//  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 [X] = CL__3b_formatOrbits(Xtref, t, MU)
// Author
// 12/02/2009 A. Blazquez (CNES, DCT/SB/MO) - initial version
// 16/11/2013 AL - rewrite (simpler and more robust)

// Recomputation of an orbit (Lissajous ou halo for example), 
// at each time t, knowing the stabilized orbit Xtref 
// Inputs: 
// Xtref: stabilized orbit (7xN) = [pvref; tref]
// t: times at which the orbit is recomputed
// Outputs: 
// X: position/velocity (6xN) corresponding to each time "t". 
// NB:
// Time must be monotonous (same variation for ref. time and t)
// %nan is returned if X cannot be computed. 


function [sgn] = tb_format_check(tref, t)
// check time vectors: must be both increasing or decreasing
// returns variation direction (1=increasing or -1=decreasing)

  nref = size(tref, 2); 
  n = size(t, 2); 
  
  dtref = 0; 
  dt = 0; 
  if (nref > 1); dtref = tref(2:$)-tref(1:$-1); end
  if (n > 1); dt = t(2:$)-t(1:$-1); end

  bref1 = find(dtref > 0) <> []; 
  bref2 = find(dtref < 0) <> []; 
  b1 = find(dt > 0) <> []; 
  b2 = find(dt < 0) <> []; 

  if ((bref1 & bref2) | (b1 & b2) | (bref1 & b2) | (bref2 & b1))
    CL__error("Invalid times"); 
  end
  
  if (bref1)
    sgn = 1; 
  elseif (bref2)
    sgn = -1; 
  elseif (b1)
    sgn = 1; 
  elseif (b2)
    sgn = -1; 
  else
    // only one ref time and only one propagation time
    sgn = 1; 
    if (t(1) < tref(1)); sgn = -1; end
  end
  
endfunction


n = size(t,2); 
X = ones(6, n) * %nan; 

// tolerance for considering that 2 times are identical
eps = 1.e-14; 

tref = Xtref(7,:); 
Xref = Xtref(1:6,:); 
nref = size(Xtref, 2); 

// Determines whether time is increasing or decreasing
sgn = tb_format_check(tref, t); 

// +/- * %inf is added but is not used except in the comparison with t
tref = [tref, sgn * %inf]; 
Xref = [Xref, %nan * ones(6,1)]; 

// integrated function
f = CL__3b_RHS; 

// For each time t, propagate from the nearest reference time. 
// Considers all ref. time intervals
for k = 1 : nref

  if (sgn > 0)
    I = find(t >= tref(k) & t < tref(k+1)); 
  else
    I = find(t <= tref(k) & t > tref(k+1)); 
  end
  
  if (I == []); continue; end

  tI = t(I); 
  // adjustment of 1st time as may cause problems 
  // with ode if times are too close
  if (abs(tI(1) - tref(k)) < eps); tI(1) = tref(k); end

  XI = CL__3b_integratorFix(Xref(:,k), tref(k), tI, f, MU);
  X(:,I) = XI; 

end

endfunction