%
% PURPOSE: Compute the motion of the radar wrto inertially fixed NED
%          coordinates, using the logged nav440 data.
%
% CHANGE LOG: 1) R. McEwen 29 Oct 2020 - Initial.
%

  if(~exist('coast_eastings')) 
    load('coastdat_MontereyBayUTM.mat')
  end

% 1. pcap_time,
% 2. lcm_time,
% 3. latitude,
% 4. longitude,
% 5. yaw_true,
% 6. pitch,
% 7. roll,
% 8. x_accel,
% 9. y_accel,
% 10. z_accel,
% 11. n_velocity,
% 12. e_velocity,
% 13. d_velocity,
% 14. altitude,
% 15. x_rate,       (omega_x)
% 16. y_rate,       (omega_y)
% 17. z_rate,       (omega_z)
% 18. x_rate_temp
 
  deci = 100;

  if(~exist('nav440')) 
    whos('Nav440Processed28Sept2020.mat');
    load('Nav440Processed28Sept2020.mat');
  end

  pcap_time = nav440(:,1);
  lcm_time  = nav440(:,2);
  time = pcap_time-pcap_time(1,1);
  v_P = nav440(:,11:13);
  
  
  
  
  whos
  
  if(~exist('position'))
    position = get(0,'screensize');
    npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
    npposition(3:4) = round([position(3)*.95 position(4)*.90]);
    position = npposition;
  end

  co = [0 0 1;
    0 0.5 0;
    1 0 0;
    0 0.75 0.75;
    0.75 0 0.75;
    0.75 0.75 0;
    0.25 0.25 0.25];
  set(groot,'defaultAxesColorOrder',co)

  if( exist( 'fig1' ) ~= 1 )
    fig1 = figure;
    figure(fig1);
  else
    figure(fig1);
    clf
  end
  set( fig1, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig1' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  if(~exist('fontsize')) fontsize = 20;end
  
  mag = 20;
  quiver( eastings(1:deci:end),  northings(1:deci:end),...
          mag*sin(yaw(1:deci:end) ),mag*cos(yaw(1:deci:end) ),0 );

  set(gca,'fontsize',fontsize);
  grid on
  hold on

  plot( coast_eastings, coast_northings,'k' )
  
  quiver(eastings(1:deci:end), northings(1:deci:end),...
         nav440(1:deci:end,12), nav440(1:deci:end,11),'g');

  legend('Heading, no units','Monterey Coast','Velocity, m/s','location','northeastoutside')
  xlabel('Eastings, meters')
  ylabel('Northings, meters')
  title('Nav440 Data from the Paragon 28 Sept 2020')

  axis equal
  set(gca,'ylim',[4071887.46    4074507.98]);
  set(gca,'xlim',[605112.83     608990.87]);
  

  if( exist( 'fig2' ) ~= 1 )
    fig2 = figure;
    figure(fig2);
  else
    figure(fig2);
    clf
  end
  set( fig2, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig2' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  time = pcap_time-pcap_time(1,1);
  
  subplot(3,1,1)
  plot(time,[v_P(:,1) v_radar(:,1)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('North, m/s')
  title('Velocity of Nav440 Ahrs and Radar in NED Coordinates');
  
  subplot(3,1,2)
  plot(time,[v_P(:,2) v_radar(:,2)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('East, m/s')
  
  subplot(3,1,3)
  plot(time,[v_P(:,3) v_radar(:,3)])
  grid on 
  hold on
  legend('Ahrs','Radar','location','northeastoutside');
  ylabel('Down, m/s')
  xlabel('Pcap Seconds Since Start')
  
  linkaxes(get(gcf,'children'),'x')
  

  if( exist( 'fig3' ) ~= 1 )
    fig3 = figure;
    figure(fig3);
  else
    figure(fig3);
    clf
  end
  set( fig3, 'Name', '28 Sept Paragon Data', 'DeleteFcn', 'clear fig3' );
  set(gcf,'position',position)
  set(gcf,'colormap',jet)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)  
  
  
  subplot(3,1,1)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,1)],'b');
  plot(time,[v_radar_B(:,1)],'g');
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Along Track, m/s and Deg.')
  title('Velocity of Nav440 Ahrs and Radar in Ship Coordinates');
  
  subplot(3,1,2)
  plot(time, roll*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,2)],'b');
  plot(time,[v_radar_B(:,2)],'g');
  grid on 
  legend('Roll','Ahrs','Radar','location','northeastoutside');
  ylabel('Cross Track, m/s and Deg.')
  
  subplot(3,1,3)
  plot(time, pitch*180/pi, 'k--','linewidth',1)
  hold on
  plot(time,[v_P_B(:,3)],'b')
  plot(time,[v_radar_B(:,3)],'g')
  grid on 
  legend('Pitch','Ahrs','Radar','location','northeastoutside');
  ylabel('Down, m/s and Deg.')
  xlabel('Pcap Seconds Since Start')
  
  linkaxes(get(gcf,'children'),'x')
  
%
% Save the relevant quantities:
  save Nav440Processed28Sept2020.mat nav440 eastings northings roll pitch yaw ...
       omega v_P_B v_radar v_radar_B rr
