%--------------------------------------------------------------------------
%
% PURPOSE: Plot the steering dynamics of the Altex simulation.
% AUTHOR:  Rob McEwen
% DATE:    99/2/12
%
%--------------------------------------------------------------------------
%
%
  str = [', Case:',caseName,' RSM ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;


  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(tout, [yout(:,uIx)]);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Meters/Sec')
  title(['Surge Rate, Meters/Second',str]);
  grid on
  subplot(3,1,2)
  if LinFlg, 
    plot(tout, [ySway(:,1) yout(:,vIx)]);
  else
    plot(tout, [yout(:,vIx)]);
  end
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Meters/Sec')
  title(['Sway Rate, Meters per Second',str]);
  grid on
  if LinFlg, legend('Linear','NonLin'), end
  subplot(3,1,3)
  if LinFlg, 
    plot(tout, [ySway(:,2) yout(:,rIx)]/d2r);
  else
    plot(tout, [yout(:,rIx)]/d2r);
  end
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Yaw Rate, Degrees/Second',str]);
  if LinFlg, legend('Linear','NonLin'), end
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(tout, [yout(:,rollIx)*180/pi]);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Roll, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(tout, [yout(:,pitchIx)]/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Pitch, Degrees',str]);
  grid on
  subplot(3,1,3)
  plot(tout, [yout(:,yawIx)*180/pi]);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Heading, Degrees',str]);
  xlabel('Seconds');
  grid on


  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(tout, yout(:,[pitchIx pitchCmdIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Pitch, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(tout, [yout(:,zPosIx)]);
  set( gca, 'ydir', 'reverse' );  
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Meters')
  title(['Depth, Meters',str]);
  xlabel('Seconds');
  grid on
  subplot(3,1,3)
  plot(tout, [yout(:,deltaEIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Elevator Angle',str]);
  xlabel('Seconds');
  grid on



%
% Heading and cross-track error, in the Waypoint frame.  Also, I'm 
% using the commanded heading angle here to convert to the Waypoint frame.
% In waypoint mode, the heading angle is really the bearing.
  xte = cos(Heading)*yout(:,yPosIx) - sin(Heading)*yout(:,xPosIx);
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(2,1,1)
  if LinFlg, 
    plot(tout, [ySway(:,4) yout(:,yawIx)]/d2r);
    legend('Linear Heading','Simulated Heading',1)
  else
    plot(tout,yout(:,[yawIx headingCmdIx])/d2r)
  end
  ylabel('Deg')
  grid on
  title(['Heading',str]);
  subplot(2,1,2)
  if LinFlg, 
    plot(tout, [ySway(:,3) xte]);
    legend('Linear Error','Simulated Error',1)
  else
    plot(tout,[xte])
  end
  title(['Cross-Track Error',str]);
  ylabel('Meters')
  grid on
  xlabel('Seconds')
%
% Plot drift angles at CG and stern.  See notes, Eqn.'s 8 & 9.
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  if LinFlg,
    plot(tout, [beta yout(:,betaIx), betaR, yout(:,betaRIx),...
             AofA, yout(:,AofAIx) yout(:,deltaRIx) ]*r2d)
    legend('Lin CG Drift', 'Nonlin CG Drift', 'Lin Stern Drift', ...
           'Nonlin Stern Drift', 'Lin Rudder Attack', ...
           'Nonlin Rudder Attack','Rudder Angle',1)
  else
    plot(tout, ...
    [yout(:,betaIx), yout(:,betaRIx), yout(:,AofAIx) yout(:,deltaRIx)]*r2d)
    legend('Horiz. Drift Angle at CG', 'Horiz. Drift Angle at Stern',...
           'Angle of Attack on the Ring','deltaR',1)
  end
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(['Horizontal Drift Angle at CG, H.D.A. at Rudder, Angle of Attack',str])
  ylabel('Degrees')
  xlabel('Seconds');
  grid on
%
% Plot actuator counts and counts per sample period
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(tout, yout(:,[rActCntsIx rActDotIx]))
  ylabel('Counts and Counts per Ts')
  xlabel('Seconds')
  title(['Actuator Counts and Counts per Sample Period ',str])
  legend('Counts','Counts per Sample Period',1)
  grid on
%
% Plot the turning moment due to the rudder.  See notes, Eqn. 32.  Also
% plot the torque about the tailcone pivot point.
%
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  if LinFlg,
    plot(tout, [Nturn yout(:,TrqNRIx) Npivot yout(:,TrqPIx)]);
    legend('Lin Vehicle CG','Nonlin Veh CG','Lin Pivot','Nonlin Pivot',0)
  else
    plot(tout, [yout(:,TrqNRIx) yout(:,TrqPIx)]);
    legend('Torque About Veh CG','Torque About Pivot Pt',0)
  end
%  if LinFlg,
%    plotyy(tout, [Nturn yout(:,TrqNRIx) Npivot yout(:,TrqPIx)],...
%           tout, [Nturn yout(:,TrqNRIx) Npivot yout(:,TrqPIx)]/ftlbf2nm);
%    legend('Lin Vehicle CG','Nonlin Veh CG','Lin Pivot','Nonlin Pivot',0)
%  else
%    plotyy(tout, [yout(:,TrqNRIx) yout(:,TrqPIx)],...
%           tout, [yout(:,TrqNRIx) yout(:,TrqPIx)]/ftlbf2nm);
%    legend('Torque About Veh CG','Torque About Pivot Pt',0)
%  end
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(['Moments about CG and Tailcone Pivot Point',str])
  ylabel('Newton-Meters')
  xlabel('Seconds');
  grid on
%
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  if LinFlg,
    plot(tout, [FYR yout(:,FYRIx)]);
    legend('Lin Rud Frc','Nonlin Rud Frc',0)
    grid on
    ylabel('Newtons')
  else
    Nunits1 = 30;  %Meters
    Ax = plotyy(tout, [yout(:,[FYRIx FYTIx FRAIx])],...
                tout, [yout(:,[FYRIx FYTIx FRAIx])]/lbf2n);
         set(gcf,'CurrentAxes',Ax(1));
%        set(gca,'ylim',[min(yout(:,FRAIx)) max(yout(:,FRAIx))*1.05]);
         grid on
         if hardflag == 3, set(gca,'FontUnits','Normalized'), end
         ylabel('Newtons')
         ylim1 = get(gca,'ylim');
         set(gca,'ytick',[ceil(ylim1(1)/10)*10:Nunits1:floor(ylim1(2)/10)*10])
         set(gcf,'CurrentAxes',Ax(2));
         ylim2 = ylim1/lbf2n;
         Nunits2 = Nunits1/lbf2n;
         set(gca,'ylim',ylim2);
%        set(gca,'ytick',[ceil(ylim2(1)/10)*10:Nunits2:floor(ylim2(2)/10)*10])
         legend('Rudder on Body Y','Thrust on Body Y','Actuator Axis',0)
         ylabel('Pounds Force')
  end
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(['Rudder Force Resolved onto the Vehicle y & Actuator Axes',str])
  xlabel('Seconds');
%
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(yout(:,yPosIx), yout(:,xPosIx))
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(['Vehicle Position',str])
% title(['Vehicle Position, Meters for a +',num2str(deltaR/d2r),...
%        ' Deg Tailcone Angle',str])
  ylabel('x Position in EF Frame, Meters');
  xlabel('y Position in EF Frame, Meters');
  axis equal
  grid on
%
if 0
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(Area(250:500),-1./poles(250:500,:))
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(' Open Loop System Time Constants Versus Rudder Area')
  ylabel('Seconds');
  xlabel(['Meters^2',str]);
  grid on
end
%
  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(tout, yout(:,[rAmpsIx rAmpSecIx]))
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title(' Rudder Actuator Current')
  ylabel('Amps');
  xlabel(['Seconds',str]);
  grid on
%
  if hardflag==0,
    return
  elseif hardflag==1,
    optstr = ' -depsc';      %Encapsulated Postscript
  elseif hardflag==2,
    optstr = ' -djpeg90';    %JPEG
  elseif hardflag==3,
    optstr = ' -dtiff';      %TIFF
  else
    disp(' Hardflag must be 0, 1, 2 or 3');
    return
  end

  nfig = 10;
  for i = 1:nfig
    eval(['print -f', num2str(i), optstr, ' simfig',num2str(i), caseName])
  end
  hardflag = 0;
