%
% This file generates a video file in Matlab, in the video are 4
% subplots. The subplots are as follows:-
%
% 1. Top left - map background with a moving dot showing the current
%               position and a blue trail showing where the receiver was
%
% 2. Top right - E/N/U error of the device under test (DUT) versus
%                POSPac truth
%
% 3. Bottom left - Video from a GoPro
%
% 4. Bottom right - number of satellites tracked/used.
%
%
% This script should be called from a customized "doPlotPos.m" this
% needs to set the following variables:-
%
% SetInvisible (debug for now so set to 0)
%
% LatMargin - defines in meters how much margin around the map we want
% LonMargin - ditto for Lat
%  - Note the google earth function will not distort the map to fit the 
%    plot aspect ratio so there may be extra space around the map
%
% SaveVideo - if 1 we'll output an MP4, otherwise we won't
%
% truthFile - POSPac.txt ("Sunnyvale" custom export format from POSPac)
%
% DUTFile.txt - realtime T04 based PVT
%             viewdat -d35:2 -mb -h DUT.T04 > DUTFile.txt
%
% InputVideoFile - file name of the GoPro video file
%
% VideoTimeOffset - Time offset of the start of the video file
%
% OutputFile - filename for the video Matlab will create
%
% StartTime - GPS time of the start of the analysis
% EndTime   - GPS time of the end of the analysis
% Rate - The code assumes position is available at 10Hz, we render 50
%        frames per second in the output video file. Rate controls how
%        many 10Hz positions we skip. If this is set to 1 each frame we
%        will advance 100ms and 50 frames per second this will result in
%        5x realtime. If we set to 5 we'll only output a frame for every
%        5th position of 500ms at 50 fps this is 25x realtime.
%
% Limitations:--
%  There is minimal error checking, we only supply a single video file
%  name, there is no checking to determine whether we have gone off the
%  end of the video file. A more robust implementation would be passed
%  an array of video files and start times and transition seamlessly
%  between files.
%
% Note is it very slow to render and save a video in Matlab!
%
% Dependencies:
%  setupMap.m
%  plot_google_map.m
%  LLH2ENU.m
%  subtightplot.m
%
%
% Copyright Trimble 2014
%


% 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('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('DUTData') )
  disp([DUTFile,' not loaded - already in memory']);
else
  if(exist([DUTFile,'.dut.mat'],'file') == 2)
    disp(['!!!!Warning MAT format ',DUTFile,' file - erase if newer TXT data available!!!!']);
    load([DUTFile,'.dut.mat']);
  else
    DUTData = load(DUTFile);        
    save([DUTFile,'.dut.mat'],'DUTData');
  end
end

disp('Got Data');


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


% T02 position type strings (as a function of "fix type" parameter)
PosStrings = [
    {'Autonomous'}
    {'DGPS'}
    {'SBAS'}
    {'RTK float'}
    {'RTK fixed'}
    {'Wide-area RTK float'}
    {'Wide-area RTK fixed'}
    {'Omnistar'} % Need to check fix_info to determine which OmniSTAR service
    {'CDGPS'}
    {'Autonomous - Kalman'}
    {'DGPS - Kalman'}
    {'SBAS - Kalman'}
    {'CDGPS - Kalman'}
    {'SBAS+ - Kalman'}
    {'CDGPS+ - Kalman'}
    {'RTX (GoldenEye)'}
    {'TLite XPS Hybrid Coarse'}
    {'TLite XPS Hybrid Fine'}
    {'INS Autonomous'}
    {'INS DGNSS'}
    {'INS RTK'}
    {'INS SBAS'}
    {'INS RTX-Carrier'}
    {'INS RTX-Code'}
    {'INS OmniSTAR-Precise'}
    {'INS Dead-Reckoning'}
    {'RTX-Stinger (Code solution)'}
];

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

i = find( (TruthData(:,dTime) >= StartTime) & (TruthData(:,dTime) <= EndTime));
TruthPVT = TruthData(i,:);
[C, A, B] = intersect(DUTData(:,2),TruthPVT(:,dTime));

RealTimePVT = DUTData(A,:);
ControlPVT  = TruthPVT(B,:);

% Get the truth data for the time of interest
disp('Found Position 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,ControlPVT,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 Rate 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

subtightplot(2,2,1,[0.04,0.04]);
% Output a marker for the current receiver position based on the truth
Point = plot(ControlPVT(1,dLon), ControlPVT(1,dLat), 'r.','MarkerSize',15);
OldX = ControlPVT(1,dLon);
OldY = ControlPVT(1,dLat);

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

% Plot the position error
subtightplot(2,2,2,[0.04,0.04]);
ENU = LLH2ENU(RealTimePVT(:,13).*pi/180, ...
              RealTimePVT(:,14).*pi/180, ...
              RealTimePVT(:,15), ...
              ControlPVT(:,dLat) .* pi/180, ...
              ControlPVT(:,dLon) .* pi/180, ...
              ControlPVT(:,dHgt));

plot(ControlPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,1));
hold on;
plot(ControlPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,2),'r');
plot(ControlPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,3),'g');
grid
AxisENU = axis;
EpochLineENU = plot([ControlPVT(1,dTime) ControlPVT(1,dTime)] - ControlPVT(1,dTime), ...
                    [AxisENU(3) AxisENU(4)], ...
                    'k','linewidth',2);
