% compareTraj.m
% Compare trajectories from ATS and rkws. 
% 
% Vivek Nadkarni
% Trimble
% November 3, 2004

% new; 
loadCfg; 

str2 = directoryname;
while (length(str2) > 1)
  [testName,str2] = strtok(str2,'\');
end

xlimits = [startTime, endTime];
% xlimits = [1.675e5, 1.705e5];

plotRkws = 0;
if (plotRkws)
  plotRkwsXYZ = 0;
  plotRkwsDeltaXYZ = 1;
  plotRkwsStatus = 0;
else
  plotRkwsXYZ = 0;
  plotRkwsDeltaXYZ = 0;
  plotRkwsStatus = 0;
end

plotMpos = 0;
if (plotMpos)
  plotMposXYZ = 0;
  plotMposDeltaXYZ = 0;
  plotMposVelXYZ = 0;
  plotMposRMS = 0;
else
  plotMposXYZ = 0;
  plotMposDeltaXYZ = 0;
  plotMposVelXYZ = 0;
  plotMposRMS = 0;  
end

plotRtk = 1;
if (plotRtk)
  plotRtkStatus = 1;
  plotRtkXYZ = 1;
  plotRtkDeltaXYZ = 1;
else
  plotRtkStatus = 0;
  plotRtkXYZ = 0;
  plotRtkDeltaXYZ = 0;  
end

%% GEQA position
% X0 = -2691552.7069; 
% Y0 = -4301178.8349; 
% Z0 = 3851750.7969; 

%% SUNN position
% X0 = -2691521.2804;
% Y0 = -4301170.7738; 
% Z0 = 3851781.4608; 

% Base station Position
X0 = surveyPos(BASE,1);
Y0 = surveyPos(BASE,2);
Z0 = surveyPos(BASE,3);

% View angles
az = 39.8321;
el = 82.7276;
% view([az, el]); 


% ATS data
atsTraj = load([directoryname 'atsTraj.txt']);
atsT = atsTraj(:,1)/1000;
atsX = atsTraj(:,2);
atsY = atsTraj(:,3);
atsZ = atsTraj(:,4);
% atsVel = load([directoryname 'ATStraj_XYZvel.txt']);
% atsVelT = atsVel(:,1);
% atsVelX = atsVel(:,2);
% atsVelY = atsVel(:,3);
% atsVelZ = atsVel(:,4);

atsVelT = atsT(2:end);
atsVelX = diff(atsX);
atsVelY = diff(atsY);
atsVelZ = diff(atsZ);


atsX = atsX - X0;
atsY = atsY - Y0;
atsZ = atsZ - Z0;

% MPOS data
if (plotMpos);
  mposTraj = load([directoryname 'mposTraj.txt']);
  mposT = mposTraj(:,1)/1000;
  mposX = mposTraj(:,2);
  mposY = mposTraj(:,3);
  mposZ = mposTraj(:,4);

  mposX = mposX - X0;
  mposY = mposY - Y0;
  mposZ = mposZ - Z0;

  mposVelT = mposT(2:end);
  mposVelX = diff(mposX);
  mposVelY = diff(mposY);
  mposVelZ = diff(mposZ);
end

% RKWS Data (actually rkwin data)
if (plotRkws)

  rkwsTraj = load([directoryname 'rkwsTraj.pos']);
  rkwsStatus = rkwsTraj(:,16);

  % /****************************************************************
  %  * INITIALIZATION STATUS REPORTING
  %  ****************************************************************/
  % typedef enum {
  %   Init_None=0,       /* 0: Initialisation status unknown */
  %   Init_NewFloat,     /* 1: transitioned from none to float */
  %   Init_HeldFloat,    /* 2: Remained in Float mode */
  %   Init_NewInit,      /* 3: Transitioned from float to fix */
  %   Init_HeldInit,     /* 4: Remained Fully initialised */
  %   Init_PartInit,     /* 5: New initialisation of >= 1 sv(s) */
  %   Init_Suspect,      /* 6: Init. suspect because of mismatch */
  %   Init_Lost_HighRMS, /* 7: Lost init due to high rms condition */
  %   Init_Lost_Mismatch,/* 8: Lost init due to init. mismatch */
  %   Init_Lost_BadRef   /* 9: Lost init due to bad ref sv */
  % } RkInit_e;
  %

  % RKWS fixed == 4
  idx = find(rkwsStatus == 4);
  rkwsTrajCut = rkwsTraj(idx, :);
  rkwsT = rkwsTrajCut(:,1);
  rkwsX = rkwsTrajCut(:,5);
  rkwsY = rkwsTrajCut(:,6);
  rkwsZ = rkwsTrajCut(:,7);

  % rkwsX = X0 + dX;
  % rkwsY = Y0 + dY;
  % rkwsZ = Z0 + dZ;
end


% RTK Data - true rtk data from rec11 
if (plotRtk)
  rtkRec11 = load([directoryname 'ref_rec11.txt']);
  rtkStatus = rtkRec11(:,7);
  % Column 7: 68 = float, 76 = fixed, 20 = rtcm , 04 = autonomous (?)
  rtkT = rtkRec11(:,1);


  DEG2RAD = pi/180;
  ell = ...
    [ DEG2RAD*rtkRec11(:,3), ...
    DEG2RAD*rtkRec11(:,2), ...
    rtkRec11(:,4)];

  % Get wgs84 ellipsoid parameters
  WGS84 = sWGS84;

  % Compute ECEF coordinates from long, lat, height.
  xyz = ell2xyz(ell,WGS84.a,WGS84.finv);

  clear ell rtkRec11;

  rtkX = xyz(:,1) - X0;
  rtkY = xyz(:,2) - Y0;
  rtkZ = xyz(:,3) - Z0;

  clear xyz;

  % If we are choosing only RTK Fixed Positions:
  idx = find(rtkStatus == 76); 
  rtkT = rtkT(idx);
  rtkX = rtkX(idx);
  rtkY = rtkY(idx);
  rtkZ = rtkZ(idx); 
  
end






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

if (plotRkwsStatus)
  newfigure;
  plot(rkwsTraj(:,1), rkwsStatus);
  title('RKWS Status');
end

if (plotRkwsXYZ)
  % Plots
  %   newfigure;
  %   plot3(atsX,atsY,atsZ, '.');
  %   hold on;
  %   plot3(rkwsX,rkwsY,rkwsZ, 'r.');
  %   view([az, el]);

  newfigure;
  subplot(3,1,1);
  plot(atsT, atsX, '.');
  hold on;
  plot(rkwsT, rkwsX, 'r.');
  title('X');
  legend('ATS', 'RKWS');

  subplot(3,1,2);
  plot(atsT, atsY, '.');
  hold on;
  plot(rkwsT, rkwsY,'r.');
  title('Y');

  subplot(3,1,3);
  plot(atsT, atsZ, '.');
  hold on;
  plot(rkwsT, rkwsZ, 'r.');
  title('Z');

end

if (plotRkwsDeltaXYZ)

  [comT, atsIdx, rkwsIdx] = intersect(atsT, rkwsT);

  %   newfigure;
  %   subplot(2,2,1);
  %   plot(comT, rkwsX(rkwsIdx) - atsX(atsIdx) , '.');
  %
  %   subplot(2,2,2);
  %   plot(comT, rkwsY(rkwsIdx) - atsY(atsIdx), '.');
  %
  %   subplot(2,2,3);
  %   plot(comT, rkwsZ(rkwsIdx) - atsZ(atsIdx), '.');


  newfigure;
  subplot(6,1,6);
  plot(comT, rkwsZ(rkwsIdx) - atsZ(atsIdx), '.');
  title('dZ: RKWS - ATS');
  % xlimits = xlim;
  xlim(xlimits);
  ylimits = [-0.05, 0.05];
  ylim(ylimits);
  xlabel('gps seconds');
  grid minor;


  subplot(6,1,1);
  plot(atsT, atsX, '.');
  hold on;
  plot(rkwsT, rkwsX, 'r.');
  title('X');
  xlim(xlimits);
  grid minor;

  subplot(6,1,2);
  plot(atsT, atsY, '.');
  hold on;
  plot(rkwsT, rkwsY,'r.');
  title('Y');
  xlim(xlimits);
  grid minor;

  subplot(6,1,3);
  plot(atsT, atsZ, '.');
  hold on;
  plot(rkwsT, rkwsZ, 'r.');
  title('Z');
  xlim(xlimits);
  ylabel('meters');
  grid minor;

  subplot(6,1,4);
  plot(comT, rkwsX(rkwsIdx) - atsX(atsIdx) , '.');
  title('dX: RKWS - ATS');
  xlim(xlimits);
  ylim(ylimits);
  grid minor;

  subplot(6,1,5);
  plot(comT, rkwsY(rkwsIdx) - atsY(atsIdx), '.');
  title('dY: RKWS - ATS');
  xlim(xlimits);
  ylim(ylimits);
  grid minor;


  set(gcf, 'Position', [1 1 1280 957]);
  suptitlebar(['RKWS(red) vs. ATS(blue): ' testName]);


  newfigure;
  plot(comT, rkwsX(rkwsIdx) - atsX(atsIdx), 'k.');
  hold on;
  plot(comT, rkwsY(rkwsIdx) - atsY(atsIdx), 'r.');
  plot(comT, rkwsZ(rkwsIdx) - atsZ(atsIdx), 'b.');
  xlim(xlimits);
  legend('dX', 'dY', 'dZ', 'Location', 'Best');
  title('RKWS minus ATS');
  xlabel('gps seconds');
  ylabel('meters');

end % plotRkwsDeltaXYZ

if (plotMposXYZ)

  newfigure;
  subplot(3,1,1);
  plot(atsT, atsX, '.');
  hold on;
  plot(mposT, mposX, 'm.');
  title('X');
  legend('MPOS', 'ATS');

  subplot(3,1,2);
  plot(atsT, atsY, '.');
  hold on;
  plot(mposT, mposY,'m.');
  title('Y');

  subplot(3,1,3);
  plot(atsT, atsZ, '.');
  hold on;
  plot(mposT, mposZ, 'm.');
  title('Z');
end

if (plotMposDeltaXYZ)

  [comT, atsIdx, mposIdx] = intersect(atsT, mposT);

  %   newfigure;
  %   subplot(2,2,1);
  %   plot(comT, mposX(mposIdx) - atsX(atsIdx) , '.');
  %
  %   subplot(2,2,2);
  %   plot(comT, mposY(mposIdx) - atsY(atsIdx), '.');
  %
  %   subplot(2,2,3);
  %   plot(comT, mposZ(mposIdx) - atsZ(atsIdx), '.');

  newfigure;
  plot(comT, mposX(mposIdx) - atsX(atsIdx), 'k.');
  hold on;
  plot(comT, mposY(mposIdx) - atsY(atsIdx), 'r.');
  plot(comT, mposZ(mposIdx) - atsZ(atsIdx), 'b.');
  legend('dX', 'dY', 'dZ', 'Location', 'Best');
  xlim(xlimits);
  title('MPOS minus ATS');
  xlabel('gps seconds');
  ylabel('meters');
end

if (plotMposVelXYZ)

  newfigure;
  subplot(3,1,1);
  plot(atsVelT, atsVelX, '.');
  hold on;
  plot(mposVelT, mposVelX, 'm.');
  title('X vel');
  legend('MPOS', 'ATS');

  subplot(3,1,2);
  plot(atsVelT, atsVelY, '.');
  hold on;
  plot(mposVelT, mposVelY,'m.');
  title('Y vel');

  subplot(3,1,3);
  plot(atsVelT, atsVelZ, '.');
  hold on;
  plot(mposVelT, mposVelZ, 'm.');
  title('Z vel');


  [comT, atsIdx, mposIdx] = intersect(atsVelT, mposVelT);

  newfigure;
  subplot(3,1,1);
  plot(comT, mposVelX(mposIdx) - atsVelX(atsIdx) , '.');
  grid minor;
  title('X vel');

  subplot(3,1,2);
  plot(comT, mposVelY(mposIdx) - atsVelY(atsIdx), '.');
  grid minor;
  title('Y vel');

  subplot(3,1,3);
  plot(comT, mposVelZ(mposIdx) - atsVelZ(atsIdx), '.');
  grid minor;
  title('Z vel');

end





%%%%%%%%%%%5




if (plotRtkXYZ)
  % Plots
  %   newfigure;
  %   plot3(atsX,atsY,atsZ, '.');
  %   hold on;
  %   plot3(rtkX,rtkY,rtkZ, 'r.');
  %   view([az, el]);

  newfigure;
  subplot(3,1,1);
  plot(atsT, atsX, '.');
  hold on;
  plot(rtkT, rtkX, 'r.');
  title('X');
  legend('ATS', 'RTK');

  subplot(3,1,2);
  plot(atsT, atsY, '.');
  hold on;
  plot(rtkT, rtkY,'r.');
  title('Y');

  subplot(3,1,3);
  plot(atsT, atsZ, '.');
  hold on;
  plot(rtkT, rtkZ, 'r.');
  title('Z');

end

if (plotRtkDeltaXYZ)

  [comT, atsIdx, rtkIdx] = intersect(atsT, rtkT);
  
  rtkErrT = comT;
  rtkErrX = rtkX(rtkIdx) - atsX(atsIdx);
  rtkErrY = rtkY(rtkIdx) - atsY(atsIdx);
  rtkErrZ = rtkZ(rtkIdx) - atsZ(atsIdx);

  %   newfigure;
  %   subplot(2,2,1);
  %   plot(comT, rtkX(rtkIdx) - atsX(atsIdx) , '.');
  %
  %   subplot(2,2,2);
  %   plot(comT, rtkY(rtkIdx) - atsY(atsIdx), '.');
  %
  %   subplot(2,2,3);
  %   plot(comT, rtkZ(rtkIdx) - atsZ(atsIdx), '.');
  %

  newfigure;
  subplot(6,1,6);
  plot(rtkErrT, rtkErrZ, '.');
  title('dZ: RTK - ATS');
  % xlimits = xlim;
  xlim(xlimits);
  ylimits = [-0.05, 0.05];
  ylim(ylimits);
  xlabel('gps seconds');
  grid minor;


  subplot(6,1,1);
  plot(atsT, atsX, '.');
  hold on;
  plot(rtkT, rtkX, 'r.');
  title('X');
  xlim(xlimits);
  grid minor;

  subplot(6,1,2);
  plot(atsT, atsY, '.');
  hold on;
  plot(rtkT, rtkY,'r.');
  title('Y');
  xlim(xlimits);
  grid minor;

  subplot(6,1,3);
  plot(atsT, atsZ, '.');
  hold on;
  plot(rtkT, rtkZ, 'r.');
  title('Z');
  xlim(xlimits);
  ylabel('meters');
  grid minor;

  subplot(6,1,4);
  plot(rtkErrT, rtkErrX, '.');
  title('dX: RTK - ATS');
  xlim(xlimits);
  ylim(ylimits);
  grid minor;

  subplot(6,1,5);
  plot(rtkErrT, rtkErrY, '.');
  title('dY: RTK - ATS');
  xlim(xlimits);
  ylim(ylimits);
  grid minor;


  set(gcf, 'Position', [1 1 1280 957]);
  suptitlebar(['RTK(red) vs. ATS(blue): ' testName]);


  newfigure;
  plot(rtkErrT, rtkErrX, 'k.');
  hold on;
  plot(rtkErrT, rtkErrY, 'r.');
  plot(rtkErrT, rtkErrZ, 'b.');
  legend('dX', 'dY', 'dZ', 'Location', 'Best');
  xlim(xlimits);
  title('RTK minus ATS');
  xlabel('gps seconds');
  ylabel('meters');

end % plotRtkDeltaXYZ

if (plotMposRMS)
  mposStatFileName = [directoryname 'MPOS\mposRmsTotal.txt'];
  mposRmsTotal = load(mposStatFileName);
  
  newfigure;
  plot(mposRmsTotal(:,1),mposRmsTotal(:,2));
  xlim(xlimits);
  xlabel('gps seconds');
  ylabel('meters');
  title('MPOS RMS position error');
end

computeStaticBias = 1; 
if (computeStaticBias)

  startStatic = 513700;
  endStatic = 514102;

  seg = find( and(startStatic < rtkErrT, rtkErrT < endStatic) );

  meanErrX = mean(rtkErrX)
  meanErrY = mean(rtkErrY)
  meanErrZ = mean(rtkErrZ)

  % Nov 22, 2004 data
  
  % Static section 1.
  %%%%%%%%%%%%%%%%%%%%
  %   startStatic = 168213;
  %   endStatic = 168457;
  % meanErrX =
  %  -5.2139e-002
  % meanErrY =
  %   6.1370e-003
  % meanErrZ =
  %  -2.7939e-003
  %

  % Static Section 2.
  %%%%%%%%%%%%%%%%%%%%%%
  %   startStatic = 169114;
  %   endStatic = 169291;
  % meanErrX =
  %  -5.2139e-002
  % meanErrY =
  %   6.1370e-003
  % meanErrZ =
  %  -2.7939e-003
  %

  %   startStatic = 168073;
  %   endStatic = 168166;
  %   %
  % meanErrX =
  %  -5.2139e-002
  % meanErrY =
  %   6.1370e-003
  % meanErrZ =
  %  -2.7939e-003
  % >>

end
