//  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_gm_stationPointing(stations,pos, res,er,obla)
// Spherical coordinates of object in local frame of location.
//
// Calling Sequence
// [res1,res2,...] = CL_gm_stationPointing(stations,pos [, res,er,obla])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the spherical coordinates (elevation, West azimuth, distance)
// of any object as seen from one or several locations (<b>stations</b>). The locations are defined by 
// their elliptical ("geodetic") coordinates. </p>
// <p>The positions (<b>pos</b>) are given in cartesian coordinates in the same frame as <b>stations</b>. </p>
// <p></p>
// <p>The outputs can be described by the <b>res</b> argument: </p>
// <p> - "azim" : azimuth (positive towards West)</p>
// <p> - "elev" : elevation </p>
// <p> - "dist" : distance </p>
// <p> - "s" : structure containing the fields "azim", "elev", "dist" </p>
// <p> - "l" : list of spherical coordinates for each station </p>
// <p></p>
// <p></p></listitem>
// <listitem>
// <p>See <link linkend="Local frames">Local frames</link> for more details on the definition of local frames.</p> 
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// stations: Stations positions in elliptical (geodetic) coordinates [long,lat,alt] [rad,m] (3xP)
// pos: Positions of object in the same frame as "stations", in cartesian coordinates [m] (3xN)
// res: (string) vector of names: "azim", "elev", "dist", "s" or "l". Default is ["azim", "elev", "dist"] (1xQ)
// er : (optional) Planet's equatorial radius. Default is %CL_eqRad [m] (1x1)
// obla : (optional) Planet's oblateness. Default is %CL_obla (1x1)
// res1, res2...: results: Elevation [rad] (PxN), azimuth [rad] (PxN), distance [m] (PxN), structure (PxN fields) or list 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_gm_stationVisiLocus
//
// Examples
// // satellite positions 
// pos = [[7.e6; 0; 0], [7.e6; 1.e3; 0], [7.e6; 2.e3; 0]]; 
// // ground stations (geodetic coordinates)
// stations = [[0;0;0], [0.1; 0.2; 0]]
// // Coordinates in topocentric North frame (size of results: 2x3)
// [azim,elev,dist] = CL_gm_stationPointing(stations, pos); 
// // List of 3x1 coordinates
// l = CL_gm_stationPointing(stations, pos, res="l"); 


// Declarations:

// Code:
if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("obla", "local")); obla = CL__dataGetEnv("obla"); end
if (~exists("res","local")); res = ["azim", "elev", "dist"]; end

// output names (NB: order matter)
NAMES = ["azim", "elev", "dist"]; 
if (typeof(res) <> "string")
  CL__error("Invalid type for argument ''res''"); 
end
if (size(res,1) > 1)
  CL__error("Invalid size for argument ''res''"); 
end
if (setdiff(res, [NAMES, "s", "l"]) <> [])
  CL__error("Invalid ''res'' argument"); 
end
if (intersect(res, ["s", "l"]) <> [] & size(res, "*") <> 1)
  CL__error("Invalid ''res'' argument"); 
end

// cres: true if azim, elev, dist (in this order = same as NAMES) is computed
cres = [%f, %f, %f]; 
if (res == "l" | res == "s") 
  cres = [%t, %t, %t];
else
  for k = 1 : 3
    if (find(res == NAMES(k)) <> []); cres(k) = %t; end; 
  end
end
 
lhs = argn(1); 
if (lhs > size(res, "*"))
  CL__error("Too many output arguments"); 
end

// number of stations
P = size(stations, 2);      

// number of positions
N = size(pos, 2);       

// cartesian positions of stations
pos_sta = CL_co_ell2car(stations, er, obla);

// mat = topoN frame (Z=vertical, X=north)
// (X_topoN = mat * X_initial_frame)
mat = CL_fr_topoNMat(stations);

// Basis unit vectors of the topoN frame, in the initial frame
north = mat' * [1;0;0];
west = mat' * [0;1;0];
loc_vert = mat' * [0;0;1];

// -----------------------
// Computation of distance
// -----------------------
if (cres(3) | cres(2))
  // dist^2 = norme(pos_sat)^2 + norme(pos_sta)^2 - 2 * pos_sat dot pos_sta
  dist = sqrt(ones(P,1) * (CL_norm(pos).^2) + (CL_norm(pos_sta).^2)' * ones(1,N) - 2 * pos_sta'*pos);
end

// -----------------------
// Computation of elevation
// -----------------------
// elev = pi/2 - acos(prod_scal_north) / dist with prod_scal_north = loc_vert dot (pos_sat - pos_sta)
// --> computes the dot product in two parts
// first part = scal1 = (local vertical) scalar (pos_sat)
//                    = loc_vert' * pos 
// second part = scal2 = (local vertical) scalar (pos_sta)
//                     = CL_dot(loc_vert,pos_sta)' * ones(1,N))
// prod_scal_north = scal1 - scal2
if (cres(2))
  elev = zeros(P,N);
  I = find(dist == 0);
  // dist set to %nan temporarily (to avoid division by zero)
  dist(I) = %nan; 
  // (%pi/2) * ones(P,N) => OK if N is 0 
  elev = (%pi/2) * ones(P,N) - real(acos((loc_vert' * pos - CL_dot(loc_vert,pos_sta)' * ones(1,N)) ./ dist));
  elev(I) = 0;
  dist(I) = 0;
  // Notes: 
  // - real to avoid numerical errors
  // - if dist == 0 ==> elev = 0
end

// ----------------------------------------------
// Computation of azimuth (positive towards Zest)
// ----------------------------------------------
if (cres(1))
  prod_scal_west = west' * pos - CL_dot(west,pos_sta)' * ones(1,N);
  prod_scal_north = north' * pos - CL_dot(north,pos_sta)' * ones(1,N);
  azim = atan(prod_scal_west,prod_scal_north);
end

// ----------------------------------------------
// outputs
// ----------------------------------------------
for i = 1 : lhs
  if (res(i) == "elev")
    varargout(i) = elev; 
  elseif (res(i) == "azim")
    varargout(i) = azim; 
  elseif (res(i) == "dist")
    varargout(i) = dist; 
  elseif (res(i) == "s")
    // structure containing everything
    varargout(i) = struct("azim", azim, "elev", elev, "dist", dist);
  else
    // list(as many elements as stations)
    l = list(); 
    for k = 1 : P
      l(k) = [azim(k,:); elev(k,:); dist(k,:)]; 
    end;
    varargout(i) = l; 
  end 
end


endfunction