#include <iostream>
#include <cmath>
#include "smesh.h"
#include "sfunction.h"
#include <plotter.h>
#include "rungek.h"
#include "doublePendulum.h"

using namespace std;

void Poincare(Plotter& plotter, double x, double l, const function1D<double>& y, double fact=1)
{
  double static py2=0;
  if (y[2]*py2<0){
    double y0 = y[0];
    double y1 = y[1];
    while (y0<-M_PI) y0 += 2*M_PI;
    while (y1<-M_PI) y1 += 2*M_PI;
    while (y0>M_PI) y0 -= 2*M_PI;
    while (y1>M_PI) y1 -= 2*M_PI;
    int iy0 = static_cast<int>(round(y0*fact)), iy1 = static_cast<int>(round(y1*fact));
    plotter.point (iy0, iy1);
    plotter.flushpl();// to flush output, otherwise some atoms might be missing on the display
  }
  py2=y[2];
}

int main(int argc, char *argv[], char *env[])
{
  int l=1000; // length of the rod (in untis of plotter)
  double E=30, phi0=0., phi1=0.0; // Energy and initial conditions
  double dh=0.003, t_start=0, t_stop=100000; // time step, starting time and ending time
  int adaptive_step=0;
  
  int i=0;
  while (++i<argc){
    std::string str(argv[i]);
    if (str=="-E" && i<argc-1)      E      = atof(argv[++i]);
    if (str=="-phi0" && i<argc-1)   phi0   = atof(argv[++i]);
    if (str=="-phi1" && i<argc-1)   phi1   = atof(argv[++i]);
    if (str=="-dh" && i<argc-1)     dh     = atof(argv[++i]);
    if (str=="-t_stop" && i<argc-1) t_stop = atof(argv[++i]);
    if (str=="-adaptive")           adaptive_step = 1;
    if (str=="-h" || str=="--help"){
      std::clog<<"**************** Double pendulum simulation program *********\n";
      std::clog<<"**   Copyright Kristjan Haule, 24.1.2006                   **\n";
      std::clog<<"*************************************************************\n";
      std::clog<<"Usage: simulation [Options]\n";
      std::clog<<"Options:   -E         Energy of the double pendulum ("<<E<<")\n";
      std::clog<<"           -phi0      Starting angle of the upper pendulum  ("<<phi0<<")\n";
      std::clog<<"           -phi1      Starting angle of the lower pendulum  ("<<phi1<<")\n";
      std::clog<<"           -dh        Time-step ("<<dh<<")\n";
      std::clog<<"           -t_stop    Maximal time ("<<t_stop<<")\n";
      std::clog<<"           -adaptive  Adaptive Step ("<<adaptive_step<<")\n";
      return 0;
    }
  }

  //************** Initialization of plotter **************************/
  PlotterParams params; // set a Plotter parameter
  params.setplparam ("BITMAPSIZE", (char *)"500x500");
  XPlotter plotter(cin, cout, cerr, params); // declare Plotter
  //  GIFPlotter plotter(cin, cout, cerr, params); // declare Plotter
  if (plotter.openpl () < 0){ // open Plotter      
    cerr << "Couldn't open Plotter\n";
    return 1;
  }

  int sz = static_cast<int>(2.3*l);
  plotter.space (-sz, -sz, sz, sz);  // specify user coordinate system
  plotter.linewidth (1);           // line thickness in user coordinates 
  plotter.filltype (0);            // objects will be filled
  plotter.bgcolorname ("white"); // background color for the window
  double fact = 0.6*l;

  int M = static_cast<int>((t_stop-t_start)/dh+0.5);
  
  DoublePendulum double_pendulum(9.8);

  function1D<double> y(4);
  y[0]=phi0;
  y[1]=phi1;
  y[2]=0;
  y[3]=double_pendulum.p2(E,phi0,phi1);
  
  plotter.fmove (phi0*fact, phi1*fact);
  
  VRungeKutta<function1D<double> > rk(t_start, t_stop, y.size(),1e-8);
  
  double ti=t_start;
  for (int i=0; i<M; i++, ti+=dh) {
    Poincare(plotter, ti, l, y, fact);
    
    if (adaptive_step) {if (!rk.Step(ti, y,  double_pendulum)) break;}
    else rungek4(ti, dh, y, double_pendulum);
    
    if (i%10000==0) clog<<"Energy="<<double_pendulum.Energy(y)<<endl;
  }
  
  /************** Plotter Done**************************/
  if (plotter.closepl () < 0){// close Plotter
    fprintf (stderr, "Couldn't close Plotter\n");
    return 1;
  }
  /*****************************************************/
  return 0;
}