%
%    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.7 2001/11/05 13:19:04 rob Exp $

global titleStr
global vehicleName

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

set( h_dpt, 'name', [vehicleName, ' Depth Control'],'DeleteFcn', 'clear h_dpt');
set(gcf,'position',position)

%
% 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

subplot( 2, 1, 1 )
cla
[xx,yy] = stairs( dataNav(:,m_present_time), dataNav(:,mDepth) );
[xx1,yy1] = stairs( dataDC(:,m_present_time), dataDC(:,cDepth) );
plot( xx, yy, xx1, yy1 );
if exist( 'tDepth' ) == 1
    colors = get( gca, 'ColorOrder' );
    [xx,yy] = stairs( dataDC(:,m_present_time), dataDC(:,tDepth) );
    line( xx, yy, 'Color', colors(3,:) );
end
set( gca, 'ydir', 'reverse' );
title( ['Depth: Commanded-Green; Smoothed Command-Red; Measured-Blue, ' ...
        titleStr] );
xlabel( 'Elapsed Time (s)' );
ylabel( 'Depth (m)' );
%legend('Meas. Depth', 'Cmded. Depth', 'Smooth Depth',0)
zoom on
grid on
if exist('filter_depth') == 1
  [xx2,yy2] = stairs( dataNav(:,1), dataNav(:,filter_depth) );
  line(xx2,yy2,'color','c')
end



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

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

subplot( 4, 1, 4 )
cla

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)' )

clear  xx yy colors
