//  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 [M,omega] = CL_rot_compose(varargin)
// Matrix and angular velocity vector resulting from frame composition 
//
// Calling Sequence
// [M,omega] = CL_rot_compose(M1,omega1,dir1, M2,omega2,dir2, ... Mn,omegan,dirn)
//
// Description
// <itemizedlist><listitem>
// <p>This function computes the matrix and angular velocity vector resulting 
// from several successive frame transformations.</p>
// <p>For each elementary transformation, 3 entries are expected : </p>
// <p>- Mk: Frame transformation matrix (framei to framej).</p>
// <p>- omegak: Angular velocity vector (framej / framei with coordinates in framei.</p>
// <p>- dirk: Direction of the kth transformation: 1=same direction as for the combined transformation; -1=opposite direction</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// M1,M2,...Mn: Frame transformation matrices (3x3xN or 3x3)
// omega1,omega2,...omegan: Angular velocity vectors [rad/s] (3xN or 3x1)
// dir1,dir2,...dirn: Directions of the transformations (1x1)
// M: Resulting transformation matrix (3x3xN)
// omega: Resulting angular velocity vector [rad/s] (3xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_pvConvert
//
// Examples
// // Composition of two transformations:
// // We know: frameA -> frameB
// // M1 is such that: X_relative_to_frameB = M1*(X_relative_to_frameA) 
// // om1 = omega(frameB/frameA) with coordinates in frameA
// M1 = CL_rot_angles2matrix(3, %pi/2)
// om1 = [0;0;0.01];
//
// // We know: frameC -> frameB
// // M2 is such that: X_relative_to_frameB = M2*(X_relative_to_frameC) 
// // om2 = omega(frameB/frameC) with coordinates in frameC
// M2 = CL_rot_angles2matrix(2, %pi/2)
// om2 = [0;0.01;0];
//
// // We compute: frameA -> frameC <=> frameA -> frameB -> frameC
// // M is such that: X_relative_to_frameC = M*(X_relative_to_frameA)
// // omega = omega(frameC/frameA) with coordinates in frameA
// [M,omega] = CL_rot_compose(M1,om1,1, M2,om2,-1);

// Declarations:

// Code:

// "nominal" case (dir1 == 1 and dir2 == 1): 
//
// F1   ----->   F2   ----->   F3
//      M1          M2
//      omega1        omega2
//      
// F1   ------------------->   F3
//              M
//              omega     
//
// M1: frame transformation matrix F1->F2: X/F2 = M1 * X/F1
// M2: frame transformation matrix F2->F3: X/F3 = M2 * X/F2
// omega1 = omega(F2/F1), coordinates in F1
// omega2 = omega(F3/F2), coordinates in F2
//
// M: frame transformation matrix F1->F3: X/F3 = M1 * X/F1
// omega = omega(F3/F2), coordinates in F2
//
// M = M2 * M1; 
// omega = M1' * omega2 + omega1; 
//
// if dirk == -1 (k = 1 or k = 2) : 
// In the expressions above, replace (simultaneously):
// omegak  by:  -Mk * omegak
// Mk    by:   Mk'


if (modulo(argn(2),3) <> 0)
  CL__error("Number of input arguments should be a multiple of 3");
end

// Check argument sizes
// NB: CL__checkInputs cannot be used in this case (list, hypermatrix...)
n = argn(2)/3;
sM = zeros(1,n);
som = zeros(1,n);
for k = 0 : n-1
  // Matrix should be 3x3xN
  if (size(varargin(3*k+1),1) <> 3 | size(varargin(3*k+1),2) <> 3); error("Invalid matrix"); end;
  sM(k+1) = size(varargin(3*k+1),"*")/9;
  // Omega should be 3xN
  if (size(varargin(3*k+2),1) <> 3); error("Invalid angular velocity vector"); end;
  som(k+1) = size(varargin(3*k+2),2);
  // Direction should be 1x1 and be equal to 1 or -1
  if (size(varargin(3*k+3),"*") <> 1); error("Invalid direction"); end;
  if (varargin(3*k+3) <> 1 & varargin(3*k+3) <> -1); error("Invalid direction"); end;
end

N = max(max(sM), max(som));
if (find((sM <> 1 & sM <> N) | (som <> 1 & som <> N)) <> [])
  CL__error("Invalid argument sizes");
end


// Index used to retrieved the matrix, omega and direction in varargin.
// ex: M1 = varargin(Imat(1))
// ex: om2 = varargin(Iom(2))
// ex: dir1 = varargin(Idir(1))
Imat = [1:3:3*n];   
Iom = [2:3:3*n];
Idir = [3:3:3*n];

M = varargin(Imat(1)); 
omega = varargin(Iom(1)); 

if (varargin(Idir(1)) < 0)
  omega = -M * omega; 
  M = M'; 
end

for i = 2 : n
  if (varargin(Idir(i)) > 0)
    omega = M' * varargin(Iom(i)) + omega
    M = varargin(Imat(i)) * M; 
  else
    omega = M' * (-varargin(Imat(i)) * varargin(Iom(i))) + omega
    M = varargin(Imat(i))' * M; 
  end
end
 
endfunction