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

using namespace std;

void plot(Plotter& plotter, double l, const function1D<double>& y)
{
  static list<pair<int,int> > positions;
    
  int x0 = static_cast<int>(round(l*sin(y[0]))), y0=static_cast<int>(round(-l*cos(y[0])));
  int x1 = static_cast<int>(round(l*(sin(y[0])+sin(y[1])))), y1 = static_cast<int>(round(-l*(cos(y[0])+cos(y[1]))));
  
  positions.push_back(make_pair(x1,y1));

  plotter.erase ();            // erase window
  
  plotter.linewidth (1);
  plotter.filltype (0);        // objects will not be filled
  plotter.colorname ("blue"); // choose black pen, with black filling
  plotter.move(positions.begin()->first,positions.begin()->second);
  for (list<pair<int,int> >::const_iterator il=positions.begin(); il!=positions.end(); il++)
    plotter.cont(il->first,il->second);
  
  plotter.colorname ("black"); // choose black pen, with black filling
  plotter.linewidth (8);
  plotter.filltype (0);        // objects will not be filled
  plotter.line(0, 0, x0, y0);
  plotter.line(x0, y0, x1, y1);
  plotter.colorname ("red"); // choose black pen, with black filling
  plotter.filltype (1);        // objects will be filled
  plotter.circle (x0, y0, 50);
  plotter.circle (x1, y1, 50);
    
}

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 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=="-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";
      return 0;
    }
  }

  //************** Initialization of plotter **************************/
  PlotterParams params; // set a Plotter parameter
  params.setplparam ("BITMAPSIZE", (char *)"1000x1000");
  params.setplparam ("USE_DOUBLE_BUFFERING", (char *)"yes");// Crucial for smooth simulation
  XPlotter 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); // size of coordinate system
  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

  
  int M = static_cast<int>((t_stop-t_start)/dh+0.5); // Number of simulation steps
  
  DoublePendulum double_pendulum(9.8); // gravitational contant
  
  function1D<double> y(4);
  y[0]=phi0;
  y[1]=phi1;
  y[2]=0;
  y[3]=double_pendulum.p2(E,y[0],y[1]);;
  
  double ti = t_start;
  for (int i=0; i<M; i++, ti+=dh) {
    if (i%100==0) clog<<"Energy is "<<double_pendulum.Energy(y)<<endl;
    plot(plotter, l, y);
    rungek4(ti, dh, y, double_pendulum);
  }
  
  /************** Plotter Done**************************/
  if (plotter.closepl () < 0){// close Plotter
    fprintf (stderr, "Couldn't close Plotter\n");
    return 1;
  }
  /*****************************************************/
  return 0;
}