//  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_topoN,vel_topoN,jacob] = CL_fr_ter2topoN(orig, pos_ter,vel_ter,er,obla)
// Terrestrial frame to topocentric North frame vector transformation - DEPRECATED
//
// Calling Sequence
// [pos_topoN,vel_topoN,jacob] = CL_fr_ter2topoN(orig [,pos_ter,vel_ter,er,obla])
//
// Description
// <itemizedlist><listitem>
// <p>This function is deprecated. </p>
// <p>Replacement function: <link linkend="CL_fr_topoNMat">CL_fr_topoNMat</link></p>
// <p></p></listitem>
// <listitem>
// <p>Converts position and (optionally) velocity vectors from the terrestrial ("Earth fixed") reference frame 
// to the topocentric North ("Earth fixed") frame.</p> 
// <p>The jacobian of the transformation is optionally computed.</p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p> 
// <p> - The jacobian of the transformation only depends on the origin of the topocentric frame. 
// It can then be computed even if the position and velocity vectors are omitted 
// (in this case they are given the value [], and the corresponding output values are also set to []). </p>
// <p> - <b>The origin of the topocentric
// North frame is not the center of the planet</b>, but
// the location defined by <b>orig</b>. </p>
// <p> - This function can be used for another planet than the Earth. </p>
// <p></p></listitem>
// <listitem>
// <p>See <link linkend="Local frames">Local frames</link> for more details on the definition of local frames.</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// orig: [lon;lat;alt] Topocentric frame origin in elliptical (geodetic) coordinates [rad;rad;m] (3x1 or 3xN)
// pos_ter: (optional) [X;Y;Z] Position vector relative to terrestrial frame [m] (3x1 or 3xN)
// vel_ter: (optional) [Vx;Vy;Vz] Velocity vector relative to terrestrial frame [m/s] (3x1 or 3xN)
// er: (optional) Planet equatorial radius (default is %CL_eqRad) [m]
// obla: (optional) Planet oblateness (default is %CL_obla)
// pos_topoN: [X;Y;Z] Position vector relative to topocentric North frame [m] (3xN)
// vel_topoN: (optional) [Vx;Vy;Vz] Velocity vector relative to topocentric North frame [m/s] (3xN)
// jacob: (optional) Transformation jacobian d(x,y,z,vx,vy,vz)_topoN/d(x,y,z,vx,vy,vz)_ter (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) CNES - MSLIB FORTRAN 90, Volume T (mt_topo_N_ref)
//
// See also
// CL_fr_topoNMat
// CL_fr_topoN2ter
//
// Examples
// orig = [%pi/4;%pi/3;100];
//
// // Compute only jacobian :
// [pos,vel,jacob] = CL_fr_ter2topoN(orig)
//
// // Compute position and velocity :
// pos_ter = [ 3500.e3; 2500.e3; 5800.e3 ];
// vel_ter = [ 1.e3; 2.e3; 6.e3 ];
// [pos,vel] = CL_fr_ter2topoN(orig,pos_ter,vel_ter) 
//
// // Compute position only (2 position vectors) :
// [pos] = CL_fr_ter2topoN(orig,[pos_ter, pos_ter])
//
// // Compute 2 positions (corresponding to 2 origins) :
// [pos] = CL_fr_ter2topoN([orig,orig],[pos_ter, pos_ter]) 
//

// Declarations:


// Code:

CL__warnDeprecated(); // deprecated function

if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("obla", "local")); obla = CL__dataGetEnv("obla"); end

// check number of output arguments 
[lhs,rhs] = argn();

if (lhs > 3)
  CL__error("Invalid number of output arguments");
end

if (rhs < 1)
  CL__error("Invalid number of input arguments");
end

compute_jacob = %f;
if (lhs == 3); compute_jacob = %t; end
  
// Check consistency of inputs

Np = 0; 
Nv = 0;
if exists('pos_ter','local'); Np = size(pos_ter,2); end
if exists('vel_ter','local'); Nv = size(vel_ter,2); end

No = size(orig,2);

// if pos and vel both defined => same size
if (Np > 0 & Nv > 0 & Np <> Nv) 
  CL__error('pos_ter and vel_ter have inconsistent sizes'); 
end

// No == 0 => Np and Nv must be == 0
if (No == 0 & (Np > 0 | Nv > 0)) 
  CL__error('orig is undefined'); 
end

// if vel present => pos must be too
if (Np == 0 & Nv > 0) 
  CL__error('vel_ter is present whereas pos_ter is not'); 
end

if ~(No == 1 | Np <= 1 | No == Np) 
  CL__error('orig and pos_ter have inconsistent sizes'); 
end

// adjust sizes 

N = max(No, Np, Nv); 

if (Np == 0); pos_ter = []; end
if (Nv == 0); vel_ter = []; end

if (Np > 0 & Np < N); pos_ter = pos_ter * ones(1,N); end 
if (Nv > 0 & Nv < N); vel_ter = vel_ter * ones(1,N); end
if (No < N); orig = orig * ones(1,N); end


// NB: the vectors u,v,w are the LINES of the transformation
// matrix from terrestrial frame to topocentric North frame

lon = orig(1,:);
lat = orig(2,:);

u = [ -sin(lat).*cos(lon); -sin(lat).*sin(lon); cos(lat) ];  
v = [ sin(lon); -cos(lon); zeros(lon) ]; 
w = [ cos(lat).*cos(lon); cos(lat).*sin(lon); sin(lat) ]; 


// --- Compute position and velocity relative to topocentric North frame 

sta = CL_co_ell2car(orig,er,obla); // "orig" in cartesian coordinates

pos_topoN = []; 
vel_topoN = []; 

if (Np > 0)
  pos_topoN = zeros(3,N); 
  pos_topoN(1,:) = CL_dot(u, pos_ter - sta); 
  pos_topoN(2,:) = CL_dot(v, pos_ter - sta); 
  pos_topoN(3,:) = CL_dot(w, pos_ter - sta); 
end

if (Nv > 0)
  vel_topoN = zeros(3,N);
  vel_topoN(1,:) = CL_dot(u, vel_ter);
  vel_topoN(2,:) = CL_dot(v, vel_ter);
  vel_topoN(3,:) = CL_dot(w, vel_ter);
end


// --- Compute Jacobian
jacob = []; 

if (compute_jacob & N > 0)
  jacob = hypermat([6,6,N]);
  mat = (hypermat([3,3,N], [u; v; w]))';
  jacob(1:3,1:3,1:N) = mat;
  jacob(4:6,4:6,1:N) = mat;
  
  if (N == 1); jacob = jacob(:,:,1); end
end


endfunction