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


// --------------------------------------------------------
// ECKSTEIN-HECHLER propagation model (internal function)
// (central force + zonal harmonics: J2..J6)
//
// Orbital elements: circular = [sma, ex, ey, inc, gom, psom]
// t0, t: in days
// all arguments are mandatory
// numres: number of results: 1 (mean) or 2 (mean+osc)
//
// Notes (validity): 
// - error if sma < 0 or inc not in [0, %pi] or ecc not in [0,1[ 
// - %nan returned if ecc > 0.1 or if inc == 0 or %pi or 
// is too close to the critical inclination
//
// Notes - algorithm: 
// - based on: "CNES - MSLIB FORTRAN 90, Volume E (me_eck_hech)"
// - Adaptation (AL) for the orbit to remain perfectly frozen: 
// => Change in ex ~ 4.e-7 (see "MODIF AL 2011-10-10" in code)
// => negligible impact compared to absolute accuracy: about 3 m in LEO 
//
// --------------------------------------------------------

function [mean_cir,osc_cir] = CL__ex_eckHech(t0, mean_cir0, t, er, mu, j1jn, numres)

if (numres <> 1 & numres <> 2)
  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 
if (size(mean_cir0,1) <> 6 | mean_cir0 == [])
  CL__error("Invalid size of orbital elements");
end

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

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

I = find(abs(mean_cir0(4,:) - critinc1) < eps_icrit | ..
         abs(mean_cir0(4,:) - critinc2) < eps_icrit | ..
         mean_cir0(2,:).^2 + mean_cir0(3,:).^2 > 0.01 | ..
         mean_cir0(4,:) <= 2*%eps | mean_cir0(4,:) >= %pi-2*%eps); 
if (I <> [])
  mean_cir0 = mean_cir0; // creates local variable
  mean_cir0(:,I) = %nan; // will be "propagated"
end

