//  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_cireq2car(cireq, mu)
// Circular equatorial to cartesian orbital elements
//
// Calling Sequence
// [pos, vel, jacob] = CL__oe_cireq2car(cireq [, mu])
//
// Description
// <itemizedlist><listitem>
// <p>Converts orbital elements adapted to near-circular near equatorial 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
// cireq: Orbital elements adapted to near-circular near equatorial orbits [sma;ex;ey;ix;iy;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
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [pos, vel] = CL__oe_cireq2car(cireq);
//
// // Example 2
// cireq = [7000.e3; 0.1; 0.2; 0.3; 0.4; 1];
// [pos, vel, jacob1] = CL__oe_cireq2car(cireq);
// [cireq2, jacob2] = CL__oe_car2cireq(pos, vel);
// cireq2 - cireq // => 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 (cireq == [])
  pos = [];
  vel = [];
  jacob = [];
  return;
end

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


a = cireq(1,:);
ex = cireq(2,:);
ey = cireq(3,:);
ix = cireq(4,:);
iy = cireq(5,:);

// F = w+gom+E = eccentric longitude
F = CL_kp_M2Ecir(ex,ey,cireq(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
cosis2 = sqrt(1-ix.^2-iy.^2); // cos(i/2)
P = [(1-2*iy.^2);  2*ix.*iy;     -2*iy.*cosis2];
Q = [2*ix.*iy;     (1-2*ix.^2);   2* ix .* cosis2];

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

// Jacobian computation (dcar/dcireq)
if (argn(1) == 3)
  // jacob(i,j) = d(car_i)/d(cireq_j)
  //
  
  N = size(cireq,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 ix,iy 
  // are computed by differianting the matrix R
  
  // Pix,Piy = first column of matrix R differentiated with respect to ix,iy
  // Qix,Qiy = second column of matrix R differentiated with respect to ix,iy
  
  Pix = [zeros(ix) ;  2*iy ; 2*ix.*iy./cosis2];
  Qix = [2*iy ;      -4*ix ; -(4*ix.^2+2*iy.^2-2)./cosis2];
  
  Piy = [-4*iy  ;     2*ix   ; (4*iy.^2+2*ix.^2-2)./cosis2];
  Qiy = [2*ix;     zeros(ix) ;  -2*ix.*iy./cosis2];
  
  // d(x,y,z,vx,vy,vz)/dix
  jacob(:,4,:) = [CL_dMult(Pix,X) + CL_dMult(Qix,Y) ; ...
                  CL_dMult(Pix,VX) + CL_dMult(Qix,VY)] ;
                              
  // d(x,y,z,vx,vy,vz)/diy
  jacob(:,5,:) = [CL_dMult(Piy,X) + CL_dMult(Qiy,Y) ; ...
                  CL_dMult(Piy,VX) + CL_dMult(Qiy,VY)] ;
  
end

endfunction