%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot DVL data.
% AUTHOR:  Rob McEwen
% DATE:    01/3/15
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%

  if exist('dataDvl') ~= 1, return, end

  str = [' ',titleStr,' Plotted on ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;


  if exist( 'dvlFig' ) ~= 1
    dvlFig = figure;
    figure(dvlFig);
%   winpos( position(1,:) );
  else
    figure(dvlFig);
  end
  set( dvlFig, 'Name', 'DVL Plot', 'DeleteFcn', 'clear dvlFig' );
  set(gcf,'position',position)

  nstrips = 7;
  ErrorVal = -32.768;
%
% The RDI Dvl substitutes a -32768 for the velocity components for
% any ping that returns invalid data.  The purpose of the find/indx
% logic below is to drop these values from the plots because they
% shrink the scale too much.
%
  subplot(nstrips,1,1)
  [xx,dvlBx]  = stairs( dataDvl(:,1), dataDvl(:,dvlBotVelx) );
  [xx,dvlWx]  = stairs( dataDvl(:,1), dataDvl(:,dvlWatVelx) );
  indxBx = find( dvlBx == ErrorVal );
  dvlBx(indxBx) = 0*ones(length(indxBx),1);
  indxWx = find( dvlWx == ErrorVal );
  dvlWx(indxWx) = 0*ones(length(indxWx),1);
  plot(xx-dataDvl(1,1), [dvlBx dvlWx ] );
  ylabel('Body X')
  title(['Dvl Measurement; Blue is wrto Bottom ',str]);
  grid on
  zoom on
  MaxVel = 2.5;
  YEMax = MaxVel;  %Prevent autoscaling to outliers
  YEMin =-MaxVel;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)

  subplot(nstrips,1,2)
  [xx,dvlBy]  = stairs( dataDvl(:,1), dataDvl(:,dvlBotVely) );
  [xx,dvlWy]  = stairs( dataDvl(:,1), dataDvl(:,dvlWatVely) );
  indxBy = find( dvlBy == ErrorVal );
  dvlBy(indxBy) = 0*ones(length(indxBy),1);
  indxWy = find( dvlWy == ErrorVal );
  dvlWy(indxWy) = 0*ones(length(indxWy),1);
  plot(xx-dataDvl(1,1), [dvlBy dvlWy ] );
  ylabel('Body Y')
  grid on;
  zoom on;
  YEMax = MaxVel;  %Prevent autoscaling to outliers
  YEMin =-MaxVel;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)

  subplot(nstrips,1,3)
  [xx,dvlBz]  = stairs( dataDvl(:,1), dataDvl(:,dvlBotVelz) );
  [xx,dvlWz]  = stairs( dataDvl(:,1), dataDvl(:,dvlWatVelz) );
  indxBz = find( dvlBz == ErrorVal );
  dvlBz(indxBz) = 0*ones(length(indxBz),1);
  indxWz = find( dvlWz == ErrorVal );
  dvlWz(indxWz) = 0*ones(length(indxWz),1);
  plot(xx-dataDvl(1,1), [dvlBz dvlWz ] );
  ylabel('Body Z')
  grid on;
  zoom on;
  YEMax = MaxVel;  %Prevent autoscaling to outliers
  YEMin =-MaxVel;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)

  subplot(nstrips,1,4)
  [xx,dvlBz]  = stairs( dataDvl(:,1), dataDvl(:,dvlBotVele) );
  [xx,dvlWz]  = stairs( dataDvl(:,1), dataDvl(:,dvlWatVele) );
  indxBz = find( dvlBz == ErrorVal );
  dvlBz(indxBz) = 0*ones(length(indxBz),1);
  indxWz = find( dvlWz == ErrorVal );
  dvlWz(indxWz) = 0*ones(length(indxWz),1);
  plot(xx-dataDvl(1,1), [dvlBz dvlWz ] );
  ylabel('E Vel')
  grid on;
  zoom on;
  YEMax = MaxVel;  %Prevent autoscaling to outliers
  YEMin =-MaxVel;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)
