// gcc c.c -Wall -std=c99 -lm
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#define C const
typedef long double R;
static R pi = 3.14159265358979323846L;
typedef struct{R x, y, t;} sl;
typedef struct{R x[3], xd[3];} co;
static R fm(R a, R m){return a - m*floorl(a/m);}
static co Kepler(R e, R L){sl w = {1, 0, 0};
  static R Cos[64], Sin[64];
  {static char first = 1;
    if(first){R b = 4; first = 0;
      for(int j=0; j<64; ++j) {Cos[j] = cosl(b); Sin[j] = sinl(b); b /=2;}}}
  R a = 1/(1 - e*e), ys = sqrtl(a), ts = 1/(a*ys);
  R M = fm(L, 2*pi);
  {R dt = 4; for(int j=0; j<64; ++j){sl q =
      {w.x*Cos[j] - w.y*Sin[j], w.x*Sin[j] + w.y*Cos[j], w.t + dt};
    dt *= .5; if(q.t - e*q.y < M) w = q;}}
  assert(fabsl((w.t - e*sinl(w.t)) - M) < 1.e-18);
  {R sp = ts/(1 - e*w.x);
    return (co){{(w.x - e)*a, w.y*ys, 0}, {-w.y*a*sp, w.x*ys*sp, 0}};}}
void pco(char * m, co x){printf("co %s:\n", m);
   for(int j=0; j<3; ++j) printf("%15.12Lf %15.12Lf\n", x.x[j], x.xd[j]);}

//typedef struct{R a, e, i, Omega, omega, L;} elm;
typedef R elm[6];
co trco(R m[3][3], co a){
  typedef struct{R m[3];} zz;
  zz mm(R y[3]){zz z; for(int k=0; k<3; ++k){
  R s=0; for(int j=0; j<3; ++j) s += y[j]*m[k][j]; z.m[k] = s;}
  return z;}
 {zz z = mm(a.x), zd = mm(a.xd); co c;
  for(int j=0; j<3; ++j) {c.x[j] = z.m[j]; c.xd[j] = zd.m[j];} return c;}}

static co initp(elm p){
R cpi = pi/180;
co n = Kepler(p[1], (p[5] - p[4])*cpi);
R op = p[4]*cpi, Om = p[3]*cpi, in = p[2]*cpi, e = p[1], A = p[0]*(1-e*e);
R uo[3][3] = {{cosl(op), -sinl(op), 0},{sinl(op), cosl(op), 0},{0,0,1}};
R ui[3][3] = {{cosl(in), 0, -sinl(in)}, {0,1,0}, {sinl(in), 0, cosl(in)}};
R uO[3][3] = {{cosl(Om), -sinl(Om), 0},{sinl(Om), cosl(Om), 0},{0,0,1}};
R ua[3][3] = {{A, 0, 0}, {0, A, 0}, {0, 0, A}};
R P = 1/sqrtl(p[0]*p[0]*p[0]); // 1/(period of orbit in years).
for(int j=0; j<3; ++j) n.xd[j] *= P;
return trco(ua, trco(uO, trco(ui, trco(uo, n))));}
#define np 9
static elm planets[np] = {
{ 0.38709893, 0.20563069,  7.00487,  48.33167,  77.45645, 252.25084},
{ 0.72333199, 0.00677323,  3.39471,  76.68069, 131.53298, 181.97973},
{ 1.00000011, 0.01671022,  0.00005, -11.26064, 102.94719, 100.46435},
{ 1.52366231, 0.09341233,  1.85061,  49.57854, 336.04084, 355.45332},
{ 5.20336301, 0.04839266,  1.30530, 100.55615,  14.75385,  34.40438},
{ 9.53707032, 0.05415060,  2.48446, 113.71504,  92.43194,  49.94432},
{19.19126393, 0.04716771,  0.76986,  74.22988, 170.96424, 313.23218},
{30.06896348, 0.00858587,  1.76917, 131.72169,  44.97135, 304.88003},
{39.48168677, 0.24880766, 17.14175, 110.30347, 224.06676, 238.92881}};
co state[np+1];
static elm planetsD[np] = {
 { 0.00000066,  0.00002527, -23.51,   -446.30,  573.57, 538101628.29},
 { 0.00000092, -0.00004938,  -2.86,   -996.89, -108.80, 210664136.06},
 {-0.00000005, -0.00003804, -46.94, -18228.25, 1198.28, 129597740.63},
 {-0.00007221,  0.00011902, -25.47,  -1020.19, 1560.78,  68905103.78},
 { 0.00060737, -0.00012880,  -4.15,   1217.17,  839.93,  10925078.35},
 {-0.00301530, -0.00036762,   6.11,  -1591.05,-1948.89,   4401052.95},
 { 0.00152025, -0.00019150,  -2.09,  -1681.40, 1312.56,   1542547.79},
 {-0.00125196,  0.0000251,   -3.64,   -151.25, -844.43,    786449.21},
 {-0.00076912,  0.00006465,  11.07,    -37.33, -132.25,    522747.90}};
 
static void ex(char * s){exit(printf("dying: %s\n", s));}
static void init(){if(sizeof(planets)/sizeof(elm) != np) ex("nxp");
  for(int j = 0; j<np; ++j) state[j+1] = initp(planets[j]);}

