//  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_equa,jacob] = CL_oe_car2cirEqua(pos_car,vel_car, mu)
// Cartesian to circular equatorial adapted orbital elements - DEPRECATED
//
// Calling Sequence
// [cir_equa,jacob] = CL_oe_car2cirEqua(pos_car,vel_car [,mu])
//
// Description
// <itemizedlist><listitem>
// <p>This function is deprecated. </p>
// <p>Replacement function: <link linkend="CL__oe_car2cireq">CL__oe_car2cireq</link></p>
// <p>"cirEqua" orbital elements are identical to "cireq" orbital elements except that 
// ix=2*sin(i/2)*cos(raan) and iy=2*sin(i/2)*sin(raan)</p>
// <p></p></listitem>
// <listitem>
// <p>Converts position and velocity to orbital elements adapted to 
// near-circular near-equatorial orbits. (elliptic orbit only)</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_car: position [X;Y;Z] [m] (3xN)
// vel_car: velocity [Vx;Vy;Vz] [m/s] (3xN)
// mu : (optional) gravitational constant [m^3/s^2] (default value is %CL_mu)
// cir_equa: orbital elements adapted to near-circular near-equatorial orbits. [sma;ex;ey;ix;iy;pom+raan+anm] [m,rad] (6xN)
// jacob: (optional) transformation jacobian (See <link linkend="Orbital elements">Orbital elements</link> for more details) (6x6xN)
//
// Bibliography
// 1) CNES - MSLIB FORTRAN 90, Volume V (mv_car_cir_equa)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Example 1
// pos_car=[-29536113;30329259;-100125];
// vel_car=[-2194;-2141;-8];
// [cirequa,jacob1] = CL_oe_car2cirEqua(pos_car,vel_car);
// [pos_car2,vel_car2,jacob2] = CL_oe_cirEqua2car(cirequa);

//!************************************************************************
//!
//! But:  Passage des parametres CARtesiens aux parametres orbitaux dits adaptes aux orbites CIRculaires EQUAtoriales.
//! ===
//!
//! Note d'utilisation: 1) Applicable aux orbites circulaires ou elliptiques, equatoriales ou non
//! ==================  2) Dans le cas d'orbites paraboliques ou hyperboliques  voir la routine:  mv_car_equa
//!                     3) Les unites en entree doivent etre coherentes entre elles. Ex.: pos_car en metres, vel_car en m/s,
//!                        mu en m**3/s**2, et les parametres orbitaux dits adaptes aux orbites CIRculaires EQUAtoriales
//!                        seront en metres et radians.
//!                     4) L'element (i,j) de la jacobienne correspond a la derivee partielle du parametre adaptes aux orbites
//!                        CIRculaires EQUAtoriales numero i par rapport a la derivee partielle du parametre cartesien numero j.
//!                     5) La transformation inverse se fait par mv_cir_equa_car.
//!************************************************************************

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


// Code:
CL__warnDeprecated(); // deprecated function

[lhs,rhs]=argn();

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

if lhs==2 then compute_jacob=%t; else compute_jacob=%f; end

N = size(pos_car,2)

//initialisation constante de test
eps100 = 100.*%eps

r = CL_norm(pos_car) //call mu_norme(pos_car, r, code_retour)
r2 = r.^2

ii = find(r < eps100)
if ii~=[] //vecteur position nul
   CL__error('null position vector');
end

//calcul du demi-grand axe
vitesse = CL_norm(vel_car)  //call mu_norme (vel_car, vitesse, code_retour)
v2 = vitesse.^2

un_sur_a = 2.0 ./ r - v2 ./ mu  //1/a = 2/r - v**2/mu   ==>  test si 1/a est negatif ou nul
                            //pour savoir si l'orbite est hyperbolique (v2>2*mu/r) ou parabolique (v2= 2*mu/r)

ii = find(un_sur_a < eps100)
if ii~=[]
  CL__error('semi major axis negative or infinit');
end

a = 1.0 ./ un_sur_a
//a ne peut pas etre negatif car 1/a a deja ete teste en ce sens
//a ne peut pas etre nul non plus, car cela signifierait que r est nul, et r a deja ete teste en ce sens

//calcul de l'eccentricite
scal = CL_dot(pos_car, vel_car)

rmua=sqrt(mu.*a)
esine=scal./rmua

rsa=r.*un_sur_a
ecose=1-rsa

e2=ecose.*ecose + esine.*esine   //par calcul e2 est positif ou nul

//test aux limites sur l'eccentricite (que l'on sait deja inferieure a 1)
e = sqrt(e2)
ii = find(e>1-EPS_ORB.parab)
//if (e > 1._pm_reel - pm_eps_parab) then  ! orbite trop eccentrique, quasi parabolique
//   code_retour%valeur = pm_err_e_non_ellip
//end if

//calcul des parametres hx et hy
vect = CL_cross(pos_car,vel_car) //call mu_prod_vect(pos_car, vel_car, vect, code_retour)  !moment cinetique
anorm = CL_norm(vect) //call mu_norme (vect, anorm, code_retour, vect_norme = v)  !norme et direction du moment cinetique

