//  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_cir2kep(cir)
// Circular to Keplerian orbital elements
//
// Calling Sequence
// [kep, jacob] = CL_oe_cir2kep(cir)
//
// Description
// <itemizedlist><listitem>
// <p>Converts orbital elements adapted to near-circular orbits 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>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// cir: Orbital elements adapted to near-circular orbits [sma;ex;ey;inc;gom;pso] [m,rad] (6xN)
// kep: 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
//
// See also
// CL_oe_kep2cir
// CL_oe_cir2car
//
// Examples
// // // Example 1
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// kep = CL_oe_cir2kep(cir);
//
// // Example 2
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// [kep, jacob1] = CL_oe_cir2kep(cir);
// [cir2, jacob2] = CL_oe_kep2cir(kep);
// cir2 - cir // => zero
// jacob2 * jacob1 // => identity

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

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

// 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)

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

kep = zeros(cir);

kep(1,:) = cir(1,:);
kep(2,:) = sqrt(cir(2,:).^2 + cir(3,:).^2);
kep(3,:) = cir(4,:);

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

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


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

if (argn(1) == 2)
  // jacob(i,j) = d(kep_i)/d(cir_j)
  //
  // Formulas used:
  // da/da = 1
  // de/dex = cos(w)
  // de/dey = sin(w)
  // di/di = 1
  // dw/dex = -sin(w)/e
  // dw/dey = cos(w)/e
  // dgom/dgom = 1
  // dM/dex = sin(w)/e
  // dM/dey = -cos(w)/e
  // dM/dalpha = 1

  // Not computable results --> set to %nan
  e = kep(2,:);
  e(Icir) = %nan;
  
  N = size(cir,2);
  jacob = zeros(6,6,N);
   
  jacob(1,1,:) = 1; // da/da
  
  jacob(2,2,:) = cos(kep(4,:)); // de/dex
  jacob(2,3,:) = sin(kep(4,:)); // de/dey
  
  jacob(3,4,:) = 1; // di/di
  
  jacob(4,2,:) = -sin(kep(4,:)) ./ e; // dw/dex
  jacob(4,3,:) = cos(kep(4,:)) ./ e; // dw/dey
  
  jacob(5,5,:) = 1; // dgom/dgom
  
  jacob(6,2,:) = sin(kep(4,:)) ./ e; // dM/dex
  jacob(6,3,:) = -cos(kep(4,:)) ./ e; // dM/dey
  jacob(6,6,:) = 1; // dM/dpso
  
end

endfunction