//  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_cir2equin(cir)
// Circular to equinoctial orbital elements
//
// Calling Sequence
// [equin,jacob] = CL__oe_cir2equin(cir)
//
// Description
// <itemizedlist><listitem>
// <p>Converts circular 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
// cir: Orbital elements adapted to near-circular orbits [sma;ex;ey;inc;gom;pso] [m,rad] (6xN)
// equinoctial: Equinoctial 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
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// equin = CL__oe_cir2equin(cir);
//
// // Example 2
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// [equin, jacob1] = CL__oe_cir2equin(cir);
// [cir2, jacob2] = CL__oe_equin2cir(equin);
// cir2 - cir // => zero
// jacob2 * jacob1 // => identity


// Declarations:


// Code:

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

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

equin = zeros(cir)

// Conversion formulas: (cir = [a;ex;ey;inc;gom;pso]
// a_equin = a
// ex_equin = e*cos(pom+gom) = ex*cos(gom) - ey*sin(gom)
// ey_equin = e*sin(pom+gom) = ex*sin(gom) + ey*cos(gom)
// ix = tan(inc/2)*cos(gom)
// iy = tan(inc/2)*sin(gom)
// L = gom + pso

cosgom = cos(cir(5,:)); 
singom = sin(cir(5,:));
taninc2 = tan(cir(4,:)/2); // tan(inc/2)

equin(1,:) = cir(1,:);
equin(2,:) = cir(2,:) .* cosgom - cir(3,:) .* singom;
equin(3,:) = cir(2,:) .* singom + cir(3,:) .* cosgom;
equin(4,:) = taninc2 .* cosgom;
equin(5,:) = taninc2 .* singom;
equin(6,:) = cir(5,:) + cir(6,:);

// Jacobian computation (dequin/dcir)
if (argn(1) == 2)
  // jacob(i,j) = d(equin_i)/d(cir_j)
  //
  // Formulas used:
  // da_equin/da = 1
  // dex_equin/dex = cos(gom)
  // dex_equin/dey = -sin(gom)
  // dex_equin/dgom = -ex*sin(gom)-ey*cos(gom) = -ey_equin
  // dey_equin/dex = sin(gom)
  // dey_equin/dey = cos(gom)
  // dey_equin/dgom = ex*cos(gom)-ey*sin(gom) = ex_equin
  // dix/di = (1+tan(i/2)^2)*cos(gom)/2
  // dix/dgom = -iy
  // diy/di = (1+tan(i/2)^2)*sin(gom)/2
  // diy/dgom = ix
  // dL/dgom = 1
  // dL/dpso = 1

  N = size(cir,2);
  jacob = zeros(6,6,N);

  jacob(1,1,:) = 1; // da_equin/da
  
  jacob(2,2,:) = cosgom; // dex_equin/dex
  jacob(2,3,:) = -singom; // dex_equin/dey
  jacob(2,5,:) = -equin(3,:); // dex_equin/dgom
  
  jacob(3,2,:) = singom; // dey_equin/dex
  jacob(3,3,:) = cosgom; // dey_equin/dey
  jacob(3,5,:) = equin(2,:); // dey_equin/dgom
  
  jacob(4,4,:) = (1+taninc2.^2) .* cosgom / 2; // dix/di
  jacob(4,5,:) = -equin(5,:); // dix/dgom
  
  jacob(5,4,:) = (1+taninc2.^2) .* singom / 2; // diy/di
  jacob(5,5,:) = equin(4,:); // diy/dgom
  
  jacob(6,5,:) = 1; // dL/dgom
  jacob(6,6,:) = 1; // dL/dpso

end

endfunction