%******************************************************************
% $Id: doRTKInitPlot.m,v 1.5 2002/08/05 18:37:12 stuart Exp $
%
% Routine to take data from the "antenna" on/off RTK test system 
% and produce some performance plots.
%
% Note - this script has been quickly hacked so that it could be
%        compiled and provided to Terrasat from a series scripts I 
%        had. Consequently the flow and structure is not how I would
%        do it again if I had some time to start from scratch!
%
% Stuart Riley
%
% Copyright Trimble Navigation 1/16/2002
%******************************************************************


function doRTKInitPlot( inits, ...
                   NumDataSets, ...
                   DataSetStrings, ...
                   MainTitle);

%*******************************************************************
%                        Start of Defines
%*******************************************************************

FALSE = 0;
TRUE  = 1;

% Provide a mechanism to switch off the "print to a file", useful when
% debugging as it saves time when printing is off.....
doPrint = TRUE;
%doPrint = FALSE;

fitAxis = FALSE;

range     = 0:1:500; % 0 -500 seconds
range5sec = 0:5:500; % 0 -500 seconds
% We want to run the matlab hist function and compute the data in the
% region 0-500 seconds. The funtion dumps any excess data (>500) into
% the last bin. So that this does not distort the plots or stats compute
% the historam for an extra bin but do not plot/use that bin in any of
% the computation.
rangecal  = 0:1:501; 
lengthcal = length(rangecal);
range5secbins  = 0:5:505; 
length5secbins = length(range5secbins);

%*******************************************************************
%                        End of Defines
%*******************************************************************


% Get the colour codes
colour_LUT = setColourCodes;

% Figure out which is the longest string so we can pad them to all be
% the same length so the labels align...
MaxLength = max(length(char(DataSetStrings(:))));


% Go through the initialization array and account for any week wrap-over
% problems

Increment = 0;
for i=2:length(inits)
  
  % First check for a day rollover - check for the difference being between
  % 0.5 and 1.5 days.
  if(    ((inits(i,1) - inits(i-1,1)) <  -43200) ...
     & ( ((inits(i,1) - inits(i-1,1)) > -129600)))
     % We have had a day rollover possible with GGA inputs
     Increment = Increment + 86400;
   % Now check for a week rollover - check for the difference being between
   % 6 and 8 days.
   elseif (  ((inits(i,1) - inits(i-1,1)) < -6*86400) ...
         & ( ((inits(i,1) - inits(i-1,1)) > -8*86400)))
     Increment = Increment + 7*86400;
   end

   inits(i,1) = inits(i,1) + Increment;
end

% Figure out how many initializations there are and the length of the
% data set in Hours so we can add this to the title of the plot.
minTime = min(inits(:,1));
maxTime = max(inits(:,1));
runLength = (maxTime - minTime)/3600; % in hours
numInits = length(inits);

% Form the Title string
TitleString = [': ',num2str(length(inits),'%4d'),' inits; ',num2str(runLength,'%.1f'),' Hours'];


%*******************************************************************
%                     Now form the histograms....
%*******************************************************************

PlotHandles  = [];
LegendMatrix = [];


CDFFig = figure;
axis([0 500 0 100])
grid
hold on

