function plotPosVideo (varargin)
% plotPosVideo: Generates video files for the Sunnyvale van runs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file generates a video file in Matlab, in the video are 5
% subplots. The subplots are as follows:
%
% 1. map background with a moving dot showing the current position and a 
%    blue trail showing where the receiver was
%
% 2. E/N/U error of the device under test (DUT) versus POSPac truth along 
%    with Stinger ENU error
%
% 3. Number of satellites tracked/used
%
% 4. Video from a dash camera
%
% 5. Optional, Video from FishEye camera looking upward (with SV's 
%    projected into the frame, if we have modeled the field of view)
% 
% See the SAMPLE INI for information on setup
% 
% Limitations:--
%  There is minimal error checking
%  It almost certainly won't handle week rollovers
%
% Note is it very slow to render and save a video in Matlab!
%
% Dependencies:
%  plot_google_map.m
%  LLH2ENU.m
%  subtightplot.m
%
% Copyright Trimble 2014-2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SAMPLE INI:
%
% % BEGIN "plotPosVideo" INI
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Set the input GNSS-INS files
% 
% % POSPac Truth (use the "Sunnyvale" custom export format from POSPac)
% truthFile='E:\Testing\SVQA\TRIPS\2019_09-Tokyo\20190919-DAY11\drive01\POSPac\export_PP.txt';
% 
% % POSLV realtime (optional, leave empty if none)
% % (use the "Sunnyvale" custom export format from POSPac)
% POSLV_RTFile='E:\Testing\SVQA\TRIPS\2019_09-Tokyo\20190919-DAY11\drive01\POSPac\export_RealTime.txt';
% 
% % T04 file
% T04File='E:\Testing\SVQA\TRIPS\2019_09-Tokyo\20190919-DAY11\drive01\BX940\BX940.T04';
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Set the input video files & settings
% 
% % Video layout Method: 1 = Dash Cam only (no fisheye)
% %                      2 = Dash Cam & Fisheye Cam (No FOV SV modeling)
% %                      3 = Dash Cam with SNVL Fisheye (+FOV SV Modeling)
% %                      4 = DVB Cam (+FOV SV Modeling, load combined video in FishEyeVideoDIR)
% LayoutMethod = 4;
% 
% % Dash video directory & time offsets (if the video lags GPS time, the offset is negative)
% DashVideoDIR='';
% DashVideoOffset=0;
% 
% % Fisheye video directory & time offsets (if the video lags GPS time, the offset is negative)
% FishEyeVideoDIR='E:\Testing\SVQA\TRIPS\2019_09-Tokyo\20190919-DAY11\drive01\video\DVBcam';
% FishEyeVideoOffset=0;
% 
% % video time settings method (1=extracted from video, 2=from video filename: 'yyyy:mm:dd HH:MM:SS')
% VideoTimeMethod=2;
% 
% % Time zone and dAT (TAI-UTC) offset (from: http://maia.usno.navy.mil/ser7/ser7.dat) 
% TimeZone=9;
% dAT=37;
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Set the output video settings
% 
% %--- Define the period of time for the analysis - Start & End times in GPS Seconds
% StartTime = 383400;
% EndTime = 383722;
% 
% % Videos can be split into segments of "SplitTime" length (off = "inf")
% SplitTime = inf;
% 
% % This switch will save a video file.
% % If this is off ("0"), debug buttons for pause, skip forward/back & debug times will be displayed.
% SaveVideo = 1;
% 
% % set the output video file
% OutputFile = 'E:\Testing\SVQA\TRIPS\2019_09-Tokyo\20190919-DAY11\drive01\video\Tokyo-01.mp4';
% 
% % Video will be rendered at "FrameRate" frames per second.
% FrameRate = 24;
% 
% % "DecRate" is the position decimation rate, it means output
% % only every Nth position will be used in the video generation.
% DecRate = 10;
% 
% % The length of the video in "realtime seconds" is:
% %
% %       (EndTime - StartTime)
% %  -------------------------------
% %  DecRate * EpochRate * FrameRate
% %
% % Where "EpochRate" is computed by the script (the rate of the POSPac Truth data).
% % For a video with 800 seconds of real time (EndTime - StartTime = 800)
% % with DecRate set to 5 and a 10Hz file the resulting video will take 32
% % seconds to view and we'll render 1,600 frames (a new frame for every
% % 0.5 seconds of realtime data at 50 frames a second).
% 
% % Set margin (in meters) around the outer limits of the Google Earth Map.
% LatMargin = 50;
% LonMargin = 50;
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % END "plotPosVideo" INI
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load processing settings from an INI

% load VARARGIN
pathed_INI='';
CacheDir='';
APIKey='';
if nargin>0
    if mod(nargin,2)~=0
        fprintf('Incorrect VARARGIN length!\n');
        return;
    else
        n=0;
        while n < nargin
            if strcmpi(varargin{n+1},'INIpath')
                pathed_INI=varargin{n+2};
            elseif strcmpi(varargin{n+1},'CacheDir')
                CacheDir=varargin{n+2};
            elseif strcmpi(varargin{n+1},'APIKey')
                APIKey=varargin{n+2};
            else
                fprintf('Unknown VARARGIN: %s\n',varargin{n+1});
                return;
            end
            n=n+2;
        end
    end
end

% prompt for INI if not set
if isempty(pathed_INI)
    % user prompt for INI file
    [INI_file,INIpathname]=uigetfile({'*.ini', 'INI Files (*.ini)';
        '*.*', 'All Files (*.*)'},'Select an INI File');
    warning off;
    pathed_INI=strcat(INIpathname,INI_file);      % concatinate strings for filename
    warning on;
end

global DUTStr;
global MarkerIndex;

% load INI file
fid = fopen(pathed_INI, 'rt');
fdata=dir(pathed_INI);
if (fid == -1)
    fprintf('Cannot open %s\n', pathed_INI);
    return;
elseif (fdata.bytes==0)
    fprintf('INI file is empty!\n');
    return;
else
    while ~feof(fid)
        readline=fgetl(fid);
        if (isempty(readline)==0)
            eval(readline);
        end
    end
    fclose(fid);
end
fprintf('Loaded file: %s\n',pathed_INI);

t0=clock;         % start the process timer
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start loading data

% POSPac Truth Data
[path,name,~] = fileparts(truthFile);
truth_MAT=sprintf('%s\\%s_video.mat',path,name);
if(exist(truth_MAT,'file') == 2)
    disp(['Loading MAT format POSPac file - erase if newer data available!!!!',truthFile]);
    load(truth_MAT);
else
    disp(['Loading POSPac file: ',truthFile]);
    TruthData = load(truthFile);
    save(truth_MAT,'TruthData');
end


