%
% Takes correlation data and produces a MP4  file
%
% Copyright 2007-2014 Trimble Navigation Ltd
% Stuart Riley
%


% Columns that the I & Q samples start in
IStart = 9;
QStart = 10;
IPunct = 41;
QPunct = 42;

% Read in the data ... but only if it doesn't exist in the Matlab
% memory. Also prefer a MAT format file as it is faster to load
if( exist('corrData') )
  disp([corrFile,' not loaded - already in memory']);
else
  if(exist([corrFile,'.dut.mat'],'file') == 2)
    disp(['!!!!Warning MAT format ',corrFile,' file - erase if newer TXT data available!!!!']);
    load([corrFile,'.dut.mat']);
  else
    corrData = load(corrFile);        
    save([corrFile,'.dut.mat'],'corrData','-v7.3');
  end
end

if( exist('TruthData') )
  disp([truthFile,' not loaded - already in memory']);
else
  if(exist([truthFile,'.dut.mat'],'file') == 2)
    disp(['!!!!Warning MAT format ',truthFile,' file - erase if newer TXT data available!!!!']);
    load([truthFile,'.dut.mat']);
  else
    TruthData = load(truthFile);        
    save([truthFile,'.dut.mat'],'TruthData');
  end
end

if( exist('ResidualData') )
  disp([residualFile,' not loaded - already in memory']);
else
  if(exist([residualFile,'.dut.mat'],'file') == 2)
    disp(['!!!!Warning MAT format ',residualFile,' file - erase if newer TXT data available!!!!']);
    load([residualFile,'.dut.mat']);
  else
    ResidualData = load(residualFile);        
    save([residualFile,'.dut.mat'],'ResidualData');
  end
end
disp('Got Data');


if( exist('InputVideoFile') )
  v1 = VideoReader(InputVideoFile);
  FrameRate = v1.FrameRate;
  disp('Opened output Video');
end

% POS Pac Column index
dTime     = 1;
dLat      = 2;
dLon      = 3;
dHgt      = 4;
dEastVel  = 5;
dNorthVel = 6;
dUpVel    = 7;

% Get the truth data for the time of interest
i = find( (TruthData(:,dTime) >= StartTime) & (TruthData(:,dTime) <= EndTime));
PVT = TruthData(i,:);
disp('Found Position Segment');

% Get the measurement residuals for the time of interest
i = find( (ResidualData(:,3) == PRN) & ...
          (ResidualData(:,4) == SatType) & ...
          (ResidualData(:,2) >= StartTime) & ... 
          (ResidualData(:,2) <= EndTime));
Res = ResidualData(i,:);
disp('Found Residual Segment');

hf = figure;
% Set the size of the figure, will impact the size of the MP4
set(hf,'units','pixels','outerposition',[0 30 1400 1100]);


% Setup the google map plot
setupMap(hf,SetInvisible,PVT,LonMargin,LatMargin);



if(SaveVideo == 1)
  videoObj = VideoWriter(OutputFile,'MPEG-4');
  videoObj.Quality = 100; 
  % 20ms per ACF = 50 ACF's per second. The video will always be
  % generated at 50fps, however if BitRate is greater than 1 we'll skip
  % bits/ACFs so we'll have a video faster than realtime
  videoObj.FrameRate = 50; 
  open(videoObj);
  disp('Open Output Video');
end

% Filter the correlators to only provide data for the time of interest.
i = find( (corrData(:,2) >= StartTime) & (corrData(:,2) <= EndTime));
Corrs = corrData(i,:);
disp('Found Correlator Segment');

% Find the first place 
clear Corr20ms;
clear Corr20msTimesISign;
clear metadata;

