//  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 [pos,vel,jacob] = CL_oe_cir2car(cir, mu)
// Circular to cartesian orbital elements
//
// Calling Sequence
// [pos,vel,jacob] = CL_oe_cir2car(cir [,mu])
//
// Description
// <itemizedlist><listitem>
// <p>Converts orbital elements adapted to near-circular orbits to 
// cartesian 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)
// mu : (optional) Gravitational constant. [m^3/s^2] (default value is %CL_mu)
// pos: Position vector [x;y;z] [m] (3xN)
// vel: Velocity vector [vx;vy;vz] [m/s] (3xN)
// jacob: (optional) Transformation jacobian (See <link linkend="Orbital elements">Orbital elements</link> for more details) (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_oe_car2cir
// CL_oe_cir2kep
//
// Examples
// // // Example 1
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// [pos, vel] = CL_oe_cir2car(cir);
//
// // Example 2
// cir = [7000.e3; 0.1; 0.2; 1; 2; 3];
// [pos, vel, jacob1] = CL_oe_cir2car(cir);
// [cir2, jacob2] = CL_oe_car2cir(pos, vel);
// cir2 - cir // => zero
// jacob2 * jacob1 // => identity

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


// Code:
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

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

a = cir(1,:);
ex = cir(2,:);
ey = cir(3,:);

// F = w+E = eccentric argument of latitude
F = CL_kp_M2Ecir(ex,ey,cir(6,:));
cosF = cos(F);
sinF = sin(F);

// Other needed quantities
r = a .* (1 - ex.*cosF - ey.*sinF);
n = sqrt(mu ./ a.^3);
nu = 1 ./ (1 + sqrt(1 - ex.^2 - ey.^2)); // nu = 1/(1+sqrt(1-e^2))

// Position and velocity in "natural" frame R
X = a .* ((1-nu.*ey.^2).*cosF + nu.*ex.*ey.*sinF - ex);
Y = a .* ((1-nu.*ex.^2).*sinF + nu.*ex.*ey.*cosF - ey);
       
VX = n.*a.^2 ./ r .* (cosF .* nu .* ex .* ey + sinF .* (nu .* ey.^2 - 1));
VY = n.*a.^2 ./ r .* (-sinF .* nu .* ex .* ey - cosF .* (nu .* ex.^2 - 1));

// Position and velocity in cartesian frame

// NB: This code is simpler but slower:
// R = R3(-gom)*R1(-i)
// R = CL_rot_angles2matrix([1,3],[-cir(4,:);-cir(5,:)]);
// pos = R * [X;Y;0];
// vel = R * [VX;VY;0];
     
// Using direct formulas is the fastest way to compute matrix R:
cg = cos(cir(5,:));
sg = sin(cir(5,:));
ci = cos(cir(4,:));
si = sin(cir(4,:));
// P = first column of matrix R
// Q = second column of matrix R
P = [cg; sg; zeros(cg)];
Q = [-sg.*ci; cg.*ci; si];

pos = CL_dMult(P,X)  + CL_dMult(Q,Y);
vel = CL_dMult(P,VX) + CL_dMult(Q,VY);

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

if (argn(1) == 3)
  // jacob(i,j) = d(car_i)/d(cir_j)
  
  N = size(cir,2);
  jacob = zeros(6,6,N);
   
  // d(x,y,z,vx,vy,vz)/da
  jacob(:,1,:) = CL_dMult(1 ./ a, [pos;-vel/2]) ;
        
  // Partial derivatives of Y,Y,VX,VY with respect to ex:
  eta = sqrt(1 - ex.^2 - ey.^2);
  dXdex = ey ./ (n .* (1+eta)) .* VX + Y.*VX./(n.*a.*eta) - a;
  dYdex = ey ./ (n .* (1+eta)) .* VY - X.*VX./(n.*a.*eta);
  dVXdex = VX.*VY./(n.*a.*eta) - n.*a.^2 ./ r.^3 .* (a.*ey./(1+eta).*X + X.*Y./eta);
  dVYdex = -VX.*VX./(n.*a.*eta) - n.*a.^2 ./ r.^3 .* (a.*ey./(1+eta).*Y - X.*X./eta);
  
  // Partial derivatives of Y,Y,VX,VY with respect to ey:
  dXdey = -ex ./ (n .* (1+eta)) .* VX + Y.*VY./(n.*a.*eta);
  dYdey = -ex ./ (n .* (1+eta)) .* VY - X.*VY./(n.*a.*eta) - a;
  dVXdey = VY.*VY./(n.*a.*eta) + n.*a.^2 ./ r.^3 .* (a.*ex./(1+eta).*X - Y.*Y./eta);
  dVYdey = -VX.*VY./(n.*a.*eta) + n.*a.^2 ./ r.^3 .* (a.*ex./(1+eta).*Y + X.*Y./eta);
  
  // Apply the same matrix R as before: 
  // d(x,y,z,vx,vy,vz)/dex
  jacob(:,2,:) = [CL_dMult(P,dXdex)  + CL_dMult(Q,dYdex) ; ...
                  CL_dMult(P,dVXdex) + CL_dMult(Q,dVYdex)] ;
                  
  // d(x,y,z,vx,vy,vz)/dey
  jacob(:,3,:) = [CL_dMult(P,dXdey)  + CL_dMult(Q,dYdey) ; ...
                  CL_dMult(P,dVXdey) + CL_dMult(Q,dVYdey)] ;
                  
  // d(x,y,z,vx,vy,vz)/dalpha
  jacob(:,6,:) = [ CL_dMult(1 ./ n, vel) ; ...
                   CL_dMult(-n .* a.^3 ./ r.^3, pos) ] ;
  
  
  // The partial derivatives with respect to i,gom 
  // are computed by differianting the matrix R
  
  // Pi,Pg = first column of matrix R differentiated with respect to i,gom
  // Qi,Qg = second column of matrix R differentiated with respect to i,gom
  
  // Pi = 0; = 
  Qi = [sg.*si; -cg.*si; ci];
  
  Pg = [-sg; cg; zeros(cg)];
  Qg = [-cg.*ci; -sg.*ci; zeros(cg)];
  
  // d(x,y,z,vx,vy,vz)/di
  jacob(:,4,:) = [CL_dMult(Qi,Y) ; ...
                  CL_dMult(Qi,VY)] ;
                              
  // d(x,y,z,vx,vy,vz)/dgom
  jacob(:,5,:) = [CL_dMult(Pg,X)  + CL_dMult(Qg,Y) ; ...
                  CL_dMult(Pg,VX) + CL_dMult(Qg,VY)] ;
  
end

endfunction