POSLV_RTData = NaN; % unless we discover otherwise
% POS LV real-time data
if(exist(POSLV_RTFile,'file') == 2)
    [path,name,~] = fileparts(POSLV_RTFile);
    POSLV_RT_MAT=sprintf('%s\\%s_video.mat',path,name);
    if(exist(POSLV_RT_MAT,'file') == 2)
        disp(['Loading MAT format POS LV file - erase if newer data available!!!!',POSLV_RTFile]);
        load(POSLV_RT_MAT);
    else
        disp(['Loading POS LV file: ',POSLV_RTFile]);
        POSLV_RTData = load(POSLV_RTFile);
        save(POSLV_RT_MAT,'POSLV_RTData');
    end
end

for i=1:length(T04File)
  % PVT data from the record 35:2
  [path,name,~] = fileparts(T04File{i});
  pos_MAT=sprintf('%s\\%s_pos_video.mat',path,name);
  if(exist(pos_MAT,'file') == 2)
      disp(['Loading MAT format PVT file - erase if newer data available!!!!',T04File{i}]);
      load(pos_MAT);
      DUTData{i} = Pos;
      clear Pos;
  else
      disp(['Loading PVT file: ',T04File{i}]);
      tmp_pos_file='tmp_pos_file_to_be_deleted.mat';
      cmd=sprintf('ViewDAT -d35:2 -h --mat="%s" "%s"',tmp_pos_file,T04File{i});
      [status,result]=dos(cmd);
      if status
          disp(result);
          fprintf('ViewDAT failed to run!\n\n');
          return;
      end

      Pos = load_viewdat(tmp_pos_file);
      DUTData{i} = Pos;
      delete(tmp_pos_file);
      save(pos_MAT,'Pos');
      clear Pos;
  end
end


% Disabled for now - modern T04 files don't include 33:14 data (update
% the script to use 35:16).
ProcessStinger = 0;
StingerData = NaN;
if(ProcessStinger)
  % Stinger PVT data from the record 33:14
  [path,name,~] = fileparts(T04File);
  stinger_MAT=sprintf('%s\\%s_stinger_video.mat',path,name);
  if(exist(stinger_MAT,'file') == 2)
      disp(['Loading MAT format Stinger PVT file - erase if newer data available!!!!',T04File]);
      load(stinger_MAT);
  else
      disp(['Loading Stinger PVT file: ',T04File]);
      tmp_stinger_file='tmp_stinger_file_to_be_deleted.d35';
      cmd=sprintf('ViewDAT -d35:16 -mb -h -o"%s" "%s"',tmp_stinger_file,T04File);
      [status,result]=dos(cmd);
      if status
          disp(result);
          fprintf('ViewDAT failed to run!\n\n');
          return;
      end
      StingerData = load(tmp_stinger_file);
      if isempty(StingerData)
          delete(tmp_stinger_file);  % delete the first temp file
          
          tmp_stinger_file='tmp_stinger_file_to_be_deleted.d33';
          cmd=sprintf('ViewDAT -d33:14 -mb -h -o"%s" "%s"',tmp_stinger_file,T04File);
          [status,result]=dos(cmd);
          if status
              disp(result);
              fprintf('ViewDAT failed to run!\n\n');
              return;
          end
          StingerData = load(tmp_stinger_file);
      else
          % change layout to match the old format
          StingerData=StingerData(:,[1 2 11 12 13]);
          StingerData(:,[3 4])=StingerData(:,[3 4]).*pi/180;
      end
      delete(tmp_stinger_file);
      save(stinger_MAT,'StingerData');
  end
end

% observable data (Optional Obs information from the record 27)
ProcessObs = 1;
OBSData=[];
TRK_PLL=[];

disp('Try to process Obs')
%%if( (exist(FishEyeVideoDIR,'dir') == 7) | (LayoutMethod > 2))          % If we don't have a Fish Eye (which is FOV modeled) file we don't need the Obs file
if(ProcessObs)
    ProcessObs = 1;
    % We want to also process an observable file
    [path,name,~] = fileparts(T04ObsFile);
    OBS_MAT=sprintf('%s\\%s_obs_video.mat',path,name);
    if(exist(OBS_MAT,'file') == 2)
        disp(['Loading MAT format OBS file - erase if newer data available!!!!',T04ObsFile]);
        load(OBS_MAT);
    else
        disp(['Loading OBS file: ',T04ObsFile]);
        tmp_obs_file='tmp_obs_file_to_be_deleted.mat';
        % Save to a .mat file (much faster to read the data). Also the
        % previous method hung if the output file was too large. 
        cmd=sprintf('ViewDAT -d27 --translate_rec35_sub9_to_rec27 --translate_rec35_sub19_to_rec27 --fix_b1_geo_phase --mat=%s -h "%s"',tmp_obs_file,T04ObsFile);
        [status,result]=dos(cmd);
        if status
            disp(result);
            fprintf('ViewDAT failed to run!\n\n');
            return;
        end
        % Read the .mat file created by viewdat. It is transposed and
        % uncompressed. We'll later save a compressed version making
        % sure we use a modern matlab format that can handle >2GB of
        % data (the file will be smaller as it is compressed).
        OBSData = load_viewdat(tmp_obs_file);
        delete(tmp_obs_file);

        save(OBS_MAT,'OBSData','-v7.3'); % Need version 7.3 for data greater than 2GB
    end

    Obs(:,1) = OBSData(:,1);
    % Multiply the system by 1000 and add the SVID to get a unique designator
    Obs(:,2) = OBSData(:,8)*1000 + OBSData(:,6);
    TRK(:,1) = unique(Obs(:,1));

    startIndex = 1;
    for index=1:length(TRK)
      % To speed the processing look in 1000 element windows. The data will
      % be a softed list, we know we have less than 1000 channels so this
      % should be a much bigger window than we need. Keep pushing the window
      % forward as we extract from the array.
      endIndex = startIndex + 1000;
      if(endIndex > length(Obs))
        endIndex = length(Obs);
      end
      i = find(Obs(startIndex:endIndex,1) == TRK(index,1));

      numSVs = unique(Obs(i-1+startIndex,2));
      TRK(index,2) = length(numSVs);
      startIndex = startIndex + i(end) ;
    end  

    % Decimate the data to 10Hz
    TRK(:,1) = round(TRK(:,1) * 10)/10; 
    index = find(diff(TRK(:,1)) ~= 0);
    TRK_PLL = TRK(index,:);
    clear Obs, TRK;
end

disp('Got OBS Data');

% Open the Dash cam video file and get the frame rate.
renderDashVideo = 0;
dash_video=[];
if( exist(DashVideoDIR,'dir') == 7) && (LayoutMethod < 4)      % Dashcam is included in the DVBcam (loaded as Fisheye)
    disp('Opening Dash Camera Video:');
    dash_video = video_time (DashVideoDIR,DashVideoOffset,VideoTimeMethod,TimeZone,dAT,StartTime,EndTime);
    renderDashVideo = 1;
end

