//  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_equin2car(equin, mu)
// Equinoctial to cartesian orbital elements
//
// Calling Sequence
// [pos, vel, jacob] = CL__oe_equin2car(equin [, mu])
//
// Description
// <itemizedlist><listitem>
// <p>Converts equinoctial orbital elements 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
// equin: Orbital elements adapted to near-circular near equatorial orbits [sma;ex;ey;hx;hy;L] [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
//
// Examples
// // // Example 1
// equin = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [pos, vel] = CL__oe_equin2car(equin);
//
// // Example 2
// equin = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [pos, vel, jacob1] = CL__oe_equin2car(equin);
// [equin2, jacob2] = CL__oe_car2equin(pos, vel);
// equin2 - equin // => 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 (equin == [])
  pos = [];
  vel = [];
  jacob = [];
  return;
end

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


a = equin(1,:);
ex = equin(2,:);
ey = equin(3,:);
hx = equin(4,:);
hy = equin(5,:);

// F = w+gom+E = eccentric longitude
F = CL_kp_M2Ecir(ex,ey,equin(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

// P = first column of matrix R
// Q = second column of matrix R
h = 1 ./ (1+hx.^2+hy.^2);
P = [(1+hx.^2-hy.^2).*h;  2*hx.*hy.*h;          -2*hy.*h];
Q = [2*hx.*hy.*h;         (1-hx.^2+hy.^2).*h;    2*hx.*h];

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

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

if (argn(1) == 3)
  // jacob(i,j) = d(car_i)/d(equin_j)
  //
  
  N = size(equin,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 hx,hy 
  // are computed by differianting the matrix R
  
  // Pix,Piy = first column of matrix R differentiated with respect to hx,hy
  // Qix,Qiy = second column of matrix R differentiated with respect to hx,hy
  
  Phx = [4*hx.*hy.^2;              -2*hy.*(hx.^2-1-hy.^2);   4*hx.*hy] .* [h.^2;h.^2;h.^2];
  Qhx = [-2*hy.*(hx.^2-1-hy.^2) ;  -4*hx.*(1+hy.^2) ;        -2*(hx.^2-1-hy.^2)] .* [h.^2;h.^2;h.^2];
  
  Phy = [-4*hy.*(1+hx.^2);    -2*hx.*(hy.^2-1-hx.^2)   ; 2*(hy.^2-1-hx.^2)] .* [h.^2;h.^2;h.^2];
  Qhy = [-2*hx.*(hy.^2-1-hx.^2);  4*hy.*hx.^2 ;  -4*hx.*hy] .* [h.^2;h.^2;h.^2];
  
  // d(x,y,z,vx,vy,vz)/dhx
  jacob(:,4,:) = [CL_dMult(Phx,X) + CL_dMult(Qhx,Y) ; ...
                  CL_dMult(Phx,VX) + CL_dMult(Qhx,VY)] ;
                              
  // d(x,y,z,vx,vy,vz)/dhy
  jacob(:,5,:) = [CL_dMult(Phy,X) + CL_dMult(Qhy,Y) ; ...
                  CL_dMult(Phy,VX) + CL_dMult(Qhy,VY)] ;
  
end

endfunction