%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot range and computed altitude.
% AUTHOR:  rsm, following MIT code.
% DATE:    00/1/5
% MODS:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
global titleStr;
global vehicleName;

 if exist( 'h_alt' ) ~= 1
   h_alt = figure;
 else
   figure( h_alt );
 end

 set( gcf, 'name', [vehicleName,' Altimeter'] );


  winpos( position(1,:) );

  %subplot( 2, 1, 1 )

  time = dataNav(:,1);
  %
  % Get rid of the 1000 meter default for plotting purposes.
  %
  procAlt       = dataNav(:,mAltitude);
  Indx          = find(procAlt >= 300);
  procAlt(Indx) = 0*ones(length(Indx),1);
  
  direction = -1;       %Upward
  if( direction == -1 )
    zdir = -1;
  elseif( direction == 1 )
    zdir =  1;  
  else
    disp('Error: Please specifiy DVL upward/downward direction')
    return
  end

  plot( time, dataNav(:,mDepth), 'm', ...
	       time, dataNav(:,mDepth) + zdir*procAlt+6.0, 'b' );
  area(time, dataNav(:,mDepth) + zdir*procAlt+6.0, ...
	'facecolor', [0 .75 .85]);
  %     'facecolor', [0 .75 .5] );
  set( gca, 'YDir', 'reverse' );
  xlim = get(gca,'XLim');
  line( time, dataNav(:,mDepth), 'color', [1 0.5 0], 'linewidth', 1 );
  title( ['Measured Depth and Calculated Ice Draft: ' titleStr] );
  ylabel( 'Depth, Meters' );
  zoom on
  grid on

  %line( time, dataNav(:,mTheta)*180/pi,'color','g')

  if( 0 )
  
  disp(' altp: Begin Dvl beam recoordinatization calculation:')
  %
  % Form the beam unit vector in DVL coordinates where +x goes from
  % beam 2 to 1, and +y goes from beam 4 to 3.
  %
  s30 = sin(pi/6);
  c30 = cos(pi/6);
  beam1_DVL = [ s30    0  c30]';
  beam2_DVL = [-s30    0  c30]';
  beam3_DVL = [   0 -s30  c30]';
  beam4_DVL = [   0  s30  c30]';
  %
  % Form the DCM from DVL to B when the Dvl points down.  This is a
  % +45 Deg about the case-fixed z axis.
  %
  T_psi45  = [  1  1  0;
	       -1  1  0;
	        0  0  1]/sqrt(2);

  T_phi180 = [  1  0  0;
		0 -1  0;
		0  0 -1];

  if( direction == 1 )
    T_B_DVL = T_psi45;
  elseif( direction == -1 )
    T_B_DVL = T_phi180 * T_psi45;
  else
    disp('Error: Please specifiy DVL upward/downward direction')
    return
  end
  %
  % Form the DCM from B to LV:
  %
  %
  % BEWARE:  The AHRS puts out projected, or space-fixed, roll and pitch 
  % angles.  THESE ARE NOT EULER ANGLES.  The following relation converts
  % the space-fixed roll angle into a 2-1 Euler.  See Wertz, p.226.
  %
  % At 20 Deg pitch, the Euler roll and the space-fixed roll differ 
  % by about 20 milli-Radians.
  %
  EulerPhi = atan( cos(dataNav(:,mTheta)).*tan(dataNav(:,mPhi) ));
  
  cpsi   = cos(dataNav(:,mPsi));
  spsi   = sin(dataNav(:,mPsi));
  ctheta = cos(dataNav(:,mTheta));
  stheta = sin(dataNav(:,mTheta));
  cphi   = cos(EulerPhi);
  sphi   = sin(EulerPhi);
  %
  % 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(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; 

  T_LV_DVL = T_LV_B * T_B_DVL;
  
  disp(' altp: Beam calculations finished.')
  
  end
  
  return






















%
% This seems to duplicate the calc in dvl/Dvl.cc and Navigation/Dvl.cc.
%
%if( exist('dvlRange')==1 ),
%  dvlRangeInterp = interp1(dataDvl(:,1),dataDvl(:,dvlRange),time);
%  dvlAlt = dvlRangeInterp .* cos(dataNav(:,mPhi)) .* cos(dataNav(:,mTheta));
%  [xx,yy]  = stairs( time, dataNav(:,mDepth)+dvlAlt );
%  line( xx, yy, 'color','g')
%end




subplot( 2, 1, 2 )
plot( time, dataNav(:,mPsaRange));
title( 'Range;   PSA - Blue, DVL - Red' );
xlabel( 'Elapsed Time (s)' );
ylabel( 'Range, Meters' );
set(gca,'XLim',xlim);
zoom on
grid on
if( exist('dvlRange')==1 ),
  [tt,yy] = stairs(dataDvl(:,1), dataDvl(:,dvlRange));
  line( tt, yy, 'color','r')
end

