//  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 [cireq,jacob] = CL__oe_car2cireq(pos,vel, mu)
// Cartesian to circular equatorial orbital elements
//
// Calling Sequence
// [cireq, jacob] = CL__oe_car2cireq(pos, vel [, mu])
//
// Description
// <itemizedlist><listitem>
// <p>Converts cartesian orbital elements to orbital elements adapted to near-circular near-equatorial orbits.</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
// pos: Position vector [x;y;z] [m] (3xN)
// vel: Velocity vector [vx;vy;vz] [m/s] (3xN)
// mu : (optional) Gravitational constant. [m^3/s^2] (default value is %CL_mu)
// cireq: Orbital elements adapted to near-circular orbits [sma;ex;ey;ix,iy;L] [m,rad] (6xN)
// jacob: (optional) Transformation jacobian (See <link linkend="Orbital elements">Orbital elements</link> for more details) (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Example 1
// pos = [7000.e3; 1000.e3; -500.e3];
// vel = [1.e3; 2.e3; 7e3];
// cireq = CL__oe_car2cireq(pos, vel);
//
// // Example 2
// pos = [7000.e3; 1000.e3; -500.e3];
// vel = [1.e3; 2.e3; 7e3];
// [cireq, jacob1] = CL__oe_car2cireq(pos, vel);
// [pos2, vel2, jacob2] = CL__oe_cireq2car(cireq); 
// pos2 - pos // => zero
// vel2 - vel // => 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 (pos == [] | vel == [])
  cireq = [];
  jacob = [];
  return;
end

// Check input sizes
[pos,vel,N] = CL__checkInputs(pos,3, vel,3);

// Check validity of inputs
[isvalid,orbit_type] = CL__oe_isValid("car",[pos;vel],mu);
if (~isvalid); CL__error("Invalid position velocity (zero norm or colinear)"); end;
if (find(orbit_type <> 1) <> []); CL__error("Invalid position velocity (parabolic or hyperbolic orbit)"); end;

// Semi major axis
r = CL_norm(pos);
V = CL_norm(vel);
W = CL_cross(pos,vel);
a = r ./ (2 - r .* V.^2/mu);
esinE = CL_dot(pos,vel) ./ sqrt(mu.*a);
// ecosE = r .* V.^2/mu - 1

// Inclination vector
[Wu, normW] = CL_unitVector(W);
ix = -Wu(2,:) ./ (2*sqrt((1+Wu(3,:))/2));
iy = Wu(1,:) ./ (2*sqrt((1+Wu(3,:))/2));

// Eccentricity vector
// Coordinates in the "natural frame"
// P = first column of matrix R
// Q = second column of matrix R
c = sqrt(1-ix.^2-iy.^2); // cos(i/2)
P = [(1-2*iy.^2);  2*ix.*iy;     -2*iy.*c];
Q = [2*ix.*iy;     (1-2*ix.^2);   2* ix .* c];

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

ex = CL_norm(W) .* VY ./ mu - X ./ r;
ey = -CL_norm(W) .* VX ./ mu - Y ./ r;

// L = mean longitude
// LE = eccentric longitude
nu = 1 ./ (1 + sqrt(1 - ex.^2 - ey.^2));
cosLE = X ./ a + ex - esinE .* nu .* ey;
sinLE = Y ./ a + ey + esinE .* nu .* ex;
L = atan(sinLE,cosLE) - ex .* sinLE + ey .* cosLE;

// Reduce L to [0,2pi[
cireq = [a;ex;ey;ix;iy;CL_rMod(L,2*%pi)];