legend('East','North','Up','location','best');
ylabel('(DUT - Reference) Error [m]');
Range = ControlPVT(end,dTime) - ControlPVT(1,dTime);
Str = PosStrings{RealTimePVT(1,11) + 1};
MarkerText = text(ControlPVT(1,dTime)+Range/100 - ControlPVT(1,dTime), ...
                  AxisENU(4) - AxisENU(4)/6, ...
                  Str,'color',[0 0.4 0.2],'FontWeight','bold', 'BackgroundColor',[1 1 1]);

subtightplot(2,2,4,[0.04,0.04]);
plot(ControlPVT(:,dTime) - ControlPVT(1,dTime), RealTimePVT(:,6));
hold on
plot(ControlPVT(:,dTime) - ControlPVT(1,dTime), RealTimePVT(:,7),'r');
grid;
AxisSV = axis;
EpochLineSV = plot([ControlPVT(1,dTime) ControlPVT(1,dTime)] - ControlPVT(1,dTime), ...
                   [AxisSV(3) AxisSV(4)], ...
                   'k','linewidth',2);
xlabel(['GPS Time [Seconds offset from ',num2str(ControlPVT(1,1,dTime)),'s]']);
ylabel('Number of Satellites');
legend('Tracked','Used','location','best');                    

% Loop around for each bit but jump forward based on the Rate we want
% to display
NumFrames = 0;
LastFrame = 0;
for epoch = 1:Rate:length(ControlPVT)
  
  % If we have video update with the video frame for the current epoch
  if(renderInputVideo == 1)
    NewFrame = 1 + round((ControlPVT(epoch,dTime) - ControlPVT(1,dTime) + 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
  delete(EpochLineSV);
  EpochLineSV = plot([ControlPVT(epoch,dTime) ControlPVT(epoch,dTime)] - ControlPVT(1,dTime), ...
                     [AxisSV(3) AxisSV(4)], ...
                     'k','linewidth',2);
  
  % 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);
  plot(OldX,OldY, 'b.','MarkerSize',5);
  % Output a marker for the current receiver position based on the truth
  Point = plot(ControlPVT(epoch,dLon), ControlPVT(epoch,dLat), 'r.','MarkerSize',15);
  OldX = ControlPVT(epoch,dLon);
  OldY = ControlPVT(epoch,dLat);


  % Plot the position error
  subtightplot(2,2,2,[0.04,0.04]);
  delete(EpochLineENU);
  EpochLineENU = plot([ControlPVT(epoch,dTime) ControlPVT(epoch,dTime)] - ControlPVT(1,dTime), ...
                      [AxisENU(3) AxisENU(4)], ...
                      'k','linewidth',2);
  delete(MarkerText);                      
  Str = PosStrings{RealTimePVT(epoch,11) + 1};
  MarkerText = text(ControlPVT(epoch,dTime) - ControlPVT(1,dTime)+Range/100, ...
                    AxisENU(4) - AxisENU(4)/6, ...
                    Str,'color',[0 0.4 0.2],'FontWeight','bold', 'BackgroundColor',[1 1 1]);

  % 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.1 * Rate,'%.2f'),'s']);
  end
end

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


