//  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 [result,Inok] = CL_rot_defFrameVec(u,v,n1,n2,res,meth)
// Frame definition based on 2 vectors. 
//
// Calling Sequence
// [M,Inok] = CL_rot_defFrameVec(u,v,n1,n2 [,res='m' ,meth])
// [q,Inok] = CL_rot_defFrameVec(u,v,n1,n2, res='q' [,meth])
//
// Description
// <itemizedlist>
// <listitem>
// <p>This function defines a new frame based on 2 vectors. </p>
// <p>Considering two vectors <b>u</b> and <b>v</b>, the new frame basis vectors are such that: </p>
// <p>- <b>n1</b> axis: aligned with u (and with same direction) </p>
// <p>- <b>n2</b> axis: in the half-plane delimited by u and containing v </p>
// <p>where n1 and n2 are axis numbers: 1=>x-axis, 2=>y-axis, 3=>z-axis. </p>
// <p></p></listitem>
// <listitem>
// <p>If u and v are (nearly) aligned or if n1 and n2 are identical, the basis vectors 
// are such that: </p>
// <p>- n1 axis: aligned with u (and same direction) </p>
// <p>- other axes: perpendicular to n1 axis (arbitrary directions). </p>
// <p></p></listitem>
// <listitem>
// <p>The outputs are: </p>
// <p>- <b>M</b>: Transformation matrix from the (implicit) reference frame to the new frame defined by u and v, </p>
// <p>- <b>q</b>: Quaternion of the rotation that transforms the axes of the (implicit) reference frame into 
// the axes of the new frame defined by u and v, </p> 
// <p>- <b>Inok</b>: Row vector that contains the indices for which the new frame is not uniquely defined. </p>
// <p></p></listitem>
// <listitem>
// <p><b>Note</b>:</p> 
// <p>- CL_rot_defFrameVec(u,[],n1,[]) is equivalent to: CL_rot_defFrameVec(u,u,n1,n1).  </p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// u: First vector (3xN or 3x1)
// v: Second vector (3xN or 3x1)
// n1: First axis number (axis colinear with u). Possibles values are: 1, 2 or 3 (1x1)
// n2: Second axis number (axis in the (u,v) plane). Possibles values are: 1, 2, 3 (1x1)
// res: (optional, string) Type of result: 'm': frame transformation matrix, q: rotation quaternion. Default is 'm' 
// meth: (optional, string) Method used: 'm': Method based on matrices (faster), 'q': method based on quaternions (result consistent with CL_rot_defRotVec). Default is 'm' 
// M/q: Frame transformation matrix (3x3xN) or rotation quaternion (dim N)
// Inok: Indices for which the new frame is not uniquely defined. 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defRotVec
//
// Examples
// CL_rot_defFrameVec([1;0;0], [1;1;0], 1, 2) // => identity
//
// // Computes local orbital frame:
// kep = [7000.e3; 0.01; %pi/3 ; 0 ; 0; 0]; // orbital elements
// [r,v] = CL_oe_kep2car(kep); // position and velocity
//
// M = CL_rot_defFrameVec(r, v, 1, 2) // => "qsw" local frame
// q = CL_rot_defFrameVec(-r, v, 3, 1, res='q')  //=> "lvlh" local frame
//

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

tol = 1.e-16; // tolerance for vector alignment (angle squared) 

// Solution for 1 vector 
// norm(u) == 1
function [M] = solve1_frame(u,n1)
  N = size(u,2);
  M = hypermat([3,3,N]);

  // second axis : n2 <> n1 and n2 <> n3 
  n2 = modulo(n1,3) + 1; // axis #2
  n3 = modulo(n2,3) + 1; // axis #3 

  v = CL_unitVector(CL__axisPerp(u)); 
  w = CL_cross(u,v);

  M(n1,:,:) = u; 
  M(n2,:,:) = v; 
  M(n3,:,:) = w; 

endfunction


// Solution for 2 vectors
// w=u^v; norms of u,v,w == 1
// (u and v not necessarily perpendicular)
function [M] = solve2_frame(u,v,w,n1,n2)
  N = size(u,2);
  M = hypermat([3,3,N]);

  // third axis : n3 <> n1 and n3 <> n2 
  n3 = modulo(n2,3)+1;
  if (n3 == n1); n3 = modulo(n3,3)+1; end
  sgn3 = sign(CL_rMod(n2 - n1, -1.5, 1.5));   

  v = CL_cross(w,u); // norm == 1

  M(n1,:,:) = u; 
  M(n2,:,:) = v; 
  M(n3,:,:) = sgn3 * w; 

endfunction


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

// Check the number of arguments, 
// in case the function is called with only 2: (u, n1) 
[lhs, rhs] = argn(); 
if (rhs < 4) 
  CL__error("At least four arguments expected: (u,v,n1,n2) or (u,u,n1,n1)");
end

// method: "matrix" or "quaternion"
if (~exists("meth", "local")); meth="m"; end 
if (meth <> "m" & meth <> "q")
  CL__error("Wrong method type");
end

// type of result: by default: matrix 
if ~exists("res", "local"); res="m"; end 
if (res <> "m" & res <> "q")
  CL__error("Wrong result type");
end

// case: empty second axis or vector
if (v == []); v = u; end
if (n2 == []); n2 = n1; end

// check argument values
if (n1 <> 1 & n1 <> 2 & n1 <> 3)
  CL__error("Wrong value for n1");
end
if (n2 <> 1 & n2 <> 2 & n2 <> 3)
  CL__error("Wrong value for n2");
end

// check dims and resize
// unit vectors before resizing (for efficiency)
// check vector norms are not 0
[u, v, N] = CL__checkInputs(u,3,v,3);
[u, nu] = CL_unitVector(u); 
[v, nv] = CL_unitVector(v); 
if (find (nu .* nv == 0) <> [])
  CL__error("Vectors should not be zero");
end

// Computation

if (meth == 'm')

  result = %nan * ones(3,3,N); // matrix type
  [w, nw] = CL_unitVector(CL_cross(u,v));
  cond_NOK = nw.*nw < tol | n1 == n2; // same as in CL_rot_defRotVec

  I = find(cond_NOK); // vectors are aligned
  if (I <> [])
    result(:,:,I) = solve1_frame(u(:,I),n1);
  end

  I = find(~cond_NOK); // vectors are not aligned
  if (I <> [])
    result(:,:,I) = solve2_frame(u(:,I),v(:,I),w(:,I),n1,n2); 
  end

  // conversion if needed 
  if (res == 'q')
    result = CL_rot_matrix2quat(result); 
  end

  Inok = find(cond_NOK); 

else

  // result is a quaternion
  Id = eye(3,3); 
  result = CL_rot_defRotVec(Id(:,n1), Id(:,n2), u, v); 

  // conversion if needed 
  if (res == 'm')
    result = CL_rot_quat2matrix(result); 
  end

end


endfunction