//  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 [result1, result2] = CL_ex_propagateMan(mod,type_oe,t1,mean_t1,t2,tman,dvman,dvframe,res,er,mu,j1jn)
// Orbit propagation with maneuvers (all analytical models) 
//
// Calling Sequence
// [result1, result2] = CL_ex_propagateMan(mod,type_oe,t1,mean_oe_t1,t2,tman,dvman,dvframe,res [,er,mu,j1jn])
//
// Description
// <itemizedlist><listitem>
// <p>Propagates orbital elements using one analytical model while taking maneuvers (velocity increments)
// into account. </p>
// <p></p>
// <p>The available propagation models are: </p>
// <p>"central": Central force (osculating elements = mean elements) </p>
// <p>"j2sec": Secular effects of J2 (osculating elements = mean elements by convention)</p>
// <p>"lydsec": Lyddane (mean elements include secular effects only) </p>
// <p>"lydlp": Lyddane (mean elements include secular and long period effects) </p>
// <p>"eckhech": Eckstein-Hechler  (mean elements include secular and long period effects) </p>
// <p></p>
// <p>Maneuvers are defined by: </p>
// <p>- <b>tman</b>: time of the maneuver.</p>
// <p>- <b>dvman</b> = [delta-Vx;delta-Vy;delta-Vz]: delta-V components in the chosen frame. </p>
// <p>- <b>dvframe</b>: name of the frame in which the components are given. The name can be 
// any name compatible with <link linkend="CL_fr_locOrbMat">CL_fr_locOrbMat</link>, or "inertial". </p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes</b>:</p> 
// <p>- If one maneuver occurs at one of the final times, the output orbital elements  
// include the effect of the maneuver at that time.</p>
// <p>- The final times as well as the maneuver times should be sorted in increasing order.</p>
// <p>- t1 should be smaller than tman(k) for any k.</p>
// <p>- t1 should be smaller than t2(k) for any k.</p>
// <p></p></listitem>
// <listitem>
// <p>See <link linkend="Propagation models">Propagation models</link> for more details.</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// mod: (string) Model name: "central", "j2sec", "lydsec", "lydlp", "eckhech". (1x1)
// type_oe: (string) Type of orbital elements used for input/output: "kep", "cir", "cireq" or "equin" (1x1)
// t1: Initial time [days] (1x1)
// mean_oe_t1: Mean orbital elements at time t1 (6x1 or 6xN) 
// t2: Final time [days] (1xN)
// tman: Maneuver times [days] (1xP)
// dvman: Delta-V with components in specified frame [dvx;dvy;dvz] [m/s] (3xP)
// dvframe: (string) Frame for the DV components: any local orbital frame or "inertial". (1x1)
// res: (string) Type of output (mean or osculating): "m", "o", "mo", "om"  (1x1)
// er: (optional) Equatorial radius [m] (default is %CL_eqRad)
// mu: (optional) Gravitational constant [m^3/s^2] (default value is %CL_mu)
// j1jn: (optional) Vector of zonal harmonics. Default is %CL_j1jn (Nz x 1)
// result1, result2: Mean or osculating orbital elements at t2 (6xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_ex_propagate
// CL_fr_locOrbMat
//
// Examples
// // Initial state (mean keplerian elements) 
// t0 = 0; // days
// kep0 = [8.e6; 0.2; 60*(%pi/180); 0; 0; 0];
// // Final times 
// t = t0 + (0 : 0.001 : 3.5); // days 
// // Maneuvers 
// tman = t0 + [1.5, 2.5]; // days
// dvman = [[10; 0; 0], [0; 0; 150]];
// dvframe = "tnw"; 
// 
// // Propagation including maneuver effects (result = osculating elements)
// kep = CL_ex_propagateMan("lydsec", "kep", t0, kep0, t, tman, dvman, dvframe, "o"); 
// 
// // Plot semi-major axis (blue) and inclination (red) 
// scf(); 
// plot(t, kep(1,:) / 1000, "b"); 
// scf(); 
// plot(t, kep(3,:) * (180/%pi), "r"); 


// Declarations:

// local function : propagates to t (1x1) and apply dv at t

