//  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_car,jacob]=CL_co_ell2car(pos_ell, er,obla)
// Elliptical coordinates to cartesian coordinates
//
// Calling Sequence
// [pos_car,jacob] = CL_co_ell2car(pos_ell [,er,obla])
//
// Description
// <itemizedlist><listitem>
// <p>Converts elliptical ("geodetic") coordinates into cartesian 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_ell : [lon;lat;alt] Position vector in elliptical ("geodetic") coordinates [rad,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_car : [x;y;z] Position vector in cartesian coordinates [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_geod_car)
//
// See also
// CL_co_car2ell
// CL_co_car2sph
//
// Examples
// // Example 1 :
// pos_ell = [CL_deg2rad(12.125); CL_deg2rad(108.2); 110.0];
// [pos_car,jacob1] = CL_co_ell2car(pos_ell);

// 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

select lhs
  case 1
    compute_jacob = %f
  case 2
    compute_jacob = %t
  else
    CL__error('check number of output arguments')
end

// If one coordinate is %nan => all set to %nan 
I = find(or(isnan(pos_ell), "r")); 
pos_ell(:, I) = %nan; 


//get latitude, longitude and altitude
lat = pos_ell(2,:);
lon = pos_ell(1,:);
hg = pos_ell(3,:);

//e and N calculation (see ref. 1)
ex2 = obla*(2-obla)
correc = 1 - ex2 .* (sin(lat).^2)  //correc>0, might be 0 if obla=1
Rphi = er./sqrt(correc) //radius at given latitude
RphiplusH = Rphi + hg

//cartesian coordinates:
x = RphiplusH.*cos(lat).*cos(lon);
y = RphiplusH.*cos(lat).*sin(lon);
z = (Rphi.*(1-ex2)+hg).*sin(lat);

pos_car = [x;y;z];

//JACOBIAN COMPUTATION
if compute_jacob
  N = size(pos_ell,2);
  jacob = zeros(3,3,N)

  K = (1 - ex2)./correc //K in [0,1]
  //if obla = 0 : ex2 = 0 ; correc = 1 ; Rphi = er; K = 1

  KRphiplusH = K.*Rphi + hg

  jacob(1,2,1:N) = - KRphiplusH .* sin(lat).*cos(lon)
  jacob(1,1,1:N) = - RphiplusH .* cos(lat).*sin(lon)
  jacob(1,3,1:N) = cos(lat) .* cos(lon)

  jacob(2,2,1:N) = - KRphiplusH .* sin(lat) .* sin(lon)
  jacob(2,1,1:N) = RphiplusH .* cos(lat) .* cos(lon)
  jacob(2,3,1:N) = cos(lat) .* sin(lon)

  jacob(3,2,1:N) = KRphiplusH .* cos(lat)
  jacob(3,1,1:N) = 0 * lat // 0 or %nan
  jacob(3,3,1:N) = sin(lat)


end

endfunction