//  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 [kep,jacob] = CL__oe_cireq2kep(cireq)
// Circular equatorial to Keplerian orbital elements
//
// Calling Sequence
// [kep, jacob] = CL__oe_cireq2kep(cireq)
//
// Description
// <itemizedlist><listitem>
// <p>Converts circular equatorial orbital elements to classical Keplerian 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)
// kep: Classical Keplerian orbital elements [sma;ecc;inc;pom;gom;anm] [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];
// kep = CL__oe_cireq2kep(cireq);
//
// // Example 2
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [kep, jacob1] = CL__oe_cireq2kep(cireq);
// [cireq2, jacob2] = CL__oe_kep2cireq(kep);
// cireq2 - cireq // => zero
// jacob2 * jacob1 // => identity


// Declarations:
EPS_ORB = CL__dataGetEnv("epsOrb", internal=%t);

// Code:

// Handle [] cases
if (cireq == [])
  kep = [];
  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;

kep = zeros(cireq);

// Notes on particular cases handled: 
// 1) If the orbit is nearly circular, pom is not defined
// (Circular treshold : ecc < EPS_ORB.cir)
// In that case pom is set to 0 (arbitrary choice)
//
// 1) If the orbit is nearly equatorial, gom is not defined
// (Equatorial treshold : sin(inc) < EPS_ORB.equa)
// In that case gom is set to 0 (arbitrary choice)

// Conversion formulas:
// a(kep) = a(cireq)
// e = sqrt(ex^2+ey^2)
// i = 2*asin(sqrt(hx^2+hy^2))
// gom = atan(hy,hx)
// pom = atan(ey,ex) - gom
// M = L - pom - gom

kep(1,:) = cireq(1,:);
kep(2,:) = sqrt(cireq(2,:).^2 + cireq(3,:).^2);
sini_sur_2 = sqrt(cireq(4,:).^2 + cireq(5,:).^2);
kep(3,:) = 2 * asin(sini_sur_2);

kep(5,:) = atan(cireq(5,:),cireq(4,:));
Ieq = find(2*sini_sur_2 < EPS_ORB.equa); // equatorial orbit
kep(5,Ieq) = 0;

kep(4,:) = atan(cireq(3,:),cireq(2,:)) - kep(5,:);
Icir = find(kep(2,:) < EPS_ORB.cir); // circular orbit
kep(4,Icir) = 0;

kep(6,:) = cireq(6,:) - kep(4,:) - kep(5,:);

// Reduce angles to 0 -> 2pi : necessary?
// kep(3:6,:) = CL_rMod(kep(3:6,:),2*%pi)

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

if (argn(1) == 2)
  // jacob(i,j) = d(kep_i)/d(cireq_j)
  //
  // Formulas used:
  // da/da = 1
  // de/dex = cos(pom+gom)
  // de/dey = sin(pom+gom)
  // di/dhx = ix / sin(i) = 2*ix/(sqrt(ix^2+iy2)*sqrt(1-(ix^2+iy^2)))
  // di/dhy = iy / sin(i) = 2*iy/(sqrt(ix^2+iy2)*sqrt(1-(ix^2+iy^2)))
  // dpom/dex = -ey/e2
  // dpom/dey = ex/e2
  // dpom/dhx = iy / (ix^2+iy^2)
  // dpom/dhy = -ix / (ix^2+iy^2)
  // dgom/dhx = -iy / (ix^2+iy^2)
  // dgom/dhy = ix / (ix^2+iy^2)
  // dM/dex = ey / e2
  // dM/dey = -ex / e2
  // dM/dL = 1
  
  // Circular orbit (not computable results --> set to %nan)
  ecir = kep(2,:);
  ecir(Icir) = %nan;
  
  // Equatorial orbit (not computable results --> set to %nan)
  ix2_ixy2 = cireq(4,:).^2 + cireq(5,:).^2;
  ix2_ixy2(Ieq) = %nan;
  
  N = size(cireq,2);
  jacob = zeros(6,6,N);
   
  jacob(1,1,:) = 1;  // da/da
   
  jacob(2,2,:) = cireq(2,:) ./ ecir;  // de/dex
  jacob(2,3,:) = cireq(3,:) ./ ecir;  // de/dey
   
  jacob(3,4,:) = 2 * cireq(4,:) ./ (sqrt(ix2_ixy2) .* sqrt(1-ix2_ixy2)) ;  // di/dhx
  jacob(3,5,:) = 2 * cireq(5,:) ./ (sqrt(ix2_ixy2) .* sqrt(1-ix2_ixy2));  // di/dhy
   
  jacob(4,2,:) = -cireq(3,:) ./ ecir.^2;  // dpom/dex
  jacob(4,3,:) = cireq(2,:) ./ ecir.^2;  // dpom/dey
  jacob(4,4,:) = cireq(5,:) ./ ix2_ixy2;  // dpom/dhx
  jacob(4,5,:) = -cireq(4,:) ./ ix2_ixy2;  // dpom/dhy
   
  jacob(5,4,:) = -cireq(5,:) ./ ix2_ixy2;  // dgom/dhx
  jacob(5,5,:) = cireq(4,:) ./ ix2_ixy2;  // dgom/dhy
   
  jacob(6,2,:) = cireq(3,:) ./ ecir.^2;  // dM/dex
  jacob(6,3,:) = -cireq(2,:) ./ ecir.^2;  // dM/dey
  jacob(6,6,:) = 1;  // dM/dL

end

endfunction