%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PURPOSE: Plot AHRS data.
% AUTHOR:  Initial; rsm
% DATE:    00/1/10
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Units and Fundamental Constants:
  units
  hardflag=0;

  time = 1;

  str = [' ',titleStr, ' ',myDate];
  PaperPosn = [.5 .5 3.75 2.5];
  FigurePosn = [5 30 815 760];
  i = 0;

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

%
% Default to plotting all available AHRS instruments.  The following flags can
% be used to shut off unwanted plots.
%
  nstrips = 3;
  if exist('plotOctans') ~= 1, plotOctans = 1; end
  if exist('plotDvl') ~= 1, plotDvl = 0; end
  if exist('plotKft') ~= 1, plotKft = 0; end
  
  if( (plotOctans == 1) & (exist('dataO') == 1) ), nstrips=4; end
    

  set( crsBwFig1, 'name', 'Attitude & Heading Reference', ...
       'DeleteFcn', 'clear crsBwFig1' );
  set(gcf,'position',position)
  clf

    subplot(nstrips,1,1)
    [xx,yy]   = stairs( dataNav(:,time), unwrap(dataNav(:,mPhi))/d2r);
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mRollCB));
      line(xx1, yy1,'Color','g');
      legend('Nav','Ahrs')
    end
    if exist('dvlRoll') == 1,
    if plotDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  unwrap(dataDvl(:,dvlRoll))/d2r);
      line(xx1, yy1,'Color','c');
      legend('Nav','Ahrs','Dvl')
    end
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  unwrap(dataO(:,mRollO))/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  unwrap(dataK(:,mRollK))/d2r);
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees')
    title(['Roll, Degrees',str]);
    grid on
    zoom on

    subplot(nstrips,1,2)
    [xx,yy]   = stairs( dataNav(:,time), dataNav(:,mTheta)/d2r);
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mPitchCB));
      line(xx1, yy1,'Color','g');
      legend('Nav','Ahrs')
    end
    if exist('dvlPitch') == 1,
    if plotDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  dataDvl(:,dvlPitch)/d2r);
      line(xx1, yy1,'Color','c');
      legend('Nav','Ahrs','Dvl')
    end
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mPitchO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mPitchK)/d2r);
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees')
    title(['Pitch, Degrees',str]);
    grid on
    zoom on

    subplot(nstrips,1,3)
    [xx,yy]   = stairs( dataNav(:,time), mod360(dataNav(:,mPsi)/d2r));
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  mod360(dataCB(:,mYawCB)));
      line(xx1, yy1,'Color','g');
      legend('Nav','Ahrs')
    end
    if exist('dvlHeading') == 1,
    if plotDvl
      [xx1,yy1] = stairs( dataDvl(:,time),  mod360(dataDvl(:,dvlHeading)/d2r));
      line(xx1, yy1,'Color','c');
      legend('Nav','Ahrs','Dvl')
    end
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  mod360(dataO(:,mYawO)/d2r));
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  mod360(dataK(:,mHeadK)/d2r));
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees')
    xlabel('Elapsed Time (s)');
    title(['Yaw, Degrees',str]);
    grid on
    zoom on

    if nstrips == 4 & (exist('mStatusO') == 1),
    subplot(nstrips,1,4)
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mStatusO));
      plot(xx2, yy2,'r');
    end
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mMonK));
      line(xx2, yy2,'color','c');
    end
    ylabel('Status')
    xlabel('Elapsed Time (s)');
    title(['Status',str]);
    grid on
    zoom on
    end

    pause(.5)
  if exist( 'CrsBwFig2' ) ~= 1
      CrsBwFig2 = figure;
  else
      CrsBwFig2 = figure( CrsBwFig2 );
  end

  set( CrsBwFig2, 'name', 'Body Rates', ...
       'DeleteFcn', 'clear CrsBwFig2' );
  set(gcf,'position',position)
  clf
    subplot(3,1,1)
    [xx,yy]   = stairs( dataNav(:,time), dataNav(:,mOmega_x)/d2r);
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_xCB));
      line(xx1, yy1,'Color','g');
      legend('Nav Log','Raw Data')
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_xO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mPrateK)/d2r);
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees/Second')
    title(['X Axis Body Rate (Omega B x), Degrees/Sec',str]);
    grid on
    zoom on

    subplot(3,1,2)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_y)/d2r);
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_yCB));
      line(xx1, yy1,'Color','g');
      legend('Nav Log','Raw Data')
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_yO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mQrateK)/d2r);
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees/Second')
    title(['Y Axis Body Rate (Omega B y), Degrees/Sec',str]);
    grid on
    zoom on

    subplot(3,1,3)
    [xx,yy] = stairs( dataNav(:,time), dataNav(:,mOmega_z)/d2r);
    plot(xx, yy);
    if exist('dataCB') == 1,
      [xx1,yy1] = stairs( dataCB(:,time),  dataCB(:,mOmega_zCB));
      line(xx1, yy1,'Color','g');
      legend('Nav Log','Raw Data')
    end
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mOmega_zO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    if exist('dataK') == 1,
    if plotKft
      [xx2,yy2] = stairs( dataK(:,time),  dataK(:,mRrateK)/d2r);
      line(xx2, yy2,'Color','c');
      legend('Nav','CrossBow','Octans','Kearfott')
    end
    end
    ylabel('Degrees/Second')
    xlabel('Elapsed Time (s)');
    title(['Z Axis Body Rate (Omega B z), Degrees/Sec',str]);
    grid on
    zoom on

    if exist('dataCB') ~= 1, return, end

    return


    pause(.5)
  if exist( 'CrsBwFig3' ) ~= 1
      CrsBwFig3 = figure;
  else
      CrsBwFig3 = figure( CrsBwFig3 );
  end

  set( CrsBwFig3, 'name', 'Crossbow Acceleration', ...
       'DeleteFcn', 'clear CrsBwFig3' );
  set(gcf,'position',position)
  clf
    subplot(3,1,1)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mAccel_xCB));
    plot(xx, yy);
    if exist('dataO') == 1,
    if plotOctans
      
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_xO));
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    ylabel('g''s')
    title(['Acceleration of Vehicle CM Resolved onto Body X, g''s',str]);
    grid on
    zoom on


    subplot(3,1,2)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mAccel_yCB));
    plot(xx, yy);
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_yO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    ylabel('g''s')
    title(['Acceleration of Vehicle CM Resolved onto Body Y, g''s',str]);
    grid on
    zoom on


    subplot(3,1,3)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mAccel_zCB));
    plot(xx, yy);
    if exist('dataO') == 1,
    if plotOctans
      [xx2,yy2] = stairs( dataO(:,time),  dataO(:,mAccel_zO)/d2r);
      line(xx2, yy2,'Color','r');
      legend('Nav','CrossBow','Octans')
    end
    end
    ylabel('g''s')
    xlabel('Elapsed Time (s)');
    title(['Acceleration of Vehicle CM Resolved onto Body Z, g''s',str]);
    grid on
    zoom on
    %
    % Compute norm of Magnetic Field:
    %
    MagneticMag = sqrt( dataCB(:,mMag_xCB).^2 + ...
                        dataCB(:,mMag_yCB).^2 + ...
                        dataCB(:,mMag_zCB).^2 );

    pause(.5)
  if exist( 'CrsBwFig4' ) ~= 1
      CrsBwFig4 = figure;
  else
      CrsBwFig4 = figure( CrsBwFig4 );
  end

  set( CrsBwFig4, 'name', 'Crossbow Magnetic Field', ...
       'DeleteFcn', 'clear CrsBwFig4' );
  set(gcf,'position',position)
  clf
    subplot(4,1,1)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_xCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body X',str]);
    grid on
    zoom on


    subplot(4,1,2)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_yCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body Y',str]);
    grid on
    zoom on


    subplot(4,1,3)
    [xx,yy] = stairs( dataCB(:,time),  dataCB(:,mMag_zCB));
    plot(xx, yy);
    ylabel('Gauss')
    title(['Magnetic Field Resolved onto Body Z',str]);
    grid on
    zoom on


    subplot(4,1,4)
    [xx,yy] = stairs( dataCB(:,time),  MagneticMag );
    plot(xx, yy);
    ylabel('Gauss')
    xlabel('Elapsed Time (s)');
    title(['Magnitude of the Magnetic Field ',str]);
    grid on
    zoom on

  return






  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[pitchIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Pitch, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), [dataNav(:,qIx)]/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate q (Omega_N_B_y)',str]);
  xlabel('Seconds');
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaEIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Elevator Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(3,1,1)
  plot(dataNav(:,tIx), dataNav(:,[yawIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Heading, Degrees',str]);
  grid on
  subplot(3,1,2)
  plot(dataNav(:,tIx), dataNav(:,[rIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate r (Omega_N_B_z)',str]);
  grid on
  subplot(3,1,3)
  plot(dataNav(:,tIx), [dataNav(:,deltaRIx)]*180/pi);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Rudder Angle',str]);
  xlabel('Seconds');
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  subplot(2,1,1)
  plot(dataNav(:,tIx), dataNav(:,[rollIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Degrees')
  title(['Roll, Degrees',str]);
  grid on
  subplot(2,1,2)
  plot(dataNav(:,tIx), dataNav(:,[pIx])/d2r);
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('Deg/Sec')
  title(['Body Rotational Rate p (Omega_N_B_x)',str]);
  grid on

  i = i+1;
  figure(i)
  if hardflag ==3, set(gcf,'PaperPosition',PaperPosn), end
  set(gcf,'position',position)
  clf
  plot(dataNav(:,yPosIx), dataNav(:,[xPosIx]));
  if hardflag == 3, set(gca,'FontUnits','Normalized'), end
  ylabel('North, Meters')
  xlabel('East, Meters')
  title(['Vehicle Location in Earth Fixed Coordinates',str]);
  grid on

