%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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'] );

%mask = find( alt_m > 0 & alt_m < 99 );
%if exist( 'medfilt1' ) == 2 & ~isempty(mask)
%    alt_c = medfilt1( alt_m(mask), 5 );
%else
%    alt_c = alt_m(mask);
%end


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 >= 900);
procAlt(Indx) = 0*ones(Indx,1);

plot( time, dataNav(:,mDepth), 'm', ...
             time, dataNav(:,mDepth) + procAlt, 'b' );
area(time, dataNav(:,mDepth) + procAlt, ...
      '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 Bottom: ' titleStr] );
ylabel( 'Depth, Meters' );
zoom on
grid on
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

