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

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

  if exist('titleStr') ~= 1
    titleStr = ['; Data Set: ', DirName];
  end
  
  str = [' ',titleStr,' Plotted on ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;
  if( exist('position') ~= 1 ) position = [3 59 1213 1070]; end

  
  if( exist('fontsize') == 0), fontsize = 20; end
  if( fontsize ==12 ), 
    linewidth = .5;
  else
    linewidth = 1;
  end
  fontweight = 'normal';


  if exist( 'dvlSideFig' ) ~= 1
    dvlSideFig = figure;
    figure(dvlSideFig);
%   winpos( position(1,:) );
  else
    figure(dvlSideFig);
  end
  set( dvlSideFig, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear dvlSideFig' );
  set(gcf,'position',position)
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  rows = 2;
  cols = 1;
  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.
%
  nplot = 0;
  nplot = nplot+1;
  dvlHandles(nplot,1)=subplot(rows,cols,nplot);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  if(exist('plotSwDvlStairs','var')==0) plotSwDvlStairs=1;end
  if(plotSwDvlStairs)
    [xx,dvlBx]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.BotVelx) );
    [xx,dvlWx]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.WatVelx) );
    [xx,dvlBy]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.BotVely) );
    [xx,dvlWy]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.WatVely) );
    [xx,dvlBz]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.BotVelz) );
    [xx,dvlWz]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.WatVelz) );
  else
    dvlBx  = dvlSide.data(:,dvlSide.BotVelx);
    dvlWx  = dvlSide.data(:,dvlSide.WatVelx);
    dvlBy  = dvlSide.data(:,dvlSide.BotVely);
    dvlWy  = dvlSide.data(:,dvlSide.WatVely);
    dvlBz  = dvlSide.data(:,dvlSide.BotVelz);
    dvlWz  = dvlSide.data(:,dvlSide.WatVelz);
    xx     = dvlSide.data(:,1);
  end
  
  
  
  BotVelMag = sqrt( dvlSide.data(:,dvlSide.BotVelx).^2 + ...
                    dvlSide.data(:,dvlSide.BotVely).^2 + ...
                    dvlSide.data(:,dvlSide.BotVelz).^2 );

  WatVelMag = sqrt( dvlSide.data(:,dvlSide.WatVelx).^2 + ...
                    dvlSide.data(:,dvlSide.WatVely).^2 + ...
                    dvlSide.data(:,dvlSide.WatVelz).^2 );

  indxB = find(BotVelMag > 10);
  BotVelMag(indxB) = NaN(length(indxB),1);
  indxW = find(WatVelMag > 10);
  WatVelMag(indxW) = NaN(length(indxW),1);
  
  if(plotSwDvlStairs)
    [xx,yyB]  = stairs( dvlSide.data(:,1), BotVelMag );
    [xx,yyW]  = stairs( dvlSide.data(:,1), WatVelMag );
  else
    yyB  = BotVelMag;
    yyW  = WatVelMag;
  end
  
  
  indxBx = find( dvlBx == ErrorVal );
  dvlBx(indxBx) = NaN(length(indxBx),1);
  indxWx = find( dvlWx == ErrorVal );
  dvlWx(indxWx) = NaN(length(indxWx),1);
  indxBy = find( dvlBy == ErrorVal );
  dvlBy(indxBy) = NaN(length(indxBy),1);
  indxWy = find( dvlWy == ErrorVal );
  dvlWy(indxWy) = NaN(length(indxWy),1);
  indxBz = find( dvlBz == ErrorVal );
  dvlBz(indxBz) = NaN(length(indxBz),1);
  indxWz = find( dvlWz == ErrorVal );
  dvlWz(indxWz) = NaN(length(indxWz),1);

  dvlPlotHandles=...
    plot(xx-dvlSide.data(1,1), [dvlBx dvlWx dvlBy dvlWy dvlBz dvlWz yyB yyW]);
  if(plotSwDvlStairs)
    set(dvlPlotHandles(1),'color','b','linestyle','-');
    set(dvlPlotHandles(2),'color','b','linestyle','--');
    set(dvlPlotHandles(3),'color','g','linestyle','-');
    set(dvlPlotHandles(4),'color','g','linestyle','--');
    set(dvlPlotHandles(5),'color','r','linestyle','-');
    set(dvlPlotHandles(6),'color','r','linestyle','--');
    set(dvlPlotHandles(7),'color','m','linestyle','-');
    set(dvlPlotHandles(8),'color','m','linestyle','--');
  else
    set(dvlPlotHandles(1),'color','b','linestyle','x');
    set(dvlPlotHandles(2),'color','b','linestyle','.');
    set(dvlPlotHandles(3),'color','g','linestyle','x');
    set(dvlPlotHandles(4),'color','g','linestyle','.');
    set(dvlPlotHandles(5),'color','r','linestyle','x');
    set(dvlPlotHandles(6),'color','r','linestyle','.');
    set(dvlPlotHandles(7),'color','m','linestyle','x');
    set(dvlPlotHandles(8),'color','m','linestyle','.');
  end
  ylabel('m/s ')
  title(['DvlSide Measurements; Solid is wrto Bottom ',str]);
  legend('v_x_N','v_x_W','v_y_N','v_y_W','v_z_N','v_z_W','|v|_N','|v|_W');
  grid on
  zoom on
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%  legend('x Btm','x Wtr',...
%         'y Btm','y Wtr',...
%         'z Btm','z Wtr',...
%         'mag Btm','mag Wtr');

  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)


  
  
  
  nplot = nplot+1;
  dvlHandles(nplot,1)=subplot(4,cols,3);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  [xx,yyB]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.BotVelStat) );
  [xx,yyW]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.WatVelStat) );
  plot(xx-dvlSide.data(1,1), [yyB yyW ~yyB*100] );
  if exist('dataK') == 1
    indx = find(   dataK(:,1) < dvlSide.data(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
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  ylabel('Bot & Wat Stat')
  grid on;
  zoom on;

  nplot = nplot+1;
  dvlHandles(nplot,1)=subplot(4,cols,4);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  [xx,yy]  = stairs( dvlSide.data(:,1), dvlSide.data(:,dvlSide.Temp) );
  plot(xx-dvlSide.data(1,1), yy );
  ylabel('Celsius')
  xlabel('Elapsed Time (s)');
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  grid on;
  zoom on;

  %linkaxes(get(gcf,'children'),'x')
  linkaxes(dvlHandles,'x')

if( exist('vpxlim') == 1 & length(vpxlim) == 2 )
  fprintf('%s\n','Reset time axis limits.')
  for( i=1:length(dvlHandles) )
    set( dvlHandles(i,1), 'xlim',vpxlim );
  end
end
  
  
  if( exist('dvlPlotMore') ~= 1 ) dvlPlotMore = 1; end

  if( dvlPlotMore ~= 1 ) return;end

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

  set( dvlSideFig2, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear dvlSideFig2' );
  set(gcf,'position',position)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  subplot(5,1,1)
  [xx,yy]  = stairs( dvlSide.data(:,1), ...
       [ bitand( dvlSide.data(:,dvlSide.BotVelStat), 2 )/2 ...
         bitand( dvlSide.data(:,dvlSide.BotVelStat), 1 )   ...
         bitand( dvlSide.data(:,dvlSide.WatVelStat), 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( dvlSide.data(:,1), ...
       [ bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^3)/2^3 ...
         bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^2)/2^2 ...
         bitand( dvlSide.data(:,dvlSide.WatVelStat), 2  )/2 ]);
  plot(xx, yy );
  ylabel('Beam 2')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;


  subplot(5,1,3)
  [xx,yy]  = stairs( dvlSide.data(:,1), ...
       [ bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^5)/2^5 ...
         bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^4)/2^4 ...
         bitand( dvlSide.data(:,dvlSide.WatVelStat), 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( dvlSide.data(:,1), ...
       [ bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^7)/2^7 ...
         bitand( dvlSide.data(:,dvlSide.BotVelStat), 2^6)/2^6 ...
         bitand( dvlSide.data(:,dvlSide.WatVelStat), 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( dvlSide.data(:,1), ...
       [ bitand( dvlSide.data(:,dvlSide.WatVelStat), 2^4)/2^4 ]);
  plot(xx, yy );
  ylabel('Too Shallow')
  set(gca,'YLim',[0 2])
  grid on;
  zoom on;
  
  linkaxes(get(gcf,'children'),'x')

if(0)  
  if exist( 'dvlSideFig4' ) ~= 1
    dvlSideFig4 = figure;
  end
  figure(dvlSideFig4);

  set( dvlSideFig4, 'Name', 'DVLSide Ahrs', 'DeleteFcn', 'clear dvlSideFig4' );
  set(gcf,'position',position)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
  dvlPsi = dvlSide.data(:,dvlSide.Heading);
  subplot(3,1,1)
  plot(dvlSide.data(:,1), mod360(dvlPsi))
  ylabel('Heading, Deg')
  title(['DvlSide AHRS, Degrees',str])
  grid on
  zoom on
  xlim = get(gca,'XLim');
  xlim(1) = 0;
  set(gca,'XLim',xlim);
  
  subplot(3,1,2)
  plot(dvlSide.data(:,1), dvlSide.data(:,dvlSide.Pitch))
  ylabel('Dvl Case Pitch, Deg')
  grid on
  zoom on

  subplot(3,1,3)
  plot(dvlSide.data(:,1), dvlSide.data(:,dvlSide.Roll))
  ylabel('Dvl Case Proj Roll, Deg')
  grid on
  zoom on
end


  %
  % Precision Control Code for example.
  %

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

  set( dvlSideFig4, 'Name', 'DvlSide Plot', 'DeleteFcn', 'clear dvlSideFig4' );
  set(gcf,'position',position)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

  
  plot(dvlSide.data(:,1), dvlSide.data(:,[dvlBeam1:dvlBeam4 dvlRange]))
  grid on
  zoom on
  title(['Beam Ranges',str])
  xlabel('Seconds')
  ylabel('Meters')
  legend('Beam 1','Beam 2','Beam 3','Beam 4', 'Range')
  if( exist('alt') == 1 )
    line(dvlSide.data(:,1), dvlSide.data(:,alt), 'color', 'y')
    legend('Beam 1','Beam 2','Beam 3','Beam 4','Range','Altitude')
  end

  return

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

  set( dvlSideFig5, 'Name', 'DVL Nav Plot', 'DeleteFcn', 'clear dvlSideFig5' );
  set(gcf,'position',position)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)

   plot( dvlSide.data(:,distBy),  dvlSide.data(:,distBx), ...
         dvlSide.data(:,DmgBEast), dvlSide.data(:,DmgBNrth))
%         dvlSide.data(:,DmgWEast), dvlSide.data(:,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(dvlSide.data)];
  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( dvlSide.data(indx,distBy),  dvlSide.data(indx,distBx), ...
	sin(dvlPsi(indx)), cos(dvlPsi(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





