//  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 [q] = CL_rot_contQuat(q)
// Makes a quaternion continuous
//
// Calling Sequence
// [qc] = CL_rot_contQuat(q)
//
// Description
// <itemizedlist><listitem>
// <p>This function tries to make each component of a quaternion continuous by
// multiplying the quaternion by -1 when necessary. </p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p>- The quaternion components must be smooth enough (as a function of the indices) otherwise  
// discontinuities may not be removed. </p>
// <p>- The quaternion is not changed by the function in the sense that it can only be multiplied by 1 or -1
// (and q and -q represent the same rotation). </p>
// <p>- Quaternions with at least one component equal to %nan are not modified. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// q: Quaternion (dim N)
// qc: Quaternion with continuous components (dim N)
//
// Authors
// CNES - DCT/SB
//
// Examples
// alpha = 0 : 0.1 : 5; 
// q = CL_rot_matrix2quat(CL_rot_angles2matrix([1,2], [alpha; 2*alpha])); 
// qc = CL_rot_contQuat(q); 
// scf(); plot(alpha, [q.r; q.i]); xtitle("Initial quaternion"); 
// scf(); plot(alpha, [qc.r; qc.i]); xtitle("Transformed quaternion"); 
//

// Declarations:


// Code:

// Number of arguments (left, right)
[lhs, rhs] = argn();
if (rhs <> 1)
  CL__error("Wrong number of input arguments");
end

if (typeof(q) <> "CLquat")
  CL__error("Wrong type of input argument. Quaternion expected");
end

N = size(q); 

// Less than 2 quaternions => nothing happens
if (N < 2)
  return; 
end

// Quaternion => 4xN matrix
qm = [q.r; q.i]; 

// Forces %nan on all components if one component is %nan
// (but the changes are not reflected in the output quaternion)
Inan = find(isnan(CL_dot(qm))); 
if (Inan <> []); qm(:,Inan) = %nan; end
Innan = find(~isnan(qm(1,:)));

// No quaternion <> %nan => nothing happens
if (Innan == [])
  return; 
end  

// Makes "continuous": 
// The largest component (index k) must keep its sign at index k+1
[qmax, I] = max(abs(qm), "r");
K1 = sub2ind(size(qm), I(1:N-1), 1:N-1); 
K2 = sub2ind(size(qm), I(1:N-1), 2:N); 
J = find(qm(K1) .* qm(K2) < 0); 

if (J <> [])
  K = zeros(1:N); 
  K(J+1) = 1; 
  K = cumsum(K); 
  qm = CL_dMult((-1) .^ K, qm); 
  // Only quaternions <> %nan are modified
  q(Innan) = CL_rot_defQuat(qm(:,Innan)); 
end


endfunction