//  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 [cir,jacob] = CL_oe_car2cir(pos,vel, mu)
// Cartesian to circular orbital elements
//
// Calling Sequence
// [cir,jacob] = CL_oe_car2cir(pos,vel [,mu])
//
// Description
// <itemizedlist><listitem>
// <p>Converts cartesian orbital elements to orbital elements adapted to near-circular 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)
// cir: Orbital elements adapted to near-circular orbits [sma;ex;ey;inc;gom;pso] [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_cir2car
// CL_oe_car2kep
//
// Examples
// // Example 1
// pos = [7000.e3; 1000.e3; -500.e3];
// vel = [1.e3; 2.e3; 7e3];
// cir = CL_oe_car2cir(pos, vel);
//
// // Example 2
// pos = [7000.e3; 1000.e3; -500.e3];
// vel = [1.e3; 2.e3; 7e3];
// [cir, jacob1] = CL_oe_car2cir(pos, vel);
// [pos2, vel2, jacob2] = CL_oe_cir2car(cir); 
// 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 == [])
  cir = [];
  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);
[Wu,normW] = CL_unitVector(W);
a = r ./ (2 - r .* V.^2/mu);
esinE = CL_dot(pos,vel) ./ sqrt(mu.*a);
// ecosE = r .* V.^2/mu - 1

// Inclination (=atan(sqrt(W(1)+W(2)),W(3)))
inc = CL_vectAngle(W, [0;0;1]);

// Raan
gom = atan(W(1,:), -W(2,:));
// NB: if W(1)==0 and W(2)==0 --> gom = 0
Ieq = find(sin(inc) < EPS_ORB.equa); // equatorial orbit
gom(Ieq) = 0;

// Eccentricity vector
// Coordinates in the "natural frame"
// P = first column of matrix R
// Q = second column of matrix R
P = [cos(gom);sin(gom);zeros(gom)];
Q = [-cos(inc).*sin(gom);cos(inc).*cos(gom);sin(inc)];

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

ex = normW .* VY ./ mu - X ./ r;
ey = -normW .* VX ./ mu - Y ./ r;

// Alpha (mean argument of latitude)
nu = 1 ./ (1 + sqrt(1 - ex.^2 - ey.^2));
cosF = X ./ a + ex - esinE .* nu .* ey;
sinF = Y ./ a + ey + esinE .* nu .* ex;
alpha = atan(sinF,cosF) - ex .* sinF + ey .* cosF;

// Reduce gom,alpha to [0,2pi[
cir = [a;ex;ey;inc;CL_rMod([gom;alpha],2*%pi)];


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

