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


#define TWOPI 6.28318530717958647692528


double pv(double x)
{
   /*** taking the Mod 2\pi of the angle ***/

   double y;

   y = x - TWOPI * (floor(x/TWOPI));

   if (y > M_PI)
      y -= TWOPI;

   return(y);
}

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

  float *t;                /* time */
  float *theta; 	   /* angular position */
  float *thetadot;         /* angular velocity */
  
  float gamma;		   /* gamma, the damping coefficient */
  float twogamma;	   /* 2 x gamma */
  float omega0, omega0sq;  /* natural frequency and its square */
  /* float omega; */       /* driving freq */
  float epsilon;           /* driving freq minus natural frequency */
  float h;                 /* amplitude of forcing */
  float initial_theta;     /* initial angular position */
  float initial_thetadot;  /* initial angular velocity */
  float finaltime;         /* length of simulation time */
  float dt;		   /* time increment or resolution */

  FILE *f_theta, *f_thetadot, *f_t;    /* temporary files to hold data */

  if (argc != 9) {
    fprintf(stdout, "Usage: drvpend gamma omega0 epsilon h initial_theta initial_thetadot finaltime dt\n");
    exit(1);
  }

  f_theta = fopen("theta", "w");       /* open file handles */
  f_thetadot = fopen("thetadot", "w");
  f_t = fopen("t", "w");
  
  gamma = atof(argv[1]);               /* C programs input routine */
  omega0 = atof(argv[2]); omega0sq = omega0*omega0;
  /* omega = atof(argv[3]); */ 
  epsilon = atof(argv[3]);
  h = atof(argv[4]);
  initial_theta = atof(argv[5]);
  initial_thetadot = atof(argv[6]);
  finaltime = atof(argv[7]);
  dt = atof(argv[8]);

/* THIS SHOULD HELP WITH THE PROBLEMS WITH POINCARE SECTIONS.
   THOUGH IT SETS dt TO BE 0.001*2*pi AUTOMATICALLY  */
  dt = 0.001 * TWOPI;

  twogamma = 2.0 * gamma;

  N = (int ) (finaltime/dt) + 1;       /* number of sample */

  /* printf("N=%d\n", N); */

  /* Allocate memories */
  if (!(t = (float *) malloc(N*sizeof(float)))) {
    fprintf(stdout, "Failed in allocating memory for t!");
    exit(1);
  }
   
  if (!(theta = (float *) malloc(N*sizeof(float)))) {
    fprintf(stdout, "Failed in allocating memory for theta!");
    exit(1);
  }

  if (!(thetadot = (float *) malloc(N*sizeof(float)))) {
    fprintf(stdout, "Failed in allocating memory for thetadot!");
    exit(1);
  }
  
  /* Initializing variables */
  for (i = 0; i < N; i++) {
    theta[i] = 0.0;
    thetadot[i] = 0.0;
    t[i] = i*dt;
  }

  theta[0] = initial_theta;          /* input initial conditions */
  thetadot[0] = initial_thetadot;

  /* THE COMPUTATION, by leap-flog Euler */
  for (i = 0; i < N - 1; i++) {
    thetadot[i+1] = thetadot[i] - (twogamma*thetadot[i] + omega0sq*(1.0 + h*cos(2*(omega0 + epsilon)*t[i]))*sin(theta[i]))*dt;
    theta[i+1] = theta[i] + thetadot[i+1]*dt;
    theta[i+1] = pv(theta[i+1]);
  }
  
  fwrite(theta, sizeof(float), N-1, f_theta);
  fwrite(thetadot, sizeof(float), N-1, f_thetadot);
  fwrite(t, sizeof(float), N-1, f_t);

  fclose(f_theta);
  fclose(f_thetadot);
  fclose(f_t);

  free(thetadot);
  free(theta);
  free(t);
  
  return;

}