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


// --------------------------------------------------------
// LYDDANE propagation model (internal function)
// (central force + zonal harmonics: J2..J5)
//
// all orbital elements: keplerian = [sma; ecc; inc; pom; gom; anm]
// t0, t: in days
// all arguments are mandatory
//
// numres: number of results: 1, 2, 3
// numres = 1 => compute secular elements (always the case)
// numres = 2 => compute secular+mean elements 
// numres = 3 => compute secular+mean+osculating elements
//
// Notes:
// secu: evolution proportional to time
// mean: secu + long periods 
// osc: mean + short periods
//
// Note (validity) : 
// - error if sma < 0 or inc not in [0, %pi] or ecc not in [0,1[ 
// - %nan returned if eccentricity > 0.9 or inc >= %pi
// - %nan returned if inc is too close to critical inclinations
// 
// Notes - algorithm: 
// - based on "CNES - MSLIB FORTRAN 90, Volume E (me_lyddane)"
// - correction made to long-periods terms (J5)
// - adaptation to add mean elements in addition to secular elements
// 
// NB: different formulas in osculating elements below and
// above eccentricity = 0.05 => discontinuity
// => causes pb in computation of mean elements from osculating
// ones (algo may not converge)
// --------------------------------------------------------

function [secu_kep, mean_kep, osc_kep] = CL__ex_lyddane(t0, secu_kep0, t, er, mu, j1jn, numres)

if (numres <> 1 & numres <> 2 & numres <> 3)
  CL__error("Invalid ''numres'' argument");
end

// Check number of output arguments (optimization)
lhs = argn(1);
numres = min(numres, lhs); 

// Check number of rows to avoid error in next lines
if (size(secu_kep0,1) <> 6 | secu_kep0 == [])
  CL__error("Invalid size of orbital elements");
end

// Inputs checking (sma, ecc, inc) => error
I = find(secu_kep0(1,:) <= 0 | ..
         secu_kep0(2,:) < 0 | secu_kep0(2,:) >= 1 | ..
         secu_kep0(3,:) < 0 | secu_kep0(3,:) > %pi); 
if (I <> [])
  CL__error("Invalid orbital elements");
end

// Inputs checking (ecc, inc) => %nan
// NB: margin for critical inclination: experimental value
// (%nan set to input arguments => will be propagated to outputs)
critinc1 = asin(2/sqrt(5));
critinc2 = %pi - critinc1;
eps_icrit = 4.e-5; 

I = find(abs(secu_kep0(3,:) - critinc1) < eps_icrit | ..
         abs(secu_kep0(3,:) - critinc2) < eps_icrit | ..
         secu_kep0(3,:) >= %pi - 2*%eps | ..
         secu_kep0(2,:) > 0.9); 
if (I <> [])
  secu_kep0 = secu_kep0; // creates local variable
  secu_kep0(:,I) = %nan; 
end

// Check j1jn
// Ensures that j1jn has at least 5 elements (fills with zeros)
j1jn = [matrix(j1jn, 1, -1), zeros(1,5)];
if (j1jn(2) == 0)
  CL__error("Invalid value for J2 (should not be 0).");
end

// Check arguments sizes
// (No uncessary resizing of initial orbital elements)
[t0, secu_kep0, N1] = CL__checkInputs(t0, 1, secu_kep0, 6); 
[t, N2] = CL__checkInputs(t, 1); 

if ~(N1 == 1 | N2 == 1 | N1 == N2)
   CL__error("Wrong size of input arguments"); 
end
N = max(N1,N2); 

// Default outputs
secu_kep = [];
mean_kep  = [];
osc_kep = [];

// Empty final times (and no error) => []
if (t == []) 
  return; // <= RETURN
end

// initializes to 0 (if necessary only)
secu_kep = zeros(6, N);
if (numres >= 2); mean_kep = zeros(6, N); end
if (numres >= 3); osc_kep = zeros(6, N); end


// ======================
// Model start
// ======================

