//  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 [equin,jacob] = CL__oe_cireq2equin(cireq)
// Circular equatorial to equinoctial orbital elements
//
// Calling Sequence
// [equin, jacob] = CL__oe_cireq2equin(cireq)
//
// Description
// <itemizedlist><listitem>
// <p>Converts circular equatorial orbital elements to equinoctial orbital elements.</p>
// <p>The transformation jacobian is optionally computed.</p>
// <p>See <link linkend="Orbital elements">Orbital elements</link> for more details</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// cireq: Circular equatorial orbital elements [sma;ex;ey;ix;iy;L] [m,rad] (6xN)
// equin: Equinoctial orbital elements [sma;ex;ey;hx;hy;L] [m,rad] (6xN)
// jacob: (optional) Transformation jacobian (See <link linkend="Orbital elements">Orbital elements</link> for more details) (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Example 1
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// equin = CL__oe_cireq2equin(cireq);
//
// // Example 2
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [equin, jacob1] = CL__oe_cireq2equin(cireq);
// [cireq2, jacob2] = CL__oe_equin2cireq(equin);
// cireq2 - cireq // => zero
// jacob2 * jacob1 // => identity


// Declarations:

// Code:

// Handle [] cases
if (cireq == [])
  equin = [];
  jacob = [];
  return;
end

// Check validity of input
[isvalid,type_orbit] = CL__oe_isValid("cireq",cireq);
if (~isvalid); CL__error("Invalid orbital elements"); end;
if (find(type_orbit <> 1) <> []); CL__error("Invalid orbital elements (parabolic or hyperbolic orbit)"); end;


// Conversion formulas: (cireq = [a; ex; ey; ix; iy; L]
// ix_equin = tan(inc/2) * cos(gom) = ix / sqrt(1 - ix^2 - iy^2)
// iy_equin = tan(inc/2) * sin(gom) = iy / sqrt(1 - ix^2 - iy^2)
// (other elements: identical)

equin = cireq;
D = real(sqrt(1 - cireq(4,:).^2 - cireq(5,:).^2));
I = find(D == 0); // case inclination = %pi
D(I) = %nan;  

equin(4,:) = cireq(4,:) ./ D; 
equin(5,:) = cireq(5,:) ./ D; 


// Jacobian computation (dequin/dcireq)
jacob = []; 

if (argn(1) == 2)
  // jacob(i,j) = d(equin_i)/d(cireq_j)
  //
  // Formulas used: cireq = [a; ex; ey; ix; iy; L]
  // da_equin/da = 1
  // dex_equin/dex = 1
  // dey_equin/dey = 1
  // dix_equin/dix = (1 - iy^2) / (1 - ix^2 - iy^2)^(3/2)
  // dix_equin/diy = -ix*iy / sqrt(1 - ix^2 - iy^2)^(3/2)
  // diy_equin/dix = -ix*iy / sqrt(1 - ix^2 - iy^2)^(3/2)
  // diy_equin/diy = (1 - ix^2) / (1 - ix^2 - iy^2)^(3/2)
  // dL_equin/dL = 1
  
  N = size(cireq,2);
  jacob = zeros(6,6,N);

  jacob(1,1,:) = 1;  // da_equin/da
  jacob(2,2,:) = 1;  // dex_equin/dex
  jacob(3,3,:) = 1;  // dey_equin/dey
  
  jacob(4,4,:) = (1 - cireq(5,:).^2) ./ D.^3;  // dix_equin/dix
  jacob(4,5,:) = (cireq(4,:) .* cireq(5,:)) ./ D.^3;  // dix_equin/diy
     
  jacob(5,4,:) = (cireq(4,:) .* cireq(5,:)) ./ D.^3;  // diy_equin/dix
  jacob(5,5,:) = (1 - cireq(4,:).^2) ./ D.^3;  // diy_equin/diy

  jacob(6,6,:) = 1;  // dpso/dL

end

endfunction