%
% PURPOSE: Compare Altex/QNX sim data with Odyssey.
%
% AUTHOR: rsm
%
%
%
% Units and Fundamental Constants:
  in2m = .0254;             % Inches to meters.
  d2r  = pi/180;            % Degrees to radians
  r2d  = 180/pi;            % Radians to degrees
  rho  = 1028;              % Density of sea water at 0 Deg C; kg/m^3
  s2kg = 14.59390;          % Slugs to kg
  ft2m = .3048;             % Feet to meters
  sft2kgm2 = 1.355818;      % Slug-ft^2 to kg-m^2
  lbf2n    = 4.448;         % Pounds force to Newtons
  kph2mps  = 1852/3600;     % Knots per hour to meters per sec
  ftlbf2nm = 1.3558179;     % Foot-pounds force to Newton-meters.
  oi2nm    = .0070615516;   % Ounce-inches to Newton-meters.
  m2c= 200*12/in2m;         % Meters-to-counts
  c2m= 1/m2c;               % Counts-to-meters
  g  = 9.806;               % Free-fall acceleration due to gravity; m/sec^2.
%
  hardflag=0;

  tIx = 1;

  caseName = 'QNX';
  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(dataSim(:,tIx), dataSim(:,sTheta), data(:,1),data(:,sim_theta));
