%
%    HEADING - Comprehensive plots of yaw and yaw control
%
%    MIT SeaGrant
%
% Copyright (C) 1995-1997, Massachussetts Institute of Technology
% Sea Grant Underwater Vehicles Laboratory
% $Revision: 1.21 $, $Date: 2014/02/28 21:01:54 $

global titleStr;
global vehicleName

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

set( h_yaw, 'Name', [vehicleName,' Yaw Control'], 'DeleteFcn', 'clear h_yaw' );
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)

time = dataDC(:,1);

if( exist('kxte') ) 
  nstrips=3;
else 
  nstrips =2;
end
stripno = 1;

if( nstrips == 3 )
  subplot( nstrips, 1, stripno )
  cla
%  stairs( time, dataDC(:,xTrackError) )
  hold on
  stairs( time, dataDC(:,kxte)*180/pi,'b' )
  stairs( time, dataDC(:,xteIntegral)*180/pi,'g' )
  stairs( time, dataDC(:,dPsi)*180/pi,'r' )
  stairs( time, dataDC(:,xTrackError),'m' )
  if(exist('xTrackErrorWay')) 
      stairs( time, dataDC(:,xTrackErrorWay),'m--' );
  end
  %set(gca,'ylim',[-12 12]);
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  stripno=stripno+1;
  grid on
  ylabel('Heading Delta, Degrees')
  title([ 'Blue-Posn Error, Green-Integral, Red-Total Cmd, ', ...
	'Magenta-Posn Error, m; ', titleStr] );
  hold off
end

subplot( nstrips, 1, stripno )
cla

[xx,yy] = stairs( time, mod360(dataDC(:,cPsi)*180/pi) );

%ydat = 180/pi*dataNav(:,mPsi);
%ind = find( abs(diff(dataDC(:,mPsi))) > pi );
%if ~isempty(ind), ydat( ind ) = NaN; end

plotptr = plot( dataNav(:,1), mod360(dataNav(:,mPsi)*180/pi), xx, yy );
hold on

if(~exist('plotSwFlags')) plotSwFlags=0;end

if(plotSwFlags)

    %Left hand turn flag:
    indxf = find(logical(bitand(dataDC(:,newEasting),2^3)));
    plotptr2 = plot( dataDC(indxf,1), mod360(dataDC(indxf,cPsi)*180/pi),'r.'); 

    %Right had turn flag
    indxf = find(logical(bitand(dataDC(:,newEasting),2^4)));
    plotptr3 = plot( dataDC(indxf,1), mod360(dataDC(indxf,cPsi)*180/pi),'g.'); 

    %Out of the lane flag.  Plot this last to overlay the first two, since
    %this also superceeds logically.
    indxf = find(logical(bitand(dataDC(:,newEasting),2^2)));
    plotptr4 = plot( dataDC(indxf,1), mod360(dataDC(indxf,cPsi)*180/pi),'b.'); 

    %Wall present flag.  
    indxf = find(logical(bitand(dataDC(:,newEasting),2^6)));
    plotptr5 = plot( dataDC(indxf,1), mod360(dataDC(indxf,cPsi)*180/pi)*0,'c.'); 
end

if(exist('wallBearing')) stairs( time, dataDC(:,wallBearing)*180/pi,'g-.' );end

plot( dataDvl(:,1), mod360(dataDvl(:,dvlHeading) - 15.2), 'm' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

if 0
  % From Jerome's lbl algorithm
if exist('heading_bias') == 1
  line(dataNav(:,1),...
       mod360((dataNav(:,heading_bias) + dataNav(:,mPsi))*180/pi),'color','c')
end
end

%lxmax   = get(plotptr(1), xmax);
%set(plotptr, 'xlim', [0 lxmax]);

%ydat = 180/pi*dataDC(:,tPsi);
%ind = find( abs(diff(data(:,tPsi))) > pi );
%if ~isempty(ind), ydat( ind ) = NaN; end

line( dataDC(:,1), mod360(dataDC(:,tPsi)*180/pi), 'Color', 'r' );
set(gca,'fontsize',fontsize,'fontweigh',fontweight)

title([ 'Heading: Commanded Bearing-Green; Smoothed Command-Red; Measured-Blue: ' ...
        titleStr] );
xlabel( 'Elapsed time (s)' );
ylabel( 'Heading (deg)' );
%legend( 'Meas.', 'Cmded', 'Smoothed',3);
zoom on
grid on
stripno=stripno+1;

subplot( nstrips, 1, stripno )
cla
stairs( dataDC(:,1), 180/pi*dataDC(:,psiProp), 'Color', 'r' );
hold on
stairs( dataDC(:,1), 180/pi*dataDC(:,psiInt),  'Color', [0 .75 .75] );
stairs( dataDC(:,1), 180/pi*dataDC(:,psiRate), 'Color', [0 .5 0] );
stairs( dataDC(:,1), 180/pi*dataDC(:,cDeltaR), 'Color', 'b' );
zoom on
grid on
set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set( gca, 'ydir', 'reverse' );
title( ['Rudder PID; Prop-Red; Deriv-Green; Rudder-Blue)'] );
%legend( 'Prop.', 'Int.', 'Deriv', 'Cmd. Rudder');
xlabel( 'Elapsed Time (s)' )
ylabel( 'Rudder Control Activity (deg)' )

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

