//  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_ter,vel_ter,jacob]=CL_fr_topoN2ter(orig, pos_topoN,vel_topoN,er,obla)
// Topocentric North frame to terrestrial frame vector transformation - DEPRECATED
//
// Calling Sequence
// [pos_ter, vel_ter, jacob] = CL_fr_topoN2ter(orig [,pos_topoN,vel_topoN,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 topocentric North ("Earth fixed") frame to 
// the terrestrial ("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 reference 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_topoN: (optional) [X;Y;Z] Position vector relative to topocentric North frame [m] (3x1 or 3xN)
// vel_topoN: (optional) [Vx;Vy;Vz] Velocity vector relative to topocentric North 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_ter: [X;Y;Z] Position vector relative to terrestrial frame [m] (3xN)
// vel_ter: (optional) [Vx;Vy;Vz] Velocity vector relative to terrestrial frame [m/s] (3xN)
// jacob: (optional) Transformation jacobian d(x,y,z,vx,vy,vz)_ter/d(x,y,z,vx,vy,vz)_topoN (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) CNES - MSLIB FORTRAN 90, Volume T (mt_topo_N_ref)
//
// See also
// CL_fr_topoNMat
// CL_fr_ter2topoN
//
// Examples
// orig = [%pi/4;%pi/3;100];
//
// // Compute only jacobian :
// [pos,vel,jacob] = CL_fr_topoN2ter(orig) 
//
// // Compute position and velocity :
// pos_topoN = [ 50; 25; 32 ];
// vel_topoN = [ 70; 100; 0 ];
// [pos,vel] = CL_fr_topoN2ter(orig,pos_topoN,vel_topoN) 
//
// // Compute position only (2 position vectors) :
// [pos] = CL_fr_topoN2ter(orig,[pos_topoN, pos_topoN]) 
//
// // Compute 2 positions (corresponding to 2 origins) :
// [pos] = CL_fr_topoN2ter([orig,orig],[pos_topoN, pos_topoN]) 


// 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_topoN','local'); Np = size(pos_topoN,2); end
if exists('vel_topoN','local'); Nv = size(vel_topoN,2); end

No = size(orig,2);

// if pos and vel both defined => same size
if (Np > 0 & Nv > 0 & Np <> Nv) 
  CL__error('pos_topoN and vel_topoN 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_topoN is present whereas pos_topoN is not'); 
end

if ~(No == 1 | Np <= 1 | No == Np) 
  CL__error('orig and pos_topoN 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_topoN = pos_topoN * ones(1,N); end
if (Nv > 0 & Nv < N); vel_topoN = vel_topoN * 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 topocentric North frame to terrestrial frame 

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

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


// --- Compute position and velocity relative to terrestrial frame

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

pos_ter = []; 
vel_ter = []; 

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

if (Nv > 0)
  vel_ter = zeros(3,N);
  vel_ter(1,:) = CL_dot(u, vel_topoN);
  vel_ter(2,:) = CL_dot(v, vel_topoN);
  vel_ter(3,:) = CL_dot(w, vel_topoN);
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