%
%    VPLANE - script to plot depth and pitch control parameters in
%    subplots of one large window.
%
%    MIT SeaGrant Lab
%
%    Copyright (C) 1995-1998
%    Massachussetts Institute of Technology
%    Sea Grant AUV Laboratory
%    $Id: vplane.m,v 1.17 2013/12/04 21:09:58 rob Exp $

global titleStr
global vehicleName

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

set( h_dpt, 'name', [vehicleName, ' Depth Control'],'DeleteFcn', 'clear h_dpt');
set(gcf,'position',position)
if( exist('fontsize') == 0), fontsize = 14; end
if( fontsize ==12 ), linewidth = .5;end
fontweight = 'normal';
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

%
% I'm not sure what he's doing with this.  I replaced "rows" in
% subplot(2,1,1) with ":".
%
%rows = 1:length(data);
%if diff(dataDC(1:2,c_depth)) > .5
%    rows(1) = [];
%end
if( exist('linewidth') ~= 1 ) linewidth = 1;end
vplaneH1=subplot( 2, 1, 1 );
cla
set(gca, 'linewidth',linewidth)
[xx,yy] = stairs( dataNav(:,m_present_time), dataNav(:,mDepth) );
[xx1,yy1] = stairs( dataDC(:,m_present_time), dataDC(:,cDepth) );
plot( xx, yy, xx1, yy1 );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
if exist( 'tDepth' ) == 1
    colors = get( gca, 'ColorOrder' );
    [xx,yy] = stairs( dataDC(:,m_present_time), dataDC(:,tDepth) );
    line( xx, yy, 'Color', colors(3,:) );
    set(gca,'fontsize',fontsize,'fontweigh',fontweight)
end
set( gca, 'ydir', 'reverse' );
vTitleStr = sprintf('%s %s \n %s',...
            'Depth: Commanded-Green; Smoothed Command-Red; Measured-Blue ',...
                    titleStr, ['If Native Alt Ctrl Green=Alt Cmd, Red=Equiv. ' ...
                    'Depth Cmd (Not Used)']);
title(vTitleStr);
%title( ['Depth: Commanded-Green; Smoothed Command-Red; Measured-Blue ' ...
%        titleStr] );
xlabel( 'Elapsed Time (s)' );
ylabel( 'Depth (m)' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
%legend('Meas. Depth', 'Cmded. Depth', 'Smooth Depth',0)
zoom on
grid on
hold on
if exist('filter_depth') == 1
  [xx2,yy2] = stairs( dataNav(:,1), dataNav(:,filter_depth) );
  line(xx2,yy2,'color','c')
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
end

if( exist('cam')==1 )
  camz = interp1(dataNav(:,1), dataNav(:,mDepth), cam.data(:,1));
  hold on
  camhand = plot(cam.data(:,1), camz,'kp', 'linewidth',linewidth)
  %lstr(length(lstr)+1,1) = {'Photos'};
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
end

if( exist( 'dataSim' ) == 1 )
  vplanes.pOn  = find( diff(dataSim(:,sOmega_P)) > 0 );
  vplanes.pOff = find( diff(dataSim(:,sOmega_P)) < 0 );
  %if( vplanes.pOn ~= [] )
  line( dataNav(vplanes.pOn,1), dataNav(vplanes.pOn,mDepth), ...
      'color', 'g', 'linestyle','x', 'linewidth', linewidth );
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  %end
  %if( vplanes.pOff ~= [] )
  line( dataNav(vplanes.pOff,1), dataNav(vplanes.pOff,mDepth), ...
      'color', 'r', 'linestyle','x', 'linewidth',  linewidth );
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  %end
end


vplaneH2=subplot( 4, 1, 3 );
cla
set(gca,'linewidth',linewidth)
[xx,yy] = stairs( dataDC(:,m_present_time), dataDC(:,cTheta)*180/pi );
plot( dataNav(:,m_present_time), dataNav(:,mTheta)*180/pi, xx, yy );
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
if exist( 'dIntegral' ) == 1
    line( dataDC(:,1), dataDC(:,dIntegral)*180/pi, 'Color', colors(4,:) );
    title( 'Vehicle Pitch: Commanded-Green; Measured-Blue; Depth Integral-Cyan')
    set(gca,'fontsize',fontsize,'fontweigh',fontweight)
    if exist( 'dIntegral2' ) == 1
      line( dataDC(:,1), dataDC(:,dIntegral2)*180/pi, 'Color', 'm' );
      title( 'Vehicle Pitch: Commanded-Green; Measured-Blue; Depth Integral-Cyan') 
      set(gca,'fontsize',fontsize,'fontweigh',fontweight)
    end
else
    title( 'Vehicle Pitch: Commanded-Green; Measured-Blue')
end
if( exist( 'mDepthRateTerm' ) == 1 )
  line( dataDC(:,1), dataDC(:,mDepthRateTerm)*180/pi, 'Color','r')
end



ylabel( 'Pitch (deg)' );
zoom on
grid on

vplaneH3=subplot( 4, 1, 4 );
cla
set(gca, 'linewidth',linewidth)

time = dataDC(:,m_present_time);
plot( time,   180/pi*dataDC(:,pitchProp), 'r' );
  line( time, 180/pi*dataDC(:,pitchRate), 'Color', [0 .5 0] );
  line( time, 180/pi*dataDC(:,pitchInt) , 'Color', [0 0.75 0.75] )
  line( time, 180/pi*dataDC(:,cDeltaE), 'Color', 'b' );
  set( gca, 'ydir', 'reverse' );
  zoom on
  grid on
  title( ['Elevator PID: (Prop-Red, Int-Cyan, Deriv-Green, Elev-Blue)'] );
  xlabel( 'Elapsed Time (s)' )
  ylabel( 'Elevator Control Activity (deg)' )
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)

  linkaxes(get(gcf,'children'),'x')
  
  clear  xx yy colors


if( exist('vpxlim') == 1 & length(vpxlim) == 2 )
  fprintf('%s\n','Reset time axis limits.')
  set(vplaneH1,'xlim',vpxlim);
  set(vplaneH2,'xlim',vpxlim);
  set(vplaneH3,'xlim',vpxlim);
end

