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);
lat     = fscanf(fid,'%f',1);
lon     = fscanf(fid,'%f',1);
hgt     = 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;

DUT = dlmread('pos.txt');

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

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

Time = DUT(:,2);

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));
disp([' Epochs :',num2str(length(Time))]);
fprintf(fid,'%d ',length(Time));
fig = figure('visible','off');
plot(Time, RealTime(:,4));
title('East Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-0.15 0.15])
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 = 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(Time, RealTime(:,5));
title('North Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-0.15 0.15])
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 = 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(Time, RealTime(:,6));
title('Up Velocity Error');
xlabel('Time [GPS Seconds]');
ylabel('Velocity Error [m/s]');
ylim([-0.15 0.15])
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 = 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) = RealTime(:,4);
VelError(:,2) = RealTime(:,5);
VelError(:,3) = 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.1 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), ...
              lat * pi/180, ...
              lon * pi/180, ...
              hgt);

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

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

subplot(3,1,3)
plot(Time,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

% 2D
TwoD = sqrt(ENU(:,1) .^ 2 + ENU(:,2) .^2);
TwoD = sort(TwoD);
per68  = TwoD(floor(0.68*length(TwoD)));
per95  = TwoD(floor(0.95*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);

% 3D
ThreeD = sqrt(ENU(:,1) .^ 2 + ENU(:,2) .^2 + ENU(:,3) .^ 2);
ThreeD = sort(ThreeD);
per68  = ThreeD(floor(0.68*length(ThreeD)));
per95  = ThreeD(floor(0.95*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);

% Height
UpMod = sort(abs(ENU(:,3)));
per68  = UpMod(floor(0.68*length(UpMod)));
per95  = UpMod(floor(0.95*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);

fig = figure('visible','off');
plot(TwoD, (1:length(TwoD))./length(TwoD),'color','r','linewidth',3);
hold on;
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('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(Time)
  fprintf(fid,'%.3f %d %.13f %.13f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n', ...
          Time(i), ...
          WeekNum, ...
          lat, ...
          lon, ...
          hgt, ...
          ENU(i,1), ...
          ENU(i,2), ...
          ENU(i,3), ...
          VelError(i,1), ...
          VelError(i,2), ...
          VelError(i,3) );

end

fclose(fid);

exit

