/* Pendulum program with Runge-Kutta integration*/


#include<math.h>
#include<iostream>
#include<fstream>
#define g 9.8 /* sets g for use throughout the program*/
#define deltat 0.01 /*sets a timestep value for use throughout the program*/
#define length 1 /*sets the length of the pendulum as a constant throughout the program */
using namespace std;

void runge_kutta_2nd(float *t, float *x, float *xprime, float (*func)(float, float, float) )
{/*The last pointer in the list of variables allows us to use an
  arbitary function here, as long as it's a function of three
  variables.  Since a 2nd order differential equation only have the
  second derivative be a function of three variables, that's all we
  need.  In some cases, that second derivative won't be an explicit
  function of one of the variables, but we can then just send a
  variable that doesn't get used. It may have other parameters, of
  course, but they will be constants.*/
  float k1,k2,k3,k4,kap1,kap2,kap3,kap4;
  k1=(*func)(*t,*x,*xprime);
  kap1=*xprime;
  k2=(*func)(*t+deltat/2,*x+deltat/2*kap1,*xprime+deltat/2*k1);
  kap2=*xprime+deltat/2*k1;
  k3=(*func)(*t+deltat/2,*x+deltat/2*kap2,*xprime+deltat/2*k2);
  kap3=*xprime+deltat/2*k2;
  k4=(*func)(*t+deltat/2,*x+deltat*kap3,*xprime+deltat*k3);
  kap4=*xprime+deltat*k3;
  *xprime=*xprime+deltat/6*(k1+2.0*k2+2.0*k3+k4);
  *x=*x+deltat/6*(kap1+2.0*kap2+2.0*kap3+kap4);
  *t=*t+deltat;
  /*The kap1...kap4 values are the K's */
  /*The k1...k4 are the k's. */
  /*The rest of this should be pretty straightforward.*/
  return;
}

    
void read_start_vals(float *theta)
{
  cout << "What is the initial value of theta in degrees?\n";
  cin >> *theta; /*theta is  a pointer so no & needed */
  /*The next line converts theta into radians */
  *theta=*theta*M_PI/180.0; /*M_PI is pi defined in math.h */
  /* An asterisk is needed for theta because we want the value*/
  /* at the memory address of the pointer, not the address itself*/
  /*Like in the case where we read in theta, we already have a pointer */
  /*So no ampersand is needed.*/
  return;
}


float angular_acceleration(float time, float theta, float omega)
{
  /*We do not use time or omega explicitly here, but we need to pass the two extra parameters so that the Runge-Kutta function can be a generic one */
  float alpha;
  alpha=-(g/length)*sin(theta);
  /* This is just first year physics, but before we make the small
     angle approximation. */
  return alpha;
}


main()
{
  int i,imax;
  float theta,omega,t,kineticenergy,potentialenergy,totalenergy,velocity;
  ofstream outfile;
  /*Above lines define the variables*/
  /*Mass is ignored here, since it falls out of all equations*/
  /*To get energy in joules, etc. would have to add a few lines of code*/
  outfile.open("./pendulumoffset"); /* opens a file for writing the output*/
  t=0;
  omega=0;
  /*The two lines above make sure we start at zero.*/
  read_start_vals(&theta); /*Reads initial offset and length of pendulum */
  for (i=0;i<50000; i=i+1) /*Starts a loop that will go through 50000 time steps */
  {
    runge_kutta_2nd(&t,&theta,&omega,&angular_acceleration);/*Calls Runge-Kutta*/
    potentialenergy=g*length*(1.0-cos(theta));/*Computes grav. pot en.*/
    velocity=omega*length;/* Computes linear velocity*/
    kineticenergy=0.5*velocity*velocity;/*Computes kin. en */
    totalenergy=potentialenergy+kineticenergy;/*Computes total energy*/
    outfile << t <<" " << theta <<" " << omega <<" " << kineticenergy <<" " << potentialenergy <<" " << totalenergy << endl;/* Prints out the key variables*/
    /*Note: keeping track of the total energy is a good way to test a*/
    /*code like this where the energy should be conserved*/
  }

}