#include <stdint.h>
#include <math.h>

/*
 * We have:
 *     E = M + 2*\sum_{s=1}^{\infty} 1/s * J_s(se)*sin(s*M)
 * J_n(nx) = (0.5*nx)^n/n! * \sum_{m=0}^{\infty} (-1)^m \frac{(0.25*n*n*x*x)^m}{m!(m+n)!}
 * J1(  e) = e*(1 - 0.125*e^2 + e^4/3*64 - e^6/9*1024)
 * J2(2*e) = 0.5*e^2*(1 - e^2/3 + e^4/24)
 * J3(3*e) = 3*e^3/8*(1 - 9e^2/16 + 9*e^4/5*128)
 * J4(4*e) = e^4/3
 * J5(5*e) = 125*e^5/256*3
 */

double EquationOfCenter(double M, double e)
{
  double e2 = e*e;
  double C = e*(   (1 + 0.5*e2*(-0.25 + e2*(0.0520833333 - 0.000217013889*e2)))*sin(M) +
                e *(0.5 + e2*(-0.16666666666667 + 0.0208333333333*e2))*sin(2*M) +
                //e2*(0.375 - 0.2109375*e2) e2*(0.375 - 0.28125*e2)
                e2*(0.375 - 0.2109375*e2)*sin(3*M) +
                0.333333333333333*e2*(0.5*e*sin(4*M) + 0.4*0.48828125*e2*sin(5*M)));
  return M+C;
}

double SolveKepler(double M, double eccentricity)
{
    // Solve Kepler's equation iteratively (Duffett-Smith, p.90)
    double delta, E = M;
    do {
        delta = E - eccentricity * sin(E) - M;
        E = E - delta / (1 - eccentricity * cos(E));
    } while (fabs(delta) > 5e-8); // epsilon = 1e-5 rad
    return E;
}