R sq(R q){return q*q;}
int main(){
  { // Demonstrate 1/r^2 acceleration towards origin.
   void ry(R e){R dL = 1e-6, a = 1/(1 - e*e), xf2 = -2*e*a, // .... simplify
           ys = sqrtl(a), ts = 1/(a*ys);
   void rx(R t){
    co a = Kepler(e, t-dL), b = Kepler(e, t), c = Kepler(e, t+dL);
    printf("t=%5.2Lf, ", t);
    for(int j = 0; j<2; ++j) assert(fabs((c.x[j] - a.x[j])/dL - 2*b.xd[j]/ts) < 1e-6);
    assert(fabs(b.x[0]*b.xd[1] - b.x[1]*b.xd[0] - 1) < 1e-12);
    assert(fabs(sqrtl(sq(b.x[0]) + sq(b.x[1])) + sqrtl(sq(b.x[0] - xf2) + sq(b.x[1]))
      - 2/(1-e*e)) < 1.e-13);  // On ellipse of orbit
    for(int j=0; j<2; ++j) printf("x=%12.9Lf xd=%12.9Lf ", b.x[j], b.xd[j]); printf("\n");
    {R rm3 = ({R r2 = sq(b.x[0]) + sq(b.x[1]); 1/(sqrtl(r2)*r2);});
      for(int j=0; j<2; ++j) printf("a=%12.9Lf p=%12.9Lf ",
         (a.x[j]+c.x[j] - 2*b.x[j])/(dL*dL), rm3*b.x[j]); printf("ac\n");}}
  printf("xf2 = %Le, e = %Le\n", xf2, e);
  rx(0); rx(1); rx(2.31); rx(3.14); rx(4.7);}
   ry(0.42); ry(0); ry(0.5); ry(0.9); if(0) ry(1.1);}

  { // Demonstrate that code produces correct orbit for special points.
   R e = 0.5; // a = 1/(1-e*e), ts = a*sqrtl(a), af = -2*e*a;
   R g(R x){return acosl(x) - sqrtl(1-x*x)*e;}
   void tt(R t){co c = Kepler(e, t);
     printf("%22.19Lf %22.19Lf %Le\n%22.19Lf %22.19Lf .\n",
       c.x[0], c.x[1], t, c.xd[0], c.xd[1]);}
    tt(g(1)); tt(g(e)); tt(g(0)); tt(g(-e)); tt(g(-1)); tt(2*pi-g(-e));
/*
 0.6666666666666666667  0.0000000000000000000 0.000000e+00
-0.0000000000000000000  1.5000000000000000000 .
 0.0000000000000000004  0.9999999999999999998 6.141848e-01
-0.9999999999999999999  0.5000000000000000004 .
-0.6666666666666666666  1.1547005383792515292 1.070796e+00
-0.8660254037844386471  0.0000000000000000001 .
-1.3333333333333333330  1.0000000000000000001 1.661382e+00
-0.6000000000000000002 -0.2999999999999999999 .
-2.0000000000000000002  0.0000000000000000002 3.141593e+00
-0.0000000000000000001 -0.5000000000000000000 .
-1.3333333333333333334 -1.0000000000000000000 4.621803e+00
 0.6000000000000000001 -0.3000000000000000000 .
*/
    }
  if(0) for(R t=0; t<5; t+=0.02) {co x = Kepler(0.4, t);  // Print an orbit.
    printf("%3.2Lf %8.5Lf %8.5Lf %8.5Lf %8.5Lf ", t, x.x[0], x.x[1], x.xd[0], x.xd[1]);
    printf("%13.10Lf\n", 1./sqrtl(sq(x.x[0]) + sq(x.x[1])) -
        0.5*(sq(x.xd[0]) + sq(x.xd[1])));}
  {R time = .163; // in centuries since January 2000
  // Next block adjusts for time.
  for(int j = 0; j<np; ++j) for(int m = 0; m<6; ++m) {R a = time*planetsD[j][m];
  planets[j][m] = m<2?planets[j][m]+a : fmodl(planets[j][m] + a/3600, 360);}
  init();
  printf("For t = %11.9Lf\n", 2000+time*100);}
  for(int j=0; j<np; ++j) {R s=0;
    for(int k=0; k<3; ++k) {R q = state[j].x[k];
      printf("%13.8Lf %11.8Lf ", q, state[j].xd[k]); s += q*q;}
	printf("  %13.8Lf\n", sqrtl(s));}
  if(0){R d2 = 0; for(int j=0; j<3; ++j) {R g = state[4].x[j] - state[3].x[j]; d2 += g*g;}
    printf("Earth to Mars = %12.9Lf\n", sqrtl(d2));}
  {R mass[np+1] = {1, 0.00000016601, 0.0000024478383, 0.00000304043263333,
      0.0000003227151, 0.00095479194, 0.0002858860, 0.00004366244,
      0.00005151389, 0.000000007396};
  }
  return 0;}

/*
For t = 2016.300000000
   0.00000000  0.00000000    0.00000000  0.00000000    0.00000000  0.00000000      0.00000000
  -0.31818725  0.45182948   -0.16271393 -1.54905673   -0.04092573 -0.10519666      0.35971358
   0.17007599 -1.13920459    0.70507304  0.26868552    0.04302140 -0.00011336      0.72657049
  -0.95490463  0.29408106   -0.31232069 -0.95045562    0.00003145 -0.00001762      1.00468257
   0.12619604  0.76778120   -1.55722521  0.13206188   -0.03560947  0.01933459      1.56273601
   0.18550881  0.41798798   -5.42782193  0.00589856   -0.12232263 -0.00162148      5.43236848
   9.97335405 -0.02121291    0.90146114  0.30562256   -0.13772775  0.01251816     10.01495842
  -1.56733001 -0.21731285   19.90637143 -0.02237077    0.25154297 -0.00108641     19.96956235
 -10.87935335 -0.16995421   27.90052194 -0.06785931    0.86681736  0.00192835     29.95915263
*/