% open the FishEye (roof) camera file
renderFishEyeVideo = 0;
fisheye_video=[];
if( exist(FishEyeVideoDIR,'dir') == 7) && (LayoutMethod > 1)
    % Open the fisheye video file, assumption is the frame rate is the same
    % as the dash cam (we're over writing the variable), if that changes
    % we'll need to read the frame rate and account for that when we grab
    % data from this file
    disp('Opening Fisheye Camera Video:');
    fisheye_video = video_time (FishEyeVideoDIR,FishEyeVideoOffset,VideoTimeMethod,TimeZone,dAT,StartTime,EndTime);
    renderFishEyeVideo = 1;
end

disp('Got Video');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% call the video processing function

if StartTime+SplitTime > EndTime
    times=[StartTime EndTime];
else
    times=StartTime:SplitTime:EndTime;
    if times(end)~=EndTime
        times=[times EndTime];    % run to the end time if that wasn't set
    end
end

db_close=0;
[PATHSTR,NAME,EXT] = fileparts(OutputFile);
for i=1:length(times)-1
    if length(times) == 2
        TmpOutputFile=OutputFile;
    else
        tmpSTR1=sprintf('00000%.0f',times(i));
        tmpSTR2=sprintf('00000%.0f',times(i+1));
        TmpOutputFile=sprintf('%s\\%s-%s_to_%s%s',PATHSTR,NAME,tmpSTR1(end-5:end),tmpSTR2(end-5:end),EXT);
    end
    
    db_close=build_video(times(i),times(i+1),TmpOutputFile,LayoutMethod,SaveVideo,FrameRate,DecRate,...
        LatMargin,LonMargin,CacheDir,APIKey,TruthData,POSLV_RTData,DUTData,StingerData,...
        ProcessObs,OBSData,TRK_PLL,renderDashVideo,dash_video,renderFishEyeVideo,fisheye_video);
    
    if db_close
        return;
    end
end

fprintf('Video Processing Completed, Runtime: %.1f seconds (%.1f hours)\n',etime(clock,t0),etime(clock,t0)/3600);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;


function [db_close] = build_video(StartTime,EndTime,OutputFile,LayoutMethod,SaveVideo,FrameRate,DecRate,...
    LatMargin,LonMargin,CacheDir,APIKey,TruthData,POSLV_RTData,DUTData,StingerData,...
    ProcessObs,OBSData,TRK_PLL,renderDashVideo,dash_video,renderFishEyeVideo,fisheye_video)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% start processing data
db_close=0;         % switch to close when debugging
t1=clock;           % start the segment timer

% T0x 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 (GVBS)'}
    {'RTX-Fast (RAIN)'}
    {'xFill-RTX'}
    {'RTX-RangePoint'}
    {'RTX-ViewPoint'}
    {'RTX-FieldPoint'}
    {'RTX-Fast (RAM)'}
    {'Titan Autonomous'}
    {'Titan DGNSS'}
    {'Titan SBAS'}
    {'SLAS'}
    {'SLAS - Kalman'}
];

% POS Pac Column index
dTime     = 1;
dLat      = 2;
dLon      = 3;
dHgt      = 4;
dEastVel  = 5;
dNorthVel = 6;
dUpVel    = 7;
dEastSig  = 8;
dNorthSig = 9;
dUpSig    = 10;
dHeading  = 13;

% Time align the POSPac truth data and the output position data
i = find( (TruthData(:,dTime) >= StartTime) & (TruthData(:,dTime) <= EndTime));
TruthPVT = TruthData(i,:);

for j=1:length(DUTData)
  [~, A, B] = intersect(DUTData{j}(:,2),TruthPVT(:,dTime));

  if isempty(B)
      disp('No Truth Data in this Segment');
      return;
  elseif isempty(A)
      disp('No UUT Data in this Segment');
      return;
  else
      RealTimePVT{j} = DUTData{j}(A,:);
      UUTRate=median(diff(RealTimePVT{j}(:,2)));
      fprintf('Epoch rate for UUT: %.3f\n',UUTRate);
      
      ControlPVT{j}  = TruthPVT(B,:);
      CtrlRate=median(diff(ControlPVT{j}(:,dTime)));
      fprintf('Epoch rate for Truth: %.3f\n',CtrlRate);
  end

  if ProcessObs
      % As we will run lots of "finds" on the Obs data filter it to only give
      % data between the start and sop epochs... could filter further by the
      % rendering rate
      i = find((OBSData(:,1) >= StartTime) & (OBSData(:,1) <= EndTime));
      OBSDataFilt = OBSData(i,:);
      
     i = find((TRK_PLL(:,1) >= StartTime) & (TRK_PLL(:,1) <= EndTime));
     TRK_PLL_Filt = TRK_PLL(i,:);
      
     ObsRate=median(diff(TRK_PLL(:,dTime)));

      fprintf('Epoch rate for OBS: %.3f\n',ObsRate);
  else
    ObsRate = 0.1;
  end

end


% Get the truth data for the time of interest
disp('Found Position Segment');

% get epoch rate for the POSPac truth
EpochRate=median(diff(TruthData(:,dTime)));
fprintf('Epoch rate for truth: %.3f\n',EpochRate);

% open the figure
hf = figure;
set(hf,'ToolBar','none');
set(hf,'MenuBar','none');

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

% some debug controls
if(SaveVideo == 0)
    
    % position vector
    i=0;
    height=20;
    width=200;
    units='pixels';
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height window_dim(3) height], ...
        'BackgroundColor',get(0,'DefaultUicontrolBackgroundColor'), ...
        'String','debug string ...', ...
        'Style','text', ...
        'HorizontalAlignment','left', ...
        'Tag','statictext1');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Close', ...
        'Style','togglebutton', ...
        'Tag','togglebutton2', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Pause', ...
        'Style','togglebutton', ...
        'Tag','togglebutton3', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Skip back 100 frames', ...
        'Style','togglebutton', ...
        'Tag','togglebutton4', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Skip back 10 frames', ...
        'Style','togglebutton', ...
        'Tag','togglebutton5', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Skip forward 10 frames', ...
        'Style','togglebutton', ...
        'Tag','togglebutton6', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Skip forward 100 frames', ...
        'Style','togglebutton', ...
        'Tag','togglebutton6', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
    
    i=i+1;
    han{i}=uicontrol('Parent',hf, ...
        'Units',units, ...
        'Position',[0 i*height-height width height], ...
        'String','Skip forward 1000 frames', ...
        'Style','togglebutton', ...
        'Tag','togglebutton7', ...
        'HorizontalAlignment','left', ...
        'Callback',' ');
end

% Now plot the estimated error from POSPac to give an idea of the
% quality of the "truth" data
global MarkerIndex;

plot(ControlPVT{MarkerIndex}(:,dTime), ControlPVT{MarkerIndex}(:,dEastSig));
hold on;
plot(ControlPVT{MarkerIndex}(:,dTime), ControlPVT{MarkerIndex}(:,dNorthSig),'r');
plot(ControlPVT{MarkerIndex}(:,dTime), ControlPVT{MarkerIndex}(:,dUpSig),'g');
grid
xlabel('GPS Time [Sec]');
ylabel('Estimated 1-\sigma Error [m]');
legend('East','North','Height','Location','Best');
title('POSPac PostProcessed Estimated Error');
%print -dpng POSPacErr.png

