//  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 [varargout] = CL_co_sph2car(varargin)
// Spherical coordinates to cartesian coordinates
//
// Calling Sequence
// [pos_car, jacob] = CL_co_sph2car(pos_sph)
// [pos_car, vel_car, jacob] = CL_co_sph2car(pos_sph, pos_sph_dot)
// [pv_car, jacob] = CL_co_sph2car(pv_sph)
//
// Description
// <itemizedlist><listitem>
// <p>Converts spherical coordinates into cartesian coordinates.</p>  
// <p>Optionally converts time derivatives of spherical coordinates into time derivatives of cartesian coordinates.</p> 
// <para/><inlinemediaobject><imageobject><imagedata fileref="spherical_coord.gif"/></imageobject></inlinemediaobject>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - The jacobian is computed if the corresponding output argument exists. </p>
// <p> - Beware that the 3rd spherical coordinate is a radius and not an altitude. </p>
// <p> - Input arguments cannot be named. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos_sph : [lon;lat;r] Position vector in spherical coordinates [m,rad] (3xN)
// pos_sph_dot : (optional) [d(lon)/dt;d(lat)/dt;d(r)/dt] Time derivatives of spherical coordinates [rad/s,m/s] (3xN)
// pos_car: [x;y;z] Position vector in cartesian coordinates [m] (3xN)
// vel_car: (optional) [vx;vy;vz] Velocity vector in cartesian coordinates [m/s] (3xN)
// pv_sph : [pos_sph; pos_sph_dot] (6xN)
// pv_car : [pos_car; vel_car] (6xN)
// jacob:  (optional) Transformation jacobian (3x3xN) or (6x6xN)
//
// 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_geoc_car)
//
// See also
// CL_co_car2sph
//
// Examples
// // Example 1
// pos_sph = [%pi/3; %pi/4; 7000.e3];
// pos_car = CL_co_sph2car(pos_sph);
// [pos_car, jacob] = CL_co_sph2car(pos_sph);
//
// // Example 2 :
// pos_sph = [%pi/3; %pi/4; 7000.e3];
// pos_sph_dot = [1.e-5; 1.e-3; 1000];
// [pos_car,vel_car,jacob] = CL_co_sph2car(pos_sph,pos_sph_dot);
// [pos_car,vel_car,jacob] = CL_co_sph2car(pos_sph,[]);

// Declarations:

