#include <iostream>
#include <vector>
#include <cmath>
#include <functional>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>

// The type of container used to hold the state vector 
typedef std::vector<double> state_type;
typedef std::function<double (double)> FunctionType;

namespace Bode = boost::numeric::odeint;
using namespace boost::numeric::odeint;


double p = 2.0;
double c = 10.0;

const double eps = 13.5;
const double M = 0.8;


double Kepler(double y)
{
 double d = y - eps*std::sin(y) - M;
 return d;
}

// NR function stuff
double f(double x)
{

 return Kepler(x);
}
double fd(double x)
{
 
 // Kepler
 return 1.0 - eps*std::cos(x);
}

//double x0 = 138.3377; // far away
//double x0 = 12.3; // far away
double x0 = 2.3000001; // try -20.0 as well

class WriteOutput
{
public:
 std::vector<double> tValues;
 std::vector<double> yValues;

public:
    WriteOutput() 
 : tValues(std::vector<double>()), yValues(std::vector<double>())
 { }
 
 void operator ()( const state_type &x , const double t )
 {
 tValues.push_back(t); yValues.push_back(x[0]); 

 std::cout << "Time and value (FO): " << t << " " << x[0] << std::endl;
 }
 };
// The rhs of x' = f(x) defined as a class 
class Davidenko
{
public:
    Davidenko() { }

    void operator() (const state_type& x, state_type& dHdt, const double t)
    {
  dHdt[0] = -f(x0)/fd(x[0]);
    }
};


void write_out( const state_type& x , const double t )
{
 if (t == 1.0)
 {
 std::cout << "Time and value: " << t << " " << x[0] << std::endl;
 }
}




int main()
{
    namespace Bode = boost::numeric::odeint;

    // Initial condition
    state_type x(1);
 x[0] = x0;

 double L = -1.0;
 double U = 2.0;
 double dt = 1.0e-3;

 // Integration_class, V2 based on Runge Kutta54 Cash Karp stepper (5th order)
 Davidenko ode;
    std::size_t steps = Bode::integrate(ode, x, L, U, dt, write_out);

 std::cout << "Number of steps " << steps << std::endl;
 std::cout << x[0] << ", " << f(x[0])<< std::endl;
 

 WriteOutput wo;
// Choice B: Fast adaptive
 typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
 typedef Bode::controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
 Bode::runge_kutta_fehlberg78< state_type, double> myStepper;
// steps = Bode::integrate_const(myStepper, ode, x, L, U, dt, boost::ref(wo));
// double abs_err = 1.0e-3 , rel_err = 1.0e-3, a_x = 1.0 , a_dxdt = 1.0; double dtODE = 1.0e-3; 

 //   controlled_stepper_type controlled_stepper(default_error_checker< double >( abs_err , rel_err , a_x , a_dxdt ) );
// steps = Bode::integrate_adaptive( controlled_stepper,ode, x , L, U, dtODE, boost::ref(wo) ); 
 
// std::cout << "Steps " << steps << ", " << f(x[0]) << std::endl;
 
 return 0;
}