//  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'.

// -----------------------------------------------------------
//> Pointing errors:
//> Impact of errors in qsw frame on pointing.
//> 
//> Each color corresponds to a different pass (with a different max 
//> elevation of spacecraft at mid-pass). 
//> There are three possible results: 
//> 1) Pointing error considering the signed error components in qsw frame
//> and the motion direction. 
//> 2) Max pointing error considering all combinations of signs of
//> error components in qsw frame.  
//> 3) Max dimension of covariance ellipse (semi major axis) in plane 
//> perpendicular to line of sight. The errors components in input are 
//> assumed to be 1-sigma values (the errors are not correlated). 
//> 
//> The sign of input error components has no impact for methods
//> 2 and 3. 
//>
//> NB: approximate (geometrical) calculation, valid only for low orbits. 
//> Hypothesis: spherical and non rotating Earth. 
//> The impact of a rotating Earth on the results can be approximately 
//> deduced from the path length and a station displacement rate of 
//> ~0.25 deg per min (=> ~2 deg max between beginning/end of pass and 
//> mid-pass for LEO). 
//
// Author: A. Lamy
// -----------------------------------------------------------

alt = 700.e3; // altitude
elevmin = 0 * %CL_deg2rad;
tab_elevmax = 0:5:90;  // degrees
mdir = 1; 
err_qsw = [1.e3, 10.e3, 1.e3]; 
plot_opt = 2; 
x_axis = 1; 

desc_param = list(..
   CL_defParam("Altitude of circular orbit", alt, units=['m', 'km'], id='$alt', valid='$alt>=0' ),..
   CL_defParam("Minimum elevation for visibility", elevmin, units=['rad', 'deg'], id='$elevmin', valid='$elevmin>=0 & $elevmin<=90' ),..
   CL_defParam("Levels of maximum elevation", tab_elevmax, units=['deg'], dim=[1, 100], id='$elevmax', valid='$elevmax>=$elevmin & $elevmax<=90'),..
   CL_defParam("Motion direction / station: 1=direct, -1=retrograde", mdir, accv=[-1,1]),..
   CL_defParam("Position error on (q, s, w) axes", err_qsw, units=['m', 'km'], dim=3), ..
   CL_defParam("Plot: 1=error, 2=max error 3=statistical error", plot_opt, accv=(1:3)), ..
   CL_defParam("X-axis: 1=elevation, 2=PSO, 3=time", x_axis, accv=(1:3))..
);

[alt,elevmin,tab_elevmax,mdir,err_qsw,plot_opt,x_axis] = CL_inputParam(desc_param);
tab_elevmax = tab_elevmax * %CL_deg2rad; 
err_qsw = matrix(err_qsw, 3, 1); 


// -----------------------------------------------------------
// Visibility angles
// elevmaxpass : elevation at mid-pass (max value)
// elev : elevation 
// R : planet radius
// => pso: PSO for each elevation
// => betaa: angle between orbit plane and station direction
//    from Earth center. 
// -----------------------------------------------------------
function [pso, betaa] = ang_visi(R, sma, elevmaxpass, elev)
  
  // angle from Earth center at mid-pass
  betaa = CL_gm_visiParams(sma, R, 'elev', elevmaxpass, 'cen'); 

  // angle from Earth center at beginning of pass
  angc = CL_gm_visiParams(sma, R, 'elev', elev, 'cen'); 

  // spherical triangle: station - midpass - beginning of pass
  // (right (trihedral) angle) 
  pso = real(acos(cos(angc) ./ cos(betaa))); 
endfunction


// -----------------------------------------------------------
// Orbit error perpendicular to line of sight
// err : 1x3
// u : line of sight in qsw frame (norm=1) 1xN
// opt: same as plot_opt
// z: error 1xN
// -----------------------------------------------------------
function [z] = orb_err_los(err, u, opt)
  N = size(u, 2); 
  
  if (opt == 1)
    z = CL_norm(CL_cross(err, u));  
    
  elseif (opt == 2)
    // all combinations of error signs in qsw
    tab = [[1;1;1],[1;1;-1],[1;-1;1],[1;-1;-1], ..
           [-1;1;1],[-1;1;-1],[-1;-1;1],[-1;-1;-1]]; 
    z = zeros(1,N); 
    for (t = tab)
      errt = CL_dMult(err, t); 
      z = max(z, CL_norm(CL_cross(errt, u)));  
    end
    
  else
    // err: 1-sigma errors
    // projection perpendicular to line of sight
    covm = matrix(repmat(diag(err.^2), 1, N), 3, 3, N); 
    M = CL_rot_defFrameVec(u, u, 3, 3); 
    covm = M * covm * M'; 
    z = zeros(1,N); 
    for (k = 1 : N)
      // max dimension of ellipse
      z(k) = sqrt(max(real(spec(covm(1:2,1:2,k))))); 
    end
  end
