%
%
% The goal is to take the Astra computed attitude and remove any
% mechanical alignment issues so we can compare to POSPac attitude
%
% Based on a memo Joe Hutton wrote 
% 
% "Coordinate frame transformations for POS AV"
% Feb 13, 2004
%
% Per Joe:
%   For this analysis it is best to assume the Vehicle frame is your 
%   Reference frame (ie set the Vehicle to Reference Mounting angles
%   to zero). That way all lever arm measurements can be done in the
%   vehicle frame as opposed to the rotated reference frame (which is
%   a nightmare). Next step is to then define and apply all the IMU 
%   misalignments wrt to the Vehicle/Reference frame.  These are applied
%   using the sequence of rotation Tz, Ty, Tx, from the Reference frame
%   to the misaligned IMU frame and a DCM. If you assume the LN200 is 
%   perfectly aligned with the Vehicle frame (which we can do for now),
%   you would enter your estimated offsets into the Reference to IMU
%   Mounting Angles field below.
%
%   For the math, attached is an oldie but a goodie.. You are interested 
%   in Cbg and Ccb.. In this example c = Camera or Sensor frame, but
%   this is also the Reference frame. Simply multiply Cbg*Ccb then
%   extract the new roll, pitch and heading using equation (7). I think
%   we should have some code kicking that will do this on an SBET; I’ll 
%   see if I can find it. We also have a Navdiff utility that is embedded 
%   in the POSPac Display function.
%
% Input:
%  WinAstra estimated roll, pitch, heading [all in degrees]
%  xRot, yRot, zRot [rotations to be applied in degrees]
%
% Returned:
%  RollHat PitchHat HeadingHat [corrected R/P/H in degrees]
%

function [RollHat PitchHat HeadingHat] = correctRollPitchHeading(rollIn,pitchIn,headIn, ...
                                                                 xRot, yRot, zRot);
% Mounting angles

x = xRot * pi/180;
y = yRot * pi/180;
z = zRot * pi/180;

Ccb(1,1) =  cos(y)*cos(z);
Ccb(1,2) =  cos(y)*sin(z);
Ccb(1,3) = -sin(y);

Ccb(2,1) = sin(x)*sin(y)*cos(z) - cos(x)*sin(z);
Ccb(2,2) = sin(x)*sin(y)*sin(z) + cos(x)*cos(z);
Ccb(2,3) = sin(x)*cos(y);

Ccb(3,1) = cos(x)*sin(y)*cos(z) + sin(x)*sin(z);
Ccb(3,2) = cos(x)*sin(y)*sin(z) - sin(x)*cos(z);
Ccb(3,3) = cos(x)*cos(y);


cosRoll = cos(rollIn.*pi/180);
sinRoll = sin(rollIn.*pi/180);

cosPitch =cos(pitchIn.*pi/180);
sinPitch =sin(pitchIn.*pi/180);

cosHeading = cos(headIn.*pi/180);
sinHeading = sin(headIn.*pi/180);


% TODO - this could be made a lot faster....
for i=1:length(rollIn)

 if 0
  roll    = rollIn(i)*pi/180;
  pitch   = pitchIn(i)*pi/180;
  heading = headIn(i)*pi/180;

  Cbg(1,1) = cos(pitch)*cos(heading);
  Cbg(1,2) = sin(roll)*sin(pitch)*cos(heading) - cos(roll)*sin(heading);
  Cbg(1,3) = cos(roll)*sin(pitch)*cos(heading) + sin(roll)*sin(heading);

  Cbg(2,1) = cos(pitch)*sin(heading);
  Cbg(2,2) = sin(roll)*sin(pitch)*sin(heading) + cos(roll)*cos(heading);
  Cbg(2,3) = cos(roll)*sin(pitch)*sin(heading) - sin(roll)*cos(heading);

  Cbg(3,1) = -sin(pitch);
  Cbg(3,2) =  sin(roll)*cos(pitch);
  Cbg(3,3) =  cos(roll)*cos(pitch);
 else
 
  ThisCosRoll  = cosRoll(i);
  ThisSinRoll  = sinRoll(i);

  ThisCosPitch = cosPitch(i);
  ThisSinPitch = sinPitch(i);

  ThisCosHeading = cosHeading(i);
  ThisSinHeading = sinHeading(i);

  Cbg(1,1) = ThisCosPitch*ThisCosHeading;
  Cbg(1,2) = ThisSinRoll*ThisSinPitch*ThisCosHeading - ThisCosRoll*ThisSinHeading;
  Cbg(1,3) = ThisCosRoll*ThisSinPitch*ThisCosHeading + ThisSinRoll*ThisSinHeading;

  Cbg(2,1) = ThisCosPitch*ThisSinHeading;
  Cbg(2,2) = ThisSinRoll*ThisSinPitch*ThisSinHeading + ThisCosRoll*ThisCosHeading;
  Cbg(2,3) = ThisCosRoll*ThisSinPitch*ThisSinHeading - ThisSinRoll*ThisCosHeading;

  Cbg(3,1) = -ThisSinPitch;
  Cbg(3,2) =  ThisSinRoll*ThisCosPitch;
  Cbg(3,3) =  ThisCosRoll*ThisCosPitch;
 end

  out = Cbg*Ccb;

  RollHat(i)  = atan(out(3,2)/out(3,3)) * 180/pi;
  PitchHat(i) = atan(-out(3,1)/sqrt(out(1,1)^2 + out(2,1)^2)) * 180/pi;
  HeadingHat(i)  = atan2(out(2,1),out(1,1)) * 180/pi;

end



