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

#define DT 0.1 // timestep

typedef struct {
  double r;
  double px, py, mx, my;
  double vpx, vpy, vmx, vmy, theta, alpha, vpr, vpth, vmr, vmth;
} state;

void update(state *s, double apx, double apy);
void printState(state *s);

int main(void) {
state s;
s.vpx=0;
s.vpy=0;
s.vmx=0;
s.vmy=0;
s.px=0;
s.py=0;
s.r = 10;
s.mx = s.px + s.r;
s.my = 0;
int ii;
printState(&s);
for(ii=0; ii<200; ii++) {
  update(&s, 0, 0.5); // accelerate straight up
  printState(&s);
}
return 0;
}

void update(state *s, double apx, double apy) {
  // compute angle of rod:
  double dx, dy, v1, v2;
  dx = s->mx - s->px;
  dy = s->my - s->py;
  double theta = atan2(dy, dx);
  printf("theta = %.4fn", theta);

  // update velocity of p:
  v1 = s->vpx;
  v2 = s->vpx += apx * DT;
  s->px += 0.5*(v1 + v2) * DT;
  v1 = s->vpy;
  v2 = s->vpy += apy * DT;
  s->py += 0.5 * (v1+v2) * DT;



  // components of velocity along and perpendicular to rod:
  s->vpr  = s->vpx * cos(theta) + s->vpy * sin(theta);
  s->vpth = -s->vpx * sin(theta) + s->vpy * cos(theta);
  s->vmr  = s->vpr;

  // update velocities:
  s->vmx = s->vmr * cos(theta) - s->vmth * sin(theta);
  s->vmy = s->vmr * sin(theta) + s->vmth * cos(theta);
  s->vmth = -s->vmx * sin(theta) + s->vmy * cos(theta);

  // and positions
  s->mx += s->vmx * DT;
  s->my += s->vmy * DT;

  // impose constraint of constant rod length...
  dx = s->mx - s->px;
  dy = s->my - s->py;
  theta = atan2(dy, dx);
  double stretch = s->r / sqrt(dx*dx + dy * dy);
  s->mx = s->px + dx * stretch;
  s->my = s->py + dy * stretch;
  s->theta = theta;
}

void printState(state *s) {
  printf("px = %5.2f; py = %5.2f; mx = %5.2f; my = %5.2fn", s->px, s->py, s->mx, s->my);
}