ii=find(anorm < eps100)
if ii~=[] //la norme du moment cinetique est nulle
  CL__error('angular momentum vector norm is  0 (position-velocity colinears');
end

v = vect./(anorm.*.ones(3,1))

rach2=0.5.*(1+v(3,:))
rach=sqrt(rach2)
cofh=1.0 ./ rach

hx=-cofh.*v(2,:)
hy=cofh.*v(1,:)

ii=find( (hx.^2+hy.^2) > (4*(1-eps100)) )
if ii~=[]                                             //la definition du vecteur inclinaison (hx,hy) impose que l'inclinaison i soit
  CL__error('inclination bigger than (pi-epsilon_retro)');//inferieure a (pi - epsilon_retro) -> test si h**2 <= 4*(cos(epsilon_retro/2)**2)
end                                                   //soit h**2 <= 4*(1-100*epsilon) si epsilon_retro = 20*sqrt(epsilon)


//calcul des parametres ex et ey

hx2=hx.^2
hy2=hy.^2

f = zeros(6,N)
f(1,:)=1 - 0.5.*hy2
f(2,:)=0.5.*hx.*hy
f(3,:)=-rach.*hy
f(4:6,:) = 0

g = zeros(6,N)
g(1,:)=f(2,:)
g(2,:)=1 - 0.5.*hx2
g(3,:) = hx.*rach
g(4:6,:) = 0

scalf = CL_dot(pos_car,f(1:3,:))
scalg = CL_dot(pos_car,g(1:3,:))

race=sqrt(1-e2) //race est different de zero car e2 < 1
al1=ecose-e2
al2=race.*esine

exsc = al1.*scalf + al2.*scalg
eysc = al1.*scalg - al2.*scalf

ex=exsc.*a./r2
ey=eysc.*a./r2

//calcul du parametre xlm
betaa=1.0 ./ (1+race) //race est > 0

cle=scalf./a + ex - esine.*betaa.*ey
sle=scalg./a + ey + esine.*betaa.*ex

//calcul de (petit omega + grand omega + e)
xle = CL__sc2angle(cle,sle) //call mu_angle2(cle, sle, xle, code_retour)

//calcul de la longitude moyenne (petit omega + grand omega + m)
xlm = xle-esine

//! ------------------------------------
//affectation des parametres osculateurs
//! --------------------------------------
cir_equa(1,:) = a
cir_equa(2,:) = ex
cir_equa(3,:) = ey
cir_equa(4,:) = hx
cir_equa(5,:) = hy
cir_equa(6,:) = xlm


