final float INIT_THETA = 40.0 * PI / 180.0; // in radians
final float INIT_THETA_DOT = 0;             // in radians/second
final float H = 1.0/30.0;                   // timestep in seconds
final float G = 10.0;                       // in m/s^2
final float L = 0.45;                       // pendulum length, in meters
// initial state
float[] state         = {INIT_THETA, INIT_THETA_DOT};
float[] returnedState = {INIT_THETA, INIT_THETA_DOT};  // this state is returned by f

void setup()
{
  size(300, 300);
  frameRate(30);
  smooth();
}

void drawState()
{
  // the ceiling
  strokeWeight(2);
  fill(0);
  line(1.0/3.0*width, height/10.0, 2.0/3.0*width, height/10.0);
  final int NUM_CEILING_LINES = 5;
  for(int i = 0; i < NUM_CEILING_LINES; i++)
    line(1.0/3.0*width * (1 + (float) i / NUM_CEILING_LINES), height/11.0, 
         1.0/3.0*width * (1 + (float) i / NUM_CEILING_LINES) + width/20.0, height/15.0);
  
  // the pendulum line
  strokeWeight(2);
  stroke(100);
  float pendulumLengthInPixelsX = L * 1.5*height;
  float pendulumLengthInPixelsY = L * 1.5*width;
  float massX = width/2.0 + pendulumLengthInPixelsX * sin(state[0]);
  float massY = height/10.0 + pendulumLengthInPixelsY * cos(state[0]);
  line(width/2.0, height/10.0, massX, massY);
  
  // the mass
  strokeWeight(0);
  fill(0, 0, 150);
  float massSize = min(height/10.0, width/10.0);
  ellipse(massX, massY, massSize, massSize);
}

void draw()
{
  background(250);
  drawState();
  rk4();    // update state
  println(frameRate);
}

// differential equation as per description
// returns values into returnedState array
void f(float theta, float thetaDot)
{
  returnedState[0] = thetaDot;
  returnedState[1] = -G/L * sin(theta);
}

// rk4 method, uses these four arrays as temp variables
float[] a_n = {0, 0};
float[] b_n = {0, 0};
float[] c_n = {0, 0};
float[] d_n = {0, 0};
void rk4()
{
  f(state[0], state[1]);                                     // a_n
  for(int i = 0; i < a_n.length; i++)
    a_n[i] = returnedState[i];
  
  f(state[0] + H/2.0 * a_n[0], state[1] + H/2.0 * a_n[1]);   // b_n
  for(int i = 0; i < b_n.length; i++)
    b_n[i] = returnedState[i];
  
  f(state[0] + H/2.0 * b_n[0], state[1] + H/2.0 * b_n[1]);   // c_n
  for(int i = 0; i < c_n.length; i++)
    c_n[i] = returnedState[i];
  
  f(state[0] + H * c_n[0], state[1] + H * c_n[1]);           // d_n
  for(int i = 0; i < d_n.length; i++)
    d_n[i] = returnedState[i];
  
  // update the state
  for(int i = 0; i < state.length; i++)
    state[i] += H/6.0 * (a_n[i] + 2*b_n[i] + 2*c_n[i] + d_n[i]);
}