for HistNum = 1:NumDataSets

  % Histogram
  data = hist(inits(:,HistNum+1),rangecal); % See comment at def of rangecal
  data = data(1:(lengthcal-1));

  % Compute the 90 percentile
  CDF = 100*cumsum(data)./numInits;
  overNinetyIndexVec = find( CDF >= 90 ) ;
  if ~isempty( overNinetyIndexVec )
    ninetyPercentile = rangecal( overNinetyIndexVec( 1 ) ); 
  else
    ninetyPercentile = NaN;
  end

  % Plot the data and grab the handle for the legend.
  Handle = plot(range,CDF, '-x',...
            'color', colour_LUT(HistNum,:));

  % Add the handle into the array of handles
  PlotHandles = [Handle PlotHandles];
  
  % Start the legend String with the supplied string
  String = [char(DataSetStrings(HistNum)),' '];

  % Check to see whether this is the shorter than the maximum length
  % String. We want them all to align so the test aligns in the legend.
  diffLength = MaxLength - length(char(DataSetStrings(HistNum)));
  if(diffLength > 0)
    pad = [];
    pad(1: (diffLength)) = ' '; 
    String = [String,pad];
  end
 
  % Now compute the 90 percentile and add to the plot, attempt to
  % maintain the text aligned.
  String = [String,'90%= '];
  PercentString = num2str(ninetyPercentile);
  if(length(PercentString) < 4)
    % Pad if there are less than 4 characters
    pad = [];
    pad(1: 4-length(PercentString)) = ' '; 
    String = [String,pad];
  end
  String = [String,PercentString,'sec; Max='];

  % Now compute the maximum initialization time and add to the legend.
  MaxText = num2str(max(inits(:,HistNum+1)));
  if(length(MaxText) < 5)
    % Pad if there are less than 5 characters
    pad = [];
    pad(1: 5-length(MaxText)) = ' '; 
    String = [String,pad];
  end
  String = [String,MaxText,'sec; Av='];

  % Now compute the average initialization time and add to the legend.
  AvText = num2str(mean(inits(:,HistNum+1)),'%.1f');
  if(length(AvText) < 5)
    % Pad if there are less than 5 characters
    pad = [];
    pad(1: 5-length(AvText)) = ' '; 
    String = [String,pad];
  end
  String = [String,AvText,'sec; Median='];

  % Now compute the median initialization time and add to the legend.
  MedianText = num2str(median(inits(:,HistNum+1)),'%.0f');
  if(length(MedianText) < 4)
    % Pad if there are less than 4 characters
    pad = [];
    pad(1: 4-length(MedianText)) = ' '; 
    String = [String,pad];
  end
  String = [String,MedianText,'sec'];

  LegendMatrix = [{String} LegendMatrix];

  % Now update the array which will store summary statistics.
  Init90(HistNum)     = ninetyPercentile;
  InitMax(HistNum)    = max(inits(:,HistNum+1));
  InitAv(HistNum)     = mean(inits(:,HistNum+1));
  InitMedian(HistNum) = median(inits(:,HistNum+1));
  InitNum(HistNum)    = length(inits);  % Number of inits
  InitHours(HistNum)  = runLength;      % Data set length in hours
end

%
% The CDF figure gets extra information annotated onto it and is printed last.
% 

% Label the graph
TextHandle(1) = legend( PlotHandles , LegendMatrix , 4);
% Use a non-proportial spaced font
set(TextHandle(1),'FontName','courier');
set(TextHandle(1),'FontSize',8);

xlabel('Satellite Acquisition + RTK Initialization Time [Seconds]');
ylabel('Cumulative Percentage');

title([MainTitle TitleString],'Interpreter','none');

figure
DataHandles = [];
DataLegend  = [];
for HistNum = 1:NumDataSets

  % Histogram
  data = hist(inits(:,HistNum+1),range5secbins); % See comment at def of rangecal
  data = data(1:(length5secbins-1));

  Handle = plot(range5sec,100*data./length(inits),'-x','linewidth',2,'markersize',6,'color', colour_LUT(HistNum,:));
  hold on;

  DataHandles = [Handle DataHandles];
  
  % Start the legend String with the supplied string
  DataLegend = [{[char(DataSetStrings(HistNum)),': 5 Second bins']} DataLegend];
end
legend(DataHandles,DataLegend);
title([MainTitle TitleString],'Interpreter','none');
grid on;
xlabel('Satellite Acquisition + RTK Initialization Time [Seconds]');
ylabel('Percentage');
if(doPrint == TRUE)
  print -dpng InitTimeDist.png;
  close;   % Clean up so we don't hog the memory
end


MarkerArray = [{'bo'} {'rs'} {'md'} {'cp'} {'kv'}];
figure;
hold on;
stemHandlesMat = [];
legendCell     = [];

for HistNum = 1:NumDataSets
  % Stem returns a column vector of handles to the stem symbol object and
  % the stem line object. Collect these for the legend.
  lineHandle = stem(inits(:,1), inits(:,HistNum+1),[char(MarkerArray(HistNum))]);
  stemHandlesMat = [ stemHandlesMat lineHandle ] ;

  % legendCell = [ legendCell { 'Trimble 4700' } ] ;  % Append legend text.
  legendCell = [ legendCell DataSetStrings(HistNum) ];
end

A = axis;
xmin = inits(1,1);
xmax = inits(length(inits),1);

