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


// ------------------------------------------------------------------
// Solution of kepler's equation (M to E) - Ellipse only
// (EQ)  M = E - e*sin(E)   
// NB: 
// - not supposed to be called directly 
// - no tests on arguments (eccentricity) 
// - e and M must have the same dimension
//
// This algorithm comes from: 
// Procedures for solving Kepler's Equation - A.W. Odell and R.H Gooding - 1986
// ------------------------------------------------------------------
function E = CL__kp_M2Eell(e, M)


  // Real cube root
  // NB: In scilab 5.4.0, a function nthroot is available
  function y = cbrt(x)
    y = sign(x) .* abs(x).^(1/3);
  endfunction
  
  // Compute initial guess according to A. W. Odell and R. H. Gooding S12 starter
  function E = initialGuess(e,reducedM)
    // A,B =  Coefficients to compute Kepler equation solver starter. */
    k1 = 3 * %pi + 2;
    k2 = %pi - 1;
    k3 = 6 * %pi - 1;
    A  = 3 * k2 * k2 / k1;
    B  = k3 * k3 / (6 * k1);

    E = zeros(e);
      
    I = find(abs(reducedM) < 1.0 / 6.0);
    E(I) = reducedM(I) + e(I) .* (cbrt(6 * reducedM(I)) - reducedM(I));

    I = find(abs(reducedM) >= 1.0 / 6.0 & reducedM < 0);
    w = %pi + reducedM(I);
    E(I) = reducedM(I) + e(I) .* (A*w ./ (B - w) - %pi - reducedM(I));

    I = find(abs(reducedM) >= 1.0 / 6.0 & reducedM >= 0);
    w = %pi - reducedM(I);
    E(I) = reducedM(I) + e(I) .* (%pi - A*w ./ (B - w) - reducedM(I));
  endfunction


// Reduce M to [-pi, pi[ interval
reducedM = CL_rMod(M,-%pi,%pi);

// Compute initial guess according to A. W. Odell and R. H. Gooding S12 starter
E = initialGuess(e,reducedM);

e1 = 1 - e;
noCancellationRisk = (e1 + E .* E / 6) >= 0.1;

f = zeros(E);
fd = zeros(E);

// Perform two iterations, each consisting of one Halley step and one Newton-Raphson step
for j = 0:1
  fdd  = e .* sin(E);
  fddd = e .* cos(E);
  
  I = find(noCancellationRisk);
  f(I) = (E(I) - fdd(I)) - reducedM(I);
  fd(I) = 1 - fddd(I);
  
  I = find(~noCancellationRisk);
  // NB: CL__E_esinE is an accurate computation of E-e*sin(E) 
  // for e close  to 1 and E close to 0
  f(I)  = CL__E_esinE(e(I),E(I)) - reducedM(I);
  s = sin(0.5 * E(I));
  fd(I) = e1(I) + 2 * e(I) .* s .* s;
  
  dee = f .* fd ./ (0.5 * f .* fdd - fd .* fd);

  // Update eccentric anomaly, using expressions that limit underflow problems
  w = fd + 0.5 * dee .* (fdd + dee .* fddd / 3);
  fd = fd + dee .* (fdd + 0.5 * dee .* fddd);
  E = E - (f - dee .* (fd - w)) ./ fd;
end

// Expand the result back to original range
E = E + (M - reducedM);

endfunction