#include <algorithm>
#include <array>
#include <boost/numeric/odeint.hpp>
#include <fstream>
#include <gsl/gsl_spline.h>
#include <iostream>
#include <numeric>
#include <string>
#include <stdexcept>
#include <vector>

#include "loadtxt.h"



// Our units are {kiloparsec, solar mass, gigayear}
constexpr double G = 4.498317481097514e-06;

class Interp {
    // This is a wrapper around GSL spline interpolation. I tried to use
    // boost::math::interpolators but as of version 1.72 none were suitable:
    // barycentric_rational is the one suitalbe for non-uniform sampling but it
    // is very slow. I also tried to resample the data uniformly using
    // barycentric rational interpolation and then using cardinal_cubic_b_spline
    // on the result, but was still slower than GSL.
public:
    Interp(std::vector<double>& x, std::vector<double>& y)
    {
        acc    = gsl_interp_accel_alloc();
        spline = gsl_spline_alloc(gsl_interp_cspline, x.size());
        gsl_spline_init(spline, x.data(), y.data(), x.size());
    }
    Interp() {}
    inline double operator()(double x) const
    {
        return gsl_spline_eval(spline, x, acc);
    }

private:
    gsl_interp_accel *acc;
    gsl_spline *spline;
};

class Plummer {
public:
    Plummer(double M, double b)
        : M(M), b(b) {}
    void calc_acceleration(const double *pos, double *acc)
    {
        double r2 = (pos[0]*pos[0] + pos[1]*pos[1] + pos[2]*pos[2] + b*b);
        double r  = sqrt(r2);
        double r3_inv = 1/(r*r2);
        acc[0] = -G*M*pos[0]*r3_inv;
        acc[1] = -G*M*pos[1]*r3_inv;
        acc[2] = -G*M*pos[2]*r3_inv;
    }

private:
    double M, b;
};

class Galaxy {
public:
    Galaxy(std::string file_name)
    {
        auto data = Loadtxt("file.dat", {1, 2, 3}).get_cols();
        auto& t_data      = data[0];
        auto& halo_m_data = data[1];
        auto& halo_b_data = data[2];
        std::transform(begin(t_data), end(t_data), begin(t_data), [t0=t_data[0]](const double& x){ return x-t0; });
        std::transform(halo_b_data.begin(), halo_b_data.end(), halo_b_data.begin(), [](const double& x){ return x*0.7664209365408798; });
        interp_halo_m = Interp(t_data, halo_m_data);
        interp_halo_b = Interp(t_data, halo_b_data);
    }
    void func(const std::array<double, 6> &y, std::array<double, 6> &f, const double t)
    {
        double halo_m = interp_halo_m(t);
        double halo_b = interp_halo_b(t);
        Plummer plummer(halo_m, halo_b);
        f[0] = y[3]; // vx -> x'
        f[1] = y[4]; // vy -> y'
        f[2] = y[5]; // vz -> z'
        plummer.calc_acceleration(y.data(), f.data()+3); // a -> v'
    }

private:
    Interp interp_halo_m;
    Interp interp_halo_b;
} galaxy("file.dat");

extern "C"
int integrate(const double y0[], const double t_max, const double stride_size, double y[])
{
    using namespace boost::numeric::odeint;
    using Coordinates = std::array<double, 6>;
    auto stepper = bulirsch_stoer<Coordinates>(1E-7, 0);
    auto function_wrapper = [](const Coordinates &x, Coordinates &dxdt, const double t) { return galaxy.func(x, dxdt, t); };
    const int stride_count = t_max / stride_size;
    Coordinates y_current;
    std::copy(y0, y0+6, begin(y_current));
    std::copy(y0, y0+6, y);
    double t = 0;
    const double h = 1./4096.;
    for (int i=0; i<stride_count; i++) {
        integrate_adaptive(stepper, function_wrapper, y_current, t, t+stride_size, h);
        // NOTE h here is just a recommended initial step size for the stepper,
        // the actual step is adapted as needed. Since the integration is
        // interrupted here in order for the data to be saved, the result
        // somewhat depends on stride_size.
        std::copy(begin(y_current), end(y_current), y+(i+1)*6);
        t += stride_size;
    }
}


int main()
{
    std::cout << "Hi" << std::endl;

    double y[12];
    double y0[] = {80,0,0,0,80,0};
    for (int i=0; i<8000; i++)
        integrate(y0, 10, 10, y);

/*double y0[] = {80,0,0,0,80,0};
double y[12];
integrate(y0, 10, 10, y);
for (int i=0; i<12; i++) std::cout << y[i] << std::endl;*/

    //std::vector<double> w = {8, 0.12, 0.04, -0.08, 200, -0.001};



    std::cout << "Bye" << std::endl;
    return 0;
}