%
% The problem here is that the various vectors dvlBx, dvlBy, etc., may be
% of different lengths.  Thus it isn't possible to use them directly to
% compute the magnitude.  So, start over.
%
%  indxB = intersect(intersect(indxBx,indxBy),indxBz);
%  indxW = intersect(intersect(indxWx,indxWy),indxWz);
   indxB = indxBx;
   indxW = indxWx;

  BotVelMag = sqrt( dataDvl(:,dvlBotVelx).^2 + ...
                    dataDvl(:,dvlBotVely).^2 + ...
                    dataDvl(:,dvlBotVelz).^2 );

  WatVelMag = sqrt( dataDvl(:,dvlWatVelx).^2 + ...
                    dataDvl(:,dvlWatVely).^2 + ...
                    dataDvl(:,dvlWatVelz).^2 );

  indxB = find(BotVelMag > 10);
  BotVelMag(indxB) = 0*ones(length(indxB),1);
  indxW = find(WatVelMag > 10);
  WatVelMag(indxW) = 0*ones(length(indxW),1);

  subplot(nstrips,1,5)
  [xx,yyB]  = stairs( dataDvl(:,1), BotVelMag );
  [xx,yyW]  = stairs( dataDvl(:,1), WatVelMag );
  plot(xx-dataDvl(1,1), [yyB yyW ] );
  ylabel('|Vel|')
  grid on;
  zoom on;
  YEMax = MaxVel;  %Prevent autoscaling to outliers
  YEMin =-MaxVel;
  Ylim = get(gca,'YLim');
  Ylim(1) = max([Ylim(1) YEMin]);
  Ylim(2) = min([Ylim(2) YEMax]);
  set(gca,'YLim',Ylim)

  subplot(nstrips,1,6)
  [xx,yyB]  = stairs( dataDvl(:,1), dataDvl(:,dvlBotVelStat) );
  [xx,yyW]  = stairs( dataDvl(:,1), dataDvl(:,dvlWatVelStat) );
  plot(xx-dataDvl(1,1), [yyB yyW ] );
  if exist('dataK') == 1
    indx = find(   dataK(:,1) < dataDvl(end,1) ...
	         & dataK(:,1) > 0 ...
		 | isnan(dataK(:,1)));    %Add back the NaNs for pen up/down
    line( dataK(indx,1), dataK(indx,mMonK),'color','m');
  end
  ylabel('Bot & Wat Stat')
  grid on;
  zoom on;

  subplot(nstrips,1,7)
  [xx,yy]  = stairs( dataDvl(:,1), dataDvl(:,dvlTemp) );
  plot(xx-dataDvl(1,1), yy );
  ylabel('Celsius')
  xlabel('Elapsed Time (s)');
  grid on;
  zoom on;

  if( exist('dvlPlotMore') ~= 1 ) dvlPlotMore = 1; end

  if( dvlPlotMore ~= 1 ) return;end

  if exist( 'dvlFig2' ) ~= 1
    dvlFig2 = figure;
  end
  figure(dvlFig2);

  set( dvlFig2, 'Name', 'DVL Plot', 'DeleteFcn', 'clear dvlFig2' );
  set(gcf,'position',position)

  subplot(5,1,1)
  [xx,yy]  = stairs( dataDvl(:,1), ...
       [ bitand( dataDvl(:,dvlBotVelStat), 2 )/2 ...
         bitand( dataDvl(:,dvlBotVelStat), 1 )   ...
         bitand( dataDvl(:,dvlWatVelStat), 1 ) ]);
  plot(xx, yy );
  ylabel('Beam 1')
  title(['Status Bits; Low Btm Trk Amplitude: Blue; Low Btm Trk Corr: Green;',...
         ' Low Water Corr-Red'])
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;

  subplot(5,1,2)
  [xx,yy]  = stairs( dataDvl(:,1), ...
       [ bitand( dataDvl(:,dvlBotVelStat), 2^3)/2^3 ...
         bitand( dataDvl(:,dvlBotVelStat), 2^2)/2^2 ...
         bitand( dataDvl(:,dvlWatVelStat), 2  )/2 ]);
  plot(xx, yy );
  ylabel('Beam 2')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;


  subplot(5,1,3)
  [xx,yy]  = stairs( dataDvl(:,1), ...
       [ bitand( dataDvl(:,dvlBotVelStat), 2^5)/2^5 ...
         bitand( dataDvl(:,dvlBotVelStat), 2^4)/2^4 ...
         bitand( dataDvl(:,dvlWatVelStat), 2^2)/2^2 ]);
  plot(xx, yy );
  ylabel('Beam 3')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;

  subplot(5,1,4)
  [xx,yy]  = stairs( dataDvl(:,1), ...
       [ bitand( dataDvl(:,dvlBotVelStat), 2^7)/2^7 ...
         bitand( dataDvl(:,dvlBotVelStat), 2^6)/2^6 ...
         bitand( dataDvl(:,dvlWatVelStat), 2^3)/2^3 ]);
  plot(xx, yy );
  ylabel('Beam 4')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;

  subplot(5,1,5)
  [xx,yy]  = stairs( dataDvl(:,1), ...
       [ bitand( dataDvl(:,dvlWatVelStat), 2^4)/2^4 ]);
  plot(xx, yy );
  ylabel('Too Shallow')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;

  
  return