// Check j1jn
// Ensures that j1jn has at least 6 elements (fills with zeros)
j1jn = [matrix(j1jn, 1, -1), zeros(1,6)];
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, mean_cir0, N1] = CL__checkInputs(t0, 1, mean_cir0, 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
mean_cir = [];
osc_cir = [];

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

// initializes to 0 (if necessary only)
mean_cir = zeros(6, N);
if (numres >= 2); osc_cir = zeros(6, N); end


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

cn0 = -j1jn;

a0   = mean_cir0(1,:);
ex0  = mean_cir0(2,:);   // e cos(w)
ey0  = mean_cir0(3,:);   // e sin(w)
i0   = mean_cir0(4,:);
om0  = mean_cir0(5,:);
l0   = mean_cir0(6,:);   // w + M

// preliminary computations: 
// gi = cn0(i) * (%CL_eqRad / a0)**i
// bi = sin(i0)^i

q  = er ./ a0;

c  = cos(i0);
b1 = sin(i0);
b2 = b1 .^2;
b4 = b1 .^4; 
b6 = b1 .^6; 

g2 = cn0(2) * q.^2;
g3 = cn0(3) * q.^3;
g4 = cn0(4) * q.^4;
g5 = cn0(5) * q.^5;
g6 = cn0(6) * q.^6;

xnot  = sqrt(mu./(a0.^3)) .* (t-t0)*86400;  

// semi major axis
am = a0;

// eccentricity vector
delta_pom  = -0.75 .* g2 .* (4 - 5 * b2);
delta_pomp = 7.5 * g4 .* (1 - (31/8) * b2 + (49/16) * b4) - ...
             13.125 * g6 .* (1 - 8 * b2 + (129/8) * b4 - (297/32) * b6);
x          = (delta_pom + delta_pomp) .* xnot; 
cx         = cos(x); 
sx         = sin(x); 
q          = (3/32) ./ delta_pom;
eps1       = q .* g4 .* b2 .* (30 - 35 * b2) - 175 * q .* g6 .* b2 .* ...
             (1 - 3 * b2 + (33/16) .* b4);
q          = (3/8) * b1 ./ delta_pom;
eps2       = q .* g3 .* (4 - 5 * b2) - q .* g5 .* (10 - 35 * b2 + 26.25 * b4);

// MODIF AL 2011-10-10 : eps1*eps2 removed from term in sx in exm 
// so that eccentricity vector of frozen orbit is perfectly constant
//
// *** PREVIOUS VERSION:  
// exm = ex0 .* cx - (1 - eps1) .* ey0 .* sx + eps2 .* sx; 
// *** MODIFIED VERSION:  
exm   = ex0 .* cx - (1 - eps1) .* (ey0 - eps2) .* sx; 
// END MODIF AL 2011-10-10 

eym   = (1 + eps1) .* ex0 .* sx + (ey0 - eps2) .* cx + eps2; 

// inclination
im = i0;

// RAAN
q   = 1.5 * g2 - 2.25 * (g2.^2) .* (2.5 - (19/6) * b2) + ...
      (15/16) * g4 .* (7 * b2 - 4) + ...
      (105/32) * g6 .* (2 - 9 * b2 + (33/4) * b4);
omm = om0 + q .* c .* xnot; 

// arg. of latitude
delta_l = 1 - 1.5 * g2 .* (3 - 4 * b2);
q   = delta_l + 2.25 * (g2.^2) .* (9 - (263/12) * b2 + (341/24) * b4) + ...
      (15/16) * g4 .* (8 - 31 * b2 + 24.5 * b4) + ...
      (105/32) * g6 .* (-(10/3) + 25 * b2 - 48.75 * b4 + 27.5 .* b6);
lm  = l0 + q .* xnot; 


// periodic terms
// ..................

// cli = cos(i*lm)  sli = sin(i*lm)
cl = cos([lm; 2*lm; 3*lm; 4*lm; 5*lm; 6*lm]); 
sl = sin([lm; 2*lm; 3*lm; 4*lm; 5*lm]); 

qq = -1.5 * g2 ./ delta_l;
qh =  (3/8) * (eym - eps2) ./ delta_pom; 
ql =  (3/8) * exm ./ b1 ./ delta_pom; 

// mean elements at final time
mean_cir = [ am .* ones(t); exm; eym; im .* ones(t); omm; lm];


// -------------------------------------------------
// Model for osculating elements
// -------------------------------------------------
if (numres >= 2)

  // => sma
  // effect of j2
  f       = (2 - 3.5 * b2) .* exm .* cl(1,:) + (2 - 2.5 * b2) .* eym .* sl(1,:) + ...
            b2 .* cl(2,:) + 3.5 * b2 .* (exm .* cl(3,:) + eym .* sl(3,:));
  delta_a = qq .* f;

  // effect of j2^2
  q       = 0.75 * (g2.^2) .* b2;
  f       = 7 * (2 - 3 * b2) .* cl(2,:) + b2 .* cl(4,:);
  delta_a = delta_a + q .* f;

  // effect of j3
  q       = -0.75 * g3 .* b1;
  f       = (4 - 5 * b2) .* sl(1,:) + (5/3) * b2 .* sl(3,:);
  delta_a = delta_a + q .* f;

  // effect of j4
  q       = 0.25 * g4 .* b2;
  f       = (15 - 17.5 * b2) .* cl(2,:) + 4.375 * b2 .* cl(4,:);
  delta_a = delta_a + q .* f;

  // effect of j5
  q       = 3.75 * g5 .* b1;
  f       = (2.625 * b4 - 3.5 * b2 + 1) .* sl(1,:) + ...
            (7/6) * b2 .* (1 - 1.125 * b2) .* sl(3,:) + (21/ 80) * b4 .* sl(5,:);
  delta_a = delta_a + q .* f;

  // effect of j6
  q       = (105/16) * g6 .* b2;
  f       = (3 * b2 - 1 - (33/16) * b4) .* cl(2,:) + ...
            0.75 * (1.1 * b4 - b2) .* cl(4,:) - (11/80) * b4 .* cl(6,:);
  delta_a = delta_a + q .* f;

  // => ex
  // effect of j2
  f        = (1 - 1.25 * b2) .* cl(1,:) + 0.5 * (3 - 5 * b2) .* exm .* cl(2,:) + ...
             (2 - 1.5 * b2) .* eym .* sl(2,:) + (7/12) * b2 .* cl(3,:) + ...
             (17/8) * b2 .* (exm .* cl(4,:) + eym .* sl(4,:));
  delta_ex = qq .* f;

  // => ey
  // effect of j2
  f        = (1 - 1.75 * b2) .* sl(1,:) + (1 - 3 * b2) .* exm .* sl(2,:) + ...
             (2 * b2 - 1.5) .* eym .* cl(2,:) + (7/12) * b2 .* sl(3,:) + ...
             (17/ 8) * b2 .* (exm .* sl(4,:) - eym .* cl(4,:));
  delta_ey = qq .* f;

  // => RAAN
  // effect of j2
  q         = -qq .* c;
  f         = 3.5 * exm .* sl(1,:) - 2.5 * eym .* cl(1,:) - 0.5 * sl(2,:) + ...
              (7/6) * (eym .* cl(3,:) - exm .* sl(3,:));
  delta_gom = q .* f;

  // effect of j3
  f         = g3 .* c .* (4 - 15 * b2);
  delta_gom = delta_gom + ql .* f;

  // effect of j5
  f         = 2.5 * g5 .* c .* (4 - 42 * b2 + 52.5 * b4);
  delta_gom = delta_gom - ql .* f;

  // => inclination
  // effect of j2
  q       = 0.5 * qq .* b1 .* c;
  f       = eym .* sl(1,:) - exm .* cl(1,:) + cl(2,:) + ..
            (7/3) * (exm .* cl(3,:) + eym .* sl(3,:));
  delta_i = q .* f;

  // effect of j3
  f       = g3 .* c .* (4 - 5 * b2);
  delta_i = delta_i - qh .* f;

  // effect of j5
  f       = 2.5 * g5 .* c .* (4 - 14 * b2 + 10.5 * b4);
  delta_i = delta_i + qh .* f;

  // => arg of latitude
  // effect of j2
  f        = (7 - (77/8) * b2) .* exm .* sl(1,:) + ...
             ((55/8) * b2 - 7.5) .* eym .* cl(1,:) + ...
             (1.25 * b2 - 0.5) .* sl(2,:) + ...
             ((77/24) * b2 - (7/6)) .* (exm .* sl(3,:) - eym .* cl(3,:));
  ddelta_l = qq .* f;

  // effect of j3
  f        = g3 .* (53 * b2 - 4 - 57.5 * b4);
  ddelta_l = ddelta_l + ql .* f;

  // effect of j5
  f        = 2.5 * g5 .* (4 - 96 * b2 + 269.5 * b4 - 183.75 * b6);
  ddelta_l = ddelta_l + ql .* f;

  // Osculating elements
  ex = exm + delta_ex;
  ey = eym + delta_ey;
  om = omm + delta_gom;
  xi = im + delta_i;
  xl = lm + ddelta_l;

  osc_cir = [ am .* (1  + delta_a); ex; ey; xi; om; xl];
  osc_cir(5:6,:) = CL_rMod(osc_cir(5:6,:), mean_cir(5:6,:)-%pi, mean_cir(5:6,:)+%pi); 
  
end

endfunction