#include "nr3.h"
#include "odeint.h"
#include "stepper.h"
#include "stepperbs.h"
#include <fstream>

const double g = 9.813;
const double C=1; // 1 N/m
const double m=0.1; //0.1 kg ball
const double l=2; // 2m rope


// coordinates : time, x[1..3],v[1..3]

struct derivatives {
   derivatives(){};
   void operator()(const double& time, const VecDoub &coord, VecDoub &deriv) {
// calculates the derivatives dydx 
	for (int i=0; i<4; i++) deriv[i]=coord[i+3];
	double r=sqrt(coord[0]*coord[0]+coord[1]*coord[1]+coord[2]*coord[2]);
	if (r<l) {
		deriv[3]=deriv[4]=0;
		deriv[5]=-g;
	} else {
		for (int i=0; i<3; i++) deriv[i+3]=-coord[i]*C*(r-l)/r/m;
		deriv[5]-=g;
	}
   }
};
          

int main() {
   ofstream outfil("pendulum.txt");
   VecDoub coord(6),deriv(6);
   derivatives d; // the derivatives struct;
   // start values: take something arbitrary
   coord[0]=1;
   coord[1]=0;
   coord[2]=0;
   coord[3]=-0.1;
   coord[4]=0.2;
   coord[5]=0;
   Output out(2000); // plot 2000 intermediate results for each time
   double t0=0;
   double stepsize=0.1;
   Odeint <StepperBS<derivatives> > ode(coord,t0,20,1e-12,1e-12,stepsize,1e-8,out,d);
   ode.integrate();
   for (int i=0; i<2000; i++) {
	outfil << out.xsave[i] << " ";
	for (int j=0; j<4; j++) outfil << out.ysave[j][i]  << " ";
	outfil << endl;
   }
}