function [mean_t] = extrap_to_man(mod, type_oe, t1, mean_t1, t, dv, dvframe, er, mu, j1jn)
  osc_t = CL_ex_propagate(mod, type_oe, t1, mean_t1, t, "o", er, mu, j1jn);
  pv = CL_oe_convert(type_oe, "pv", osc_t, mu);
  if (dvframe == "inertial")
    dv_inert = dv; 
  else
    M = CL_fr_locOrbMat(pv(1:3,:), pv(4:6,:), dvframe); 
    dv_inert = (M') * dv;
  end
  pv(4:6,:) = pv(4:6,:) + dv_inert;
  osc_t = CL_oe_convert("pv", type_oe, pv, mu);
  mean_t = CL_ex_osc2mean(mod, type_oe, osc_t, er, mu, j1jn);
endfunction

// possible values for argument "res"
RES = ["mo", "om", "o", "m"]; 

// possible values for argument "dvframe"
DVFRAMES = ["qsw", "tnw", "inertial"]; 


// Code:

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

// --------------------------------------
// Error checking
// --------------------------------------

if (argn(2) < 9)
  CL__error("invalid number of input arguments"); 
end

if (size(t1,"*") <> 1)
  CL__error("Invalid size for argument t1: should be (1x1)"); 
end

if (size(mean_t1,1) <> 6 | size(mean_t1,2) <> 1)
  CL__error('Invalid size for argument mean_t1: should be (6x1)'); 
end

// check argument "res"
if (typeof(res) <> "string" | size(res, "*") <> 1)
  CL__error("Invalid type or size for argument res"); 
end
if (find(res == RES) == [])
  CL__error("Invalid value for argument res"); 
end

// check argument "dvframe"
if (typeof(dvframe) <> "string" | size(dvframe, "*") <> 1)
  CL__error("Invalid type or size for argument: dvframe"); 
end
if (find(dvframe == DVFRAMES) == [])
  CL__error("Invalid value for argument: dvframe"); 
end

// other checks
Nman = size(tman,2); 
Nt2 = size(t2,2); 

if (Nman <> size(dvman,2))
   CL__error('tman et dvman not of the same size. Should be (1xP) and (3xP)');
end

// check maneuver dates
if (Nman <> 0)
  if (tman(1) < t1)
    CL__error("Invalid maneuver dates: t1 should be <= tman"); 
  end
  if (Nman > 1)
    if (find(tman(2:$) - tman(1:$-1) < 0))
      CL__error("Non increasing maneuver dates"); 
    end
  end
end

// check final dates
if (Nt2 <> 0)
  if (t2(1) < t1)
    CL__error("Invalid final dates: t1 should be <= t2"); 
  end
  if (Nt2 > 1)
    if (find(t2(2:$) - t2(1:$-1) < 0))
      CL__error("Non increasing final dates"); 
    end
  end
end

// --------------------------------------
// Special cases
// --------------------------------------

if (Nt2 == 0) then 
   result1 = []; 
   result2 = []; 
   return; // *** EXIT ***  
end

// optimisation: reduce number of maneuvers
I = find(tman <= t2($));
tman = tman(I); 
dvman = dvman(:,I); 
Nman = length(I);  

// Case of no maneuver:
if (Nman == 0) then 
   [result1, result2] = CL_ex_propagate(mod, type_oe, t1, mean_t1, t2, res, er, mu, j1jn);
   return; // *** EXIT ***  
end

// --------------------------------------
// Propagation with maneuvers 
// (at least one) 
// --------------------------------------
result1 = %nan * ones(6, Nt2);   
result2 = %nan * ones(6, Nt2);

// Initial state
tref = t1; 
mean_tref = mean_t1; 

// Propagation between successive maneuvers
for kman = 1 : Nman

   I = find(t2 >= tref & t2 < tman(kman));
   if (I <> [])
      [result1(:,I),result2(:,I)] = CL_ex_propagate(mod, type_oe, tref, mean_tref, t2(I), res, er, mu, j1jn);
   end
  
   // state after kman_th maneuver:
   mean_tref = extrap_to_man(mod, type_oe, tref, mean_tref, tman(kman), dvman(:,kman), dvframe, er, mu, j1jn); 
   tref = tman(kman); 
   
end

// Propagation -> final time 
I = find(t2 >= tman(Nman));
if (I <> [])
   [result1(:,I),result2(:,I)] = CL_ex_propagate(mod, type_oe, tref, mean_tref, t2(I), res, er, mu, j1jn);
end

endfunction