%
% Now integrate over the 20ms GPS bits
%
EpochCnt = 1;
k = (20 - Corrs(1,3)) + 1;
[Row,Col] = size(Corrs);
% Pre-allocate to speed up the loop
Corr20ms = zeros(ceil(length(Corrs)/20),Col);
while(k< (length(Corrs)-20))
  % In case of a loss of lock we can lose bit sync, re-sync by looking
  % for the millisecond count to be zero (column 3)
  while(Corrs(k,3) ~= 0)
    disp(['Resyncing Index = ',num2str(k)]);
    k = k + 1;
  end
  
  metadata.timeTag(EpochCnt)  = Corrs(k,2) + 0.019; % + 19ms to end of bit
  DeltaT = abs(Res(:,2) - metadata.timeTag(EpochCnt));
  j = find(min(DeltaT) == DeltaT);

  metadata.Residuals(EpochCnt,:) = Res(j(1),:);
  
  DeltaT = abs(PVT(:,dTime) - metadata.timeTag(EpochCnt));
  j = find(min(DeltaT) == DeltaT);
  metadata.PVT(EpochCnt,:) = PVT(j(1),:);

  % Corr20ms this 20ms
  Corr20ms(EpochCnt,:) =  Corrs(k,:);
  k = k+1;
  for i = 2:20
    Corr20ms(EpochCnt,IStart:end) = Corr20ms(EpochCnt,IStart:end) + Corrs(k,IStart:end);
    k = k+1;
  end
  EpochCnt = EpochCnt + 1;
  if(mod(EpochCnt,200) == 0)
    disp(['Processed Bit: ',num2str(EpochCnt)]);
  end
end
% as we may have dropped some bits (re-sync) erase any spare elements
% from the pre-allocated array
Corr20ms(EpochCnt:end,:) = [];

disp('Got Corr Sums');

tmp = max(max(abs(Corr20ms(:,IStart:end))));
if(tmp > YMax)
  YMax = tmp;
end

clear Point;
Time0   = metadata.timeTag(1);
TimeEnd = metadata.timeTag(end);
Range = TimeEnd - Time0;


% Crude bit flip estimator
Corr20msTimesISign = Corr20ms;
for i=1:length(Corr20ms)
  if(Corr20msTimesISign(i,IPunct) < 0)
    Corr20msTimesISign(i,IStart:2:end) = -1*Corr20ms(i,IStart:2:end);
  end
end

if( exist('InputVideoFile') )
  subtightplot(2,2,4,[0.04,0.04]);
else
  subtightplot(2,1,2,[0.04,0.04]);
end
% Draw the time series of the I/Q punct corr
plot(metadata.timeTag(:) - StartTime, Corr20ms(:,IPunct));
hold on
plot(metadata.timeTag(:) - StartTime, Corr20ms(:,QPunct),'r');
EpochLine = plot([metadata.timeTag(1) metadata.timeTag(1)] - Time0,[-YMax YMax],'k','linewidth',2);
axis( [0 (EndTime-StartTime) -YMax YMax] );
grid;
xlabel('Time [Sec]');
ylabel('P(I) & P(Q)');
legend('P(I)','P(Q)','Current Epoch','location','SouthEast');


[Row,Col] = size(Corr20ms);

% Setup the ACF sub figure, it is much quicker to setup the figure and
% then delete and replot the ACF each bit that fully drawing the figure
% each time
subtightplot(2,2,2,[0.04,0.04]);
RangeI = ((IStart:2:Col) - IPunct) .* 0.0625;
RangeQ = ((QStart:2:Col) - QPunct) .* 0.0625;
IACF = Corr20msTimesISign(:,IStart:2:end);
QACF = Corr20ms(:,QStart:2:end);

IQCorrLine = plot(RangeQ,sqrt(IACF(1,:).^2 + QACF(1,:).^2),'g');
hold on
ICorrLine = plot(RangeI,IACF(1,:));
QCorrLine = plot(RangeQ,QACF(1,:),'r');
legend('\surd(I^2+Q^2)','I.Sign(P(I))','Q','location','NorthEast');
axis([-2 2 -YMax/10 YMax]);
grid
title(['C/A Corr fn: T= ', num2str(floor(metadata.timeTag(1))),'s', ...
       ' rho(Err) = ',num2str(metadata.Residuals(1,7),'%7.1f'),'m', ... 
       ' Doppler(Err) = ',num2str(metadata.Residuals(1,8),'%6.2f'),'m/s']);
xlabel('Offset [Chips]');

% Plot a line showing the azimuth to the satellite
Line = plot( [metadata.PVT(1,dLon) metadata.PVT(1,dLon) + sin(metadata.Residuals(1,11) * pi / 180) ], ...
             [metadata.PVT(1,dLat) metadata.PVT(1,dLat) + cos(metadata.Residuals(1,11) * pi / 180) ], ...
             'y');
