/* Written by Clayton Otey October 27, 2007
  Compile with 
  gcc -o jlt jlt.c -lm -O3
 */

#include 
#include 
#include 

typedef long double real;
typedef real func(real *, real *, real);

#define PI 3.14159265358979

#define K 128
real A = .1;
real I = 1;

real V(real r) 
{
  return expl(-r*r)/r;
}

real Hrot(real *q, real *p) {
  return 0.5*p[0]*p[0]/I;
}

real H(real *q, real *p) {
  return 0.5*p[1]*p[1] +  Hrot(q,p) + V(q[1] + A*cosl(q[0]));
}

real dHdp(real *dHdp, real *p, real t) {
  dHdp[0] = p[0]/I;
  dHdp[1] = p[1];
}

real dHdq(real *dHdq, real *q, real t) {
  real theta = q[0];
  real x = q[1];
  //real xa = x - A*fabsl(cosl(theta));
  real xa = x + A*cosl(theta);
  real f = -expl(-xa*xa)*(2.0 + 1.0/(xa*xa));
  dHdq[0] = -A*sinl(theta)*f;
  //if(cosl(theta)<0)
  //  dHdq[0] = -dHdq[0];
  dHdq[1] = f;
}

void rk_explicit(real **aq, real **ap, real *bq, real *bp,
		 func dHdp, func dHdq,
		 int N, int M,
		 real *cq, real *cp, real **q, real **p, real **fq, real **fp,
		 real *qin, real *pin, real *qout, real *pout, real t, real tau) {


  int i,j,k;
  for(i=0;i