// Eccentricity threshold 
eccel = 0.05

q = er ./ secu_kep0(1,:); 
g2 = j1jn(2) * (q.^2) / 2;

e2 = secu_kep0(2,:).^2;
tt1 = sqrt(1-e2);

h2 = g2 ./ (tt1.^4);
h3 = -j1jn(3) * (q.^3) ./ (tt1.^6);
h4 = -j1jn(4) * (q.^4) * (3 / 8) ./ (tt1.^8);
h5 = -j1jn(5) * (q.^5) ./ (tt1.^10);

ant = sqrt(mu ./ secu_kep0(1,:).^3) .* (t-t0) * 86400;

c1 = cos(secu_kep0(3,:));
dc = 1 - 5 * (c1.^2);

s1 = sin(secu_kep0(3,:));

r1 = 4 + 3 * e2;
r2 = 4 + 9 * e2;
r4 = e2 .* (c1.^6) ./ dc.^2;


// -------------------------------------------------
// Model for secular elements
// (effects of J2, J2^2, J4)
// -------------------------------------------------

// Secular effects on argp, raan et mean anomaly
// Warning: function uses "global" variables
// (function only useful to hide temporary variables)
function [po1,go1,am1] = secu_drift_lyd()
  // argument of perigee: po1
  h22 = -35 + 24*tt1 + 25*(tt1.^2) + (90-192*tt1-126*(tt1.^2)) .* (c1.^2)+(385+360*tt1+45*(tt1.^2)) .* (c1.^4);
  h41 = 21 - 9*(tt1.^2)+(-270 + 126*(tt1.^2)) .* (c1.^2) + (385 - 189*(tt1.^2)) .* (c1.^4);
  po1 = secu_kep0(4,:) + ant .* (1.5 * h2 .* (-dc + h2 .* h22/16) + (5/16) * h4 .* h41); 

  // raan: go1 :
  h22 = (-5 + 12*tt1 + 9 * (tt1.^2)) .* c1 + (-35 -36*tt1 -5*(tt1.^2)) .* (c1.^3);
  h41 = (5 - 3*(tt1.^2)) .* c1 .* (3 - 7*(c1.^2));
  go1 = secu_kep0(5,:) + ant .* (3 * h2 .* (-c1 + h2 .* h22/8) + 5 * h4 .* h41/4);    

  // mean anomaly: am1
  h22 = -15 + 16*tt1 + 25*(tt1.^2) + (30 - 96*tt1 - 90*(tt1.^2)) .* (c1.^2) + (105 + 144*tt1 + 25 *(tt1.^2)) .* (c1.^4);
  h41 = (3 - 30*(c1.^2) + 35*(c1.^4));
  am1 = secu_kep0(6,:) + ant .* (1 + 1.5 * h2 .* (tt1 .* (-1 + 3*(c1.^2)) + h2 .* tt1 .* h22/16) + 15 * h4 .* tt1 .* e2 .* h41/16);

endfunction

[po1,go1,am1] = secu_drift_lyd();


// Secular elements final time

secu_kep(1,:) = secu_kep0(1,:); 
secu_kep(2,:) = secu_kep0(2,:); 
secu_kep(3,:) = secu_kep0(3,:); 

// only pom, gom and anm vary
secu_kep(4,:) = po1;  // g '' in publication
secu_kep(5,:) = go1;  // h '' in publication
secu_kep(6,:) = am1;  // l'' in publication
am1 = CL_rMod(am1,2 * %pi);


// -------------------------------------------------
// Model for mean/osculating elements
// -------------------------------------------------