% Setup the google map plot
if LayoutMethod==1
    subtightplot(2,2,1,[0.04,0.04]);                  % Axis: Google Map
elseif LayoutMethod > 1
    subtightplot(2,3,1,[0.04,0.04]);
else
    disp('Unknown "LayoutMethod"');
    return;
end

lat = mean(ControlPVT{MarkerIndex}(:,dLat)) * pi / 180;
%lon = mean(ControlPVT(:,dLon)) * pi / 180;

a  = 6378137.0; % Earth radius
f  = 1 / 298.257223563 ;
e2 = 2*f - f*f ;

% Earth flattening
W = sqrt( 1 - e2 * sin(lat) * sin(lat)) ;
N = a / W ;
M = a * ( 1 - e2 ) / (W * W * W) ;
NcosRefLat = N * cos(lat);

% Compute the degrees / metres at the current position
LatScale   = 180 / (M*pi);
LongScale  = 180 / (NcosRefLat*pi);

axis_lim(1) = min(ControlPVT{MarkerIndex}(:,dLon)) - LongScale*LonMargin;
axis_lim(2) = max(ControlPVT{MarkerIndex}(:,dLon)) + LongScale*LonMargin;
axis_lim(3) = min(ControlPVT{MarkerIndex}(:,dLat)) - LatScale*LatMargin;
axis_lim(4) = max(ControlPVT{MarkerIndex}(:,dLat)) + LatScale*LatMargin;
axis(axis_lim);
disp('Getting satellite map data from Google');
if isempty(CacheDir) || isempty(APIKey)
    plot_google_map('maptype','satellite');
else
    plot_google_map('maptype','satellite','CacheDir',CacheDir,'APIKey',APIKey);
end
disp('Got Sat Map');

% build the video object
if(SaveVideo == 1)
  videoObj = VideoWriter(OutputFile,'MPEG-4');
  videoObj.Quality = 100; 
  videoObj.FrameRate = FrameRate;  % Set output frames per second
  open(videoObj);
  disp('Open Output Video');
end

if LayoutMethod==1
    subtightplot(2,2,1,[0.04,0.04]);                  % Axis: Google Map
elseif LayoutMethod > 1
    subtightplot(2,3,1,[0.04,0.04]);
end
% Output a marker for the current receiver position based on the truth
Point = plot(ControlPVT{MarkerIndex}(1,dLon), ControlPVT{MarkerIndex}(1,dLat), 'r.','MarkerSize',15);
OldX = ControlPVT{MarkerIndex}(1,dLon);
OldY = ControlPVT{MarkerIndex}(1,dLat);

% Plot the position error
if LayoutMethod==1
    subtightplot(2,2,2,[0.04,0.04]);                  % Axis: Position Error
elseif LayoutMethod > 1
    subtightplot(4,3,2:3,[0.04,0.04]);
end
l_text={};
t_text='';


colorMap = {'r','b','c','m','g','#ffa500'};

