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

#define L0 1.0 // lenght pendulum+uncompressed spring
#define K 50.0 // spring constant C divided by m, in m/s^2

struct derivatives {
   derivatives(){};
   void operator()(const double& time, VecDoub &y, VecDoub &dydx) {
// calculates the derivatives dydx on base of the coordinates y
// y[0..2] = x,y,z
// y[3..5] = vx,vy,vz = dx/dt,dy/dt,dz/dt
// R0+l0= 1 meter (pendulum+uncompressed spring
      for (int i=0; i<3; i++) dydx[i]=y[i+3]; //dx/dt = vx
      double r=sqrt(y[0]*y[0]+y[1]*y[1]+y[2]*y[2]);
      
      for (int i=3; i<6; i++) { 
         if (r<L0) dydx[i]=0;
         else dydx[i]=-y[i-3]/r*(r-L0)*K; // force in the direction of the origin
      }
      dydx[5]-=9.813; // gravitational constant
   }
};
          

int main() {
   int nvar=6;
   VecDoub coord(nvar),dydx(nvar);
   derivatives d; // the derivatives struct;
   // start values: let bob fall from a rest position with a little bit of tangential speed
   coord[0]=0.2;
   coord[1]=0;
   coord[2]=0;
   coord[3]=0;
   coord[4]=0.1; // 10cm/s y-speed
   coord[5]=0;
   Output out(100);
   double t0=0;
   double t1=10;
   double stepsize=0.1;
   Odeint <StepperDopr5<derivatives> > ode(coord,t0,t1,1e-10,1e-10,stepsize,1e-7,out,d);
// start time 0, stop time 10 sec., tolerances 1e-10, start stepsize 0.1, minimum stepsize 1e-7, output out, derivatives d
   ode.integrate();
   cout << "at end of integration : " << endl;
   cout << "time, x, y, z, vx, vy, vz"<< endl;
   cout << "10 ";
   for (int i=0; i<6; i++) cout << coord[i] << " ";
   cout << endl;
   cout << endl;
   cout << "time, x, y, z, vx, vy, vz"<< endl;
   for (int i=0; i<100; i++) {
       cout << out.xsave[i] << " ";
       for (int j=0; j<6; j++) cout << out.ysave[j][i] << " ";
       cout << endl;
   }
   return 0;
}