/* solve_sdpend.c
 *
 * Example code to simulate the chaotic pendulum in the main corridor in 
 * the School of Physics.
 *
 * Parameters are passed at the command line:
 * 
 * $./solve_dpend TMIN TMAX TH10 W10 TH20 W20 NSTEP > pendulum.txt 
 *
 * where TMIN and TMAX are the starting and ending times (time is non-
 * dimensionalised in units of the square root of L/g, where L is the
 * length of one side of the square plates of the pendulum), TH10 and 
 * TH20 are the initial angles (degrees), and W10 and W20 are the initial 
 * angular velocities (degrees per unit time), and NSTEP is the number 
 * of integrations steps. Note that there is no checking for accuracy, 
 * so the user needs to choose a suitable NSTEP. However, the energy is 
 * calculated at each time step, and this provides a check on accuracy. 
 * Angles written to file are in radians.
 *
 * M.S. Wheatland, 2007
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

/* Hardwired parameters */

#define PI 3.14159265
#define N 4 /* Number of equations to be solved */

void runge_kutta(float xin, float yin[], float yout[], float h);
void derivs(float xin, float yin[], float dydx[]);
float energy(float th1v, float w1v, float th2v, float w2v);

int main(int argc, char *argv[])
{

  int i = 0, NSTEP;
  float h, TMIN, TMAX, TH10, W10, TH20, W20;
  float yin[N], yout[N];
  float *t, *th1, *th2, *w1, *w2, *E;

  /* Obtain command line values */

  TMIN = atof(argv[1]);
  TMAX = atof(argv[2]);
  TH10 = atof(argv[3]);
  W10 = atof(argv[4]);
  TH20 = atof(argv[5]);
  W20 = atof(argv[6]);
  NSTEP = atoi(argv[7]);

  /* Allocate memory for arrays of values of time, angles 1 and 2,
     and angular velocities 1 and 2 respectively */ 

  t = (float *) malloc(NSTEP*sizeof(float)); 
  th1 = (float *) malloc(NSTEP*sizeof(float)); 
  w1 = (float *) malloc(NSTEP*sizeof(float));
  th2 = (float *) malloc(NSTEP*sizeof(float));
  w2 = (float *) malloc(NSTEP*sizeof(float));
  E = (float *) malloc(NSTEP*sizeof(float));

  /* Step size for integration */

  h = (TMAX - TMIN)/(NSTEP - 1.0);
 
  /* Define array of t values */

  for (i = 0; i < NSTEP; i++)
    t[i] = TMIN + h*i;

  /* Initial values - convert all angles to radians */

  th1[0] = TH10*PI/180.0;
  w1[0] = W10*PI/180.0;
  th2[0] = TH20*PI/180.0;
  w2[0] = W20*PI/180.0; 

  E[0]=energy(th1[0],w1[0],th2[0],w2[0]);

  /* Perform the integration */

  printf("%f %f %f %f %f %f\n", t[0], th1[0], w1[0], th2[0], w2[0], E[0]);
  for (i = 0; i < NSTEP - 1; i++)
  { 
    yin[0] = th1[i];
    yin[1] = w1[i];
    yin[2] = th2[i];
    yin[3] = w2[i];
    runge_kutta(t[i], yin, yout, h);
    th1[i+1] = yout[0];
    w1[i+1] = yout[1];
    th2[i+1] = yout[2];
    w2[i+1] = yout[3];
  
    E[i+1]=energy(th1[i+1],w1[i+1],th2[i+1],w2[i+1]);
  
    printf("%f %f %f %f %f %f\n", t[i+1], th1[i+1], w1[i+1], th2[i+1],
      w2[i+1],E[i+1]);
  }

  return 0;
 
}

void derivs(float xin, float yin[], float dydx[])
{

  /* Function to fill array of derivatives dydx at xin */

  float th1, th2, w1, w2, phi, den, del, sphi, cphi;

  th1=yin[0];
  w1=yin[1];
  th2=yin[2];
  w2=yin[3];

  dydx[0]=w1; 
 
  phi=0.25*PI-th1+th2; 
  sphi=sin(phi);
  cphi=cos(phi);
  den=5./3.-0.75*cphi*cphi;
  dydx[1]=(0.75*w1*w1*sphi*cphi+0.75*sin(th2)*cphi
          +w2*w2*sphi/sqrt(2.)-sin(th1)/sqrt(2.)
          +sin(0.25*PI-th1))/den;   

  dydx[2]=w2;
  dydx[3]=-1.5*(dydx[1]*cphi+w1*w1*sphi+sin(th2))/sqrt(2.);

  return;

}

void runge_kutta(float xin, float yin[], float yout[], float h)
{
  /* Fourth order Runge-Kutta - see e.g. Numerical Recipes */
 
  int i;
  float hh, xh, dydx[N], dydxt[N], yt[N], k1[N], k2[N], k3[N], k4[N];
  
  hh = 0.5*h;
  xh = xin + hh; 
  
  derivs(xin, yin, dydx); /* first step */
  for (i = 0; i < N; i++) 
  {
    k1[i] = h*dydx[i];
    yt[i] = yin[i] + 0.5*k1[i];
  }

  derivs(xh, yt, dydxt); /* second step */ 
  for (i = 0; i < N; i++)
  {
    k2[i] = h*dydxt[i];
    yt[i] = yin[i] + 0.5*k2[i];
  }   

  derivs(xh, yt, dydxt); /* third step */
  for (i = 0; i < N; i++)
  {
    k3[i] = h*dydxt[i];
    yt[i] = yin[i] + k3[i];
  }

  derivs(xin + h, yt, dydxt); /* fourth step */
  for (i = 0; i < N; i++)
  {
    k4[i] = h*dydxt[i];
    yout[i] = yin[i] + k1[i]/6. + k2[i]/3. + k3[i]/3. + k4[i]/6.;
  }
 
  return;

}

float energy(float th1v, float w1v, float th2v, float w2v)
{
  /* Evaluate the total energy */

  float phi, en;

  phi=0.25*PI-th1v+th2v; 

  en=0.5*(5.*w1v*w1v/3.+2.*w2v*w2v/3.+sqrt(2.)*w1v*w2v*cos(phi));
  en=en-cos(th1v)/sqrt(2.)-cos(th2v)/sqrt(2.)-cos(0.25*PI-th1v);

  return en;
}