% Output a marker for the current receiver position based on the truth
Point = plot(metadata.PVT(1,dLon), metadata.PVT(1,dLat), 'r.','MarkerSize',15);
OldX = metadata.PVT(1,dLon);
OldY = metadata.PVT(1,dLat);

if( exist('InputVideoFile','var') )
  renderInputVideo = 1;
else
  renderInputVideo = 0;
end

clear MarkerPoint;

% Loop around for each bit but jump forward based on the BitRate we want
% to display
NumFrames = 0;
LastFrame = 0;
for epoch=1:BitRate:length(Corr20ms)
  
  % Draw the correlation function for I & Q
  subtightplot(2,2,2,[0.04,0.04]);
  delete(IQCorrLine);
  delete(ICorrLine);
  delete(QCorrLine);
  IQCorrLine = plot(RangeQ,sqrt(IACF(epoch,:).^2 + QACF(epoch,:).^2),'g');
  ICorrLine = plot(RangeI,IACF(epoch,:));
  QCorrLine = plot(RangeQ,QACF(epoch,:),'r');

  title(['C/A Corr fn: T= ', num2str(floor(metadata.timeTag(epoch))),'s', ...
         ' |\rho(Err)| = ',num2str(abs(metadata.Residuals(epoch,7)),'%.1f'),'m', ... 
         ' |Doppler(Err)| = ',num2str(abs(metadata.Residuals(epoch,8)),'%.2f'),'m/s']);

  % If we have video update with the video frame for the current epoch
  if(renderInputVideo == 1)
    NewFrame = 1 + round((metadata.timeTag(epoch) - Time0 + VideoTimeOffset)*FrameRate);
    if(NewFrame ~= LastFrame)
      % This part of the code is very slow ... if we don't need to
      % render a new frame skip this section
      subtightplot(2,2,3,[0.04,0.04]);
      image(v1.read(NewFrame));
      axis off;
      LastFrame = NewFrame;
    end
    subtightplot(2,2,4,[0.04,0.04]);
  else
    subtightplot(2,1,2,[0.04,0.04]);
  end
  
  % Update the time series by deleting the old current epoch line 
  % and draw the new one
  delete(EpochLine);
  EpochLine = plot([metadata.timeTag(epoch) metadata.timeTag(epoch)] - Time0,[-YMax YMax],'k','linewidth',2);

  % If enabled to add a marker in the correlator time series graph, the
  % old marker gets deleted and a new one is added to the graph.
  if(drawMarker == 1)
    if(exist('MarkerPoint','var'))
      delete(MarkerPoint);
      delete(MarkerText);
    end

    % Add a marker to the time series with some meta data
    MarkerPoint = plot(metadata.timeTag(epoch)-Time0,YMax - YMax/10,'ks','markerfacecolor',[0 0 0]);
    if( metadata.Residuals(epoch,6) == 16);
      Str = 'PLL';
    else
      Str = 'AFC';
    end

    % Note the "10" forces a new line in the text displayed on the
    % screen
    Str = [Str, ' C/No = ',num2str(metadata.Residuals(epoch,5)/10,'%.1f'),'dBHz',10, ...
           '\rho(Err) = ', num2str(metadata.Residuals(epoch,7),'%.1f'),'m' 10, ...
           'Dop(Err) = ',  num2str(metadata.Residuals(epoch,8),'%.2f'),'m/s' 10, ...
           'velocity = ',  num2str(sqrt(metadata.PVT(epoch,dEastVel).^2 + metadata.PVT(epoch,dNorthVel).^2),'%.2f'),'m/s'];
    
    MarkerText = text(metadata.timeTag(epoch)-Time0+Range/100,YMax - YMax/6,Str,'color',[0 0.4 0.2],'FontWeight','bold', 'BackgroundColor',[1 1 1]);
  end

  % Update the map subplot
  subtightplot(2,2,1,[0.04,0.04]);

  % Erase the position marker and satellite LOS line from the last
  % epoch, the update the position track
  delete(Point);
  delete(Line);
  plot(OldX,OldY, 'b.','MarkerSize',5);

  % Plot a line in yellow showing the azimuth to the satellite
  Line = plot( [metadata.PVT(epoch,dLon) metadata.PVT(epoch,dLon) + sin(metadata.Residuals(epoch,11) * pi / 180) ], ...
               [metadata.PVT(epoch,dLat) metadata.PVT(epoch,dLat) + cos(metadata.Residuals(epoch,11) * pi / 180) ], ...
               'y');

  % Output a marker for the current receiver position based on the truth
  Point = plot(metadata.PVT(epoch,dLon), metadata.PVT(epoch,dLat), 'r.','MarkerSize',15);
  OldX = metadata.PVT(epoch,dLon);
  OldY = metadata.PVT(epoch,dLat);

  if( metadata.Residuals(epoch,6) == 16);
    Str = 'PLL';
  else
    Str = 'AFC';
  end

  % Title for the map subplot
  title(['PRN = ',num2str(PRN), ...
         ' Elev = ',num2str(metadata.Residuals(epoch,9),'%.1f'),'deg ', ...
         ' Azi = ', num2str(metadata.Residuals(epoch,11),'%.2f'),'deg ', ...
         ' C/No = ',num2str(metadata.Residuals(epoch,5)/10,'%.1f'),'dBHz', ...
         ' Mode = ',Str]);
  
  % Save the video if enabled 
  if(SaveVideo == 1)
    F = getframe(hf);
    writeVideo(videoObj,F);
  else
    % If we aren't saving video need a small pause otherwise the image
    % doesn't get rendered
    pause(0.00001);
  end

  NumFrames = NumFrames + 1;
  if(mod(NumFrames,20) == 0)
    disp(['Processed: ',num2str(NumFrames * 0.02 * BitRate,'%.2f'),'s']);
  end