%
% Expand this plot to have all 4 beam ranges.  Add the individual
% beam ranges to dvl.log.
%
  
  if exist( 'dvlFig3' ) ~= 1
    dvlFig3 = figure;
  end
  figure(dvlFig3);

  set( dvlFig3, 'Name', 'DVL Range', 'DeleteFcn', 'clear dvlFig3' );
  set(gcf,'position',position)

  plot(dataDvl(:,1),dataDvl(:,dvlRange))
  grid on
  ylabel('Meters')
  xlabel('Seconds')
  title(['Dvl Range ',str]);
  
  

  if exist( 'dvlFig3' ) ~= 1
    dvlFig3 = figure;
  end
  figure(dvlFig3);

  set( dvlFig3, 'Name', 'DVL Plot', 'DeleteFcn', 'clear dvlFig3' );
  set(gcf,'position',position)
  dvlHeading = dataDvl(:,dvlHeading);
  subplot(3,1,1)
  plot(dataDvl(:,1), mod360(dvlHeading*180/pi))
  ylabel('Heading, Deg')
  title(['Dvl AHRS, Degrees',str])
  grid on
  zoom on
  xlim = get(gca,'XLim');
  xlim(1) = 0;
  set(gca,'XLim',xlim);
  
  subplot(3,1,2)
  plot(dataDvl(:,1), dataDvl(:,dvlPitch)*180/pi)
  ylabel('Dvl Case Pitch, Deg')
  grid on
  zoom on

  subplot(3,1,3)
  plot(dataDvl(:,1), dataDvl(:,dvlRoll)*180/pi)
  ylabel('Dvl Case Proj Roll, Deg')
  grid on
  zoom on
  

  if exist( 'dvlFig4' ) ~= 1
    dvlFig4 = figure;
  end
  figure(dvlFig4);

  set( dvlFig4, 'Name', 'DVL Plot', 'DeleteFcn', 'clear dvlFig4' );
  set(gcf,'position',position)

  plotAlt=0;
  
  subplot(2,1,1)
  plot(dataDvl(:,1), dataDvl(:,[range1:range4]))
  grid on
  zoom on
  title(['Beam Ranges',str])
  xlabel('Seconds')
  ylabel('Meters')
  legend('Beam 1','Beam 2','Beam 3','Beam 4')
  if( exist('dataAvp') == 1 & plotAlt ==1 )
    line(avp.time, dataAvp(:,Altitude), 'color','m')
    legend('Beam 1','Beam 2','Beam 3','Beam 4','Altimeter')
    xlim = get(gca,'XLim');
    xlim(1) = 0;
    set(gca,'XLim',xlim);
  end
  if( exist('alt') == 1 )
    line(dataDvl(:,1), dataDvl(:,alt), 'color', 'y')
    legend('Beam 1','Beam 2','Beam 3','Beam 4','Altitude')
  end
  if( exist('dataAvp') == 1 & plotAlt == 1 & exist('alt') == 1 )
    legend('Beam 1','Beam 2','Beam 3','Beam 4','Altimeter','Altitude')
  end
  
  subplot(2,1,2)
  if( exist('alt') ~= 1 )
    plAng = asin( sqrt( dataDvl(:,dn1).^2 + dataDvl(:,dn2).^2 ) );
    plot(dataDvl(:,1), plAng*180/pi)
  else
  plot(dataDvl(:,1), dataDvl(:,[planeAng planeHdg])*180/pi)
  grid on
  zoom on
  title('Angle between Normal and Vehicle z')
  xlabel('Seconds')
  ylabel('Degrees')
  legend('Ang from Normal to b_z','Ang from Uphill to b_x')
  end  

  if exist( 'dvlFig5' ) ~= 1
    dvlFig5 = figure;
  end
  figure(dvlFig5);

  set( dvlFig5, 'Name', 'DVL Nav Plot', 'DeleteFcn', 'clear dvlFig5' );
  set(gcf,'position',position)

   plot( dataDvl(:,distBy),  dataDvl(:,distBx), ...
         dataDvl(:,DmgBEast), dataDvl(:,DmgBNrth))
%         dataDvl(:,DmgWEast), dataDvl(:,DmgWNrth))
  xlabel('Meters, East')
  ylabel('Meters, North')
  axis equal
  grid on
  zoom on
  hold on
  legend('Mbari','Rdi Bottom')
  title(['Cartesian Position',str])
  indx = [1:20:length(dataDvl)];
  if 0
    if( exist('dataAvp') == 1 )
      quiver( avp.position(indx,2), avp.position(indx,1), ...
	  sin(avp.compass(indx)), cos(avp.compass(indx)) )
    end
    quiver( dataDvl(indx,distBy),  dataDvl(indx,distBx), ...
	sin(dvlHeading(indx)), cos(dvlHeading(indx)) )
  end  %quiver
  if( exist('dataAvp') == 1 )
    line(avp.position(:,2), avp.position(:,1), 'color','r')
    legend('Mbari','Rdi Bottom','Vision')
  end
  hold off
  return