// -------------------------------
// Internal function
// -------------------------------
function [pos_car, vel_car, jacob] = sph2car(pos_sph, pos_sph_dot, compute_der, compute_jac, dimjac)
  // Default initialization 
  pos_car = []; 
  vel_car = []; 
  jacob = []; 

  // -------------------------------
  // Computation of cartesian coordinates
  // -------------------------------
  clon = cos(pos_sph(1,:)); 
  slon = sin(pos_sph(1,:)); 
  clat = cos(pos_sph(2,:)); 
  slat = sin(pos_sph(2,:)); 
  d = pos_sph(3,:);

  clat_clon = clat .* clon;
  clat_slon = clat .* slon;

  x = d .* clat_clon;
  y = d .* clat_slon;
  z = d .* slat;
  pos_car = [x; y; z];

  // Inan => if one of (lon,lat,r) is %nan 
  // => for later use (%nan propagation)
  Inan = find(isnan(x)); 

  // data for derivatives or jacobian
  slat_slon = slat .* slon;
  slat_clon = slat .* clon;

  // -------------------------------
  // Computation of derivatives
  // -------------------------------
  if (compute_der)
    londot = pos_sph_dot(1,:);
    latdot = pos_sph_dot(2,:);
    ddot = pos_sph_dot(3,:);

    vx = ddot .* clat_clon - z .* clon .* latdot - y .* londot;
    vy = ddot .* clat_slon - z .* slon .* latdot + x .* londot;
    vz = ddot .* slat + d .* clat .* latdot;
    vel_car = [vx; vy; vz];
  end

  // -------------------------------
  // Computation of jacobian
  // -------------------------------
  if (compute_jac)
    N = size(pos_sph,2);
    jacob = zeros(dimjac,dimjac,N);

    // x partial derivative
    jacob(1,2,1:N) = -d .* slat_clon;
    jacob(1,1,1:N) = -y;
    jacob(1,3,1:N) = clat_clon;
    // y partial derivative
    jacob(2,2,1:N) = -d .* slat_slon;
    jacob(2,1,1:N) = x;
    jacob(2,3,1:N) = clat_slon;
    // z partial derivative
    jacob(3,2,1:N) = d .* clat;
    jacob(3,3,1:N) = slat;

    if (dimjac > 3)
  
    // vx partial derivative
    jacob(4,2,1:N) = -vz .* clon + z .* slon .* londot;
    jacob(4,1,1:N) = -vy;
    jacob(4,3,1:N) = -slat_clon .* latdot - clat_slon .* londot;
    jacob(4,5,1:N) = -d .* slat_clon;
    jacob(4,4,1:N) = -y;
    jacob(4,6,1:N) = clat_clon;
    // vy partial derivative
    jacob(5,2,1:N) = -vz .* slon - z .* clon .* londot;
    jacob(5,1,1:N) = vx;
    jacob(5,3,1:N) = clat_clon .* londot - slat_slon .* latdot;
    jacob(5,5,1:N) = -d .* slat_slon;
    jacob(5,4,1:N) = x;
    jacob(5,6,1:N) = clat_slon;
    // vz partial derivative
    jacob(6,2,1:N) = ddot .* clat - z .* latdot;
    jacob(6,3,1:N) = clat .* latdot;
    jacob(6,5,1:N) = d .* clat;
    jacob(6,6,1:N) = slat;
  
    end // dimjac > 3
  end

  // -------------------------------
  // propagates (initial) %nan
  // -------------------------------
  if (Inan <> [])
    pos_car(:, Inan) = %nan; 
    if (vel_car <> []); vel_car(:,Inan) = %nan; end
    if (jacob <> []); jacob(:,:,Inan) = %nan; end
  end

endfunction


// Code:
// ===============================
// MAIN
// ===============================

// left/right numbers of arguments
[lhs,rhs] = argn();

// -------------------------------
// Checks input/output arguments
// -------------------------------
if (rhs <> 1 & rhs <> 2)
  CL__error("Bad number of input arguments"); 
end

dim = size(varargin(1), 1); 
if (dim <> 3 & dim <> 6)
  CL__error("Bad size for 1st input arguments"); 
end

if (rhs == 2 & dim == 6)
  CL__error("Bad number of input arguments"); 
end

if (rhs == 1 & lhs >= 3)
  CL__error("Bad number of output arguments"); 
end

if (rhs == 1 & dim == 3) 
  numcase = 1; 
  pos_sph = varargin(1); 
  pos_sph_dot = []; 
  compute_der = %f; 
  compute_jac = (lhs >= 2);
  dimjac = 3; // if computed

elseif (rhs == 1 & dim == 6) 
  numcase = 2; 
  pos_sph = varargin(1)(1:3,:); 
  pos_sph_dot = varargin(1)(4:6,:); 
  compute_der = %t; 
  compute_jac = (lhs >= 2); 
  dimjac = 6; // if computed

elseif (rhs == 2) 
  numcase = 3; 
  pos_sph = varargin(1); 
  pos_sph_dot = varargin(2); 
  compute_der = (pos_sph_dot <> [] & lhs >= 2); 
  compute_jac = (pos_sph_dot <> [] & lhs >= 3); 
  dimjac = 6; // if computed

else
  error("Unexpected situation!"); 
end

// -------------------------------
// computes
// -------------------------------
[pos_car, vel_car, jacob] = sph2car(pos_sph, pos_sph_dot, compute_der, compute_jac, dimjac); 

// -------------------------------
// generates outputs
// -------------------------------
if (numcase == 1)
  varargout(1) = pos_car; 
  if (lhs >= 2); varargout(2) = jacob; end

elseif (numcase == 2)
  varargout(1) = [pos_car; vel_car]; 
  if (lhs >= 2); varargout(2) = jacob; end

else
  varargout(1) = pos_car; 
  if (lhs >= 2); varargout(2) = vel_car; end
  if (lhs >= 3); varargout(3) = jacob; end
end

endfunction