if (numres >= 2)
  
  cdai = cos(0.5 * secu_kep0(3,:));
  sdai = sin(0.5 * secu_kep0(3,:));

  //  long period terms in J2,J3,J4,J5

  cc = 2 * (c1.^2) ./ dc;
  q0 = 16 * ((c1.^2) ./ dc) + 40 * ((c1.^4) ./ dc.^2);

  a = 0.5 * (1- 5 .* cc) .* s1 .* h2 - 5 * (1- cc) .* s1 .* h4 ./ (3 .* h2);

  // CORRECTION VM: eta^2 in numerator and not denominator...
  // OLD VERSION: 
  // b = ((tt1.^2) .* h3 + (5/16) * r1 .* (3 * (1- cc) .* s1.^2 - 2) ./ (tt1.^2) .* h5) ./ h2;
  b = ((tt1.^2) .* h3 + (5/16) * r1 .* (3 * (1- cc) .* s1.^2 - 2) .* (tt1.^2) .* h5) ./ h2;
  c = (35/32) * h5 .* ((1- 2 * cc) .* s1.^2) ./ (3 * h2);

  dai2 = secu_kep0(2,:) .* (-a .* cos(2 * po1) + secu_kep0(2,:) .* c .* sin(3 * po1));
  //delta2 e (1)
  dex2 = 0.25 * (-(tt1.^2) .* dai2 + b .* sin(po1)) .* s1;   
  // delta i2 (2)
  dai2 = 0.25 * (dai2 - b .* sin(po1) ./ (tt1.^2)) .* (secu_kep0(2,:) .* c1);  

  edm2 = - h3 - (5/16) * r2 .* h5 .* (3 * (1- cc) .* s1.^2 - 2);
  edm2 = edm2 .* cos(po1) ./ h2 + secu_kep0(2,:) .* a .* sin(2 * po1);
  edm2 = edm2 + e2 .* c .* cos(3 * po1);
  // edl (3)
  edm2 = 0.25 * edm2 .* (tt1.^3) .* s1; 

  a = -0.5 * (11+5 * q0) .* h2 + 5 * (3+q0) .* h4 ./ (3 * h2);
  a = (secu_kep0(2,:) .* s1) .* a .* sin(2 * po1);
  b = (5/8) * r1 .* (0.5 * (3 .* (1- cc) .* s1.^2 - 2) + 3 * (s1.^2) .* (3+q0));
  b = (h3 + b .* h5) .* cos(po1) ./ h2;
  c = (35/16) * (0.5 * ((1- 2 * cc) .* s1.^2) + (s1.^2) .* (5+2 * q0)) .* h5 ./ (9 * h2);
  c = e2 .* c .* cos(3 * po1);

  // sin i d gom 2
  sidg2 = 0.25 * (secu_kep0(2,:) .* c1) .* (a+b-c); 

  a = c1 ./ (1+c1);
  b = tt1 ./ (1+tt1);
  c = tt1 + 1 ./ (1+tt1);

  som1 = -(c+2.5) .* (1- 5 .* cc) .* s1.^2 - c1 .* (11+5 * q0) - 11 * (c1.^2);
  som1 = (som1 + 2) .* e2 + 200 * r4;
  som2 = (c+2.5) .* (1- cc) .* s1.^2 + c1 .* (3+q0) + 3 * (c1.^2);
  som2 = (som2 -2) .* e2 - 40 * r4;
  som3 = (a+c) .* (secu_kep0(2,:) .* s1);
  som4 = (a + tt1 .* b) .* r1;
  som4 = som4 + e2 .* (9 + 6 * c) +20;
  som4 = som4 .* (3 * (1- cc) .* s1.^2 - 2) + 6 * r1 .* c1 .* (1-c1) .* (3+q0);
  som4 = (secu_kep0(2,:) .* s1) .* som4;
  som5 = -0.5 * ((1- 2 * cc) .* s1.^2) .* (a + 3 * c + 2);
  som5 = (som5 - c1 .* (1-c1) .* (5+2 * q0)) .* (secu_kep0(2,:) .* s1) .* e2;

  pgm2 = 0.125 * (h2 .* som1 + 10 * h4 .* som2 ./ (3 * h2)) .* sin(2 * po1);
  pgm2 = pgm2 + (0.25 * h3 .* som3 + (5/64) * h5 .* som4) .* cos(po1) ./ h2;
  // pom gom anom 2
  pgm2 = pgm2 + (35/64) * h5 .* som5 .* cos(3 * po1) ./ (9 * h2);  
  // pgm2 includes secular + long periods
  pgm2 = pgm2 + (po1+go1+am1);   

  // -------------------------------------------------
  // Changes / initial algorithm for long period + 
  // osculating effects
  // -------------------------------------------------

  // No short periods => no term in edm3
  // (edm = e''dl dans le papier)
  edm = edm2; 
  //  No short periods => no term in sidg3 
  // (sidg = sin i''. dh  dans le papier)
  sidg = sidg2; 
  //only add long period (ede = e'' + de in publication)
  ede = secu_kep0(2,:) + dex2; 

  // mean anomaly including LP
  am2 = CL__sc2angle(ede .* cos(am1) - edm .* sin(am1) , ede .* sin(am1) + edm .* cos(am1)); 

  // term in dai2 only 
  // (dai2 = delta2 i in publication, computes term "di" (p33))
  sidi = sdai + 0.5 * (dai2) .* cdai; 
  // identity: sin(2x) = 2sin x cos x !
  sidg = 0.5 * sidg ./ cdai; 

  // gom including LP
  go2 = CL__sc2angle(sidi .* cos(go1) - sidg .* sin(go1) , sidi .* sin(go1) + sidg .* cos(go1)); 

  
  // "mean" elements
  // No long periods on sma
  mean_kep(1,:) = (secu_kep0(1,:)); 
  mean_kep(2,:) = sqrt(ede .* ede + edm .* edm);
  mean_kep(3,:) = 2 * asin(sqrt(sidi .* sidi + sidg .* sidg));
  // => (pom+gom+anom) - gomLP - anomLP
  mean_kep(4,:) = pgm2 - go2 - am2; 
  mean_kep(5,:) = go2;
  mean_kep(6,:) = am2;

  mean_kep(4:6,:) = CL_rMod(mean_kep(4:6,:), secu_kep(4:6,:)-%pi, secu_kep(4:6,:)+%pi); 

