#ifndef DOUBLE_PENDULUM
#define DOUBLE_PENDULUM
#include <iostream>

class DoublePendulum{
  double om02;
public:
  DoublePendulum(double om02_) : om02(om02_){}
  void operator()(double x, function1D<double>& y, function1D<double>& dydx)
  {
    double cs = cos(y[0]-y[1]), ss = sin(y[0]-y[1]);
    double t = 1/(1+ss*ss);
    dydx[0] = (y[2]-y[3]*cs)*t;
    dydx[1] = (2*y[3]-y[2]*cs)*t;
    double c1 = y[2]*y[3]*ss*t;
    double c2 = (y[2]*y[2]+2*y[3]*y[3]-2*y[2]*y[3]*cs)*cs*ss*t*t;
    dydx[2] = -2*om02*sin(y[0])-c1+c2;
    dydx[3] =   -om02*sin(y[1])+c1-c2;
  }
  double Energy(const function1D<double>& y)
  {
    double cs = cos(y[0]-y[1]), ss = sin(y[0]-y[1]);
    return 0.5*(y[2]*y[2]+2*y[3]*y[3]-2*y[2]*y[3]*cs)/(1+ss*ss)+om02*(3-2*cos(y[0])-cos(y[1]));
  }
  double p2(double E, double y0, double y1)
  {
    if (E<=0) {std::cerr<<"Error in starting conditions!"<<std::endl; return 0;}
    double E2 = (E-om02*(3-2*cos(y0)-cos(y1)))*(1+sqr(sin(y0-y1)));
    if (E2<0) {std::cerr<<"Error in starting conditions!"<<std::endl; return 0;}
    else return sqrt(E2);
  }
};
#endif