if (argn(1) == 2)
  // jacob(i,j) = d(cir_i)/d(car_j)

  jacob = zeros(6,6,N);

  // Partial derivatives of W(1,:), W(2,:) and W(3,:) with respect to (x,y,z) and (vx,vy,vz)
  dW1dr = [ zeros(a) ; vel(3,:) ; -vel(2,:)];
  dW2dr = [-vel(3,:) ; zeros(a) ;  vel(1,:)];
  dW3dr = [ vel(2,:) ; -vel(1,:);  zeros(a)];
  
  dW1dv = [ zeros(a) ; -pos(3,:); pos(2,:)];
  dW2dv = [ pos(3,:) ; zeros(a) ;  -pos(1,:)];
  dW3dv = [-pos(2,:) ; pos(1,:) ; zeros(a)];
  
  // Useful values:
  s = sqrt(W(1,:).^2+W(2,:).^2);
  // s, set to %nan if equatorial orbit
  s(Ieq) = %nan;
  ci = W(3,:) ./ normW;  // cos(inc)
  si = s ./ normW;       // sin(inc)
  cg = -W(2,:) ./ s;     // cos(gom)
  sg = W(1,:) ./ s;      // sin(gom)
  
  // 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);

  for j = 1 : 3
  
    // da/d(x,y,z):
    dadr = 2 * a.^2 ./ r.^3 .* pos(j,:);
    // da/d(vx,vy,vz):
    dadv = 2 * a.^2 ./ mu .* vel(j,:);
  
    // Partial derivative of s = sqrt(W(1)^2+W(2)^2)
    dsdr = (W(1,:).*dW1dr(j,:) + W(2,:).*dW2dr(j,:)) ./ s;
    dsdv = (W(1,:).*dW1dv(j,:) + W(2,:).*dW2dv(j,:)) ./ s;
    
    // Partial derivatives of cos(inc)  with respect to (x,y,z) and (vx,vy,vz)
    dcidr = dW3dr(j,:) ./ normW - W(3,:) .* dnormWdr(j,:) ./ normW.^2;
    dcidv = dW3dv(j,:) ./ normW - W(3,:) .* dnormWdv(j,:) ./ normW.^2;
    
    // Partial derivatives of sin(inc) with respect to (x,y,z) and (vx,vy,vz)
    dsidr = dsdr ./ normW - s .* dnormWdr(j,:) ./ normW.^2;
    dsidv = dsdv ./ normW - s .* dnormWdv(j,:) ./ normW.^2;
    
    // Partial derivatives of cos(gom)  with respect to (x,y,z) and (vx,vy,vz)
    dcgdr = -dW2dr(j,:) ./ s + W(2,:) .* dsdr ./ s.^2;
    dcgdv = -dW2dv(j,:) ./ s + W(2,:) .* dsdv ./ s.^2;
    
    // Partial derivatives of sin(gom)  with respect to (x,y,z) and (vx,vy,vz)
    dsgdr = dW1dr(j,:) ./ s - W(1,:) .* dsdr ./ s.^2;
    dsgdv = dW1dv(j,:) ./ s - W(1,:) .* dsdv ./ s.^2;
    
    // Partial derivatives of VX = cos(gom)*vx + sin(gom)*vy
    dVXdr = dcgdr.*vel(1,:) + dsgdr.*vel(2,:);
    dVXdv = dcgdv.*vel(1,:) + cg * (j==1) + dsgdv.*vel(2,:) + sg * (j==2);
    
    // Partial derivatives of VY = -sin(gom)*cos(i)*vx + cos(gom)*cos(i)*vy + sin(i)*vz
    dVYdr = -dsgdr.*ci.*vel(1,:) -sg.*dcidr.*vel(1,:) + dcgdr.*ci.*vel(2,:) + cg.*dcidr.*vel(2,:) + dsidr.*vel(3,:);
    dVYdv = -dsgdv.*ci.*vel(1,:) -sg.*dcidv.*vel(1,:) - sg.*ci*(j==1) +  ...
            dcgdv.*ci.*vel(2,:) + cg.*dcidv.*vel(2,:) + cg.*ci*(j==2) + dsidv.*vel(3,:) + si*(j==3);
            
    // Partial derivatives of X = cos(gom)*x + sin(gom)*y
    dXdr = dcgdr.*pos(1,:) + cg*(j==1) + dsgdr.*pos(2,:) + sg*(j==2);
    dXdv = dcgdv.*pos(1,:) + dsgdv.*pos(2,:);
    
    // Partial derivatives of Y = -sin(gom)*cos(i)*x + cos(gom)*cos(i)*y + sin(i)*z
    dYdr = -dsgdr.*ci.*pos(1,:) -sg.*dcidr.*pos(1,:) - sg.*ci*(j==1) +  ...
            dcgdr.*ci.*pos(2,:) + cg.*dcidr.*pos(2,:) + cg.*ci*(j==2) + dsidr.*pos(3,:) + si*(j==3);
    dYdv = -dsgdv.*ci.*pos(1,:) -sg.*dcidv.*pos(1,:) + dcgdv.*ci.*pos(2,:) + cg.*dcidv.*pos(2,:) + dsidv.*pos(3,:);
    
    // Partial derivatives of ex = normW*VY/mu -X/r
    dexdr = dnormWdr(j,:) .* VY ./ mu + normW .* dVYdr ./ mu - dXdr ./ r + X .* pos(j,:) ./ r.^3;
    dexdv = dnormWdv(j,:) .* VY ./ mu + normW .* dVYdv ./ mu - dXdv ./ r;
    
    // Partial derivatives of ey = -normW*VX/mu -Y/r
    deydr = -dnormWdr(j,:) .* VX ./ mu - normW .* dVXdr ./ mu - dYdr ./ r + Y .* pos(j,:) ./ r.^3;
    deydv = -dnormWdv(j,:) .* VX ./ mu - normW .* dVXdv ./ mu - dYdv ./ r;
    
    // Partial derivatives of i = atan(s/W3)
    didr = (W(3,:) .* dsdr - dW3dr(j,:) .* s) ./ (W(3,:).^2 + s.^2);
    didv = (W(3,:) .* dsdv - dW3dv(j,:) .* s) ./ (W(3,:).^2 + s.^2);
    
    // Partial derivatives of gom = atan(W(1)/-W(2))
    dgdr = -(W(2,:) .* dW1dr(j,:) - dW2dr(j,:) .* W(1,:)) ./ s.^2;
    dgdv = -(W(2,:) .* dW1dv(j,:) - dW2dv(j,:) .* W(1,:)) ./ s.^2;
    
    // Partial derivatives of alpha:
    // d(esinE)/d(x,y,z)
    desinEdr = vel(j,:) ./ (sqrt(mu.*a)) - a .* esinE ./ r.^3 .* pos(j,:);
    desinEdv = pos(j,:) ./ (sqrt(mu.*a)) - a .* esinE ./ mu .* vel(j,:);
    
    // Partial derivatives of nu
    dnudr = (ex.*dexdr + ey.*deydr) ./ sqrt(1 - ex.^2 - ey.^2) .* nu.^2;
    dnudv = (ex.*dexdv + ey.*deydv) ./ sqrt(1 - ex.^2 - ey.^2) .* nu.^2;
    
    dcosFdr = dXdr ./ a - X .* dadr ./ a.^2 + dexdr - (desinEdr .* nu .* ey + esinE .* dnudr .* ey + esinE .* nu .* deydr);
    dsinFdr = dYdr ./ a - Y .* dadr ./ a.^2 + deydr + (desinEdr .* nu .* ex + esinE .* dnudr .* ex + esinE .* nu .* dexdr);
    dFdr = (cosF .* dsinFdr - dcosFdr .* sinF) ./ (cosF.^2 + sinF.^2);
    dalphadr = dFdr - dexdr .* sinF - ex .* dsinFdr + deydr .* cosF + ey .* dcosFdr;
    
    dcosFdv = dXdv ./ a - X .* dadv ./ a.^2 + dexdv - (desinEdv .* nu .* ey + esinE .* dnudv .* ey + esinE .* nu .* deydv);
    dsinFdv = dYdv ./ a - Y .* dadv ./ a.^2 + deydv + (desinEdv .* nu .* ex + esinE .* dnudv .* ex + esinE .* nu .* dexdv);
    dFdv = (cosF .* dsinFdv - dcosFdv .* sinF) ./ (cosF.^2 + sinF.^2);
    dalphadv = dFdv - dexdv .* sinF - ex .* dsinFdv + deydv .* cosF + ey .* dcosFdv;
    
    // Assign results to the output jacobian:
    jacob(1,j,:) = dadr;
    jacob(2,j,:) = dexdr;
    jacob(3,j,:) = deydr;
    jacob(4,j,:) = didr;
    jacob(5,j,:) = dgdr;
    jacob(6,j,:) = dalphadr;
    
    jacob(1,j+3,:) = dadv;
    jacob(2,j+3,:) = dexdv;
    jacob(3,j+3,:) = deydv;
    jacob(4,j+3,:) = didv;
    jacob(5,j+3,:) = dgdv;
    jacob(6,j+3,:) = dalphadv;
  end
end

endfunction