%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Process the data loaded by loadNavData to prepare it for plotting.
% AUTHOR:  Rob McEwen
% DATE:    03/3/14
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  %
  % ********Process the data**********
  %
  % Plot the ship's track timeMargin seconds before the first mission starts.
  %
  timeMargin = 7200;
  %
  % Sync relative time properly with nav time.  See cvtwf.m, tmin code.
  %
  tmin = startTime - tusbl(1) - timeMargin;
  %tmax = startTime - tusbl(1) + dataK(end,1) + 1200;
  %
  % This call clips the time vector with tmin, tmax.
  %
  cvtwf 
  %
  % Locate the start and stop times of each mission on the Winfrog ship track.
  % Be careful to synchronize the usbl relative time with the relative
  % navigation time, startTime.
  %
  tusblrStartIndx = []; tusblrEndIndx = [];
  for( i=1:length(Mstart) )
    startIndx =   find(tusblr - startTime >= Mstart(i) ...
	        & tusblr -startTime <= Mstart(i)+2);
    endIndx   =   find(tusblr -startTime >= Mend(i) ...
	        & tusblr - startTime <= Mend(i)+2);
    if( ~isempty( startIndx ) )
      tusblrStartIndx  = [ tusblrStartIndx; startIndx(1) ];
    else
      disp(sprintf([' Mission ', num2str(i) ' start time not found.\n']));
      tusblrStartIndx  = [ tusblrStartIndx; 0 ];
    end
    if( ~isempty( endIndx ) )
      tusblrEndIndx  = [ tusblrEndIndx; endIndx(1) ];
    else
      disp(sprintf([' Mission ', num2str(i) ' end time not found.\n']));
      tusblrEndIndx  = [ tusblrEndIndx; 0 ]; 
    end
  end
  
  if( ~loadDvl ), return; end
  %
  % Renavigate each mission by dead-reckoning using INS heading and Dvl
  % velocity. First initialize the transform from RDI's goofy ship axes, with
  % EA=45 Degrees, to the SNAME standard.  The DCM below is a +90 yaw followed
  % by a 180 roll. 
  %
  T_SNAME_RDI =  [0., 1., 0.;  
                  1., 0., 0.;
                  0., 0.,-1.];
  %		
  T_SNAME_286 =  [0.,-1., 0.;  
                 -1., 0., 0.;
                  0., 0.,-1.];
  %
  % Extract good data:
  %
  if( strcmp(pathNames(pathIndx).tracking, 'water') )
    indx1 = find( dataDvl(:,dvlWatVelStat) == 0 );
    %
    % Apparently when watVelStatus is zero (good data), the data can still really
    % be bad if the velocity components are set to -32768 mm/sec.  Eliminate
    % these.  BEWARE that indx2 now indexes indx1, NOT dataDvl.
    %
    indx2  = find( dataDvl(indx1,dvlWatVelx) > -32 );  
    dvlIndx = indx1(indx2);
  elseif( strcmp(pathNames(pathIndx).tracking, 'ice') )
    indx1 = find( dataDvl(:,dvlBotVelStat) == 0 );
    %
    % Apparently when watVelStatus is zero (good data), the data can still really
    % be bad if the velocity components are set to -32768 mm/sec.  Eliminate
    % these.  BEWARE that indx2 now indexes indx1, NOT dataDvl.
    %
    indx2  = find( dataDvl(indx1,dvlBotVelx) > -32 );  
    dvlIndx = indx1(indx2);
  else
    disp(sprintf(' Unknown Ins/Dvl orientation %s.\n',...
	pathNames(pathIndx).tracking) )
  end
  %
  % Transform to LVLH coords (NED) using the Kearfott Ahrs.  
  %
  % NOTE: Make sure you compensate for roll sign errors, especially when it's
  % pointed up.
  %
  tdvl = diff(dataDvl(dvlIndx,1));
  %
  % Remove the large time intervals between missions:
  %
  indx3 = find(isnan(dataDvl(:,1)));
  jmpIndx = [];
  j=1;
  for( i=1:length(indx3)/2 )
    tmpIndx = find( dvlIndx > indx3(2*i) );
    if( ~isempty(tmpIndx) )
      jmpIndx(j,1) = tmpIndx(1)-1;
      if( jmpIndx(j,1) == 0 ) jmpIndx(j,1) = 1;end
      j=j+1;
    end
  end
  tdvl(jmpIndx) = zeros(size(jmpIndx));
  %
  dvlDist = [0 0];
  %
  % Elminiate double abcissa points, otherwise interp1 gags.  Also, elminate the
  % NaNs, for the same reason.
  %
  indxK1 = find(  diff(dataK(:,1)) ~= 0  );
  indxK2 = find( ~isnan(dataK(:,1)) );
  indxK  = intersect(indxK1,indxK2);
  %
  % The AHRS angles that the Kearfott puts out ARE standard Euler 
  % angles, 3-2-1.
  %
  ahrs1 = interp1(dataK(indxK,1),dataK(indxK,[mRollK:mHeadK]),...
      dataDvl(dvlIndx,1));
  phi   = ahrs1(:,1);
  theta = ahrs1(:,2);
  psi   = ahrs1(:,3);
  %
  cpsi   = cos( psi );
  spsi   = sin( psi );
  ctheta = cos( theta );
  stheta = sin( theta );
  cphi   = cos( phi );
  sphi   = sin( phi );
  %
  % Form the direction cosine matrix that recoordinatizes a vector from the 
  % body frame (B) into the local-level-local-vertical (LV) navigation 
  % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
  %
  % N is the inertially fixed, or Newtonian frame.
  %
  % The transformation T_B_LV, is 3-2-1 Euler.  The matrix below, T_LV_B, is
  % the transpose.
  %
  T_LV_B = zeros(3,3,length(dvlIndx));     %Initialize it.
  %
  T_LV_B(1,1,:) =  cpsi.*ctheta; 
  T_LV_B(2,1,:) =  spsi.*ctheta; 
  T_LV_B(3,1,:) = -stheta; 

  T_LV_B(1,2,:) = -spsi.*cphi + cpsi.*stheta.*sphi; 
  T_LV_B(2,2,:) =  cpsi.*cphi + sphi.*stheta.*spsi; 
  T_LV_B(3,2,:) =  ctheta.*sphi; 

  T_LV_B(1,3,:) =  spsi.*sphi + cpsi.*cphi.*stheta; 
  T_LV_B(2,3,:) = -cpsi.*sphi + stheta.*spsi.*cphi;
  T_LV_B(3,3,:) =  ctheta.*cphi; 

  disp(sprintf(' Beginning the Dvl Dead Reckoning Computation.\n'));
  for( i=1:length(dvlIndx)-1)
    if( strcmp(pathNames(pathIndx).tracking, 'water') )
      %
      % X is either a water-mass or ice-fixed reference frame.
      %
      vel_B_X_LV = T_LV_B(:,:,i)...
                   *dataDvl(dvlIndx(i),[dvlWatVelx:dvlWatVelz])';
    elseif( strcmp(pathNames(pathIndx).tracking, 'ice') )
      T = T_SNAME_RDI;
      %
      % Dvl was up, -y was forward, EA = ???
      %
      % Assume the user didn't change directories after loadNavData...
      %
      if( strncmp(DirName,'2001.286',8)) T = T_SNAME_286; end
      vel_B_X_LV  = T_LV_B(:,:,i)*T...
	           *dataDvl(dvlIndx(i),[dvlBotVelx:dvlBotVelz])';
    else
      disp(sprintf(' Unknown Ins/Dvl orientation %s.\n',...
	  pathNames(pathIndx).tracking) );
      break;
    end
    %
    % Accumulate velocity to get position, assuming constant velocity over each
    % sampling instant.
    %
    dvlDist(i+1,:) = dvlDist(i,:) + tdvl(i)*vel_B_X_LV(1:2)';
  end
  disp(sprintf(' Done!.\n'));

  clear T_LV_B ahrs1 phi theta psi cpsi spsi ctheta stheta cphi sphi   

  return