%
% _p denotes Euler angles in the  primed 2-3 system, that is, pitch first,
% then yaw.

  theta_p = pi/6;
  psi_p   = pi/4;
%
% Now do the 2-3

T_P = [ cos(theta_p)    0   -sin(theta_p);
              0         1         0;
        sin(theta_p)    0    cos(theta_p)];

T_Y = [ cos(psi_p)    sin(psi_p)    0;
       -sin(psi_p)    cos(psi_p)    0;
            0          0            1];
%
% 
beamHat2 = [-cos(pi/4.)*sin(pi/6.) sin(pi/4)*sin(pi/6.) cos(pi/6.)]'
%
%
% beamHat2 is beam 2 in the body (or Dvl) frame.
%
% Then the 2-3 rotation (NOT 3-2) below should re-create beam 2 in the
% Body frame: T_B_N = T_Y * T_P

T_Y * T_P * [ 0 0 1]'
%
% The _2 reminds us that it was created from a 2-3 sequence:
%
T_N_B_2 = T_P' * T_Y';
%
% This points beamHat2 along nadir:
%
T_N_B_2 * beamHat2

% Let's walk through all 4 beams just to be sure:
%
c3s2 = 1./sqrt(2.)/2.;   %cos(pi/4)*sin(pi/6),   pi/6 = 30 degrees.
s3s2 = 1./sqrt(2.)/2.;   %sin(pi/4)*sin(pi/6)
c2   = sqrt(3.)/2.;      %cos(pi/6)

beamHat = [ c3s2 -c3s2 c3s2 -c3s2;
           -s3s2  s3s2 s3s2 -s3s2;
	     c2    c2   c2    c2  ];

for( i=1:4 )
  %
  % Now find the 3-2-1 Euler sequence (psi, theta, phi) that creates T_N_B_2 above:
  %
  % Multiply it out by hand and compare to this web page:
  %
  % https://en.wikiversity.org/wiki/PlanetPhysics/Direction_Cosine_Matrix_to_Euler_321_Angles
  %

  psi_p = pi/4 + (i-1)*pi/2;

  phi   = atan2( sin(psi_p)*sin(theta_p), cos(theta_p) );
  theta =  asin( cos(psi_p)*sin(theta_p ) );
  psi   = atan2( sin(psi_p), cos(theta_p)*cos(psi_p) );

  % The above formulas give the 3-2-1 sequence to form T_B_N, NOT T_N_B.

  %Instead try it numerically:
  %phi   =  atan2( T_N_B_2(2,3), T_N_B_2(3,3) )
  %theta = -asin(  T_N_B_2(1,3) )
  %psi   =  atan2( T_N_B_2(1,2), T_N_B_2(1,1) )

  %[psi theta phi]=dcm2angle(T_N_B_2');
  
  %
  % Putting the above E.A.'s in the standard 3-2-1 should produce a nadir vector:
  %
  cpsi   = cos( psi );
  spsi   = sin( psi );
  ctheta = cos( theta );
  stheta = sin( theta );
  cphi   = cos( phi );
  sphi   = sin( phi );
  %
  % Form the direction cosine matrix that recoordinatizes a vector from the 
  % body frame (B) into the local-level-local-vertical (LV) navigation 
  % frame, which is (North, East, Down).  See Fossen, p.10, Eqn 2.11.
  % Here we will assume the LV frame is intertially fixed and thus
  % also a Newtonian (N) frame.
  %
  % N is the inertially fixed, or Newtonian frame.
  %
  % The transformation T_B_N, is 3-2-1 Euler.  The matrix below, T_N_B, is
  % the transpose.
  %
  %
  T_N_B(1,1) =  cpsi.*ctheta; 
  T_N_B(2,1) =  spsi.*ctheta; 
  T_N_B(3,1) = -stheta; 

  T_N_B(1,2) = -spsi.*cphi + cpsi.*stheta.*sphi; 
  T_N_B(2,2) =  cpsi.*cphi + sphi.*stheta.*spsi; 
  T_N_B(3,2) =  ctheta.*sphi; 

  T_N_B(1,3) =  spsi.*sphi + cpsi.*cphi.*stheta; 
  T_N_B(2,3) = -cpsi.*sphi + stheta.*spsi.*cphi;
  T_N_B(3,3) =  ctheta.*cphi; 

  beamIndx = [2 3 1 4];
  fprintf('Beam %d:\n',beamIndx(i));
  vec_N = T_N_B * beamHat(:,beamIndx(i))

end