endfunction


// -----------------------------------------------------------
// Computation
// -----------------------------------------------------------

R = %CL_eqRad; 
sma = R + alt; 
T = CL_kp_params("per", sma); 
V = sqrt(%CL_mu / sma); // velocity
nbpso = 301;  // number of pso per pass (odd number)

// pass angles
[psomax, betaa] = ang_visi(R, sma, tab_elevmax, elevmin);

// order: elevmax1, elevmax2, ...
pso = matrix(CL_intervLinspace([-psomax; psomax], nbpso), 1, -1); 
betaa = matrix(repmat(betaa, nbpso, 1), 1, -1); 
elevmaxpass = matrix(repmat(tab_elevmax, nbpso, 1), 1, -1);
  
// Frame: 
// Orbit in (X,Y) plane. X axis: mid pass. Y increasing. 
// Station in (X,Z) plane (Z positive if direct pass) 
// pso: angle in orbit plane from X axis. 
 
pos = sma * [cos(pso); sin(pso); zeros(pso)]; 
vel = V * [-sin(pso); cos(pso); zeros(pso)];
  
// station (not moving / orbit plane)
psta = R * [cos(betaa); zeros(betaa); mdir * sin(betaa)]; 
   
// line of sight: station -> spacecraft
[u, dist] = CL_unitVector(pos - psta); 
  
// elevation of spacecraft from station
elev = max(elevmin, min(elevmaxpass, %pi/2 - CL_vectAngle(u, psta)));  
  
// line of sight in local frame 
u_qsw = CL_fr_inertial2qsw(pos, vel, u); 

// ang: orbit error perpendicular to line of sight (rad)
ang = atan(orb_err_los(err_qsw, u_qsw, plot_opt) ./ dist); 

// results (each column <-> elevmax)
result = struct(); 
result.elev = matrix(elev, nbpso, -1); 
result.pso = matrix(pso, nbpso, -1); 
result.ang = matrix(ang, nbpso, -1); 
  

// -----------------------------------------------------------
// Plot
// -----------------------------------------------------------
f=scf();
f.figure_size = [800,600]; 
f.visible="on";
f.immediate_drawing="off";

a=gca();

nbcol = 100; 
f.color_map = jetcolormap(nbcol) * 0.9; // darker

colorbar(min(tab_elevmax*%CL_rad2deg), max(tab_elevmax*%CL_rad2deg), colminmax=[1,nbcol]); 
e = gce(); 
e.parent.title.text = "Max elevation (deg)"; 

for k = 1 : length(tab_elevmax) 
  icol = round((k-1) * (nbcol-1) / (length(tab_elevmax)-1)) + 1; 
  if (x_axis == 1)
    x = result.elev(:,k) * %CL_rad2deg;
  elseif (x_axis == 2)
    x = result.pso(:,k) * %CL_rad2deg;
  else 
    x = result.pso(:,k) * (T/(2*%pi)) / 60;
  end
  plot2d(x, result.ang(:,k) * %CL_rad2deg, style=icol);
end

// general setting
str_alt = msprintf("%.1f", alt/1000) + " km"; 
str_err = "err = [" + strcat(msprintf("%.1f\n", err_qsw/1000),", ") + "] km"; 
text1 = "(" + str_alt + ", " + str_err + ")"; 

text2 = ""; 
if (plot_opt == 2); text2 = " - worst case"; end
if (plot_opt == 3); text2 = " - stat"; end

a.title.text = "Pointing error " + text1 + " - non rotating Earth" + text2;
a.x_label.text = "Elevation (deg)";
if (x_axis == 2); a.x_label.text = "In-orbit position / mid-pass (deg)"; end
if (x_axis == 3); a.x_label.text = "Time / mid-pass (min)"; end
a.y_label.text = "Angular error (deg)";
CL_g_stdaxes(a,ft=3);

// change properties
h = CL_g_select(a, "Polyline");
h.thickness = 2;

f.immediate_drawing="on";
f.visible="on";