//  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 [pv] = CL_cw_propagateMan(t0,pv0,t,alt,acc,tman,dvman,er,mu)
// Propagation using Clohessy-Whiltshire model with maneuvers
//
// Calling Sequence
// pv = CL_cw_propagateMan(t0,pv0,t,alt [,acc,tman,dvman,er,mu])
//
// Description
// <itemizedlist><listitem>
// <p>Propagates using clohessy-Whiltshire equations (including a constant relative acceleration and possible impulsive maneuvers).</p> 
// <p></p>
// <p>The reference frame used is the target's LVLH local frame (origin = target). </p>
// <p>The reference altitude <b>alt</b> is the altitude of the target = semi major axis of target's (circular) orbit
// minus equatorial radius. </p>
// <p></p>
// <p><b>acc</b> is a (constant) differential inertial acceleration with components in the reference frame. </p>
// <p><b>tman</b> and <b>dvman</b> are optional maneuvers performed by the "chaser" (components in reference frame). </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// t0: Initial time [days] (1x1)
// pv0: Initial (relative) position and velocity vectors in reference frame [m,m/s] (6x1)
// t: Final (increasing) times [days] (1xN)
// alt: Reference altitude (= altitude of target) [m] (1x1)
// acc: (optional) Differential (constant) acceleration in reference frame (m/s^2). Default is []. (3x1)
// tman: (optional) times of maneuvers. Must be sorted increasingly [days]. Default is []. (1xP)
// dvman: (optional) Velocity increments [m/s]. Default is []. (3xP)
// er: (optional) Equatorial radius [m]. Default is %CL_eqRad
// mu: (optional) Gravitational constant [m^3/s^2]. Default value is %CL_mu
// pv: Relative position and velocity vectors at time t in reference frame [m,m/s] (6xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_cw_propagate
//
// Bibliography
// 1) Mecanique spatiale, CNES - Cepadues 1995, Tome II
//
// Examples
// alt = 450.e3;
// t0 = 0;
// pv0 = [1;0;0;1;0;0];  
// t = (100:100:500)/86400; 
// tman = 150/86400; 
// dvman = [1;0;0]; 
// pv = CL_cw_propagateMan(t0, pv0, t, alt, [], tman, dvman)


// Declarations:

// Code:
if (~exists('acc','local')); acc=[0;0;0]; end
if (~exists('tman','local')); tman=[]; end
if (~exists('dvman','local')); dvman=[]; end
if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

// only one initial date. 
if (size(t0, 2) > 1 | size(pv0,2) > 1 | size(acc, 2) > 1 | size(alt,2) > 1)
  CL__error("Invalid input arguments sizes"); 
end

// no input dates
if (t == [])
  pv = []; 
  return; 
end

I = find(t < t0)
if (I <> [])
  CL__error("Invalid input arguments (times)"); 
end

if (find(t(2:$) - t(1:$-1) < 0) <> [])
  CL__error("Not increasing extrapolation dates"); 
end

if (tman <> [])
  if (find(tman(2:$) - tman(1:$-1) < 0) <> [])
    CL__error("Not increasing maneuver dates"); 
  end
end

if (size(tman,2) <> size(dvman,2))
  CL__error("Invalid size for tman or dvman"); 
end

// only maneuvers >= t0 and <= t($) are considered
I = find(tman < t0 | tman > t($)); 
tman(I) = []; 
dvman(:,I) = []; 

nbman = size(tman,2);

N = size(t, 2); 
pv = zeros(6, N); 

// tlast: latest date processed
// (avoid considering a date twice)
tlast = -%inf; 

// effects of maneuvers
for k = 1 : nbman
  
  I = find(t > tlast & t < tman(k)); 

  pv1 = CL_cw_propagate(t0, pv0, [t(I), tman(k)], alt, acc, er, mu); 
  t0 = tman(k); 
  pv0 = pv1(:,$); 
  pv0(4:6,1) = pv0(4:6,1) + dvman(:,k); 

  if (I <> [])
    pv(:,I) = pv1(:,1:$-1); 
    if (t($) == tman(k))
      pv(:,$) = pv0(:,1); 
    end
    tlast = t(I($)); 
  end

end

// until last date
I = find(t > tlast); 
if (I <> [])
  pv(:,I) = CL_cw_propagate(t0, pv0, t(I), alt, acc, er, mu); 
end

endfunction