#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>

using namespace std;

template <class functor, class container>
void Euler(double x, double h, container& y, functor& derivs)
{
  container fk(y.size());
  derivs(x, y, fk);
  for (int i=0; i<y.size(); i++) y[i] += h*fk[i]; 
}

/////////////////////////////////////////////////////////////////////////////
///////////////// METHOD OF RUNKGE-KUTTA 4-th ORDER   ///////////////////////
/////////////////////////////////////////////////////////////////////////////
template <class functor, class container>
void RK4(double x, double h, container& y, functor& derivs)
{
  static container k1(y.size()), k2(y.size()), k3(y.size()), k4(y.size());// temporary arrays
  static container yt(y.size());
  
  double h2=h*0.5;
  double xh = x + h2;
  derivs(x, y, k1);     // First step : evaluating k1
  for (int i=0; i<y.size(); i++) yt[i] = y[i] + h2*k1[i];// Preparing second step by  ty <- y + k1/2
  derivs(xh, yt, k2);                                    // Second step : evaluating k2
  for (int i=0; i<y.size(); i++) yt[i] = y[i] + h2*k2[i];// Preparing third step by   yt <- y + k2/2
  derivs(xh, yt, k3);                                    // Third step : evaluating k3
  for (int i=0; i<y.size(); i++) yt[i] = y[i] +  h*k3[i];// Preparing fourth step  yt <- y + k3
  derivs(x+h, yt, k4);                                   // Final step : evaluating k4
  for (int i=0; i<y.size(); i++) y[i] += h/6.0*(k1[i]+2.0*(k2[i]+k3[i])+k4[i]);
}


void simple_pendulum(double x, const vector<double>& y, vector<double>& dydx)
{ // use time units omega*t->t
  // d^2 u/dt^2 = - omega^2 u    is written as
  // y[0,1] = [u(t), du/dt]
  // d y/dt = [du/dt, d^2u/dt] = [du/dt, -u] = [y[1],-y[0]]
  // Exact solution is y[0,1] = [xmax*sin(t), xmax*cos(t)]
  dydx[0] = y[1];
  dydx[1] = -y[0];
}
double Energy(const vector<double>& y)
{// Energy of the pendulum is E = 1/2*xmax ( u^2 +  (du/dt)^2 )
  return 0.5*(y[0]*y[0]+y[1]*y[1]);
}

int main()
{
  double E = 1;                 // our choice for energy in dimensionless units
  double t_start=0, t_stop=200; // elapsed time in dimensionless units
  double dh=0.1;                // time step

  double xmax = sqrt(2*E);      // Energy uniquely determines maximum amplitude
  vector<double> y(2);
  y[0]=0;                       // Pendulum is initially at zero but has maximum momentum
  y[1]=xmax;
  // The exact solution of this problem is:
  //    y[0,1] = [xmax*sin(t), xmax*cos(t)]
  
  cout.precision(15);
  int M = static_cast<int>((t_stop-t_start)/dh+0.5); // Number of timesteps
  double ti = t_start;
  for (int i=0; i<M; i++, ti+=dh) {
    cout<<setw(25)<<ti<<" "<<setw(25)<<y[0]<<" "<<setw(25)<<y[1]<<" "<<setw(25)<<y[0]-xmax*sin(ti)<<" "<<setw(25)<<Energy(y)<<endl;
    //Euler(ti, dh, y, simple_pendulum);
    RK4(ti, dh, y, simple_pendulum);
  }
  cout<<setw(25)<<ti<<" "<<setw(25)<<y[0]<<" "<<setw(25)<<y[1]<<" "<<setw(25)<<y[0]-xmax*sin(ti)<<" "<<setw(25)<<Energy(y)<<endl;
  
  return 0;
}