%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% 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;
   figure( h_alt );
 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
if( exist('fontsize') == 0), fontsize = 14; end
if( fontsize ==12 ), linewidth = .5;end
fontweight = 'normal';
if( exist('plotSwIce') ~= 1 ) plotSwIce = 0; end
if( exist('postprocalt') ~= 1 ) postprocalt = 1; end
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

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 >= 290);
procAlt(Indx) = 0*ones(length(Indx),1);
%procAlt(Indx) = NaN(length(Indx),1);    %Fill won't fill with NaN's.
%
%
%if( exist('dvlRange')==1 ),
%  dvlAlt2 = dataDvl(:,dvlRange);
%  indx2 = find( dvlAlt2 == 0. );  
%  dvlAlt2(indx2) = NaN(length(indx2),1);
%  dvlRangeInterp = interp1(dataDvl(:,1),dvlAlt2,time);
%  if( postprocalt) 
%    procAlt = dvlRangeInterp .* cos(dataNav(:,mPhi)) .* ...
%	cos(dataNav(:,mTheta));
%  end
%end
%
% Compute distance made good (x-y plane arc length)
if( exist('posx') & exist('posy') )
  %
  % For 2004.174.10, the ones() is 24kx24k, which exceeds an internal size limit.
  %arclen=tril( ones(length(posx), length(posx)) )*...
  %	 [0; sqrt( abs(diff(posx).*diff(posy)) ) ];
  fprintf('Starting for loop ... \n');
  tic
  arcsegs = [0; sqrt( diff(posx).^2 + diff(posy).^2 ) ];
  arclen = 0*arcsegs;
  for( i=2:length(arcsegs) )
    arclen(i) = arclen(i-1) + arcsegs(i);
  end
  fprintf('For loop finished.\n');
  toc
end
if( exist('dmgSw') == 0 ) dmgSw = 1;end
if( length(arclen) == length(dataNav(:,1)) & dmgSw )
  navxaxis = arclen;
  if( exist('dataDvl') == 1 )
    dvlxaxis    = interp1(dataNav(:,1), navxaxis, dataDvl(:,1) );
  end
  if( exist('dataPSA') == 1 & 0)
    psaxaxis    = interp1(dataNav(:,1), navxaxis, psa916(:,1) );
  end
  if( exist('PA200LR') == 1 )
    pa200xaxis    = interp1(dataNav(:,1), navxaxis, PA200LR(:,1) );
  end
  useArcLen = 1;
else
  navxaxis = time;
  useArcLen = 0;
end


if( plotSwIce == 0 )
plot( navxaxis, dataNav(:,mDepth), 'm', ...
             navxaxis, dataNav(:,mDepth) + procAlt, 'b' );
if(useArcLen == 1) axis equal; end
area(navxaxis, dataNav(:,mDepth) + procAlt, 'facecolor', [0 .75 .85]);
if( exist('mDvlAltitude') == 1 )
  line( navxaxis, dataNav(:,mDepth) + dataNav(:,mDvlAltitude), ...
      'color','m','linestyle','--');
end

  if( exist('alt2') == 1 )
  line( navxaxis, dataNav(:,mDepth) + alt2, ...
      'color','m','linestyle','--'); 
  end

if( exist( 'dataSim' ) == 1 & exist( 'sWaterDepth' ) == 1 )
  simxaxis    = interp1(dataNav(:,1), navxaxis, dataSim(:,1) );
  interpDepth = interp1(dataSim(:,1), dataSim(:,sWaterDepth), navxaxis );
  line( simxaxis, dataSim(:,sWaterDepth),...
      'linewidth', linewidth, 'Color','g' );
%  line( navxaxis, interpDepth,...
%      'linewidth', linewidth, 'Color','g' );
end %if( plotSwIce == 0 )


elseif( plotSwIce == 1 )
plot( navxaxis, dataNav(:,mDepth), 'm', ...
             navxaxis, dataNav(:,mDepth) - procAlt, 'b' );
area(navxaxis, dataNav(:,mDepth), 'facecolor', [0 .75 .85]);
%area(time, dataNav(:,mDepth)-procAlt, ...
%      'facecolor','g');
%     'facecolor', [0 .75 .85]);
%     'facecolor', [0 .75 .5] );

end
set( gca, 'YDir', 'reverse' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
altpXlim = get(gca,'XLim');
line( navxaxis, dataNav(:,mDepth), 'color', [1 0.5 0], 'linewidth', linewidth );
%title( ['Measured Depth and Calculated Bottom: ' titleStr] );
title( ['Vehicle and Bottom Depth: ' titleStr] );
ylabel( 'Depth, Meters' );

if(useArcLen)
  xlabel( 'Distance Made Good (Arc Length), Meters' );
else
  xlabel( 'Elapsed Time, Seconds' );
end
zoom on
grid on
%
% Turn this off for now.
%
if( exist('dvlRange')==1 & 0),
  dvlRangeInterp = interp1(dataDvl(:,1),dataDvl(:,dvlRange),navxaxis);
  dvlAlt = dvlRangeInterp .* cos(dataNav(:,mPhi)) .* cos(dataNav(:,mTheta));
  [xx,yy]  = stairs( navxaxis, dataNav(:,mDepth)+dvlAlt );
  line( xx, yy, 'color','g')
end

strip2 = subplot( 2, 1, 2 );
  if(useArcLen) xaxisD = dvlxaxis;
  else          xaxisD = dataDvl(:,1);
  end
%  plot(xaxisD, dataDvl(:,[dvlRange dvlBeam1 dvlBeam2 dvlBeam3 dvlBeam4]), ...
%      navxaxis, procAlt)
lstr = {};
if( exist('dvlBeam1') == 1 ),
  plot(xaxisD, dataDvl(:,[dvlBeam1 dvlBeam2 dvlBeam3 dvlBeam4]), ...
      navxaxis, procAlt)
  lstr(:,1) = { 'Beam1','Beam2','Beam3','Beam4','Altitude'};
else
  plot(navxaxis, procAlt)
  lstr(:,1) = {'Altitude'};
end
  set(strip2,'XLim',altpXlim);
  set(strip2,'fontsize',fontsize,'fontweigh',fontweight)
  if( exist('psa916') == 1 )
    line( navxaxis, dataNav(:,mPsaRange), 'color','k');
    lstr(length(lstr)+1,1) = {'Psa916 Range'};
  end
  if( exist('PA200LR') == 1 )
    line( pa200xaxis, PA200LR(:,pa200range), 'color','k');
    lstr(length(lstr)+1,1) = {'Pa200LR Range'};
  end
  if(useArcLen)
    xlabel( 'Distance Made Good (Arc Length), Meters' );
    xaxisD = dvlxaxis;
  else
    xaxisD = dataDvl(:,1);
    xlabel( 'Elapsed Time, Seconds' );
  end
  legend(lstr);
  title( 'Dvl Range' );
  ylabel('Range, Meters')
  zoom on
  grid on


