//  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_car2sph(varargin)
// Cartesian coordinates to spherical coordinates
//
// Calling Sequence
// [pos_sph, jacob] = CL_co_car2sph(pos_car)
// [pos_sph, pos_sph_dot, jacob] = CL_co_car2sph(pos_car [,vel_car])
// [pv_sph, jacob] = CL_co_car2sph(pv_car)
//
// Description
// <itemizedlist><listitem>
// <p>Converts cartesian coordinates into spherical coordinates.</p>  
// <p>Optionally converts time derivatives of cartesian coordinates into time derivatives of spherical coordinates.</p> 
// <para/><inlinemediaobject><imageobject><imagedata fileref="spherical_coord.gif"/></imageobject></inlinemediaobject>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p> 
// <p> - Longitudes of points along the Z-axis are arbitrarily set to 0. </p>
// <p> - The jacobian is computed if the corresponding output argument exists. </p>
// <p> - Elements of the jacobian that cannot be computed (on the Z-axis) are set to %nan. </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_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)
// pos_sph : [lon;lat;r] Position vector in spherical coordinates [rad,m](3xN)
// pos_sph_dot : (optional) [d(lon)/dt;d(lat)/dt;d(r)/dt] Time derivatives of spherical coordinates [rad/s,m/s] (3xN)
// pv_car : [pos_car; vel_car] (6xN)
// pv_sph : [pos_sph; pos_sph_dot] (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_car_geoc)
//
// See also
// CL_co_sph2car
//
// Examples
// // Example 1
// pos_car = [3842403.1; -5057704.6; 577780.5];
// [pos_sph] = CL_co_car2sph(pos_car)
//
// // Example 2
// vel_car = [1000; 1000; 100];
// [pos_sph,pos_sph_dot,jacob] = CL_co_car2sph(pos_car,vel_car)

// Declarations:

// -------------------------------
// Internal function
// -------------------------------
function [pos_sph, pos_sph_dot, jacob] = car2sph(pos_car, vel_car, compute_der, compute_jac, dimjac)

  // Default initialization 
  pos_sph = []; 
  pos_sph_dot = []; 
  jacob = []; 

  // -------------------------------
  // Computation of spherical coordinates
  // -------------------------------
  x = pos_car(1,:);
  y = pos_car(2,:);
  z = pos_car(3,:);

  // radius
  d = CL_norm(pos_car); 

  // Inan => if one of (x,y,z) is %nan 
  // => for later use (%nan propagation)
  Inan = find(isnan(d)); 

  I = find(d == 0); 
  d(I) = %nan;

  // latitude
  lat = asin(z ./ d);

  // longitude
  // (x and y == 0) => longitude = 0 (by convention)
  lon = CL_rMod(atan(y,x), 0, 2*%pi); 

  pos_sph = [lon; lat; d];

  d2_xy = x.^2 + y.^2;
  I = find(d2_xy == 0);
  d2_xy(I) = %nan;
  d_xy = sqrt(d2_xy);

  // -------------------------------
  // Computation of derivatives
  // -------------------------------
  if (compute_der)
    vx = vel_car(1,:);
    vy = vel_car(2,:);
    vz = vel_car(3,:);

    // derivatives of radius,lat,lon
    ddot = CL_dot(pos_car, vel_car) ./ d; 
    latdot = (vz - z .* (ddot ./ d)) ./ d_xy;
    londot = (x .* vy - y .* vx) ./ d2_xy; 

    pos_sph_dot = [londot; latdot; ddot];
  end

  // -------------------------------
  // Computation of jacobian
  // -------------------------------
  if (compute_jac)
    N = size(pos_car, 2);

    jacob = zeros(dimjac,dimjac,N);

    d2 = d .* d;
    z_over_d = z ./ d;
    z_over_d2 = z ./ d2;
    coslon = cos(lon); 
    sinlon = sin(lon); 
    
    d_lat_dx = x .* (-z_over_d2 ./ d_xy);
    d_lat_dy = y .* (-z_over_d2 ./ d_xy);
    d_lat_dz = d_xy ./ d2;

    d_lon_dx = -y ./ d2_xy;
    d_lon_dy = x ./ d2_xy;
    
    d_d_dx = x ./ d;
    d_d_dy = y ./ d;

    // latitude partial derivatives
    jacob(2,1,1:N) = d_lat_dx;
    jacob(2,2,1:N) = d_lat_dy;
    jacob(2,3,1:N) = d_lat_dz;

    // longitude partial derivatives
    jacob(1,1,1:N) =  d_lon_dx;
    jacob(1,2,1:N) = d_lon_dy;

    // d partial derivatives
    jacob(3,1,1:N) = d_d_dx;
    jacob(3,2,1:N) = d_d_dy;
    jacob(3,3,1:N) = z_over_d;

    if (dimjac > 3)
  
    z_ddot_over_d3 = z_over_d2 .* ddot ./ d; 

    vartmp = ddot ./ d; 
    d_ddot_dx = (vx - x .* vartmp) ./ d;
    d_ddot_dy = (vy - y .* vartmp) ./ d;
    d_ddot_dz = (vz - z .* vartmp) ./ d;

    // velocity derivatives over d
    jacob(6,1,1:N) = d_ddot_dx;
    jacob(6,2,1:N) = d_ddot_dy;
    jacob(6,3,1:N) = d_ddot_dz;

    jacob(6,4,1:N) = d_d_dx;
    jacob(6,5,1:N) = d_d_dy;
    jacob(6,6,1:N) = z_over_d;

    // velocity partial derivatives over latitude
    vartmp = z_ddot_over_d3 - (latdot./d_xy);
    jacob(5,1,1:N) = (x .* vartmp - z_over_d .* d_ddot_dx) ./ d_xy;
    jacob(5,2,1:N) = (y .* vartmp - z_over_d .* d_ddot_dy) ./ d_xy;
    jacob(5,3,1:N) = (z_ddot_over_d3 .* z - z_over_d .* d_ddot_dz - ddot ./ d) ./ d_xy;
    jacob(5,4,1:N) = d_lat_dx;
    jacob(5,5,1:N) = d_lat_dy;
    jacob(5,6,1:N) = d_lat_dz;

    // velocity partial derivatives over longitude
    jacob(4,1,1:N) = (vy - 2 * x .* londot) ./ d2_xy;
    jacob(4,2,1:N) =  -(vx + 2 * y .* londot) ./ d2_xy;
    jacob(4,4,1:N) = d_lon_dx;
    jacob(4,5,1:N) = d_lon_dy;

    end // dimjac > 3
  end

  // -------------------------------
  // propagates (initial) %nan
  // -------------------------------
  if (Inan <> [])
    pos_sph(:, Inan) = %nan; 
    if (pos_sph_dot <> []); pos_sph_dot(:, 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_car = varargin(1); 
  vel_car = []; 
  compute_der = %f; 
  compute_jac = (lhs >= 2);
  dimjac = 3; // if computed

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

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

else
  error("Unexpected situation!"); 
end

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

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

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

else
  varargout(1) = pos_sph; 
  if (lhs >= 2); varargout(2) = pos_sph_dot; end
  if (lhs >= 3); varargout(3) = jacob; end
end

endfunction