//calcul jacobien
if compute_jacob
  dadx = zeros(6,N)
  dadx(1:3,:)=2.0.*(a.*a.*.ones(3,1)).*pos_car ./ ((r.^3).*.ones(3,1))
  dadx(4:6,:)=2.0.*(a.*a.*.ones(3,1)).*vel_car ./ mu

  drdx = zeros(6,N)
  drdx(1:3,:)=pos_car./(r.*.ones(3,1))
  drdx(4:6,:)=0

  decose = (un_sur_a.*.ones(6,1)) .* (-drdx+dadx.*(rsa.*.ones(6,1)))
  desine = zeros(6,N)
  desine(1:3,:) = -0.5.*(esine.*un_sur_a.*.ones(3,1)).*dadx(1:3,:) + vel_car./(rmua.*.ones(3,1))
  desine(4:6,:) = -0.5.*(esine.*un_sur_a.*.ones(3,1)).*dadx(4:6,:) + pos_car./(rmua.*.ones(3,1))

  dce = dadx./(r2.*.ones(6,1)) - 2.0 .* (a./r2./r.*.ones(6,1)).*drdx

  dvect = hypermat([3 6 N])
  dvect(3,1,1:N)=vel_car(2,:)
  dvect(3,2,1:N)=-vel_car(1,:)
  dvect(3,3,1:N)=0
  dvect(3,4,1:N)=-pos_car(2,:)
  dvect(3,5,1:N)=pos_car(1,:)
  dvect(3,6,1:N)=0

  dvect(1,1,1:N)=0
  dvect(1,2,1:N)=vel_car(3,:)
  dvect(1,3,1:N)=-vel_car(2,:)
  dvect(1,4,1:N)=0
  dvect(1,6,1:N)=pos_car(2,:)
  dvect(1,5,1:N)=-pos_car(3,:)

  dvect(2,1,1:N)=-vel_car(3,:)
  dvect(2,2,1:N)=0
  dvect(2,3,1:N)=vel_car(1,:)
  dvect(2,4,1:N)=pos_car(3,:)
  dvect(2,5,1:N)=0
  dvect(2,6,1:N)=-pos_car(1,:)

  anorm2= anorm.^2;
  
  dan = dvect'*vect;
  dan = dan ./(anorm2.*.ones(6,1));
  // old code (not vectorized)
  //dan = zeros(6,N);
  //for i=1:N 
  //  dan(:,i) = ( vect(:,i)'*dvect(:,:,i) )';
  //end
  //dan = dan./(anorm2.*.ones(6,1));
  
  
  
  anorm_hm = hypermat([3,6,N] , [anorm.*.ones(1,18)]);
  v_hm = hypermat([3,6,N] , [v.*.ones(1,6)]);
  dan_hm = hypermat([3,6,N] , [(dan' .*. ones(1,3))']);
  dv = dvect ./ anorm_hm - v_hm .* dan_hm ;
  // old code (not vectorized)
  //dv = hypermat([3 6 N]);
  //for i=1:N
  //  dv(:,:,i) = dvect(:,:,i)./anorm(i) - (v(:,i).*.ones(1,6)).*(dan(:,i)'.*.ones(3,1));
  //end
  
  dcdv3 = -0.25.*cofh./rach2;
  dch = CL_dMult(dcdv3,matrix(dv(3,:,:),-1,N));
  dhxdx = -CL_dMult(v(2,:) , dch) - CL_dMult(cofh,matrix(dv(2,:,:),-1,N));
  dhydx = CL_dMult(v(1,:) , dch) + CL_dMult(cofh,matrix(dv(1,:,:),-1,N));
  // old code (not vectorized)
  //for i=1:N
  //  dch(:,i) = (dcdv3(i).*dv(3,:,i))';
  //  dhxdx(:,i) = -dch(:,i).*v(2,i) - (cofh(i).*dv(2,:,i))';
  //  dhydx(:,i) = dch(:,i).*v(1,i) + (cofh(i).*dv(1,:,i))';
  //end
  
  
  
  hdh = ( (hx.*.ones(6,1)).*dhxdx + (hy.*.ones(6,1)).*dhydx ) .* 0.25;
  dsf = (pos_car(1,:).*.ones(6,1)) .* ( -(hy.*.ones(6,1)).*dhydx ) + ...
        (pos_car(2,:).*.ones(6,1)) .* 0.5 .* ( (hx.*.ones(6,1)).*dhydx + (hy.*.ones(6,1)).*dhxdx ) - ...
        (pos_car(3,:).*.ones(6,1)) .* ( (rach.*.ones(6,1)).*dhydx - (hy.*.ones(6,1)).*hdh./(rach.*.ones(6,1)) ) + f;

  dsg = (pos_car(1,:).*.ones(6,1)) .* 0.5 .* ( (hx.*.ones(6,1)).*dhydx + (hy.*.ones(6,1)).*dhxdx ) + ...
        (pos_car(2,:).*.ones(6,1)) .* ( -(hx.*.ones(6,1)).*dhxdx ) + ...
        (pos_car(3,:).*.ones(6,1)) .* ( (rach.*.ones(6,1)).*dhxdx - (hx.*.ones(6,1)).*hdh./(rach.*.ones(6,1))) + g;

  de2 = 2.0 * ( decose.*(ecose.*.ones(6,1)) + desine.*(esine.*.ones(6,1)) )
  drace = -0.5 * de2./(race.*.ones(6,1))

  dal1 = decose-de2
  dal2 = drace.*(esine.*.ones(6,1)) + (race.*.ones(6,1)).*desine

  dexdx = dce.*(exsc.*.ones(6,1)) + ...
          (a./r2.*.ones(6,1)) .* ...
                                ( dal1.*(scalf.*.ones(6,1)) + ...
                                  (al1.*.ones(6,1)).*dsf + ...
                                  dal2.*(scalg.*.ones(6,1)) + ...
                                  (al2.*.ones(6,1)).*dsg );

  deydx = dce.*(eysc.*.ones(6,1)) + ...
          (a./r2.*.ones(6,1)) .* ...
                                ( dal1.*(scalg.*.ones(6,1)) + ...
                                  (al1.*.ones(6,1)).*dsg - ...
                                  dal2.*(scalf.*.ones(6,1)) - ...
                                  (al2.*.ones(6,1)).*dsf );

  dbeta = -drace.*((betaa.^2).*.ones(6,1))

  dl2 = dsf./(a.*.ones(6,1)) - (scalf.*.ones(6,1)).*dadx./((a.^2).*.ones(6,1))
  dm2 = dsg./(a.*.ones(6,1)) - (scalg.*.ones(6,1)).*dadx./((a.^2).*.ones(6,1))

  dt2b = desine.*(betaa.*.ones(6,1)) + (esine.*.ones(6,1)).*dbeta

  dcle = dl2 + dexdx - ( dt2b.*(ey.*.ones(6,1)) + (esine.*betaa.*.ones(6,1)).*deydx )
  dsle = dm2 + deydx + ( dt2b.*(ex.*.ones(6,1)) + (esine.*betaa.*.ones(6,1)).*dexdx )

  dxle = (cle.*.ones(6,1)).*dsle - (sle.*.ones(6,1)).*dcle

  dmdx = dxle - desine

  jacob = hypermat([6,6,N], [dadx;dexdx;deydx;dhxdx;dhydx;dmdx]);
  jacob = jacob';
  
  if (N == 1); jacob = jacob(:,:,1); end

end

endfunction