//  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 [varargout] = CL_rot_defRot2Ax(u,v,axis1,axis2, numsol)
// Determination of 2 successive rotations 
//
// Calling Sequence
// [qa,angsa,Inok] = CL_rot_defRot2Ax(u,v,axis1,axis2 [,numsol=1])
// [qa,angsa,qb,angsb,Inok] = CL_rot_defRot2Ax(u,v,axis1,axis2, numsol=2)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the rotation, combination of 2 rotations around 2 axes, that transforms 
// a vector (<b>u</b>) into another vector (<b>v</b>).</p>
// <p>The rotation is returned as a quaternion, and the corresponding rotation angles are also returned.</p>
// <p>There are two solutions (in usual situations): </p>
// <p>(qa, angsa): solution such that the second rotation angle is minimal.</p>
// <p>(qb, angsb): solution such that the second rotation angle is maximal.</p>
// <p></p>
// <p><b>axis1</b> and <b>axis2</b> can either 
// be vectors (3xN) or axes numbers (1 for x-axis, 2 for y-axis or 3 for z-axis). The axes should be perpendicular to one another.
// If they are numbers, the vectors u and v must be given in the frame where the axes are basis vectors (coordinates = [1;0;0], [0;1;0], or [0;0;1]). </p>
// <p>Indices for which the rotation doesn't exist or is not uniquely defined are returned in Inok. </p>
// <p></p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p>- If no rotation exists, the rotation computed is such that the image of u is as close to v as possible. </p>
// <p>- If the axes are given as vectors, the coordinates are those of the vectors not considering the effect of the rotations. </p>
// <p>- If the axes are colinear, the results are set to %nan. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// u: Vector to be rotated (3xN or 3x1)
// v: Desired image of u by the rotation (3xN or 3x1)
// axis1: Axis vector (3xN or 3x1) or axis number (1x1) for the first rotation 
// axis2: Axis vector (3xN or 3x1) or axis number (1x1) for the second rotation 
// numsol: (optional, integer) Number of solutions returned: 1 or 2. Default is 1
// qa: Quaternion for the 1st solution (dim N)
// angsa: Rotation angles for the 1st solution (2xN)
// qb: Quaternion for the 2nd solution (dim N)
// angsb: Rotation angles for the 2nd solution (2xN)
// Inok: Indices for which the rotation is not uniquely defined, or the desired target vector cannot be reached
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_defRotVec
// CL_rot_defRot1Ax
//
// Examples
// // Example1: X axis then Y
// u = [ 1 ; 2 ; 3 ];
// v = [ 3 ; 1 ; 2 ];
// n1 = 1;
// n2 = 2;
// [qa, angsa, Inok] = CL_rot_defRot2Ax(u, v, n1, n2);
// // Check results :
// v2 = CL_rot_rotVect(qa,u) // == v
//
// // Example2: axes = vectors
// u = [ 1 ; 2 ; 3 ];
// v = [ 3 ; 1 ; 2 ];
// axis1 = [-2;3;4];
// axis2 = [1;0;1];
// [qa, angsa, qb, angsb, Inok] = CL_rot_defRot2Ax(u, v, axis1, axis2, numsol=2);
// // Check results :
// v2a = CL_rot_rotVect(qa,u) // == v
// v2b = CL_rot_rotVect(qb,u) // == v

// ----------------------------------------------------------
// Declarations:
// ----------------------------------------------------------

function [qa, angsa, qb, angsb, ind] = def_rot_2ax(u, v, n1, n2)
// Case where axes are given by numbers 
// u and v: same size and norms == 1
// n1 and n2: 1, 2 or 3 and n1 <> n2 (not checked)
// note: 
// Inaccurate => ind == 1
// impossible => ind == 2
// (qa, angsa) : solution 1
// (qb, angsb) : solution 2
  
  tol = 1.e-8; // tolerance value
  N = size(u,2); 
  ind = zeros(1:N); // validity indicator
  Id = eye(3,3); 

  // axis 3, sgn3 = -1 if frame not direct  
  n3 = pmodulo(n2+(n2-n1)-1,3) + 1; 
  sgn3 = sign(CL_rMod(n2 - n1, -1.5, 1.5)); 

  phi = atan(sgn3 * u(n3,:), u(n1,:)); 
  psi = atan(sgn3 * v(n3,:), v(n2,:)); 
  A = CL_norm([u(n1,:); u(n3,:)]); 

  expr_valid = A - abs(v(n1,:)); // has to be > 0
  // Correction of v : 
  // NB: v(n2,:) and v(n3,I) not changed because only 
  // the ratio matters (see psi = ...) 
  I = find(expr_valid < 0);
  v(n1,I) = sign(v(n1,I)) .* A(I);  

  x = zeros(A); 
  I = find(A > 0); 
  x(I) = real(acos(v(n1,I) ./ A(I))); // zero if A == 0
  y = atan(A .* sin(x), u(n2,:));  

  sol_ang1 = CL_rMod([psi+y; psi-y], -%pi, %pi); // 1st rot
  sol_ang2 = CL_rMod([phi+x; phi-x], -%pi, %pi); // 2nd rot

  [m, I] = min(abs(sol_ang2),"r"); // |ang2| minimal
  ka = sub2ind([2,N], I, 1:N);   // I = 1 or 2
  kb = sub2ind([2,N], 3-I, 1:N); // 3-I = 2 or 1
    
  angsa = [matrix(sol_ang1(ka), 1, -1); matrix(sol_ang2(ka), 1, -1)];
  angsb = [matrix(sol_ang1(kb), 1, -1); matrix(sol_ang2(kb), 1, -1)];

  qa = CL_rot_axAng2quat(Id(:,n1), angsa(1,:)) * ..
       CL_rot_axAng2quat(Id(:,n2), angsa(2,:));
  qb = CL_rot_axAng2quat(Id(:,n1), angsb(1,:)) * ..
       CL_rot_axAng2quat(Id(:,n2), angsb(2,:));

  // assess validity
  I = find(expr_valid < tol); // valid but close 
  ind(I) = 1; 
 
