#include "nr3.h"
#include "odeint.h"
#include "stepper.h"
#include "stepperdopr5.h"
#include "stepperbs.h"

// Note: in odeint, I changed maxsteps to 10 million
// units: time in mega-seconds (about 12 days), distances in Gigameters (1e9 m),// velocities in Gm/Ms = km/s
// masses in 10^24 kg
// G =6.67e-11 kg^-1m^3s^-2 = 6.67e-11 *1e24/1e24kg*1e-27/(1e9m^3)*1e12/(1e6s)^2

const double G = 6.67e-2;
const double mass[4] = {2e6,6,0.1,1000};

// variables : planets : sun = 0 , a = 1 , moon =2, B =3
// coordinates : coord[6*planet+i], i=0...5, coord[i]=x,y,z,vx,vy,vz
#define NPLANET 4
#define NCOORD 6
#define NTOT 24

struct derivatives {
   derivatives(){};
   void operator()(const double& time, const VecDoub &coord, VecDoub &deriv) {
// calculates the derivatives dydx on base of the coordinates y
      for (int planet=0; planet<NPLANET; planet++) for (int i=0; i<3; i++) {
          deriv[planet*NCOORD+i]=coord[planet*NCOORD+i+3]; //dx/dt = vx
          deriv[planet*NCOORD+i+3]=0; // initialize accelerations to 0
      }
      for (int planet=0; planet<NPLANET; planet++) {
         for (int plan2=planet+1; plan2<NPLANET; plan2++) {
            double x[3];
            for (int i=0; i<3; i++) x[i]=coord[plan2*NCOORD+i]-coord[planet*NCOORD+i];
	    double r=0;
	    for (int i=0; i<3; i++) r+=x[i]*x[i]; 
            r=sqrt(r);
            double F1=G/r/r/r;
            for (int i=0; i<3; i++) {
	       deriv[planet*NCOORD+i+3]+=F1*mass[plan2]*x[i]; // force of plan2 on planet in i-direction
	       deriv[plan2*NCOORD+i+3]-=F1*mass[planet]*x[i]; // force of planet on plan2 in i-direction
	    }
	 }
      }
   }
};
          

int main() {
   VecDoub coord(NTOT),deriv(NTOT);
   VecDoub coordcop(NTOT);
   derivatives d; // the derivatives struct;
   // start values: let bob fall from a rest position with a little bit of tangential speed
   for (int i=0; i<NTOT; i++) coord[i]=0;
   coord[6]=150; // x planet A
   coord[10]=30; // vy planet A
   coord[12]=150; // x moon
   coord[13]=0.38; // y moon
   coord[15]=-1; // vx moon
   coord[16]=30; // vy moon
   coord[17]=0.1; // vz moon
   coord[18]=400; // x planet B
   coord[22]=18.3; // x planet B
   coord[23]=-1.6; // x planet B
   for (int planet=1; planet<NPLANET; planet++) for (int i=3; i<6; i++) coord[i]-=coord[planet*NCOORD+i]*mass[planet]/mass[0]; // calculate velocity star
   Output out(0);
   double t0=0;
   double stepsize=0.1;
   for (double t1=10; t1<2e7; t1*=10.0) {
      for (int i=0; i<NTOT; i++) coordcop[i]=coord[i];
      Odeint <StepperBS<derivatives> > ode(coordcop,t0,t1,1e-14,1e-15,stepsize,2e-6,out,d);
      ode.integrate();
      cout << t1 << " ";
      for (int plan=0; plan<4; plan++) {
         for (int i=0; i<6; i++) cout << coordcop[i+NCOORD*plan] << " ";
         cout << endl;
      } 
      Odeint <StepperBS<derivatives> > odeback(coordcop,t1,t0,1e-14,1e-15,stepsize,2e-6,out,d);
      odeback.integrate();
      cout << " BS after reverse integration : " << endl;
      cout << t1 << " ";
      for (int plan=0; plan<4; plan++) {
         for (int i=0; i<6; i++) cout << coordcop[i+NCOORD*plan]-coord[i+NCOORD*plan] << " ";
         cout << endl;
      } 
      cout << " number of OK steps " << ode.nok << endl << endl;
      if (ode.nok>5e6) break; // stop if next loop would require more than 50mil steps
   }
   for (double t1=10; t1<2e7; t1*=10.0) {
      for (int i=0; i<NTOT; i++) coordcop[i]=coord[i];
      Odeint <StepperDopr5<derivatives> > ode(coordcop,t0,t1,1e-13,1e-14,stepsize,2e-6,out,d);
      ode.integrate();
      cout << t1 << " ";
      for (int plan=0; plan<4; plan++) {
         for (int i=0; i<6; i++) cout << coordcop[i+NCOORD*plan] << " ";
         cout << endl;
      } 
      Odeint <StepperDopr5<derivatives> > odeback(coordcop,t1,t0,1e-13,1e-14,stepsize,2e-6,out,d);
      odeback.integrate();
      cout << " Dopr 5 after reverse integration : " << endl;
      cout << t1 << " ";
      for (int plan=0; plan<4; plan++) {
         for (int i=0; i<6; i++) cout << coordcop[i+NCOORD*plan]-coord[i+NCOORD*plan] << " ";
         cout << endl;
      } 
      cout << " number of OK steps " << ode.nok << endl;
      if (ode.nok>5e6) break; // stop if next loop would require more than 50mil steps
   }
   return 0;
}