% POS Pac Column index

dTime     = 1;
dLat      = 2;
dLon      = 3;
dHgt      = 4;
dEastVel  = 5;
dNorthVel = 6;
dUpVel    = 7;
dEastSig  = 8;
dNorthSig = 9;
dUpSig    = 10;
dRoll     = 11;
dPitch    = 12;
dHeading  = 13;

isOctave = exist('OCTAVE_VERSION', 'builtin') ~= 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now read the matlab configuration file
fid = fopen( 'matlab.config.txt', 'r' );

dataSet = fscanf(fid,'%s',1);
runNum  = fscanf(fid,'%d',1);
start   = fscanf(fid,'%d',1);
stop    = fscanf(fid,'%d',1);
type    = fscanf(fid,'%d',1);
RefLat  = fscanf(fid,'%f',1);
RefLon  = fscanf(fid,'%f',1);
RefHgt  = fscanf(fid,'%f',1);
WeekNum = fscanf(fid,'%d',1);
ENMax   = fscanf(fid,'%f',1);
UMax    = fscanf(fid,'%f',1);
engine  = fscanf(fid,'%s',1);

fclose(fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FileString = dataSet;

POSPac = dlmread('truth.txt');

if(stop > -1)
  i = (POSPac(:,dTime) >= start) & (POSPac(:,dTime) <= stop);
  POSPac = POSPac(i,:);
end

if(isOctave)
  set(0,'defaulttextfontname','*');
end

fid = fopen('stats.txt','w');
fprintf(fid,'%d ',runNum);

CurrentDate = datevec(date);
% Y/M/D
fprintf(fid,'%d %d %d ', CurrentDate(1), CurrentDate(2), CurrentDate(3));

%
% To give the data some context plot the raw truth.
%

fig = figure('visible','off');
plot(POSPac(:,dLon), POSPac(:,dLat));
xlabel('Longitude [degree]');
ylabel('Latitude [degree]');
axis square;
grid on;
title('POSPac position');

if(isOctave)
  print('-dpng',[FileString,'_POSPac_Position.png'],'-S960,720');
else
  print('-dpng',[FileString,'_POSPac_Position.png']);
  saveas(fig, [FileString,'_POSPac_Position'],'fig');
end

fig = figure('visible','off');
plot(POSPac(:,dTime), POSPac(:,dHgt));
xlabel('GPS Time [seconds]');
ylabel('Height [m]');
grid on;
title('POSPac Height');

if(isOctave)
  print('-dpng',[FileString,'_POSPac_Hgt.png'],'-S960,720');
else
  print('-dpng',[FileString,'_POSPac_Hgt.png']);
  saveas(fig,[FileString,'_POSPac_Hgt'],'fig');
end

fig = figure('visible','off');
plot(POSPac(:,dTime), sqrt(POSPac(:,dEastVel).^2 + POSPac(:,dNorthVel).^2));
xlabel('GPS Time [seconds]');
ylabel('Horizontal Velocity[m/s]');
grid on;
title('POSPac Horizontal Velocity');

if(isOctave)
  print('-dpng',[FileString,'_POSPac_HorzVel.png'],'-S960,720');
else
  print('-dpng',[FileString,'_POSPac_HorzVel.png']);
  saveas(fig,[FileString,'_POSPac_HorzVel'],'fig');
end

%
% Load in the data we are analyzing
%

DUT = dlmread('pos.txt');

% Load common data points with matching time tags
[A, B] = intersect_sorted(DUT(:,2), POSPac(:,dTime));

% Get the true LLH position and ENU velocity
if(type == 2)  % T02
  RealTime = DUT(A,[11:13, 15, 14, 16]);
else           % T04
  RealTime = DUT(A,[13:15, 17, 16, 18]);
end

PostProcess = POSPac(B,:);

disp([' Epochs :',num2str(length(A))]);
fprintf(fid,'%d ',length(A));

fig = figure('visible','off');
plot(PostProcess(:,dTime), PostProcess(:,dEastSig));
hold on;
plot(PostProcess(:,dTime), PostProcess(:,dNorthSig),'r');
plot(PostProcess(:,dTime), PostProcess(:,dUpSig),'g');
grid on;
xlabel('GPS Time [Sec]');
ylabel('Estimated 1-\sigma Error [m]');
legend('East','North','Height');
title('POSPac Estimated Error');

if(isOctave)
  print('-dpng',[FileString,'_POSPac_Error.png'],'-S960,720');
else
  print('-dpng',[FileString,'_POSPac_Error.png']);
  saveas(fig,[FileString,'_POSPac_Error'],'fig');
end

fig = figure('visible','off');
plot(PostProcess(:,dTime), PostProcess(:,dEastVel) - RealTime(:,4));
title('East Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-1 1])
grid on;

if(isOctave)
  print('-dpng',[FileString,'_EastVel.png'],'-S960,720');
else
  print('-dpng',[FileString,'_EastVel.png']);
  saveas(fig,[FileString,'_EastVel'],'fig');
end

i = isnan(RealTime(:,4)) == 0;
VEast = PostProcess(i, dEastVel) - RealTime(i,4);

RMS = num2str(rms(VEast));
disp(['RMS(E_Vel) :', RMS, ' m/s']);
EastVel  = sort(abs(VEast));
per68  = EastVel(floor(0.68*length(EastVel)));
per95  = EastVel(floor(0.95*length(EastVel)));
per100 = EastVel(end);

disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

fig = figure('visible','off');
plot(PostProcess(:,dTime), PostProcess(:,dNorthVel) - RealTime(:,5));
title('North Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-1 1])
grid on;

if(isOctave)
  print('-dpng',[FileString,'_NorthVel.png'],'-S960,720');
else
  print('-dpng',[FileString,'_NorthVel.png']);
  saveas(fig,[FileString,'_NorthVel'],'fig');
end

i = isnan(RealTime(:,5)) == 0;
VNorth = PostProcess(i, dNorthVel) - RealTime(i,5);
RMS = num2str(rms(VNorth));
disp(['RMS(N_Vel) :', RMS, ' m/s']);
NorthVel  = sort(abs(VNorth));
per68  = NorthVel(floor(0.68*length(NorthVel)));
per95  = NorthVel(floor(0.95*length(NorthVel)));
per100 = NorthVel(end);

disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

fig = figure('visible','off');
plot(PostProcess(:,dTime), PostProcess(:,dUpVel) - RealTime(:,6));
title('Up Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-1 1])
grid on;

if(isOctave)
  print('-dpng',[FileString,'_UpVel.png'],'-S960,720');
else
  print('-dpng',[FileString,'_UpVel.png']);
  saveas(fig,[FileString,'_UpVel'],'fig');
end

i = isnan(RealTime(:,6)) == 0;
VUp = PostProcess(i, dUpVel) - RealTime(i,6);
RMS = num2str(rms(VUp));
disp(['RMS(U_Vel) :', RMS, ' m/s']);
UpVel  = sort(abs(VUp));
per68  = UpVel(floor(0.68*length(UpVel)));
per95  = UpVel(floor(0.95*length(UpVel)));
per100 = UpVel(end);

disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);


% Capture the velocity error
VelError(:,1) = PostProcess(:,dEastVel)  - RealTime(:,4);
VelError(:,2) = PostProcess(:,dNorthVel) - RealTime(:,5);
VelError(:,3) = PostProcess(:,dUpVel)    - RealTime(:,6);


fig = figure('visible','off');
plot(EastVel, (1:length(EastVel))./length(EastVel),'b','linewidth',3);
hold on
plot(NorthVel, (1:length(NorthVel))./length(NorthVel),'r','linewidth',3);
plot(UpVel, (1:length(UpVel))./length(UpVel),'g','linewidth',3);
grid on;
xlabel('Velocity Error [m/s]');
ylabel('CDF');
title('Cumulative Distribution : Velocity');
axis([0 0.2 0 1]);
legend('East','North','Up');

if(isOctave)
  print('-dpng',[FileString,'_VelCDF.png'],'-S960,720');
else
  print('-dpng',[FileString,'_VelCDF.png']);
  saveas(fig,[FileString,'_VelCDF'],'fig');
end

% Calculate position errors

ENU = LLH2ENU(RealTime(:,1) .* pi/180, ...
              RealTime(:,2) .* pi/180, ...
              RealTime(:,3), ...
              PostProcess(:,dLat) .* pi/180, ...
              PostProcess(:,dLon) .* pi/180, ...
              PostProcess(:,dHgt));
    

% Plot ENU errors
fig = figure('visible','off');
subplot(3,1,1)
plot(PostProcess(:,dTime),ENU(:,1));
ylabel('East Error [m]');
grid on;
ylim([-ENMax ENMax])

subplot(3,1,2)
plot(PostProcess(:,dTime),ENU(:,2));
ylabel('North Error [m]');
grid on;
ylim([-ENMax ENMax])

subplot(3,1,3)    
plot(PostProcess(:,dTime),ENU(:,3));
ylabel('Height Error [m]');
grid on;
ylim([-UMax UMax])
xlabel('Time [GPS Seconds]');

if(isOctave)
  print('-dpng',[FileString,'_ENU.png'],'-S960,720');
else
  print('-dpng',[FileString,'_ENU.png']);
  saveas(fig,[FileString,'_ENU'],'fig');
end

% Compute the cross-track error
CosHead = cos(PostProcess(:,dHeading) * pi / 180);
SinHead = sin(PostProcess(:,dHeading) * pi / 180);

CrossTrack = sqrt((SinHead .* ENU(:,2)) .^ 2 + (CosHead .* ENU(:,1)) .^2);

% Crosstrack
CrossTrack = sort(CrossTrack);
per68  = CrossTrack(floor(0.68*length(CrossTrack)));
per95  = CrossTrack(floor(0.95*length(CrossTrack)));
per100 = CrossTrack(end);

disp('Cross-Track Stats:');
disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

% 2D
TwoDOrig = sqrt(ENU(:,1) .^ 2 + ENU(:,2) .^2);
TwoD = sort(TwoDOrig);
per68  = TwoD(floor(0.68*length(TwoD)));
per95  = TwoD(floor(0.95*length(TwoD)));
per99  = TwoD(floor(0.99*length(TwoD)));
per100 = TwoD(end);

disp('2D Stats:');
disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

% Now write a file that contains the 1% outliers
i = TwoDOrig > per99;
writeKML( [FileString,'-2D-Outliers'], ...
          'SNPC', ...
          '2D Error', ...
          TwoDOrig(i), ...
          PostProcess(i,dTime), ...
          PostProcess(i,dLat), ...
          PostProcess(i,dLon), ...
          PostProcess(i,dHgt), ...
          RealTime(i,1), ...
          RealTime(i,2), ...
          RealTime(i,3));

% 3D
ThreeDOrig = sqrt(ENU(:,1) .^ 2 + ENU(:,2) .^2 + ENU(:,3) .^ 2);
ThreeD = sort(ThreeDOrig);
per68  = ThreeD(floor(0.68*length(ThreeD)));
per95  = ThreeD(floor(0.95*length(ThreeD)));
per99  = ThreeD(floor(0.99*length(ThreeD)));
per100 = ThreeD(end);

disp('3D Stats:');
disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');

fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

i = ThreeDOrig > per99;
writeKML( [FileString,'-3D-Outliers'], ...
          'SNPC', ...
          '3D Error', ...
          ThreeDOrig(i), ...
          PostProcess(i,dTime), ...
          PostProcess(i,dLat), ...
          PostProcess(i,dLon), ...
          PostProcess(i,dHgt), ...
          RealTime(i,1), ...
          RealTime(i,2), ...
          RealTime(i,3));

% Height
UpModOrig = abs(ENU(:,3));
UpMod = sort(UpModOrig);
per68  = UpMod(floor(0.68*length(UpMod)));
per95  = UpMod(floor(0.95*length(UpMod)));
per99  = UpMod(floor(0.99*length(UpMod)));
per100 = UpMod(end);

disp('Height Stats:');
disp(['( 68%):',num2str(per68)]);
disp(['( 95%):',num2str(per95)]);
disp(['(100%):',num2str(per100)]);
disp(' ');
fprintf(fid,'%.3f %.3f %.3f ',per68,per95,per100);

i = UpModOrig > per99;
writeKML( [FileString,'-Hgt-Outliers'], ...
          'SNPC', ...
          'Height Error', ...
          UpModOrig(i), ...
          PostProcess(i,dTime), ...
          PostProcess(i,dLat), ...
          PostProcess(i,dLon), ...
          PostProcess(i,dHgt), ...
          RealTime(i,1), ...
          RealTime(i,2), ...
          RealTime(i,3));

fig = figure('visible','off');
plot(CrossTrack, (1:length(CrossTrack))./length(CrossTrack),'b','linewidth',3);
hold on;
plot(TwoD, (1:length(TwoD))./length(TwoD),'color','r','linewidth',3);
plot(ThreeD, (1:length(ThreeD))./length(ThreeD),'color','m','linewidth',3);
plot(UpMod, (1:length(UpMod))./length(UpMod),'color','g','linewidth',3);
xlabel('Error [m]');
ylabel('CDF');
title('Cumulative Distribution: Position');
A = axis;
A(2)=UMax;
axis(A);
grid on;
legend('Crosstrack','2D','3D','Height');

if(isOctave)
  print('-dpng',[FileString,'_PosCDF.png'],'-S960,720');
else
  print('-dpng',[FileString,'_PosCDF.png']);
  saveas(fig,[FileString,'_PosCDF'],'fig');
end

fprintf(fid,'%s\n',engine);
fclose(fid);

% Now create an E/N/U Error file

fid = fopen( [FileString,'-ENU-Err.txt'], 'w' );

for i=1:length(RealTime(:,1))
  fprintf(fid,'%.3f %d %.13f %.13f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n', ...
          PostProcess(i,dTime), ...
          WeekNum, ...
          PostProcess(i,dLat), ...
          PostProcess(i,dLon), ...
          PostProcess(i,dHgt), ...
          ENU(i,1), ...
          ENU(i,2), ...
          ENU(i,3), ...
          VelError(i,1), ...
          VelError(i,2), ...
          VelError(i,3) );
end

fclose(fid);

exit