endfunction


// ----------------------------------------------------------
// main 
// ----------------------------------------------------------

if (~exists('numsol', 'local')); numsol = 1; end // one solution

if (numsol <> 1 & numsol <> 2)
  CL__error("Invalid value for numsol");
end

// Check argument sizes, and resize if necessary: 
// unit vectors before resizing (for efficiency)
// check vectors norms are not 0)
// (NB: norms compared to 0 volontarily)
[u, nu] = CL_unitVector(u); 
[v, nv] = CL_unitVector(v);

nraxis = size(axis1,1); // nb rows of axes 

if (nraxis == 1)
  [u, v, N] = CL__checkInputs(u,3, v,3);
  [axis1, axis2, Nax] = CL__checkInputs(axis1,1, axis2,1);
  if (Nax > 1 | (axis1 <> 1 & axis1 <> 2 & axis1 <> 3) | .. 
                (axis2 <> 1 & axis2 <> 2 & axis2 <> 3))
    CL__error("Invalid axes"); 
  end 
else
  naxis1 = CL_norm(axis1);
  naxis2 = CL_norm(axis2);
  [u, v, axis1, axis2, N] = CL__checkInputs(u,3, v,3, axis1,3, axis2,3);
  if (find(naxis1 .* naxis2 == 0) <> []); CL__error("Zero norm axes"); end
end

// Handle cases where one input is []
if (u == [] | v == [] | axis1 == [] | axis2 == [])
  qa = []; angsa = []; qb = []; angsb = []; Inok = [];
  return;
end

if (find(nu .* nv == 0) <> [])
  CL__error("Vectors should not be zero");
end


// ----------------------------------------------------------

// NB: N = number of columns of arguments (see above) 

ind = zeros(1:N); 

if (size(axis1,1) == 1)
  // case axes == numbers
  n1 = axis1; 
  n2 = axis2;
  
  [qa, angsa, qb, angsb, ind] = def_rot_2ax(u, v, n1, n2); 
  if (n1 == n2)
    ind = 2 * ones(1:N); // previous result: invalid!
  end

else
  // case axes == vectors

  // new frame: X = "axis1", Y = "axis2" 
  // (quaternion to save memory)
  
  [q0, Inokfr] = CL_rot_defFrameVec(axis1, axis2, 1, 2, res="q"); 

  u = CL_rot_rotVect(q0', u);  // coordinates in new frame
  v = CL_rot_rotVect(q0', v); 

  [qa, angsa, qb, angsb, ind] = def_rot_2ax(u, v, 1, 2); 
  qa = q0 * qa * q0';
  qb = q0 * qb * q0'; 

  // if axes are colinear => invalid
  ind(Inokfr) = 2; 
  
end


// update results (zero angles if invalid) 
I = find(ind == 2); 
if (I <> [])
  n = length(I); 
  qa(I) = %nan * CL_rot_defQuat(ones(4,n));
  angsa(:,I) = %nan * ones(2,n);
  qb(I) = qa(I);
  angsb(:,I) = angsa(:,I); 
end

Inok = find(ind <> 0); 


// Selection of outputs 

if (numsol == 1)
  if (argn(1) > 3); CL__error("Too many output arguments"); end
  varargout = list(qa, angsa,Inok);
else
  if (argn(1) > 5); CL__error("Too many output arguments"); end
  varargout = list(qa, angsa, qb, angsb, Inok);
end


endfunction