end 

if (numres >= 3)
  
  // Short periods (J2)
  am2 = am1; // will be modifiedif e > 0.05 (=eccel)
  ecc2 = secu_kep0(2,:);
  po2 = po1;

  x = zeros(am1);
  y = zeros(am1);
  ede2 = zeros(am1);
  gom2 = zeros(am1);
  aux = zeros(am1);
  auxi = zeros(am1);
  mkt12 = secu_kep0(2,:);
  
  // special conditions for eccentricity
  I = find(secu_kep0(2,:) > eccel);

  // if input orbital elements = one column 
  // => enlargment of some variables to max size 
  
  if (size(secu_kep0,2) == 1 & I ~= [])
    I = 1 : length(t);
    mkt12 = mkt12 * ones(t);
    sdai = sdai * ones(t);
    cdai = cdai * ones(t);
  end
    
  ede2(I) = mkt12(I) + dex2(I);
  x(I) = ede2(I) .* cos(am1(I)) - edm2(I) .* sin(am1(I));
  y(I) = ede2(I) .* sin(am1(I)) + edm2(I) .* cos(am1(I));
  am2(I) = CL__sc2angle(x(I),y(I));
    
  ecc2(I) = sqrt(x(I).^2 + y(I).^2);
  aux(I) = sdai(I) + 0.5 * dai2(I) .* cdai(I);
  auxi(I) = 0.5 * sidg2(I) ./ cdai(I);
  x(I)=aux(I) .* cos(go1(I)) - auxi(I) .* sin(go1(I));
  y(I)=aux(I) .* sin(go1(I)) + auxi(I) .* cos(go1(I));

  gom2(I) = CL__sc2angle(x(I),y(I));

  po2(I) = pgm2(I) - gom2(I) - am2(I);

  // end I
  
  e = CL_kp_M2E(ecc2,am2);
  v = CL__sc2angle(cos(e) - ecc2 , sqrt(1 - ecc2 .* ecc2) .* sin(e));
  xc = (3 * (1+secu_kep0(2,:) .* cos(v)) + e2 .* cos(v).^2) .* cos(v);

  dax3 = secu_kep0(2,:) .* (-1+3 * (c1.^2)) .* (xc+secu_kep0(2,:) .* c) + (1 + secu_kep0(2,:) .* xc) .* (3 * (s1.^2) .* cos(2 * po2+2 * v));
  dax3 = secu_kep0(1,:) .* dax3 .* g2 ./ (tt1.^6);

  de1 = ((-1+3 * (c1.^2)) .* (xc+secu_kep0(2,:) .* c) + (3 * (s1.^2) .* cos(2 * po2+2 * v)) .* (xc+secu_kep0(2,:))) ./ (tt1.^6);
  de2 = cos(2 * po2+3 .* v) + 3 * cos(2 * po2+v);

  dex3 = 0.5 * (tt1.^2) .* (de1 .* g2 - de2 .* (s1.^2) .* h2);

  dai3 = (secu_kep0(2,:) .* de2 + 3 * cos(2 * po2+2 * v)) / 2;
  dai3 = c1 .* s1 .* dai3 .* h2;

  a1 = 1+secu_kep0(2,:) .* cos(v);
  a2 = a1 .* (1+a1) ./ (tt1.^2);
  d1 = 2 * (3 * (c1.^2)-1) .* (1+a2) .* sin(v);
  d1 = d1+(s1.^2) .* (3 * (1-a2) .* sin(2 * po2+v) + (3 * a2+1) .* sin(2 * po2+3 * v));

  d21 = 6 * (v + secu_kep0(2,:) .* sin(v) - am2);
  d22 = 3 * sin(2 * po2+2 * v) + secu_kep0(2,:) .* (3 .* sin(2 * po2+v) + sin(2 * po2+3 * v));

  a1 = 5 * (c1.^2) - 2 * c1 -1;

  dpgm3 = secu_kep0(2,:) .* b .* tt1 .* d1 + a1 .* d21 + (2-a1) .* d22;
  dpgm3 = 0.25 * dpgm3 .* h2;

  // The second term is in fact "edm3"
  // edm = edm2 + - 0.25 * (tt1.^3) .* d1 .* h2; 
  // The second term is in fact "sidg3"; cl = cos i0  sl = sin i0
  // sidg = sidg2 + 0.5 * c1 .* s1 .* (d22-d21) .* h2; 
  
  // Re-written compared to original
  edm3 = - 0.25 * (tt1.^3) .* d1 .* h2;
  edm = edm2 + edm3;
  
  sidg3 = 0.5 * c1 .* s1 .* (d22-d21) .* h2;
  sidg = sidg2 + sidg3;

  ede = secu_kep0(2,:) + dex2 + dex3;

  am3 = CL__sc2angle(ede .* cos(am1) - edm .* sin(am1) , ede .* sin(am1) + edm .* cos(am1));

  // sdai = sin(i0/2); cdai = cos(i0/2); 
  sidi = sdai + 0.5 * (dai2 + dai3) .* cdai 
  sidg = 0.5 * sidg ./ cdai;  

  go3 = CL__sc2angle(sidi .* cos(go1) - sidg .* sin(go1) , sidi .* sin(go1) + sidg .* cos(go1)); 

  // Osculating elements

  osc_kep(1,:) = (secu_kep0(1,:) + dax3); 
  osc_kep(2,:) = sqrt(ede .* ede + edm .* edm);
  osc_kep(3,:) = 2 * asin(sqrt(sidi .* sidi + sidg .* sidg));
  osc_kep(4,:) = pgm2 + dpgm3 - go3 - am3;
  osc_kep(5,:) = go3;
  osc_kep(6,:) = am3;

  osc_kep(4:6,:) = CL_rMod(osc_kep(4:6,:), secu_kep(4:6,:)-%pi, secu_kep(4:6,:)+%pi); 

end 


endfunction