//  Copyright (c) CNES  2008
//
//  This software is part of CelestLab, a CNES toolbox for Scilab
//
//  This software is governed by the CeCILL  license under French law and
//  abiding by the rules of distribution of free software.  You can  use,
//  modify and/ or redistribute the software under the terms of the CeCILL
//  license as circulated by CEA, CNRS and INRIA at the following URL
//  'http://www.cecill.info'.

function [om,omp] = CL_rot_angularVelocity(naxes,angles,angles_der,angles_der2)
// Rotation angles to angular velocity and acceleration vectors - DEPRECATED
//
// Calling Sequence
// [om,omp] = CL_rot_angularVelocity(naxes,angles,angles_der,angles_der2)
//
// Description
// <itemizedlist><listitem>
// <p>This function is deprecated. </p>
// <p>Replacement function: <link linkend="CL_rot_angVelocity">CL_rot_angVelocity</link></p>
// <p></p></listitem>
// <listitem>
// <p>Computes the angular rotation and acceleration vectors resulting from a combination of 3 
// rotations.</p> 
// <p>The rotation axes are defined by numbers: 1=x-axis, 2=y-axis, 3=z-axis.</p>  
// <p>If: F1 = initial frame, F2 = final frame (= image of F1 by the successive rotations given by (naxes, angles)), 
// then the angular velocity vector as computed is omega(F2/F1) with coordinates in F2.</p>  
// </listitem>
// </itemizedlist>
//
// Parameters
// naxes : Axes numbers: 1=x-axis, 2=y-axis or 3=z-axis (1x3 or 3x1)
// angles : Rotation angles around respective axes [rad] (3xN) 
// angles_der : First derivatives of angles with respect to time [rad/s] (3xN) 
// angles_der2 : Second derivatives of angles with respect to time [rad/s^2] (3xN) 
// om : Angular rotation vector (3xN)
// omp : Angular acceleration vector (3xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_matrix2quat
//
// Examples
// naxes = [1,2,3]; // XYZ
// angles = [%pi/4 ; %pi/8 ; %pi/6]
// angles_der = [%pi/4 ; %pi/8 ; %pi/6]
// angles_der2 = [%pi/4 ; %pi/8 ; %pi/6]
// [om,omp] = CL_rot_angularVelocity(naxes,angles,angles_der,angles_der2)

// Declarations:


// Code:

CL__warnDeprecated(); // deprecated function

// Validity checking

naxes = matrix(naxes, 1, -1); // => row vector
Nrot = length(naxes);
N = size(angles,2);

if (Nrot <> 3)
  CL__error("Invalid argument sizes"); 
end

// Resize arguments if necessary
[angles, angles_der, angles_der2] = CL__checkInputs(angles,3, angles_der,3, angles_der2,3);

// Check that axis numbers are equal to 1, 2 or 3
I = find(naxes <> 1 & naxes <> 2 & naxes <> 3); 
if (~isempty(I))
  CL__error("Invalid axis numbers");
end


phi = angles(1,:);
theta = angles(2,:);
psi = angles(3,:);

phip = angles_der(1,:);
thetap = angles_der(2,:);
psip = angles_der(3,:);

phipp = angles_der2(1,:);
thetapp = angles_der2(2,:);
psipp = angles_der2(3,:);

// correspondance
// axis sequence - index value - kinematic equations of motions
rot_sequences = [[1,2,3]; [2,3,1]; [3,1,2]; 
                 [1,3,2]; [2,1,3]; [3,2,1];
                 [1,2,1]; [2,3,2]; [3,1,3]; 
                 [1,3,1]; [2,1,2]; [3,2,3]];

// find sequence 
I = vectorfind(rot_sequences, naxes);

if (I == [])
  CL__error("Invalid rotation sequence");
end

om = zeros(3,N);
omp = zeros(3,N);

i1 = rot_sequences(I,1); 
i2 = modulo(i1,3) + 1; 
i3 = modulo(i2,3) + 1; 

if (I >= 1 & I <= 3)

  om(i1,:)  = phip.*cos(theta).*cos(psi) + thetap.*sin(psi);
  om(i2,:)  = -phip.*cos(theta).*sin(psi) + thetap.*cos(psi);
  om(i3,:)  = psip + phip.*sin(theta);

  omp(i1,:) = phipp.*cos(theta).*cos(psi) - phip.*thetap.*sin(theta).*cos(psi) - phip.*cos(theta).*psip.*sin(psi) + thetapp.*sin(psi) + thetap.*psip.*cos(psi);
  omp(i2,:) = -phipp.*cos(theta).*sin(psi) + phip.*thetap.*sin(theta).*sin(psi) - phip.*cos(theta).*psip.*cos(psi) + thetapp.*cos(psi) - thetap.*psip.*sin(psi);
  omp(i3,:) = psipp + phipp.*sin(theta) + phip.*thetap.*cos(theta);

elseif (I >= 4 & I <= 6)

  om(i1,:)  = phip.*cos(theta).*cos(psi) - thetap.*sin(psi);
  om(i2,:)  = psip - phip.*sin(theta);
  om(i3,:)  = phip.*cos(theta).*sin(psi) + thetap.*cos(psi);

  omp(i1,:) = phipp.*cos(theta).*cos(psi) - phip.*thetap.*sin(theta).*cos(psi) - phip.*cos(theta).*psip.*sin(psi) - thetapp.*sin(psi) - thetap.*psip.*cos(psi);
  omp(i2,:) = psipp - phipp.*sin(theta) - phip.*thetap.*cos(theta);
  omp(i3,:) = phipp.*cos(theta).*sin(psi) - phip.*thetap.*sin(theta).*sin(psi) + phip.*cos(theta).*psip.*cos(psi) + thetapp.*cos(psi) - thetap.*psip.*sin(psi);

elseif (I >= 7 & I <= 9)

  om(i1,:)  = psip + phip.*cos(theta);
  om(i2,:)  = phip.*sin(theta).*sin(psi) + thetap.*cos(psi);
  om(i3,:)  = phip.*sin(theta).*cos(psi) - thetap.*sin(psi);

  omp(i1,:) = psipp + phipp.*cos(theta) - phip.*thetap.*sin(theta);
  omp(i2,:) = phipp.*sin(theta).*sin(psi) + phip.*thetap.*cos(theta).*sin(psi) + phip.*sin(theta).*psip.*cos(psi) + thetapp.*cos(psi) - thetap.*psip.*sin(psi);
  omp(i3,:) = phipp.*sin(theta).*cos(psi) + phip.*thetap.*cos(theta).*cos(psi) - phip.*sin(theta).*psip.*sin(psi) - thetapp.*sin(psi) - thetap.*psip.*cos(psi);

else

  om(i1,:) = psip + phip.*cos(theta);
  om(i2,:) = -phip.*sin(theta).*cos(psi) + thetap.*sin(psi);
  om(i3,:) = phip.*sin(theta).*sin(psi) + thetap.*cos(psi);

  omp(i1,:) = psipp + phipp.*cos(theta) - phip.*thetap.*sin(theta);
  omp(i2,:) = -phipp.*sin(theta).*cos(psi) - phip.*thetap.*cos(theta).*cos(psi) + phip.*sin(theta).*psip.*sin(psi) + thetapp.*sin(psi) + thetap.*psip.*cos(psi);
  omp(i3,:) = phipp.*sin(theta).*sin(psi) + phip.*thetap.*cos(theta).*sin(psi) + phip.*sin(theta).*psip.*cos(psi) + thetapp.*cos(psi) - thetap.*psip.*sin(psi);

end

endfunction