//  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 [cir,jacob] = CL__oe_cireq2cir(cireq)
// Circular equatorial to circular orbital elements
//
// Calling Sequence
// [cir, jacob] = CL__oe_cireq2cir(cireq)
//
// Description
// <itemizedlist><listitem>
// <p>Converts circular equatorial orbital elements to orbital elements adapted to near-circular orbits.</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)
// cir: Orbital elements adapted to near-circular orbits [sma;ex;ey;inc;gom;pso] [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];
// cir = CL__oe_cireq2cir(cireq);
//
// // Example 2
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [cir, jacob1] = CL__oe_cireq2cir(cireq);
// [cireq2, jacob2] = CL__oe_cir2cireq(cir);
// cireq2 - cireq // => zero
// jacob2 * jacob1 // => identity


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

// Code:

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

cir = zeros(cireq);

// Notes on particular cases: 
// 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: (cireq = [a; ex; ey; ix; iy; L]
// a_cir = a
// ex_cir = ex*cos(gom) + ey*sin(gom)
// ey_cir = -ex*sin(gom) + ey*cos(gom)
// inc = 2*asin(sqrt(ix^2+iy^2))
// gom = atan(iy,ix)
// pso = L - gom

cir(1,:) = cireq(1,:);

sininc2 = sqrt(cireq(4,:).^2 + cireq(5,:).^2); // sin(inc/2)
cir(4,:) = 2 * asin(sininc2); // inc

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

cosgom = cos(cir(5,:)); 
singom = sin(cir(5,:)); 

cir(2,:) = cireq(2,:) .* cosgom + cireq(3,:) .* singom;
cir(3,:) = -cireq(2,:) .* singom + cireq(3,:) .* cosgom;

cir(6,:) = cireq(6,:) - cir(5,:);

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


// Jacobian computation (dcir/dcireq)
if (argn(1) == 2)
  // jacob(i,j) = d(cir_i)/d(cireq_j)
  //
  // Formulas used: cireq = [a; ex; ey; ix; iy; L]
  // da_cir/da = 1
  // dex_cir/dex = cos(gom)
  // dex_cir/dey = sin(gom)
  // dex_cir/dix = ey_cir * dgom/dix
  // dex_cir/diy = ey_cir * dgom/diy
  // dey_cir/dex = -sin(gom)
  // dey_cir/dey = cos(gom)
  // dey_cir/dix = -ex_cir * dgom/dix
  // dey_cir/diy = -ex_cir * dgom/diy
  // di/dix = ix / sin(i) = 2*ix/(sqrt(ix^2+iy2)*sqrt(1-(ix^2+iy^2)))
  // di/diy = iy / sin(i) = 2*iy/(sqrt(ix^2+iy2)*sqrt(1-(ix^2+iy^2)))
  // dgom/dix = -iy / (ix^2+iy^2)
  // dgom/diy = ix / (ix^2+iy^2)
  // dpso/ix = -dgom/dix
  // dpso/iy = -dgom/diy
  // dpso/dL = 1
  
  // Equatorial orbit (not computable results --> set to %nan)
  D2 = cireq(4,:).^2 + cireq(5,:).^2;
  D2(Ieq) = %nan;
  D = sqrt(D2); 
  
  N = size(cireq,2);
  jacob = zeros(6,6,N);
   
  jacob(1,1,:) = 1;  // da/da

  jacob(2,2,:) = cosgom;  // dex_cir/dex
  jacob(2,3,:) = singom;  // dex_cir/dey
  jacob(2,4,:) = -cir(3,:) .* cireq(5,:) ./ D2;  // dex_cir/ix
  jacob(2,5,:) = cir(3,:) .* cireq(4,:) ./ D2;  // dex_cir/iy
   
  jacob(3,2,:) = -singom;  // dey_cir/dex
  jacob(3,3,:) = cosgom;  // dey_cir/dey
  jacob(3,4,:) = cir(2,:) .* cireq(5,:) ./ D2;  // dey_cir/ix
  jacob(3,5,:) = -cir(2,:) .* cireq(4,:) ./ D2;  // dey_cir/iy

  jacob(4,4,:) = 2 * cireq(4,:) ./ (D .* sqrt(1-D2)) ;  // di/dix
  jacob(4,5,:) = 2 * cireq(5,:) ./ (D .* sqrt(1-D2));  // di/diy
    
  jacob(5,4,:) = -cireq(5,:) ./ D2;  // dgom/dix
  jacob(5,5,:) = cireq(4,:) ./ D2;  // dgom/diy
         
  jacob(6,4,:) = cireq(5,:) ./ D2;  // dpso/dix
  jacob(6,5,:) = -cireq(4,:) ./ D2;  // dpso/diy
  jacob(6,6,:) = 1;  // dpso/dL

end

endfunction