//  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_kep2equin(kep)
// Keplerian to equinoctial orbital elements
//
// Calling Sequence
// [equin, jacob] = CL__oe_kep2equin(kep)
//
// Description
// <itemizedlist><listitem>
// <p>Converts classical Keplerian 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
// kep: Classical Keplerian orbital elements [sma;ecc;inc;pom;gom;anm] [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
//
// See also
// CL_oe_equin2kep
// CL_oe_kep2car
//
// Examples
// // Example 1
// kep = [7000.e3; 0.1; 0.5; 1; 2; 3];
// equin = CL_oe_kep2equin(kep);
//
// // Example 2
// kep = [7000.e3; 0.1; 0.5; 1; 2; 3];
// [equin, jacob1] = CL_oe_kep2equin(kep);
// [kep2, jacob2] = CL_oe_equin2kep(equin);
// kep2 - kep // => zero
// jacob2 * jacob1 // => identity

// Declarations:


// Code:

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

// Check validity of input (must be an elliptical orbit too!)
[isvalid,type_orbit] = CL__oe_isValid("kep",kep);
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(kep)

// Conversion formulas:
// a(equin) = a(kep)
// ex = e*cos(pom+gom)
// ey = e*sin(pom+gom)
// hx = tan(i/2)cos(gom)
// hy = tan(i/2)sin(gom)
// L = M + pom + gom

equin(1,:) = kep(1,:);
equin(2,:) = kep(2,:) .* cos(kep(4,:)+kep(5,:));
equin(3,:) = kep(2,:) .* sin(kep(4,:)+kep(5,:));
equin(4,:) = tan(kep(3,:)/2) .* cos(kep(5,:));
equin(5,:) = tan(kep(3,:)/2) .* sin(kep(5,:));
equin(6,:) = kep(4,:) + kep(5,:) + kep(6,:);

// Jacobian computation (dequin/dkep)
if (argn(1) == 2)
  // jacob(i,j) = d(equin_i)/d(kep_j)
  //
  // Formulas used:
  // da/da = 1
  // dex/de = cos(pom+gom)
  // dex/dhx = -ey
  // dex/dhy = -ey
  // dey/de = sin(pom+gom)
  // dey/dhx = ex
  // dey/dhy = ex
  // dhx/di = (1+tan(i/2)^2)*cos(gom)/2
  // dhx/dgom = -hy
  // dhy/di = (1+tan(i/2)^2)*sin(gom)/2
  // dhy/dgom = hx
  // dL/dpom = 1
  // dL/dgom = 1
  // dL/dM = 1

  N = size(kep,2);
  jacob = zeros(6,6,N);
  
  jacob(1,1,:) = 1; // da/da
  
  jacob(2,2,:) = cos(kep(4,:)+kep(5,:)); // dex/de
  jacob(2,4,:) = - equin(3,:); // dex/dhx
  jacob(2,5,:) = - equin(3,:); // dex/dhy
  
  jacob(3,2,:) = sin(kep(4,:)+kep(5,:)); // dey/de
  jacob(3,4,:) = equin(2,:); // dey/dhx
  jacob(3,5,:) = equin(2,:); // dey/dhy
  
  jacob(4,3,:) = (1 + tan(kep(3,:)/2).^2) .* cos(kep(5,:))/2; // dhx/di
  jacob(4,5,:) = - equin(5,:); // dhx/dgom
  
  jacob(5,3,:) = (1 + tan(kep(3,:)/2).^2) .* sin(kep(5,:))/2; // dhy/di
  jacob(5,5,:) = equin(4,:); // dhy/dgom
  
  jacob(6,4,:) = 1; // dL/dpom
  jacob(6,5,:) = 1; // dL/dgom
  jacob(6,6,:) = 1; // dL/dM


end

endfunction