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

// -----------------------------------------------------------
//> Various definitions of "altitude": 
//> Meaning of terms used: 
//> - spherical: radius (= distance from Earth centre) minus Earth equatorial radius
//> - geodetic: altitude wrt ellipsoid
//> - mean: value obtained from mean parameters
//> - true: value obtained from true (osculating) parameters
//> - sma: semi-major axis
//>
//> NB: Orbit propagated using "lyddaneLp" model (see help pages for validity). 

// Author: A. Lamy
// -----------------------------------------------------------

sma = %CL_eqRad + 700.e3; // sma minus equatorial radius
pom = %pi/2; 
inc = 98*%CL_deg2rad; 
ecc = 1.e-3; 
x_axis = 1;  // PSO
y_axis = 1;  // altitude

desc_param = list(..
   CL_defParam("Mean semi-major axis", sma, units=['m', 'km'], valid='$x>0'),..
   CL_defParam("Mean eccentricity", ecc,  valid="$x>=0 & $x<1"  ),..
   CL_defParam("Mean inclination", inc, units=['rad', 'deg'], valid="$x>=0 & $x<=180"  ),..
   CL_defParam("Mean argument of perigee", pom, units=['rad', 'deg']),..
   CL_defParam("X-axis: 1=mean arg. of lat, 2=true arg. of lat, 3=(sph.) latitude", x_axis, accv=1:3),..
   CL_defParam("Plot: 1=altitude, 2=1st time derivative, 3=2nd time derivative", y_axis, accv=1:3)..
);

[sma, ecc, inc, pom, x_axis, y_axis] = CL_inputParam(desc_param);


// -----------------------------------------------------------
// Computation
// -----------------------------------------------------------
ex = ecc*cos(pom); 
ey = ecc*sin(pom); 

circ0 = [sma; ex; ey; inc; 0; 0]; // mean elements adapted to (nearly) circular orbits

T = CL_kp_params('per', sma)/86400;  // orbital period in seconds
t0 = 0; 
mean_cir_T = CL_ex_propagate("lydlp", "cir", t0, circ0, T, "m");  
n = CL_rMod(mean_cir_T(6), %pi, 3*%pi)/T; // rad/day
T = 2*%pi / n; // more exact (mean) period. 

t = linspace(0,T,500); // final times (days)

[mean_cir,osc_cir] = CL_ex_propagate("lydlp", "cir", t0, circ0, t, "mo");  // orbit propagation
pso = CL_rMod(mean_cir(6,:), (t/T) * 2*%pi -%pi, (t/T) * 2*%pi + %pi); // garanteed continuous

[posm,velm] = CL_oe_cir2car(mean_cir);
[pos,vel] = CL_oe_cir2car(osc_cir);

[pos_ell] = CL_co_car2ell(pos);
[pos_sph] = CL_co_car2sph(pos);

[posm_ell] = CL_co_car2ell(posm);
[posm_sph] = CL_co_car2sph(posm);

// altitudes and time derivatives
tsec = t * 86400; 

// altitudes (m)
alt_sma = (sma - %CL_eqRad)*ones(tsec); 
alt_sph = pos_sph(3,:) - %CL_eqRad; 
alt_sphm = posm_sph(3,:) - %CL_eqRad; 
alt_ell = pos_ell(3,:); 
alt_ellm = posm_ell(3,:); 

// 1st derivatives (m/s)
[X, alt_sma_dot] = CL_interpLagrange(tsec, alt_sma, tsec); 
[X, alt_sph_dot] = CL_interpLagrange(tsec, alt_sph, tsec); 
[X, alt_sphm_dot]  = CL_interpLagrange(tsec, alt_sphm, tsec); 
[X, alt_ell_dot] = CL_interpLagrange(tsec, alt_ell, tsec); 
[X, alt_ellm_dot] = CL_interpLagrange(tsec, alt_ellm, tsec); 

// 2nd derivatives (m/s^2)
[X, alt_sma_2dot] = CL_interpLagrange(tsec, alt_sma_dot, tsec); 
[X, alt_sph_2dot] = CL_interpLagrange(tsec, alt_sph_dot, tsec); 
[X, alt_sphm_2dot]  = CL_interpLagrange(tsec, alt_sphm_dot, tsec); 
[X, alt_ell_2dot] = CL_interpLagrange(tsec, alt_ell_dot, tsec); 
[X, alt_ellm_2dot] = CL_interpLagrange(tsec, alt_ellm_dot, tsec); 


// -----------------------------------------------------------
// plot
// -----------------------------------------------------------
function PLOT(x, y, icol, thick)
  plot(x, y, "thickness", thick); 
  e = CL_g_select(gce(), "Polyline"); 
  e.foreground = icol; 
endfunction

f = scf();
f.visible="on";
f.immediate_drawing="off";

a=gca();

if (y_axis == 1)
  a.title.text = "Altitude";
  a.y_label.text = "Altitude (km)";
elseif (y_axis == 2)
  a.title.text = "Altitude 1st time derivative";
  a.y_label.text = "Altitude 1st time derivative (m/s)";
else
  a.title.text = "Altitude 2nd time derivative";
  a.y_label.text = "Altitude 2nd time derivative (m/s^2)";
end

if (x_axis == 1)
  x = pso*%CL_rad2deg; 
  a.x_label.text = "Mean argument of latitude (deg)";
elseif (x_axis == 2)
  psov = CL_rMod(pom+CL_kp_M2v(ecc,pso-pom),pso-%pi,pso+%pi); 
  x = psov*%CL_rad2deg; 
  a.x_label.text = "True argument of latitude (deg)";
else
  x = pos_sph(2,:)*%CL_rad2deg; 
  a.x_label.text = "Latitude (deg)";
end

if (y_axis == 1)
  PLOT(x, alt_sma/1000, 1, 3); 
  PLOT(x, alt_sph/1000, 11, 3); 
  PLOT(x, alt_sphm/1000, 12, 2); 
  PLOT(x, alt_ell/1000, 28, 3); 
  PLOT(x, alt_ellm/1000, 30, 2); 
  
elseif (y_axis == 2)
  PLOT(x, alt_sma_dot, 1, 3); 
  PLOT(x, alt_sph_dot, 11, 3); 
  PLOT(x, alt_sphm_dot, 12, 2); 
  PLOT(x, alt_ell_dot, 28, 3); 
  PLOT(x, alt_ellm_dot, 30, 2); 

else
  PLOT(x, alt_sma_2dot, 1, 3); 
  PLOT(x, alt_sph_2dot, 11, 3); 
  PLOT(x, alt_sphm_2dot, 12, 2); 
  PLOT(x, alt_ell_2dot, 28, 3); 
  PLOT(x, alt_ellm_2dot, 30, 2); 

end

// adjustments

CL_g_stdaxes(a);
a.tight_limits = "on"; 

CL_g_legend(a, ["Spherical: sma", "Spherical: true", "Spherical: mean", "Geodetic: true", "Geodetic: mean"]); 

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