#include <iostream>
#include <cmath>
#include "smesh.h"
#include "sfunction.h"

/////////////////////////////////////////////////////////////////////////////
///////////////// METHOD OF RUNKGE-KUTTA 4-th ORDER   ///////////////////////
/////////////////////////////////////////////////////////////////////////////
template <class functor, class container>
void rungek4(double x, double h, container& y, functor& derivs)
{// Runke-Kutta of fourth order for fixed time-step h.
 // derivs is a function which evaluates RHS of the ODE system, y contains values of dependent variables
  static container yt(y.size()), fk1(y.size()), fk2(y.size()), fk3(y.size()), fk4(y.size()); // temporary arrays
  double h2=h*0.5;
  double xh = x + h2;
  int N = y.size();      
  derivs(x, y, fk1);                                  // First step : evaluating k1
  for (int i=0; i<N; i++) yt[i] = y[i] + h2*fk1[i];   // Preparing second step by  ty <- y + k1/2
  derivs(xh, yt, fk2);                                // Second step : evaluating k2
  for (int i=0; i<N; i++) yt[i] = y[i] + h2*fk2[i];   // Preparing third step by   yt <- y + k2/2
  derivs(xh, yt, fk3);                                // Third step : evaluating k3
  for (int i=0; i<N; i++) yt[i] = y[i] + h*fk3[i];    // Preparing fourth step  yt <- y + k3
  derivs(x+h, yt, fk4);                               // Final step : evaluating k4
  double h6=h/6.0;                                    // Accumulate increments with proper weights
  for (int i=0; i<N; i++) y[i] += h6*(fk1[i]+2.0*(fk2[i]+fk3[i])+fk4[i]);
}

void simple_pendulum(double x, const function1D<double>& y, function1D<double>& dydx)
{
  dydx[0] = y[1];
  dydx[1] = -y[0];
}
double Energy(const function1D<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);
  function1D<double> y(2);
  y[0]=0;
  y[1]=xmax;

  cout.precision(15);
  int M = static_cast<int>((t_stop-t_start)/dh+0.5);
  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;
    rungek4(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;
}