//  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_ell,jacob] = CL_co_car2ell(pos_car, er,obla)
// Cartesian coordinates to elliptical coordinates 
//
// Calling Sequence
// [pos_ell ,jacob] = CL_co_car2ell(pos_car [,er,obla])
//
// Description
// <itemizedlist><listitem>
// <p>Converts cartesian coordinates into elliptical ("geodetic") coordinates.</p> 
// <p> Elliptical coordinates are longitude, latitude and altitude with respect to 
// some reference ellipsoid. </p>
// <p> The reference ellipsoid is an ellipsoid of revolution around the Z axis. It is then 
// characterized by 2 parameters: </p>
// <p> - semi-major axis of the ellipse obtained as the intersection of the ellipsoid with any 
// plane containing the Z axis, </p>
// <p> - oblateness (or flattening) of this ellipse. </p>
// <p><inlinemediaobject><imageobject><imagedata fileref="elliptical_coord.gif"/></imageobject></inlinemediaobject></p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - The transformation jacobian is computed if the corresponding output argument exists. </p>
// <p> - Be careful that the 3rd elliptical coordinate is an altitude and not the distance to the planet center. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos_car : Position vector in cartesian coordinates [X;Y;Z] [m] (3xN)
// er : (optional) Equatorial radius of reference ellipsoid [m] (default is %CL_eqRad)
// obla : (optional) Oblateness of reference ellipsoid (default is %CL_obla)
// pos_ell : Position vector in elliptical ("geodetic") coordinates [longitude;latitude;alt] [rad,m] (3xN)
// jacob : (optional) Transformation jacobian (3x3xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) Mecanique Spatiale, Cnes - Cepadues Editions, Tome I, section 3.2.3 (Les reperes de l'espace et du temps, Relations entre les coordonnees)
// 2) CNES - MSLIB FORTRAN 90, Volume T (mt_car_geod)
//
// See also
// CL_co_ell2car
// CL_co_car2sph
//
// Examples
// // Example 1
// pos_car = [4637885.347; 121344.608; 4362452.869];
// [pos_ell] = CL_co_car2ell(pos_car);
//
// // Example 2 : consistency test
// pos_car = [4637885.347; 121344.608; 4362452.869];
// [pos_ell,jacob1] = CL_co_car2ell(pos_car);
// [pos_car2,jacob2] = CL_co_ell2car(pos_ell);
// M = jacob1*jacob2;  // identity matrix

// Declarations:


// Code:
[lhs,rhs]=argn();

if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("obla", "local")); obla = CL__dataGetEnv("obla"); end
if obla>=1 then CL__error('oblateness >= 1'); end

N = size(pos_car,2)

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

eps100 = 100*%eps
eps = max(1e-9,10*eps100)
epsreq = er*eps
niter = 30

x = pos_car(1,:)
y = pos_car(2,:)
z = pos_car(3,:)

disxy2 = x.^2 + y.^2
disxy = sqrt(disxy2)
dist2 = z.^2 + disxy2

if find(dist2<(epsreq.^2))~=[] then CL__error('zero norm vector'); end

//LONGITUDE COMPUTATION
rlon = zeros(x)

//if longitude is 0 or pi
  //if x>=0
  i_lon1 = find( (abs(y)<epsreq)&(x>=0) ) //longitude is 0
  rlon(i_lon1) = 0
  //if x<0
  i_lon2 = find( (abs(y)<epsreq)&(x<0) ) //longitude is pi
  rlon(i_lon2) = %pi
//else
  i_lon3 = find( ~(abs(y)<epsreq) )
  rlon(i_lon3) = CL__sc2angle(x(i_lon3),y(i_lon3))
//end
//END LONGITUDE COMPUTATION

//LATITUDE AND HEIGHT COMPUTATION
rlat = zeros(x)
rhaut = zeros(x)
ecc2 = zeros(x)
correc = zeros(x)
Rphi = zeros(x)

//if latitude = +-pi/2 (situation on the poles axe)
i_lh1 = find(disxy<epsreq)
  rlat(i_lh1) = sign(z(i_lh1)).*(%pi/2)
  rhaut(i_lh1) = abs(z(i_lh1)) - er*(1-obla)
  //NOTE: jacobian non computable in this case

