%
% PURPOSE: Post-process the nav data from the arctic.
% AUTHOR:  rsm
% DATE:    03/2/21
%
% Interpolate the Crossbow data to the Kearfott:
  phi   = interp1(dataCB(:,1), dataCB(:,mRollCB),  dataK(:,1), 'linear', 0)*d2r;
  theta = interp1(dataCB(:,1), dataCB(:,mPitchCB), dataK(:,1), 'linear', 0)*d2r;
%
% Convert from projected roll to Euler roll:
%
  phi = atan( cos(theta).*tan(phi) );
%
% Form the 2-1 direction cosine matrix from the Crossbow:
%
  i = 1;
  while( i <= length(dataK) )
    T_CB_N = [cos(theta(i))                   0       -sin(theta(i));
	      sin(phi(i))*sin(theta(i))   cos(phi(i))  sin(phi(i))*cos(theta(i))
	      cos(phi(i))*sin(theta(i))  -sin(phi(i))  cos(phi(i))*cos(theta(i))
	     ];
    thetaK = dataK(i,mPitchK);
    phiK   = dataK(i,mRollK);
    T_Kf_N = [cos(thetaK)                   0       -sin(thetaK);
	      sin(phiK)*sin(thetaK)   cos(phiK)  sin(phiK)*cos(thetaK)
	      cos(phiK)*sin(thetaK)  -sin(phiK)  cos(phiK)*cos(thetaK)
	     ];
    T_CB_Kf= T_CB_N * T_Kf_N';
    thetaCB(i) = -asin(T_CB_Kf(1,3));
    phiCB(i)   = -asin(T_CB_Kf(3,2));
    i= i+1;
  end
%
% Compute the mean and standard deviation of the offset angles for the
% specified ranges.
%
  clear phiCBbar;
  if( exist('range') == 1 )
    if( rem(length(range),2) ~= 0 )
      disp('range must contain pairs of (t_min,t_max) points.')
    else
      tt = dataK(:,1);
      indx = [];
      i=1;
      while( i <= length(range) )
	indx = [indx; find(tt>range(i) & tt<range(i+1))];
	i = i+2;
      end
      phiCBbar      = mean(phiCB(indx))
      sigma_phiCB   = std(phiCB(indx))
      thetaCBbar    = mean(thetaCB(indx))
      sigma_thetaCB = std(thetaCB(indx))
    end
  end
  
%
% Plot it!
%
  if exist( 'ProcNavFig1' ) ~= 1
      ProcNavFig1 = figure;
  end
  ProcNavFig1 = figure( ProcNavFig1 );

  set( ProcNavFig1, 'name', 'Processed Nav Data: Crossbow Misalignment', ...
       'DeleteFcn', 'clear ProcNavFig1' );
  set(gcf,'position',position)
  clf
 
 subplot(2,1,1)
 plot( dataK(:,1), phiCB*180/pi)
 if( exist('phiCBbar') == 1 )
   line( tt, phiCBbar/d2r, 'color','g')
   line( tt, (ones(size(tt))*[3 -3]*sigma_phiCB + phiCBbar)/d2r, 'color','r')
 end
 ylabel('Roll, Degrees')
 title(['Crossbow/Kearfott Misalignment; 2-1 Euler',str]);
 if( exist('phiCBbar') == 1 )
   xlabel(['Ave = ',num2str(phiCBbar*180/pi),' Sigma = ',...
	 num2str(sigma_phiCB*180/pi),'; Degrees.'])
 end
 %legend(lstr);
 grid on
 zoom on
 YEMax = 2;  %Prevent autoscaling to outliers
 YEMin =-2;
 Ylim = get(gca,'YLim');
 Ylim(1) = max([Ylim(1) YEMin]);
 Ylim(2) = min([Ylim(2) YEMax]);
 set(gca,'YLim',Ylim)
 
 subplot(2,1,2)
% disp('Before plot')
 plot( dataK(:,1), thetaCB*180/pi)
 if( exist('thetaCBbar') == 1 )
 disp('Before ave line')
   line( tt, thetaCBbar/d2r, 'color','g')
 disp('Before std line')
   line( tt, ...
	 (ones(size(tt))*[3 -3]*sigma_thetaCB + thetaCBbar)/d2r, 'color','r')
 end
 ylabel('Pitch; Degrees')
 if( exist('thetaCBbar') == 1 )
   xlabel(['Ave = ',num2str(thetaCBbar*180/pi),' Sigma = ',...
	 num2str(sigma_thetaCB*180/pi),'; Degrees.'])
 end
 %legend(lstr);
 grid on
 zoom on
 Ylim = get(gca,'YLim');
 Ylim(1) = max([Ylim(1) YEMin]);
 Ylim(2) = min([Ylim(2) YEMax]);
 set(gca,'YLim',Ylim)
 disp('Done')
 
 