TimeDiff = xmax - xmin;
if(TimeDiff > 24*3600)
  % For longer than a day plot lines every 6 hours.
  xmin = floor(xmin / (3600*6)) *3600 *6;
  xmax = ceil(xmax / (3600*6)) *3600 *6;
  TimeDiff = xmax - xmin;
  Labels = TimeDiff / (6*3600);
else
  % For longer than a day plot lines every 3 hours.
  xmin = floor(xmin / (3600*3)) *3600 *3;
  xmax = ceil(xmax / (3600*3)) *3600 *3;
  TimeDiff = xmax - xmin;
  Labels = TimeDiff / (3*3600);
end
A(1) = xmin;
A(2) = xmax;
axis(A);

labelAxes(Labels,gcf,axis,0,0);
grid

% Use the stem symbols (at stem ends) for the legend.
legend( stemHandlesMat( 1, :) , legendCell ) ;

xlabel('Time of initialization attempt start [GPS Time HH:MM]');
ylabel('Time taken to Initialize [Seconds]');
title([MainTitle TitleString],'Interpreter','none');
if(doPrint == TRUE)
  print -dpng InitTimeVTime.png;
  close;   % Clean up so we don't hog the memory
end

for HistNum = 1:NumDataSets
  figure;
  plot(inits(:,1), inits(:,2 + NumDataSets + (HistNum-1)*3),'r-x');
  E_sigma = std(inits(:,2 + NumDataSets + (HistNum-1)*3));
  E_mean  = mean(inits(:,2 + NumDataSets + (HistNum-1)*3));

  hold on;
  plot(inits(:,1), inits(:,3 + NumDataSets + (HistNum-1)*3),'g-x');
  N_sigma = std(inits(:,3 + NumDataSets + (HistNum-1)*3));
  N_mean  = mean(inits(:,3 + NumDataSets + (HistNum-1)*3));

  plot(inits(:,1), inits(:,4 + NumDataSets + (HistNum-1)*3),'b-x');
  U_sigma = std(inits(:,4 + NumDataSets + (HistNum-1)*3));
  U_mean  = mean(inits(:,4 + NumDataSets + (HistNum-1)*3));

  %
  % Assume a bad init is a height error > 22cm or a horizontal > 12cm
  % 
  
  i = find(  ( sqrt(  inits(:,2 + NumDataSets + (HistNum-1)*3).^2 ... 
                    + inits(:,3 + NumDataSets + (HistNum-1)*3).^2 ) > 0.12) ...
           | ( abs(inits(:,4 + NumDataSets + (HistNum-1)*3)) > 0.22) );

  numBad = length(i);
  PercentGood(HistNum) = 100 * (numInits - numBad) / numInits;

  xlabel('Time of initialization attempt start [GPS Time HH:MM]');
  ylabel('Position Error [metres]');
  legend(['East:   Sig=',num2str(E_sigma,'%.3f'),'m; Mean=',num2str(E_mean,'%.3f'),'m'],...
         ['North:  Sig=',num2str(N_sigma,'%.3f'),'m; Mean=',num2str(N_mean,'%.3f'),'m'],...
         ['Height: Sig=',num2str(U_sigma,'%.3f'),'m; Mean=',num2str(U_mean,'%.3f'),'m']);
  
  title([char(DataSetStrings(HistNum)),'; ',num2str(numInits),' Good Inits; ',num2str(numBad),' Bad Inits; Reliability = ',num2str(PercentGood(HistNum),'%.3f'),'%' ]);
  
  A = axis;
  A(1) = xmin;
  A(2) = xmax;
  axis(A);
  labelAxes(Labels,gcf,axis,0,0);
  grid;
  if(doPrint == TRUE)
    print('-dpng',[char(DataSetStrings(HistNum)),'_PosData.png']);
    close;   % Clean up so we don't hog the memory
  end
  
  % If there are some bad inits remove them and create a plot of the good
  % initializations only
  if(numBad > 0)

    % Print to the console all the bad inits
    
    disp([char(DataSetStrings(HistNum)),': Bad Init List (Init Num / Time / dE / dN / dU)'])
    for k=1:length(i)
      disp(['Init Num = ',num2str(i(k)), ...
            ' Time = ',num2str(inits(i(k),1)), ...
            ' dE = ',num2str(inits(i(k),2 + NumDataSets + (HistNum-1)*3),'%.2f'), ...
            ' dN = ',num2str(inits(i(k),3 + NumDataSets + (HistNum-1)*3),'%.2f'), ...
            ' dU = ',num2str(inits(i(k),4 + NumDataSets + (HistNum-1)*3),'%.2f')])
    end
    
    figure;

    j = find(  ( sqrt(  inits(:,2 + NumDataSets + (HistNum-1)*3).^2 ... 
                      + inits(:,3 + NumDataSets + (HistNum-1)*3).^2 ) <= 0.12) ...
             & ( abs(inits(:,4 + NumDataSets + (HistNum-1)*3)) <= 0.22) );
    plot(inits(j,1), inits(j,2 + NumDataSets + (HistNum-1)*3),'r-x');
    E_sigma = std(inits(j,2 + NumDataSets + (HistNum-1)*3));
    E_mean  = mean(inits(j,2 + NumDataSets + (HistNum-1)*3));

    hold on;
    plot(inits(j,1), inits(j,3 + NumDataSets + (HistNum-1)*3),'g-x');
    N_sigma = std(inits(j,3 + NumDataSets + (HistNum-1)*3));
    N_mean  = mean(inits(j,3 + NumDataSets + (HistNum-1)*3));

    plot(inits(j,1), inits(j,4 + NumDataSets + (HistNum-1)*3),'b-x');
    U_sigma = std(inits(j,4 + NumDataSets + (HistNum-1)*3));
    U_mean  = mean(inits(j,4 + NumDataSets + (HistNum-1)*3));

    xlabel('Time of initialization attempt start [GPS Time HH:MM]');
    ylabel('Position Error [metres]');

    legend(['East:   Sig=',num2str(E_sigma,'%.3f'),'m; Mean=',num2str(E_mean,'%.3f'),'m'],...
           ['North:  Sig=',num2str(N_sigma,'%.3f'),'m; Mean=',num2str(N_mean,'%.3f'),'m'],...
           ['Height: Sig=',num2str(U_sigma,'%.3f'),'m; Mean=',num2str(U_mean,'%.3f'),'m']);
           
    title(['Good Inits. only: ',char(DataSetStrings(HistNum)),'; ',num2str(numInits),' Good Inits; ',num2str(numBad),' Bad Inits; Reliability = ',num2str(PercentGood(HistNum),'%.3f'),'%' ]);
  
    A = axis;
    A(1) = xmin;
    A(2) = xmax;
    axis(A);
    labelAxes(Labels,gcf,axis,0,0);
    grid;
    if(doPrint == TRUE)
      print('-dpng',[char(DataSetStrings(HistNum)),'_PosData_GoodOnly.png']);
      close;   % Clean up so we don't hog the memory
    end
  end
  InitEMean(HistNum)  = E_mean;
  InitNMean(HistNum)  = N_mean;
  InitUMean(HistNum)  = U_mean;
  InitESigma(HistNum) = E_sigma;
  InitNSigma(HistNum) = N_sigma;
  InitUSigma(HistNum) = U_sigma;

  % Now produce a series of position histograms and plot them....
  range = -0.2:0.005:0.2;
  len = length(range) - 1;

  for component=1:3
    figure
  
    N=histc(inits(:,component + 1 + NumDataSets + (HistNum-1)*3),range);
    compData(component,:) = (100*N(2:len)./numInits)';
    % do not plot the last and first points as any data outside of the defined
    % range gets included in these points.
    %bar(range(2:len),100*N(2:len)./numInits,1);
    bar(range(2:len),compData(component,:),1);

    A = axis;
    % 
    % Find the first and last zero so we can figure out the best axis.
    %
    S = length(N);
    START = TRUE;
  
    for i=1:S
      if(N(i) ~= 0)
        if(START == TRUE)
          FirstNonZero = i;
          START = FALSE;
        end
        LastZero = i;
      end    
    end
  
    % Closest mm for fixed RTK
    if(fitAxis == TRUE)
      MinX = floor(range(FirstNonZero)*1000)/1000;
      MaxX = ceil(range(LastZero)*1000)/1000;
      A(1) = MinX;
      A(2) = MaxX;
      axis(A);
    end

    ylabel('Number of Epochs [%]');
    grid
  
    % Compute the RMS
    Truth  = 0.0;
    SumAll = 0.0;
  
    for i=1:numInits
      SumAll = SumAll + (inits(i,component + 1 + NumDataSets + (HistNum-1)*3) - Truth)^2;
    end
  
    RMS = sqrt( SumAll / (numInits - 1));
     
    if(component == 1)
      xlabel('East Position Error [m]');
      xString = [char(DataSetStrings(HistNum)), ...
            ': East - Sigma = ',num2str(E_sigma,2), ...
            'm; Mean Error = ',num2str(E_mean,2),...
            'm'];
            %'m; RMS = ',num2str(RMS,2), ...
      title(xString);
      if(doPrint == TRUE)
        print('-dpng',[char(DataSetStrings(HistNum)),'_East_Hist.png']);
        close;   % Clean up so we don't hog the memory
      end
    elseif(component == 2)
      xlabel('North Position Error [m]');
      yString = [char(DataSetStrings(HistNum)), ...
            ': North - Sigma = ',num2str(N_sigma,2), ...
            'm; Mean Error = ',num2str(N_mean,2),...
            'm'];
            % 'm; RMS = ',num2str(RMS,2), ...
      title(yString);
      if(doPrint == TRUE)
        print('-dpng',[char(DataSetStrings(HistNum)),'_North_Hist.png']);
        close;   % Clean up so we don't hog the memory
      end
    else
      xlabel('Height Position Error [m]');
      zString = [char(DataSetStrings(HistNum)), ...
            ': Height - Sigma = ',num2str(U_sigma,2), ...
            'm; Mean Error = ',num2str(U_mean,2),...
            'm'];
            % 'm; RMS = ',num2str(RMS,2), ...
      title(zString);
      if(doPrint == TRUE)
        print('-dpng',[char(DataSetStrings(HistNum)),'_Height_Hist.png']);
        close;   % Clean up so we don't hog the memory
      end
    end
  end
  
  Es(HistNum) = E_sigma;
  Ns(HistNum) = N_sigma;
  Us(HistNum) = U_sigma;
  
  % Now plot dE/dN/dU as a histogram on the same graph.
  figure
  bar(range(2:len),compData','group');
  legend(xString,yString,zString);
  grid on
  title([MainTitle TitleString],'Interpreter','none');
  xlabel('East/North/Height Position Error [m]');
  ylabel('Number of Epochs [%]');
  if(doPrint == TRUE)
    print('-dpng',[char(DataSetStrings(HistNum)),'_ENU_Hist.png']);
    close;   % Clean up so we don't hog the memory
  end
end

figure(CDFFig);

for HistNum = 1:NumDataSets
  str(HistNum) = {[char(DataSetStrings(HistNum)), ...
                  ':  Rel. = ',num2str(PercentGood(HistNum),'%.3f'),'%; ', ...
                  'East Sig = ',num2str(Es(HistNum),'%.3f'),'m; ', ...
                  'North Sig = ',num2str(Ns(HistNum),'%.3f'),'m; ', ...
                  'Height Sig = ',num2str(Us(HistNum),'%.3f'),'m; ']};
end
text(30,25,str);

if(doPrint == TRUE)
  print -dpng InitTimeCDF.png
  close;   % Clean up so we don't hog the memory
end

%
% Finally print all the results to a file.
%
fid = fopen( 'stats.csv', 'wt' ) ;
fprintf(fid,'Test Title,Data set,Run Time (hour),Reliability [%%],90%% Init [Sec],Max Init [Sec],Mean. Init [Sec],Median Init [Sec],Num Inits,E-Mean [m],E-Sigma [m],N-Mean [m],N-Sigma [m],U-Mean [m],U-Sigma [m]\n');
for HistNum = 1:NumDataSets
  fprintf(fid,'%s,%s,%f,%f,%f,%d,%f,%d,%d,%f,%f,%f,%f,%f,%f\n', ...
         MainTitle, ...
         char(DataSetStrings(HistNum)), ...
         InitHours(HistNum), ...
         PercentGood(HistNum), ...
         Init90(HistNum), ...
         round(InitMax(HistNum)), ...
         InitAv(HistNum), ...
         floor(InitMedian(HistNum)), ...
         InitNum(HistNum), ...
         InitEMean(HistNum), ...
         InitESigma(HistNum), ...
         InitNMean(HistNum), ...
         InitNSigma(HistNum), ...
         InitUMean(HistNum), ...
         InitUSigma(HistNum));
end
fclose(fid);


