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

float length;
float gravity;
float initialAngle;

void deriv(float* y_in, float* dy_out)
{
  //Index of angle
  dy_out[0] = y_in[1];

  //Index of angular velocity
  dy_out[1] = -(gravity/length) * sin(y_in[0]);
}

void rk4(void (*deriv)(float* y_in, float* dy_out), 
	 int n, float *x, float dx, float *y) 
{
  int i; 
  float x0 = *x;
  float y0[2];
  y0[0] = y[0];
  y0[1] = y[1];
 
  float dy1[2]; 

  deriv(y, dy1);

  for(i=0; i<n; i++) {
    y[i] = y0[i] + 0.5 * dx * dy1[i];
  }

  float dy2[2]; 
  deriv(y, dy2);

  for(i=0; i<n; i++) {
    y[i] = y0[i] + 0.5 * dx * dy2[i];
  }
  
  float dy3[2];
  deriv(y, dy3);

  for(i=0; i<n; i++) {
    y[i] = y0[i] + dx * dy3[i];
  }

  deriv(y, y);

  for(i=0; i<n; i++) {
    y[i] = (dy1[i] + 2*(dy2[i] + dy3[i]) + y[i])/6.0;
    y[i] = y0[i] + dx * y[i];
  }

  *x = x0 + dx;
}

int main() 
{
  //Fiddle with these for the desired result.
  length = 1.0;
  gravity = 9.807;
  initialAngle = 45.0;
  int granularity = 500;
  int duration = 1000;

  float period = 2 * M_PI * sqrt(length/gravity);
  float tfinal = 2*period;
  float dt = tfinal/(granularity - 1);

  float* y = (float*)malloc(2*sizeof(float));

  y[0] = initialAngle * M_PI/180;
  y[1] = 0;

  float t = 0.0;

  int i;
  for(i=2; i<duration; i++) {
    rk4(deriv, 2, &t, dt, y);
    printf("Time %f Angle %f\n", t, y[0]*180.0*M_PI);
  }

  free(y);

  return 0;
}