for j=1:length(RealTimePVT)
  ENU = LLH2ENU(RealTimePVT{j}(:,13).*pi/180, ...
                RealTimePVT{j}(:,14).*pi/180, ...
                RealTimePVT{j}(:,15), ...
                ControlPVT{j}(:,dLat) .* pi/180, ...
                ControlPVT{j}(:,dLon) .* pi/180, ...
                ControlPVT{j}(:,dHgt));

  % Plot the type 35:2 error
  plot(ControlPVT{j}(:,dTime) - ControlPVT{j}(1,dTime), sqrt(ENU(:,1).^2 + ENU(:,2).^2),'color',colorMap{(j*2)-1},'linewidth',2)
  hold on;
  plot(ControlPVT{j}(:,dTime) - ControlPVT{j}(1,dTime), ENU(:,3),'color',colorMap{j*2},'linewidth',2);

  set(gca,'XLim',[StartTime - ControlPVT{j}(1,dTime)  EndTime - ControlPVT{j}(1,dTime)])
  RMS_DUT_2D = rms(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
  RMS_DUT_UP = rms(ENU(:,3));

  Horz = sort(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
  len = length(Horz);
  if len > 0
      disp(['INS/GNSS Record 35:2 - 2D: 68%=',...
          num2str(Horz( round(len*0.68)),'%.2f'), ...
          ' 95%=',num2str(Horz( round(len*0.95)),'%.2f'), ...
          ' 98%=',num2str(Horz( round(len*0.98)),'%.2f'), ...
          ' 99%=',num2str(Horz( round(len*0.99)),'%.2f'), ...
          ' 100%=',num2str(Horz(end),'%.2f')]);
      Up = sort(abs(ENU(:,3)));
      len = length(Up);
      disp(['INS/GNSS Record 35:2 - Up: 68%=',num2str(Up( round(len*0.68)),'%.2f'), ...
          ' 95%=',num2str(Up( round(len*0.95)),'%.2f'), ...
          ' 98%=',num2str(Up( round(len*0.98)),'%.2f'), ...
          ' 99%=',num2str(Up( round(len*0.99)),'%.2f'), ...
          ' 100%=',num2str(Up(end),'%.2f')]);
      
    global DUTStr;

    l_text{end+1}=[DUTStr{j},' - 2D'];
    l_text{end+1}=[DUTStr{j},' - Up'];
    t_text=sprintf('RMS -- GNSS/INS: 2D = %.2f Up = %.2f',RMS_DUT_2D,RMS_DUT_UP);
  end
end

% - Hack - had to set POSLV_RTData to be NaN, the variable needs to
% exist to call this function.
if( (exist('POSLV_RTData','var') == 1) & ~isnan(POSLV_RTData))
  [~, A, B] = intersect(POSLV_RTData(:,dTime),TruthPVT(:,dTime));
  if isempty(A) || isempty(B)
      disp('No POS LV Data matched in this segment, skipping POS LV.');
  else
      RealTimePOSLV  = POSLV_RTData(A,:);
      ControlLVPVT = TruthPVT(B,:);
      
      ENU = LLH2ENU(RealTimePOSLV(:,dLat).*pi/180, ...
          RealTimePOSLV(:,dLon).*pi/180, ...
          RealTimePOSLV(:,dHgt), ...
          ControlLVPVT(:,dLat) .* pi/180, ...
          ControlLVPVT(:,dLon) .* pi/180, ...
          ControlLVPVT(:,dHgt));
      
      plot(ControlLVPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,1),'y','linewidth',4);
      plot(ControlLVPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,2),'color',[1 0.5 0],'linewidth',4);
      plot(ControlLVPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,3),'color',[0.7 0.7 0.7],'linewidth',4);
      
      RMS_POSLV_2D = rms(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
      RMS_POSLV_UP = rms(ENU(:,3));
      Horz = sort(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
      len = length(Horz);
      if len > 0
          disp(['POSLV Realtime - 2D: 68%=',num2str(Horz( round(len*0.68)),'%.2f'), ...
              ' 95%=',num2str(Horz( round(len*0.95)),'%.2f'), ...
              ' 98%=',num2str(Horz( round(len*0.98)),'%.2f'), ...
              ' 99%=',num2str(Horz( round(len*0.99)),'%.2f'), ...
              ' 100%=',num2str(Horz(end),'%.2f')]);
          Up = sort(abs(ENU(:,3)));
          len = length(Up);
          disp(['POSLV Realtime - Up: 68%=',num2str(Up( round(len*0.68)),'%.2f'), ...
              ' 95%=',num2str(Up( round(len*0.95)),'%.2f'), ...
              ' 98%=',num2str(Up( round(len*0.98)),'%.2f'), ...
              ' 99%=',num2str(Up( round(len*0.99)),'%.2f'), ...
              ' 100%=',num2str(Up(end),'%.2f')]);
          l_text{end+1}='POSLV RT E';
          l_text{end+1}='POSLV RT N';
          l_text{end+1}='POSLV RT U';
          t_text=sprintf('%s | POSLV RT: 2D = %.2f Up = %.2f',t_text,RMS_POSLV_2D,RMS_POSLV_UP);
      end
  end
end

if(~isnan(StingerData))
  [~, A, B] = intersect(StingerData(:,2),TruthPVT(:,dTime));
  RealTimeStinger = StingerData(A,:);
  ControlStingerPVT = TruthPVT(B,:);


  ENU = LLH2ENU(RealTimeStinger(:,3), ...
                RealTimeStinger(:,4), ...
                RealTimeStinger(:,5), ...
                ControlStingerPVT(:,dLat) .* pi/180, ...
                ControlStingerPVT(:,dLon) .* pi/180, ...
                ControlStingerPVT(:,dHgt));

  % Plot the type 33:14 error
  plot(ControlStingerPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,1),'c');
  plot(ControlStingerPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,2),'m');
  plot(ControlStingerPVT(:,dTime) - ControlPVT(1,dTime), ENU(:,3),'k');
  RMS_Stinger_2D = rms(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
  RMS_Stinger_UP = rms(ENU(:,3));
  Horz = sort(sqrt(ENU(:,1).^2 + ENU(:,2).^2));
  len = length(Horz);
  if len > 0
      disp(['Stinger Record - 2D: 68%=',num2str(Horz( round(len*0.68)),'%.2f'), ...
          ' 95%=',num2str(Horz( round(len*0.95)),'%.2f'), ...
          ' 98%=',num2str(Horz( round(len*0.98)),'%.2f'), ...
          ' 99%=',num2str(Horz( round(len*0.99)),'%.2f'), ...
          ' 100%=',num2str(Horz(end),'%.2f')]);
      Up = sort(abs(ENU(:,3)));
      len = length(Up);
      disp(['Stinger Record - Up: 68%=',num2str(Up( round(len*0.68)),'%.2f'), ...
          ' 95%=',num2str(Up( round(len*0.95)),'%.2f'), ...
          ' 98%=',num2str(Up( round(len*0.98)),'%.2f'), ...
          ' 99%=',num2str(Up( round(len*0.99)),'%.2f'), ...
          ' 100%=',num2str(Up(end),'%.2f')]);
      
      l_text{end+1}='Stinger E';
      l_text{end+1}='Stinger N';
      l_text{end+1}='Stinger U';
      t_text=sprintf('%s | Stinger: 2D = %.2f Up = %.2f',t_text,RMS_Stinger_2D,RMS_Stinger_UP);
  end
else
  RealTimeStinger = NaN;
  ControlStingerPVT = NaN;
end

grid
AxisENU = axis;
global MarkerIndex;
EpochLineENU = plot([ControlPVT{MarkerIndex}(1,dTime) ControlPVT{MarkerIndex}(1,dTime)] - ControlPVT{MarkerIndex}(1,dTime), ...
                    [AxisENU(3) AxisENU(4)], ...
                    'k','linewidth',2);

l_text{end+1} = [DUTStr{MarkerIndex},' PVT Type'];
legend(l_text, 'location','best');
title('2D | Up - Comparison');

ylabel('(DUT - Reference) Error [m]');
Range = ControlPVT{MarkerIndex}(end,dTime) - ControlPVT{MarkerIndex}(1,dTime);
Str = PosStrings{RealTimePVT{MarkerIndex}(1,11) + 1};
MarkerText = text(ControlPVT{MarkerIndex}(1,dTime)+Range/100 - ControlPVT{MarkerIndex}(1,dTime), ...
                  AxisENU(4) - AxisENU(4)/6, ...
                  Str,'color',[0 0.4 0.2],'FontWeight','bold', 'BackgroundColor',[1 1 1]);

% Plot the satellites tracked/used                  
if LayoutMethod==1
    subtightplot(2,2,4,[0.04,0.04]);                  % Axis: Tracked/Used
elseif LayoutMethod > 1
    subtightplot(4,3,5:6,[0.04,0.04]);
end
sv_label={};
sv_circle={};
plot(ControlPVT{MarkerIndex}(:,dTime) - ControlPVT{MarkerIndex}(1,dTime), RealTimePVT{MarkerIndex}(:,6),'b');
hold on
if ProcessObs && isempty(TRK_PLL_Filt)==0
    plot(TRK_PLL_Filt(:,1) - ControlPVT{MarkerIndex}(1,dTime), TRK_PLL_Filt(:,2),'g');
end
plot(ControlPVT{MarkerIndex}(:,dTime) - ControlPVT{MarkerIndex}(1,dTime), RealTimePVT{MarkerIndex}(:,7),'r');
grid;
set(gca,'XLim',[StartTime - ControlPVT{MarkerIndex}(1,dTime)  EndTime - ControlPVT{MarkerIndex}(1,dTime)])
AxisSV = axis;
EpochLineSV = plot([ControlPVT{MarkerIndex}(1,dTime) ControlPVT{MarkerIndex}(1,dTime)] - ControlPVT{MarkerIndex}(1,dTime), ...
                   [AxisSV(3) AxisSV(4)], ...
                   'k','linewidth',2);
xlabel(['GPS Time [Seconds offset from ',num2str(ControlPVT{MarkerIndex}(1,1,dTime)),'s]']);
ylabel('Number of Satellites');
title('Stinger M7 Observables');
if ProcessObs && isempty(TRK_PLL_Filt)==0
    legend('Tracked (PLL+FLL)','Tracked (PLL)','Used',[DUTStr{MarkerIndex},' PVT Type'],'location','best');
else
    legend('Tracked','Used','location','best');
end
drawnow;

% Loop through the DUT (type 35:2) position data, skipping "DecRate" epochs
NumFrames = 0;
LastFrame = 0;
cir_han={};
N_han=[];
lastObsEpoch=[];
lastEpoch=0;
EpochTimes=StartTime:max([CtrlRate UUTRate ObsRate]):EndTime;
epoch=0;
while epoch < length(ControlPVT{MarkerIndex})
    if epoch < 1
        epoch=1;
    else
        epoch = epoch + DecRate;
        if epoch > length(ControlPVT{MarkerIndex})
            epoch = length(ControlPVT{MarkerIndex});
        end
    end
    
    % make sure the time epoch intervals lines up between Truth & UUT Obs rates
    % i.e.: Truth & UUT PVT may be at 100Hz but Observables are at 10Hz
    if rem(ControlPVT{MarkerIndex}(epoch,dTime),ObsRate) > 0.001
        current_time=ControlPVT{MarkerIndex}(epoch,dTime);
        i=0;
        epoch=[];
        while isempty(epoch) && i<length(EpochTimes)
            i=i+1;
            if EpochTimes(i) > current_time
                epoch=find(round(1000*ControlPVT{MarkerIndex}(:,dTime))==round(1000*EpochTimes(i)));
            end
        end
    end
    
    % make sure the epoch isn't larger than the truth vector (or empty)
    if isempty(epoch) || (epoch > length(ControlPVT{MarkerIndex}))
        epoch = length(ControlPVT{MarkerIndex});
    end
    
    % ---Fish Eye Plot
    FishEyeFrame=[];
    if( renderFishEyeVideo && LayoutMethod > 1)
        if LayoutMethod == 2
            subtightplot(2,2,3,[0.04,0.04]);
        elseif LayoutMethod == 3
            subtightplot(2,3,4,[0.04,0.04]);
        elseif (LayoutMethod >= 4)
            subtightplot(2,1,2,[0.02,0.04]);                  % Axis: Fish Eye Camera
        end
        axis off;
        tic
        FishEye_idx=find(ControlPVT{MarkerIndex}(epoch,dTime)>=fisheye_video.time(:,1) & ControlPVT{MarkerIndex}(epoch,dTime)<=fisheye_video.time(:,2));
        if isempty(FishEye_idx)==0
            FishEyeFrame = 1 + round((ControlPVT{MarkerIndex}(epoch,dTime) - fisheye_video.time(FishEye_idx(1),1))*fisheye_video.time(FishEye_idx(1),3));
            try
                FE1 = fisheye_video.video{FishEye_idx(1)}.read(FishEyeFrame);
                if LayoutMethod==2
                    FE3 = fliplr(FE1);
                    axis equal;
                elseif LayoutMethod==3
                    % SNVL cam
                    FE2 = FE1(151:930,521:1400,:);
                    % Image was taken looking up, to visualize looking down mirror the
                    % image horizontally.
                    FE3 = fliplr(FE2);
                    axis square;
                elseif LayoutMethod==4
                    FE3 = flipud(FE1);
                    axis equal;
                    imageScale = 2;
                elseif LayoutMethod==5
                    FE3 = flipud(FE1);

                    % Latest Sunnyvale camera 
                    % 1 - 586 pixels = Dash Cam
                    % 587 - 1226 - Fisheye
                    % There are some extra pixels at the end of the
                    % image we'll remove
                    % We also move the FE to the left to match what
                    % the script expects
                    Dash = FE3(:,1:586,:);
                    Eye  = fliplr(FE3(:,587:1226,:));
                    FE3 = [Eye,Dash];
                    axis equal;
                    imageScale = 1226/640;
                end
                hold on;
                image(FE3);
            catch
                fprintf('  Bad frame %i in %s\n',FishEyeFrame,fisheye_video.video{FishEye_idx(1)}.Name);
            end
        end
        
        % If we have an observable file we can get the elevation/azimuth and
        % show in the sky plot
        if(ProcessObs == 1) && (LayoutMethod > 2) && (isempty(FishEye_idx)==0)
            ObsEpoch = find(OBSDataFilt(:,1) == ControlPVT{MarkerIndex}(epoch,dTime));
            
            % store or reload the last epoch list (if OBS & POS rates are the same)
            if isempty(ObsEpoch)
                ObsEpoch=lastObsEpoch;        % if there isn't an exact time match, use the last match
            else
                lastObsEpoch=ObsEpoch;
                lastEpoch=ControlPVT{MarkerIndex}(epoch,dTime);
            end
            
            % delete old SV labels & allocate space
            for i = 1:length(sv_label)
                delete(sv_label{i});
                delete(sv_circle{i});
            end
            
            % for layout method 4 plot a circle at the 10 degree elevation mask
            if LayoutMethod>=4
                delete(N_han);
                TMP_size=size(FE3)./[1 imageScale 1];
                if isempty(cir_han)
                    TMP_az=(0:1:360)';
                    TMP_elev=10*ones(size(TMP_az));
                    [TMP_x,TMP_y] = get_fisheye_spot2( TMP_size, TMP_az, TMP_elev );
                    cir_han = plot(TMP_x,TMP_y,'LineStyle','-','Color','k');
                    txt_han = text(TMP_x(181),TMP_y(181),'Skyview Tracked (PLL)','HorizontalAlignment','center','VerticalAlignment','top','Color','k','FontWeight','bold');
                end
                [TMP_x,TMP_y] = get_fisheye_spot2( TMP_size, -ControlPVT{MarkerIndex}(epoch,dHeading), 10 );
                N_han = text(TMP_x,TMP_y,'N','FontSize',14,'HorizontalAlignment','center','FontWeight','bold','Color','r');
            end
            
            if (ControlPVT{MarkerIndex}(epoch,dTime)-lastEpoch) < 2*ObsRate
                sv_label=cell(length(ObsEpoch),1);
                sv_circle=cell(length(ObsEpoch),1);
                for i = 1:length(ObsEpoch)
                    % satellite azimuth - vehicle heading
                    tot_azim = OBSDataFilt(ObsEpoch(i),10)- ControlPVT{MarkerIndex}(epoch,dHeading);
                    if LayoutMethod==3
                        [x,y] = get_fisheye_spot( size(FE3), tot_azim, OBSDataFilt(ObsEpoch(i),11) );
                    elseif LayoutMethod>=4
                        [x,y] = get_fisheye_spot2( TMP_size, tot_azim, OBSDataFilt(ObsEpoch(i),11) );
                    end
                    if(OBSDataFilt(ObsEpoch(i),8) == 0)        % GPS
                        PRN_colour='b';
                    elseif(OBSDataFilt(ObsEpoch(i),8) == 1)    % SBAS
                        PRN_colour='m';
                    elseif(OBSDataFilt(ObsEpoch(i),8) == 2)    % GLN
                        PRN_colour='r';
                    elseif(OBSDataFilt(ObsEpoch(i),8) == 3)    % Galileo
                        PRN_colour=[0.95  0.85  0.15];
                    elseif(OBSDataFilt(ObsEpoch(i),8) == 4)    % QZSS
                        PRN_colour=[0.7   0.55  0.9];
                    elseif( (OBSDataFilt(ObsEpoch(i),8) == 5) | ...
                            (OBSDataFilt(ObsEpoch(i),8) == 7) | ...
                            (OBSDataFilt(ObsEpoch(i),8) == 10) )   % BDS
                        PRN_colour='g';
                    elseif(OBSDataFilt(ObsEpoch(i),8) == 9)    % IRNSS
                        PRN_colour='c';
                    else                                       % Everything else
                        PRN_colour='k';
                    end
                    if OBSDataFilt(ObsEpoch(i),11) >= 10       % only plot if above the elevation mask
                        sv_label{i} = text(x,y,num2str(OBSDataFilt(ObsEpoch(i),6)),'FontSize',14,'HorizontalAlignment','center','Color',PRN_colour,'FontWeight','bold');
                        sv_circle{i} = plot(x,y,'Marker','o','Color',PRN_colour,'MarkerSize',19);
                    else
                        sv_label{i} = [];
                        sv_circle{i} = [];
                    end
                end
            end
        else
            % delete old SV labels
            for i = 1:length(sv_label)
                delete(sv_label{i});
                delete(sv_circle{i});
            end
            delete(N_han);
        end
        hold off;
        toc
    end
    
    % If we have video update with the video frame for the current epoch
    dashFrame=[];
    if(renderDashVideo == 1)
        dash_idx=find(ControlPVT{MarkerIndex}(epoch,dTime)>=dash_video.time(:,1) & ControlPVT{MarkerIndex}(epoch,dTime)<=dash_video.time(:,2));
        if isempty(dash_idx)==0
            dashFrame = 1 + round((ControlPVT{MarkerIndex}(epoch,dTime) - dash_video.time(dash_idx(1),1))*dash_video.time(dash_idx(1),3));
            if(dashFrame ~= LastFrame)
                % This part of the code is very slow ... if we don't need to
                % render a new frame skip this section
                if LayoutMethod==1
                    subtightplot(2,2,3,[0.04,0.04]);                  % Axis: Dash Camera
                elseif LayoutMethod == 2
                    subtightplot(2,2,4,[0.08,0.04]);
                elseif LayoutMethod == 3
                    subtightplot(2,3,5:6,[0.08,0.04]);
                end
                try
                    image(dash_video.video{dash_idx(1)}.read(dashFrame));
                catch
                    fprintf('  Bad frame %i in %s\n',dashFrame,dash_video.video{dash_idx(1)}.Name);
                end
                axis off;
                axis equal;
                LastFrame = dashFrame;
            end
        end
    end
    
    % Update Tracked/Used Plot
    if LayoutMethod==1
        subtightplot(2,2,4,[0.04,0.04]);                  % Axis: Tracked/Used Plot
    elseif LayoutMethod > 1
        subtightplot(4,3,5:6,[0.04,0.04]);
    end
    if(exist('EpochLineSV','var')==1)
        delete(EpochLineSV);
    end
    EpochLineSV = plot([ControlPVT{MarkerIndex}(epoch,dTime) ControlPVT{MarkerIndex}(epoch,dTime)] - ControlPVT{MarkerIndex}(1,dTime), ...
        [AxisSV(3) AxisSV(4)], ...
        'k','linewidth',2);
    
    % Update the map subplot
    if LayoutMethod==1
        subtightplot(2,2,1,[0.04,0.04]);                  % Axis: Google Map
    elseif LayoutMethod > 1
        subtightplot(2,3,1,[0.04,0.04]);
    end
    
    % 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{MarkerIndex}(epoch,dLon), ControlPVT{MarkerIndex}(epoch,dLat), 'r.','MarkerSize',15);
    OldX = ControlPVT{MarkerIndex}(epoch,dLon);
    OldY = ControlPVT{MarkerIndex}(epoch,dLat);
    
    % Plot the position error
    if LayoutMethod==1
        subtightplot(2,2,2,[0.04,0.04]);                  % Axis: Position Error Plot
    elseif LayoutMethod > 1
        subtightplot(4,3,2:3,[0.04,0.04]);
    end
    delete(EpochLineENU);
    EpochLineENU = plot([ControlPVT{MarkerIndex}(epoch,dTime) ControlPVT{MarkerIndex}(epoch,dTime)] - ControlPVT{MarkerIndex}(1,dTime), ...
        [AxisENU(3) AxisENU(4)], ...
        'k','linewidth',2);
    delete(MarkerText);
    Str = PosStrings{RealTimePVT{MarkerIndex}(epoch,11) + 1};
    MarkerText = text(ControlPVT{MarkerIndex}(epoch,dTime) - ControlPVT{MarkerIndex}(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.001);
        
        % debug stuff
        if isempty(dashFrame)==0 && isempty(FishEyeFrame)==0
            set(han{MarkerIndex},'string',sprintf('Epoch: %i, GPS sec: %.3f, Dash Frame: %i (time: %.3f), FE Frame: %i (time: %.3f)',...
                epoch,ControlPVT{MarkerIndex}(epoch,dTime),...
                dashFrame,dash_video.time(dash_idx(1),1)+dashFrame/dash_video.time(dash_idx(1),3),...
                FishEyeFrame,fisheye_video.time(FishEye_idx(1),1)+FishEyeFrame/fisheye_video.time(FishEye_idx(1),3)));
        elseif isempty(FishEyeFrame)==0 && (LayoutMethod>=4)
            set(han{MarkerIndex},'string',sprintf('Epoch: %i, GPS sec: %.3f, Dash/FE Frame: %i (time: %.3f)',...
                epoch,ControlPVT{MarkerIndex}(epoch,dTime),...
                FishEyeFrame,fisheye_video.time(FishEye_idx(1),1)+FishEyeFrame/fisheye_video.time(FishEye_idx(1),3)));
        end
        if get(han{2},'value')==1
            db_close=1;
            delete(hf);
            return;
        end
        while get(han{3},'value')==1
            pause(0.1);
        end
        if get(han{4},'value')==1
            set(han{4},'value',0)
            epoch = epoch - 100*DecRate;
        end
        if get(han{5},'value')==1
            set(han{5},'value',0)
            epoch = epoch - 10*DecRate;
        end
        if get(han{6},'value')==1
            set(han{6},'value',0)
            epoch = epoch + 10*DecRate;
        end
        if get(han{7},'value')==1
            set(han{7},'value',0)
            epoch = epoch + 100*DecRate;
        end
        if get(han{8},'value')==1
            set(han{8},'value',0)
            epoch = epoch + 1000*DecRate;
        end
        
        % check that values are in range
        if epoch < 1
            epoch=0;
        elseif epoch > length(ControlPVT{MarkerIndex})
            epoch=length(ControlPVT{MarkerIndex});
        end
        
        drawnow;
    end
    
    NumFrames = NumFrames + 1;
    if(mod(NumFrames,20) == 0)
        disp(['Processed: ',num2str(NumFrames * EpochRate * DecRate,'%.2f'),'s']);
    end
end

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

% close the figure when done
delete(hf);

fprintf('Segment Processing Completed, Runtime: %.1f seconds (%.1f hours)\n',etime(clock,t1),etime(clock,t1)/3600);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;


function [video_return] = video_time (VideoDIR,TimeOffset,VideoTimeMethod,TimeZone,dAT,StartTime,EndTime)
% video_time: 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% video time determination

disp('in get video_time');

video_return.time=[];

% setup & get list
if (strcmp(VideoDIR(end),'\')==0)
    VideoDIR=strcat(VideoDIR,'\');
end
video_list=dir(sprintf('%s\\*.MP4',VideoDIR));

% get file times and look for overlaps
overlap=[];
for i=1:size(video_list,1)
    start_time=[];
    duration=[];
    fprintf('%s%s\n',VideoDIR,video_list(i).name);
    
    if VideoTimeMethod == 1
        [status1,result1]=dos(sprintf('exiftool -CreateDate "%s%s"',VideoDIR,video_list(i).name));
        [status2,result2]=dos(sprintf('exiftool -Duration "%s%s"',VideoDIR,video_list(i).name));

        if (status1==1) || (status2==1)
            fprintf('exiftool failed.  Result 1: %s\n',result1);
            fprintf('                  Result 2: %s\n',result2);
        else
            % get file start time
            find_str='Create Date                     :';
            j01=strfind(result1,find_str);
            if isempty(j01)==0
                j02=result1(j01(1)+length(find_str):end);
                j03 = datenum(j02,'yyyy:mm:dd HH:MM:SS');
                if j03 < 0
                    fprintf('extraction failed.  output: %.9f\n',j03);
                else
                    j04=datevec(j03);
                    [JD] = time2JD(j04);
                    JD=JD-TimeZone/24+TimeOffset/86400;
                    [~,start_time] = JD2GPS (JD,dAT);
                end
            end
            
            % get duration (change and get duration from: v = VideoReader(filename))
            find_str='Duration                        :';
            j11=strfind(result2,find_str);
            if isempty(j11)==0
                j12=result2(j11(1)+length(find_str):end);
                j13=textscan(j12,'%f:%f:%f');
                if isempty(j13)==0 && size(j13,2)==3
                    duration=3600*j13{1}+60*j13{2}+j13{3};
                else
                    fprintf('extraction failed.  output: %.9f\n',j12);
                end
            end
        end
    elseif VideoTimeMethod == 2
        % get file start time
        [~,datename,~]=fileparts(video_list(i).name);
        if length(datename)==19
            start_time = [str2double(datename(1:4)) str2double(datename(6:7)) str2double(datename(9:10)) str2double(datename(12:13)) str2double(datename(15:16)) str2double(datename(18:19))];
            [JD] = time2JD(start_time);
            JD=JD-TimeZone/24+TimeOffset/86400;
            [~,start_time] = JD2GPS (JD,dAT);
        else
            fprintf('INVALID "VideoTimeMethod" 2 start time\n');
            return;
        end

        % get duration (change and get duration from: v = VideoReader(filename))
        TMP_vid = VideoReader(sprintf('%s%s',VideoDIR,video_list(i).name));
        duration=TMP_vid.Duration;
        clear TMP_vid;
    else
        fprintf('INVALID "VideoTimeMethod": %i\n',VideoTimeMethod);
        return;
    end
    
    % find out if this overlaps with the start/end duration
    if isempty(start_time)==0 && isempty(duration)==0
        end_time=start_time+duration;
        fprintf('  Start: %.0f & Duration: %i\n',start_time,duration);
        if (start_time>=StartTime && start_time<EndTime) || ...
                (end_time>StartTime && end_time<=EndTime) || ...
                (start_time<=StartTime && end_time>=EndTime)
            fprintf('    Overlap found in file: %s%s\n',VideoDIR,video_list(i).name);
            overlap(end+1,1:3)=[start_time end_time i];
        end
    else
        fprintf('Start and/or duration not found in file: %s%s\n',VideoDIR,video_list(i).name);
    end
end
fprintf('\n');

% sort the overlap files and load the video
if isempty(overlap)==0
    overlap=sort(overlap,1);
    for i=1:size(overlap,1)
        fprintf('Reading file %i: %s%s\n',i,VideoDIR,video_list(overlap(i,3)).name);
        video_return.video{i} = VideoReader(sprintf('%s%s',VideoDIR,video_list(overlap(i,3)).name));
        video_return.time(end+1,1:3)=[overlap(i,1:2) video_return.video{i}.FrameRate];
        fprintf('  Rate: %.2f\n',video_return.time(end,3));
    end
    fprintf('\n');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;


function [x,y] = get_fisheye_spot2(size_image,azim_deg,elev_deg)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% azim of 0 = top of plot.  azim of 90 is right side of plot
% polar plot 0 = right side of plot, polar plot 90 = top of plot.

% polar_deg = 90 - azim_deg;
% height = size_image(1);
% width = size_image(2);
% polar_rad = polar_deg*pi/180;
% r = (90-elev_deg)/90;
% x = width/2*r*cos(polar_rad);
% y = height/2*r*sin(polar_rad);
% x = x + width/2;
% y = -y + height/2; % "-y" because image was flipped

% sqrt(sum(([480/2 640/2]-[y x]).^2))
% ~340 pixels == 80 degress below zenith (to 10 degrees above the horizon)

polar_az = (90 - azim_deg)*pi/180;
r = (90 - elev_deg)*340/80;               % convert elevation to pixel distance
                                          % (~340 pixels == 80 degress below 
                                          % zenith = 10 degrees above the horizon)
x = r.*cos(polar_az) + size_image(2)/2;
y = r.*sin(polar_az) + size_image(1)/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;


function [JD] = time2JD(time)
% time2JD: convert a gregorian time to a Julian Date
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function: time2JD.m
% purpose: Convert time to julian date
% author: David J. VandenBerg
% status: developed
% version: Matlab 5
% last date of development: May '98
% variables:
%   time=1x6 mtx of [year month day hour min sec]
%   JD=julian date
% example usage: time2JD(mtx)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% convert gregorian date/time to julian date
dd=time(:,4)/24+time(:,5)/60/24+time(:,6)/60/60/24;  % decimal part of day
D=time(:,3)+dd;                                      % day
M=time(:,2);                                         % month
Y=time(:,1);                                         % year
JD=367.0*Y-floor(7.0*(Y+floor((M+9.0)/12.0))/4.0)+floor(275*M/9.0)+D+1721013.5;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;


function [week,sec] = JD2GPS (JD,dAT)
% JD2GPS: convert Julian Date (ref: UTC) to GPS time (weeks & seconds)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get GPS time

JD_GPS=JD+(dAT-19)/86400.0;                     % JD in GPS time
week=floor((JD_GPS-2444244.5)/7.0);             % GPS week
DOW=floor(JD_GPS-2444244.5-7.0*week);           % day of week
day_sec=(JD_GPS-2444244.5-7.0*week-DOW)*86400;  % seconds into current day
sec=day_sec+DOW*86400.0;                        % GPS week sec's

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
return;