end

if(SaveVideo == 1)
  % Close the video file
  close(videoObj);
end


%
%
% Now plot a few figures and save as PNG and FIG (SavePlot).
%
%

figure
mesh(((IStart:2:Col) - IPunct) .* 0.0625,1:length(Corr20msTimesISign), Corr20msTimesISign(:,IStart:2:end))
view(330,60)
colorbar;
xlabel('Offset [Chips]');
ylabel('Bit Number');
zlabel('I.Sign(P(I))');
% SavePlot saves to a PNG and FIG file and also cleans up the font to
% make the graphs look a little better for powerpoint etc.
savePlot([FileBase,'.ICorrMesh.png'],1);


figure
mesh(((IStart:2:Col) - IPunct) .* 0.0625,1:length(Corr20msTimesISign), sqrt(IACF.^2 + QACF.^2));
view(330,60)
colorbar;
xlabel('Offset [Chips]');
ylabel('Bit Number');
zlabel('\surd(I^2+Q^2)');
% SavePlot saves to a PNG and FIG file and also cleans up the font to
% make the graphs look a little better for powerpoint etc.
savePlot([FileBase,'.IQCorrMesh.png'],1);


figure
subplot(2,1,1)
% Draw the time series of the I/Q punct corr
plot(metadata.timeTag - StartTime, Corr20ms(:,IPunct));
hold on
plot(metadata.timeTag - StartTime, Corr20ms(:,QPunct),'r');
axis( [0 (EndTime-StartTime) -YMax YMax] );
grid;
xlabel('Time [Sec]');
ylabel('P(I) & P(Q)');
legend('P(I)','P(Q)','location','SouthEast');

subplot(2,1,2)
plot(Res(:,2) - StartTime, Res(:,5)/10)
grid
xlabel('Time [Sec]');
ylabel('C/No [dBHz]');
savePlot([FileBase,'.ICorrTimeSeries.png'],1);



figure
subplot(3,1,1)
plot(Res(:,2) - StartTime, Res(:,5)/10)
grid
xlabel('Time [Sec]');
ylabel('C/No [dBHz]');

subplot(3,1,2)
plot(Res(:,2) - StartTime, Res(:,7))
grid
xlabel('Time [Sec]');
ylabel('Pseudorange Error [m]');


subplot(3,1,3)
plot(Res(:,2) - StartTime, Res(:,8))
grid
xlabel('Time [Sec]');
ylabel('Doppler Error [m/s]');
savePlot([FileBase,'.Errors.png'],1);


figure
IQ = sqrt(IACF(:,:).^2 + QACF(:,:).^2);
mesh(-2:0.125:-0.125,1:length(IACF),abs(IQ(:,1:16) - IQ(:,33:-1:18)))
title('Correlation function Asymmetry');
delta = (abs(IQ(:,1:16) - IQ(:,33:-1:18)));
savePlot([FileBase,'.Asym.png'],1);


