#include <iostream>
#include <iomanip>
#include <cmath>
#include <vector>
#include "rungek.h"

using namespace std;

void simple_pendulum(double x, const vector<double>& y, vector<double>& dydx)
{
  dydx[0] = y[1];
  dydx[1] = -y[0];
}
double Energy(const vector<double>& y)
{
  return 0.5*(y[0]*y[0]+y[1]*y[1]);
}


using namespace std;
int main()
{
  double E = 1;
  double t_start=0, t_stop=200;
  double dh=0.1;

  double xmax = sqrt(2*E);
  vector<double> y(2);
  y[0]=0;
  y[1]=xmax;

  VRungeKutta<vector<double> > rk(t_start, t_stop, y.size(), 1e-8); // RK class for adaptive step

  cout.precision(15);
  double ti = t_start;
  while (ti<t_stop){
    cout<<setw(25)<<ti<<" "<<setw(25)<<y[0]<<" "<<setw(25)<<y[1]<<" "<<setw(25)<<y[0]-xmax*sin(ti)<<" "<<setw(25)<<Energy(y)<<endl;
    if (!rk.Step(ti, y,  simple_pendulum)) break;
  }
  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;
}