//else (situation out of the poles axe)

  //if abs(z)<epsreq (situation on the equator plan)
  i_lh2 = find( (abs(z)<epsreq)&(disxy>epsreq) )
    rlat(i_lh2) = 0
    rhaut(i_lh2) = disxy(i_lh2) - er
    if compute_jacob  //variables for jacobian computation
      ecc2(i_lh2) = obla * (2-obla)
      correc(i_lh2) = 1-ecc2(i_lh2).*(sin(rlat(i_lh2)).^2)  //correc is > 0
                                                            //correc could be 0 if apla=1: impossible at this stade of computation
      Rphi(i_lh2) = er ./ sqrt(correc(i_lh2)) //radius at computed latitude
    end

  //else (Newton schema initialisation)
  i_lh3 = find( (~(abs(z)<epsreq))&(disxy>epsreq) )
    iter = zeros(x)
    //initialisation of computation constants
    corapl = 1 - obla  //in ]0.,1.]
    ecc2(i_lh3) = obla * (2 - obla)
    crapl2 = corapl^2
    reqcor = er*corapl
    requa2 = er^2
    rqcor2 = reqcor^2
    coralt = disxy2/requa2 + (z.^2)/rqcor2

    rhaut(i_lh3) = sqrt(dist2(i_lh3)).*(1 - 1 ./ sqrt(coralt(i_lh3)))  //aproximated height value

    corxy = ones(x)
    corxy(i_lh3) = crapl2 * disxy(i_lh3)
    rasb  = z./corxy
    glat0 = atan(rasb)

    alpha = disxy
    intell = ( (disxy2 ./ requa2 + (z.^2)./rqcor2)  <  1 )  //latitude initialisation

    //if intell (point inside the ellipsoid)
    i_intell = find( (intell==%t)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
      cas3ra = ((alpha - ecc2 * er)  <  0)

      //if cas3ra (complex case with 3 roots)
      i_cas3ra = find( (cas3ra==%t)&(intell==%t)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
        rlat(i_cas3ra) = sqrt((er ./ alpha(i_cas3ra)).^2 - 1) / corapl
        rlat(i_cas3ra) = atan(rlat(i_cas3ra))
        //if z<0
        i_neg = find( (z<0)&(cas3ra==%t)&(intell==%t)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
          rlat(i_neg) = -rlat(i_neg)
      //else cas3ra
      i_cas3ra_2 = find( (cas3ra==%f)&(intell==%t)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
        rlat(i_cas3ra_2) = glat0(i_cas3ra_2) - obla/reqcor .* sin(2.*glat0(i_cas3ra_2)).*rhaut(i_cas3ra_2)
      //end cas3ra

    //elseif intell (rhaut<=er)&(obla<=1/2) (point outside the ellipsoid)
    i_intell_2 = find( (rhaut<=er)&(obla<=1/2)&(intell==%f)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
      rlat(i_intell_2) = glat0(i_intell_2) - obla/reqcor .* sin(2.*glat0(i_intell_2)).*rhaut(i_intell_2) //height <= er and obla <= 1/2

    //else intell
    i_intell_3 = find( (~((rhaut<=er)&(obla<=1/2)))&(intell==%f)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
      rlat(i_intell_3) = glat0(i_intell_3)  //height > er or obla > 1/2

    //end if intell

    //if rlat>pi/2 (recal initial latitude between -pi/2 and pi/2)
    i_recal = find( (rlat>%pi/2)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
      rlat(i_recal) = %pi - rlat(i_recal)
    //else if
    i_recal_2 = find( (rlat<-%pi/2)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
      rlat(i_recal_2) = -%pi - rlat(i_recal_2)
    //end if

    //Newton schema iterations
    rlat0 = zeros(x)
    sinl0 = zeros(x)
    tanl0 = zeros(x)
    denomi = zeros(x)
    fonc = zeros(x)
    dfonc = zeros(x)
    testcv = ones(x) //1:true, 0:false
    ii = find( (testcv==1)&(iter<niter)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
    while ii~=[]
      iter(ii) = iter(ii) + 1
      rlat0(ii) = rlat(ii)
      sinl0(ii) = sin(rlat0(ii))
      tanl0(ii) = tan(rlat0(ii))
      denomi(ii) = sqrt(1 - ecc2(ii).*sinl0(ii).^2)
      fonc(ii)  = -sinl0(ii).*ecc2(ii)*er./denomi(ii) + alpha(ii).*tanl0(ii) - z(ii)
      dfonc(ii) = er./denomi(ii).^3
      dfonc(ii) = -dfonc(ii).*ecc2(ii).*cos(rlat0(ii)) + alpha(ii).*(1 + tanl0(ii).^2)

      rlat(ii)  = rlat0(ii) - fonc(ii)./dfonc(ii) //mise a jour de la latitude par Newton

      //if rlat>pi/2 (latitude eventual recallage between  -pi/2 and pi/2
      ii_2 = find( (rlat>%pi/2)&(testcv==1)&(iter<niter)&(disxy>epsreq)&(~(abs(z)<epsreq)) ) //recalage eventuel de la latitude entre -pi/2 et pi/2
        rlat(ii_2) = 0.5.*(%pi/2 + rlat0(ii_2))
      //else if rlat<-pi/2
      ii_3 = find( (rlat<-%pi/2)&(testcv==1)&(iter<niter)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
        rlat(ii_3) = 0.5.*(-%pi/2 + rlat0(ii_3))
      //end if

      ii_test =find( (~(abs(rlat-rlat0)>eps))&(testcv==1)&(iter<niter)&(disxy>epsreq)&(~(abs(z)<epsreq)) )  //test de convergence: testcv vaut true si on n'a pas la convergence, false sinon
        testcv(ii_test) = 0

      ii = find( (testcv==1)&(iter<niter)&(disxy>epsreq)&(~(abs(z)<epsreq)) )
    end //end while

    rx1 = zeros(x)
    ry1 = zeros(x)
    rz1 = zeros(x)

    correc(i_lh3) = 1 - ecc2(i_lh3).*(sin(rlat(i_lh3)).^2)  //correc est > 0.
                                                            //correc pourrait etre nul si apla=1: impossible a ce stade des calculs
    Rphi(i_lh3) = er./sqrt(correc(i_lh3)) //rayon a la latitude calculee
    rhaut(i_lh3) = alpha(i_lh3)./cos(rlat(i_lh3))-Rphi(i_lh3) //hauteur a la latitude calculee

    rx1(i_lh3) = (Rphi(i_lh3) + rhaut(i_lh3)).*cos(rlat(i_lh3)).*cos(rlon(i_lh3))
    ry1(i_lh3) = (Rphi(i_lh3) + rhaut(i_lh3)).*cos(rlat(i_lh3)).*sin(rlon(i_lh3))
    rz1(i_lh3) = (Rphi(i_lh3)*crapl2 + rhaut(i_lh3)).*sin(rlat(i_lh3))

    //Convergence test on positions
    oknewt = zeros(x)
    i_oknewt =find( (abs(rx1 - x) <= 1.0e2 .* epsreq) &...
                    (abs(ry1 - y) <= 1.0e2 .* epsreq) &...
                    (abs(rz1 - z) <= 1.0e2 .* epsreq) &...
                    (disxy>epsreq)&(~(abs(z)<epsreq)) )
    oknewt(i_oknewt) = 1

    //----------------------------------------------------------------
    //on verifie qu'il y a solution et si la solution obtenue par Newton est correcte
    //i.e. que -pi/2 <= rlat <= pi/2, que rhaut >=0 quand on se trouve
    //a l'exterieur de l'ellipsoide et que les residus en rx, ry et rz
    //sont faibles
    //----------------------------------------------------------------

    ii_verif = find( (  (testcv==1) |...
                        ( (intell==%f)&(rhaut < -1.0e+3 .* epsreq) ) |...
                        (oknewt==0) |...
                        ( (Rphi.*crapl2 + rhaut) < 0 )...
                      ) & (disxy>epsreq)&(~(abs(z)<epsreq))...
                   );
    if ii_verif~=[] then CL__error('no convergence'); end

  //end Newton (out equator plan)

//end poles axe case

pos_ell = [rlon;rlat;rhaut]

// propagates %nan 
cond_nan = or(isnan(pos_car), "r"); 
I = find(cond_nan); 
pos_ell(:, I) = %nan; 

if compute_jacob

  jacob = %nan * ones(3,3,N);
 
  // case where jacobian is computable and input <> nan  
  i_npa = find(~(disxy<epsreq) & ~cond_nan); 
  
  if (i_npa <> [])

    K = ones(x)
    KRphiplusH = ones(x)
    RphiplusH = ones(x)
    K(i_npa) = (1 - ecc2(i_npa))./correc(i_npa) //K in [0,1]
    //if obla = 0 : ecc2 = 0 ; correc = 1 ; Rphi = er; K = 1

    KRphiplusH(i_npa) = K(i_npa).*Rphi(i_npa) + rhaut(i_npa)
    RphiplusH(i_npa) = Rphi(i_npa) + rhaut(i_npa)
  
    if (find(KRphiplusH < epsreq) <> [])
      CL__error('Jacobian non computable. Negative altitude'); 
    end

    jacob(2,1,i_npa) = -sin(rlat(i_npa)).*cos(rlon(i_npa))./KRphiplusH(i_npa)
    jacob(2,2,i_npa) = -sin(rlat(i_npa)).*sin(rlon(i_npa))./KRphiplusH(i_npa)
    jacob(2,3,i_npa) = cos(rlat(i_npa))./KRphiplusH(i_npa)

    jacob(1,1,i_npa) = -sin(rlon(i_npa))./(RphiplusH(i_npa).*cos(rlat(i_npa)))
    jacob(1,2,i_npa) = cos(rlon(i_npa))./(RphiplusH(i_npa).*cos(rlat(i_npa)))
    jacob(1,3,i_npa) = 0

    jacob(3,1,i_npa) = cos(rlat(i_npa)).*cos(rlon(i_npa))
    jacob(3,2,i_npa) = cos(rlat(i_npa)).*sin(rlon(i_npa))
    jacob(3,3,i_npa) = sin(rlat(i_npa))

  end

end


endfunction