// Jacobian computation (dcireq/dcar)
if (argn(1) == 2)
  // jacob(i,j) = d(cireq_i)/d(car_j)
  
  jacob = zeros(6,6,N);
  eta = sqrt(1-ex.^2-ey.^2);
  nu = 1 ./ (1+eta);
  n = sqrt(mu ./ a.^3);
  na2 = sqrt(mu.*a);

  // Partial derivatives of X,Y,VX,VY with respect to ex:
  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) - na2 ./ r.^3 .* (a.*ey./(1+eta).*X + X.*Y./eta);
  dVYdex = -VX.*VX./(n.*a.*eta) - na2 ./ r.^3 .* (a.*ey./(1+eta).*Y - X.*X./eta);
  
  // Partial derivatives of X,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) + na2 ./ r.^3 .* (a.*ex./(1+eta).*X - Y.*Y./eta);
  dVYdey = -VX.*VY./(n.*a.*eta) + na2 ./ r.^3 .* (a.*ex./(1+eta).*Y + X.*Y./eta);
  
  // Partial derivatives of W(1,:), W(2,:) and W(3,:) with respect to (x,y,z) and (vx,vy,vz)
  dW1dr = [ zeros(n) ; vel(3,:) ; -vel(2,:)];
  dW2dr = [-vel(3,:) ; zeros(n) ;  vel(1,:)];
  dW3dr = [ vel(2,:) ; -vel(1,:);  zeros(n)];
  
  dW1dv = [ zeros(n) ; -pos(3,:); pos(2,:)];
  dW2dv = [ pos(3,:) ; zeros(n) ;  -pos(1,:)];
  dW3dv = [-pos(2,:) ; pos(1,:) ; zeros(n)];
  
  // Partial derivatives of the norm of W with respect to (x,y,z) and (vx,vy,vz)
  dnormWdr = CL_dMult(CL_cross(vel, W), 1 ./ normW);
  dnormWdv = -CL_dMult(CL_cross(pos, W), 1 ./ normW);
  
  // Partial derivatives of Wu(1,:), Wu(2,:) and Wu(3,:) with respect to (x,y,z) and (vx,vy,vz)
  dWu1dr = CL_dMult(dW1dr, 1 ./ normW) - CL_dMult(W(1,:) ./ normW.^2, dnormWdr);
  dWu2dr = CL_dMult(dW2dr, 1 ./ normW) - CL_dMult(W(2,:) ./ normW.^2, dnormWdr);
  dWu3dr = CL_dMult(dW3dr, 1 ./ normW) - CL_dMult(W(3,:) ./ normW.^2, dnormWdr);
  
  dWu1dv = CL_dMult(dW1dv, 1 ./ normW) - CL_dMult(W(1,:) ./ normW.^2, dnormWdv);
  dWu2dv = CL_dMult(dW2dv, 1 ./ normW) - CL_dMult(W(2,:) ./ normW.^2, dnormWdv);
  dWu3dv = CL_dMult(dW3dv, 1 ./ normW) - CL_dMult(W(3,:) ./ normW.^2, dnormWdv);
  
  for j = 1 : 3
    // d(vx,vy,vz)/dex
    dvdex = dVXdex .* P(j,:) + dVYdex .* Q(j,:);
    // d(vx,vy,vz)/dey
    dvdey = dVXdey .* P(j,:) + dVYdey .* Q(j,:);

    
    // da/d(x,y,z) and da/d(vx,vy,vz)
    dadr = 2 * a.^2 ./ r.^3 .* pos(j,:);
    dadv = 2 ./ (n.^2 .* a) .* vel(j,:);
    
    // dex/d(x,y,z) and dex/d(vx,vy,vz)
    dexdr = -a.*nu.*ex.*eta./r.^3 .* pos(j,:) - ey.*(iy.*VX-ix.*VY)./(na2.*eta.*c) .* Wu(j,:) + eta./na2 .* dvdey;
    dexdv = 1 ./ mu .* ((2*X.*VY - VX.*Y) .* Q(j,:) - Y.*VY.*P(j,:))  - ey.*(ix.*Y-iy.*X)./(na2.*eta.*c) .* Wu(j,:);
    
    // dey/d(x,y,z) and dey/d(vx,vy,vz)
    deydr = -a.*nu.*ey.*eta./r.^3 .* pos(j,:) + ex.*(iy.*VX-ix.*VY)./(na2.*eta.*c) .* Wu(j,:) - eta./na2 .* dvdex;
    deydv = 1 ./ mu .* ((2*VX.*Y - X.*VY) .* P(j,:) - X.*VX.*Q(j,:))  + ex.*(ix.*Y-iy.*X)./(na2.*eta.*c) .* Wu(j,:);
    
    // dix/d(x,y,z) and dix/d(vx,vy,vz)
    
    d1 = 1 ./ (2*sqrt((1+Wu(3,:))/2));
    dixdr = -Wu(2,:) .* (-dWu3dr(j,:) .* d1.^3) - dWu2dr(j,:) .* d1;
    dixdv = -Wu(2,:) .* (-dWu3dv(j,:) .* d1.^3) - dWu2dv(j,:) .* d1;
    
    // diy/d(x,y,z) and diy/d(vx,vy,vz)
    diydr = Wu(1,:) .* (-dWu3dr(j,:) .* d1.^3) + dWu1dr(j,:) .* d1;
    diydv = Wu(1,:) .* (-dWu3dv(j,:) .* d1.^3) + dWu1dv(j,:) .* d1;
    
    
    // dL/d(x,y,z) and dL/d(vx,vy,vz)
    dLdr = - vel(j,:) ./ na2 + (iy.*VX - ix.*VY) ./ (na2.*eta.*c) .* Wu(j,:) - nu.*eta./na2 .* (ey.*dvdey + ex.*dvdex);
    dLdv = -2 ./ na2 .* pos(j,:) + nu .* (ex.*deydv - ey.*dexdv) + 1 ./ (na2.*c) .* (ix.*Y - iy.*X) .* Wu(j,:);
    
    // Assign results to the output jacobian:
    jacob(1,j,:) = dadr;
    jacob(2,j,:) = dexdr;
    jacob(3,j,:) = deydr;
    jacob(4,j,:) = dixdr;
    jacob(5,j,:) = diydr;
    jacob(6,j,:) = dLdr;
    
    jacob(1,j+3,:) = dadv;
    jacob(2,j+3,:) = dexdv;
    jacob(3,j+3,:) = deydv;
    jacob(4,j+3,:) = dixdv;
    jacob(5,j+3,:) = diydv;
    jacob(6,j+3,:) = dLdv;
  end
end

endfunction