%      dataDC(:,tIx), dataDC(:,cTheta));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Pitch, Radians',str]);
  grid on
  subplot(3,1,2)
  plot(dataSim(:,tIx), dataSim(:,sPos_z), data(:,1), data(:,m_depth));
  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(dataSim(:,tIx), [dataSim(:,[sDeltaE scDeltaE])],...
       data(:,1),      data(:,[m_elevator_angle c_elevator_angle]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Elevator Angle',str]);
  xlabel('Seconds');
  legend('Alt deltaE', 'Alt deltaECmd',...
         'Ody deltaE', 'Ody deltaECmd')
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(4,1,1)
  plot(dataDC(:,tIx),dataDC(:,[pitchProp]), ...
   data(:,1),   data(1,uo_control_kpp)*(data(:,sim_theta)-data(:,c_pitch)));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad')
  title(['Pitch Term, ',str]);
  grid on
  subplot(4,1,2)
  plot(dataDC(:,tIx), dataDC(:,pitchInt),...
       data(:,1),    data(:,p_integral));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad')
  title(['Integral Term',str]);
  grid on
  subplot(4,1,3)
  plot(dataDC(:,tIx), dataDC(:,pitchIntInp),...
       data(:,1),     data(:,p_dbg));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad')
  title(['Integrator Input ',str]);
  grid on
  subplot(4,1,4)
  plot(dataDC(:,tIx), dataDC(:,pitchRate),...
                      data(:,1), data(1,uo_control_kdp)*data(:,m_pitch_rate));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians/Sec')
  title(['Pitch Rate Term',str]);
  xlabel('Seconds');
  legend('Altex', 'Odyssey')
  grid on

  sz = size(dataDC);
  if (sz(2) >= cDeltaEBL ) & exist('Del24BL')
    i = i+1;
    figure(i)
    if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
    set(gcf,'position',FigurePosn)
    clf
    subplot(2,1,1)
    plot(dataDC(:,tIx), dataDC(:,[cDeltaE cDeltaEBL]));
    if hardflag == 3, set(gca,'FontUnits','Normalized'), end
    ylabel('Rad')
    title(['ElevatorCmd, ElevatorCmd Before Limit ',str]);
    legend('Altex Elev', 'Altex Elev Before Limit');
    grid on
%
    subplot(2,1,2)
    plot(data(:,1), data(:,[c_elevator_angle Del24BL]));
    if hardflag == 3, set(gca,'FontUnits','Normalized'), end
    ylabel('Rad')
    title(['ElevatorCmd, ElevatorCmd Before Limit ',str]);
    legend('Ody Elev', 'Ody Elev Before Limit');
    grid on
  end

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(dataDC(:,tIx), dataDC(:,[cDeltaE]), ...
       dataSim(:,1),   dataSim(:,sDeltaE),...
       data(:,1),    data (:,m_elevator_angle));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Elevator, ',str]);
  legend('DynCont', 'Simulator', 'Odyssey');
  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(dataSim(:,tIx), dataSim(:,[sTheta]), data(:,1),data(:,sim_theta));
%      dataDC(:,tDC), dataDC(:,[pitchDC]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Pitch, Radians',str]);
  grid on
  subplot(3,1,2)
  plot(dataSim(:,tIx), dataSim(:,sOmega_q), ...
       data(:,1),      data(:,sim_q));
%      dataDC(:,tDC), dataDC(:,qDC));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad/Sec')
  title(['Body Rotational Rate q (Omega_N_B_y)',str]);
  xlabel('Seconds');
  grid on
  subplot(3,1,3)
  plot(dataSim(:,tIx), [dataSim(:,[sDeltaE scDeltaE])],...
                     data(:,1), data(:,[m_elevator_angle c_elevator_angle]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Elevator Angle',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(dataSim(:,tIx), unwrap(dataSim(:,sPsi)),...
       data(:,1),      unwrap( data(:,sim_psi)));
%      dataDC(:,tDC), unwrap(dataDC(:,[yawDC])));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Heading, Radians',str]);
  grid on
  subplot(3,1,2)
  plot(dataSim(:,tIx), dataSim(:,[sOmega_r]),...
       data(:,1),     data(:,sim_r));
%      dataDC(:,tDC), dataDC(:,[rDC]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad/Sec')
  title(['Body Rotational Rate r (Omega_N_B_z)',str]);
  grid on
  subplot(3,1,3)
  plot(dataSim(:,tIx), dataSim(:,sDeltaR),...
       data(:,1),      data(:,m_rudder_angle));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Rudder Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(2,1,1)
  plot(dataSim(:,tIx), dataSim(:,sPhi),...
       data(:,1),      data(:,sim_phi));
%      dataDC(:,tDC), dataDC(:,[rollDC]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Radians')
  title(['Roll, Radians',str]);
  grid on
  subplot(2,1,2)
  plot(dataSim(:,tIx), dataSim(:,[sOmega_p]),...
       data(:,1),      data(:,sim_p));
%      dataDC(:,tDC),  dataDC(:,[pDC]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Rad/Sec')
  title(['Body Rotational Rate p (Omega_N_B_x)',str]);
  grid on

if 0

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(4,1,1)
  plot(dataSim(:,tIx), dataSim(:,[sM1_1]),...
       data(:,1),  data(:,sim_M1_1));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Roll Torque, Newton-Meters')
  title([' Upper Rudder',str]);
  grid on
  subplot(4,1,2)
  plot(dataSim(:,tIx), dataSim(:,[sM2_1]),...
       data(:,1),  data(:,sim_M2_1));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Roll Torque, Newton-Meters')
  title([' Elevator',str]);
  grid on
  subplot(4,1,3)
  plot(dataSim(:,tIx), dataSim(:,[sM3_1]),...
       data(:,1),  data(:,sim_M3_1));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Roll Torque, Newton-Meters')
  title([' Lower Rudder',str]);
  grid on
  subplot(4,1,4)
  plot(dataSim(:,tIx), dataSim(:,[sM4_1]),...
       data(:,1),  data(:,sim_M4_1));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title([' Elevator',str]);
  ylabel('Newton-Meters')
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(4,1,1)
  plot(dataSim(:,tIx), dataSim(:,[sM1_2]),...
       data(:,1),  data(:,sim_M1_2));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Pitch Torque, Newton-Meters')
  title([' Upper Rudder',str]);
  grid on
  subplot(4,1,2)
  plot(dataSim(:,tIx), dataSim(:,[sM2_2]),...
       data(:,1),  data(:,sim_M2_2));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Pitch Torque, Newton-Meters')
  title([' Elevator',str]);
  grid on
  subplot(4,1,3)
  plot(dataSim(:,tIx), dataSim(:,[sM3_2]),...
       data(:,1),  data(:,sim_M3_2));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Pitch Torque, Newton-Meters')
  title([' Lower Rudder',str]);
  grid on
  subplot(4,1,4)
  plot(dataSim(:,tIx), dataSim(:,[sM4_2]),...
       data(:,1),  data(:,sim_M4_2));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  title([' Elevator',str]);
  ylabel('Newton-Meters')
  grid on

end

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  subplot(3,1,1)
  plot(dataSim(:,tIx), dataSim(:,sVel_u),...
       data(:,1),      data(:,sim_u));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Surge Rate, m/s')
  grid on
  subplot(3,1,2)
  plot(dataSim(:,tIx), dataSim(:,sVel_v),...
       data(:,1),      data(:,sim_v));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Sway Rate, m/s')
  grid on
  subplot(3,1,3)
  plot(dataSim(:,tIx), dataSim(:,sVel_w),...
       data(:,1),      data(:,sim_w));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Heave Rate, m/s')
  title(['Translational Velocity in Body Coordinates',str]);
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(dataSim(:,sPos_y), dataSim(:,sPos_x),...
       data(:,sim_y),     data(:,sim_x),...
       dataNav(:,mPos_y), dataNav(:,mPos_x));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('North, Meters')
  xlabel('East, Meters')
  legend('DorOdy Truth', 'Odyssey', 'DorOdy DR');
  title(['Vehicle Location in Earth Fixed Coordinates',str]);
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',FigurePosn)
  clf
  plot(dataSim(:,tIx), dataSim(:,cThrAmps),...
       data(:,1),      data(:,c_thruster_current));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Amperes')
  xlabel('Seconds')
  legend('Altex', 'Odyssey')
  title(['Thruster Current',str]);
  grid on

  TsC = .2;
  szC = size(dataDC);
  dimC = szC(1)-1;
  bdCTime = dataDC(2:szC(1),1) - dataDC(1:dimC,1);
  i=i+1;
  figure(i)
  clf
  set(gcf,'position',FigurePosn)
  [tS, bdCTimeS] = stairs(dataDC(2:szC,1), bdCTime);
  [tS, bdCTimeS1]= stairs(dataDC(2:szC,1), dataDC(2:szC,2));
  plot(tS, [bdCTimeS bdCTimeS1]*1000);
  grid on
  legend('Dyn Cnt Loop Time', 'Nav Loop Time')
  xlabel('Seconds')
  ylabel('milli-Seconds')
  title(['Backdifference of Control and Navigation Time Stamps',str]);
