#define BOOST_TEST_MODULE  quantity

#include <physical/runtime.h>
#include <physical/math.h>

#include <boost/test/unit_test.hpp>

#include <complex>
#include <iostream>

BOOST_AUTO_TEST_SUITE( quantity );//{

  BOOST_AUTO_TEST_CASE( getCoeff ) {
    using runtime::physical::quantity;
    quantity< std::complex<double> > a = std::complex<double>(1.0);
    quantity<double> b = 1.0;
    //std::cout << "a<complex>:  " << a << std::endl;
    /* default (GNU/Units like) printing */
    BOOST_CHECK_EQUAL( a.toString(), "<1>");
    BOOST_CHECK_EQUAL( a.getCoeff<double>(), b.getCoeff<double>() );
    BOOST_CHECK_EQUAL( b.getCoeff(), b.getCoeff<double>() );
    BOOST_CHECK_EQUAL( &b.getCoeff(), &b.coeff );
  }

  BOOST_AUTO_TEST_CASE( ugly_print ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;
    Quantity::setPrintMode(Quantity::UGLY_PRINT);

    Quantity a = Quantity(std::complex<double>(1.0,1.0), units_pair("kg",1));

    BOOST_CHECK_EQUAL( a.toString(), "(1,1) * {(kg : 1), }");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1,-1) * {(kg : 1), }");

    a *= Quantity(1.0, units_pair("s",-1));

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "(10,10) * {(kg : 1), (s : -1), }");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-10,-10) * {(kg : 1), (s : -1), }");

    a /= 100.0;
    BOOST_CHECK_EQUAL( a.toString(), "(0.1,0.1) * {(kg : 1), (s : -1), }");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-0.1,-0.1) * {(kg : 1), (s : -1), }");
  }

  BOOST_AUTO_TEST_CASE( math_print ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;
    Quantity::setPrintMode(Quantity::MATH_PRINT);

    Quantity a = Quantity(std::complex<double>(1.0,1.0), units_pair("kg",1));

    BOOST_CHECK_EQUAL( a.toString(), "((1 + i*1) * kg)" );
    BOOST_CHECK_EQUAL( (-1*a).toString(), "((-1 - i*1) * kg)" );

    a *= Quantity(1.0, units_pair("s",-1));

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "((10 + i*10) * kg / s)" );
    BOOST_CHECK_EQUAL( (-1*a).toString(), "((-10 - i*10) * kg / s)" );

    a /= 100.0;
    BOOST_CHECK_EQUAL( a.toString(), "((0.1 + i*0.1) * kg / s)" );
    BOOST_CHECK_EQUAL( (-1*a).toString(), "((-0.1 - i*0.1) * kg / s)" );

    a *= Quantity(1.0, units_pair("m",-1));

    a *= 100.0;
    BOOST_CHECK_EQUAL( a.toString(), "((10 + i*10) * kg / (m * s))" );
    BOOST_CHECK_EQUAL( (-1*a).toString(), "((-10 - i*10) * kg / (m * s))" );

    a /= 100.0;
    BOOST_CHECK_EQUAL( a.toString(), "((0.1 + i*0.1) * kg / (m * s))" );
    BOOST_CHECK_EQUAL( (-1*a).toString(), "((-0.1 - i*0.1) * kg / (m * s))" );
  }

  BOOST_AUTO_TEST_CASE( latex_print ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;
    Quantity::setPrintMode(Quantity::LATEX_PRINT);

    Quantity a = Quantity(std::complex<double>(1.0,1.0), units_pair("kg",1));

    BOOST_CHECK_EQUAL( a.toString(), "(1 + i1)~\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1 - i1)~\\mathrm{kg}");

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "(10 + i10)~\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-10 - i10)~\\mathrm{kg}");

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "(100 + i100)~\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-100 - i100)~\\mathrm{kg}");

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "(1 \\times 10^{3} + i1 \\times 10^{3})~\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1 \\times 10^{3} - i1 \\times 10^{3})~\\mathrm{kg}");

    a *= 10.0;
    BOOST_CHECK_EQUAL( a.toString(), "(1 \\times 10^{4} + i1 \\times 10^{4})~\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1 \\times 10^{4} - i1 \\times 10^{4})~\\mathrm{kg}");

    a = Quantity(std::complex<double>(1.0,1.0), units_pair("kg",-1));

    Quantity::setPrintMode(Quantity::LATEX_ONELINE_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(1 + i1)~1/\\mathrm{kg}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1 - i1)~1/\\mathrm{kg}");
    Quantity::setPrintMode(Quantity::LATEX_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(1 + i1)~\\frac{1}{\\mathrm{kg}}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-1 - i1)~\\frac{1}{\\mathrm{kg}}");

    a *= Quantity(1.0, units_pair("kg",-1));

    a /= 10.0;
    Quantity::setPrintMode(Quantity::LATEX_ONELINE_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(0.1 + i0.1)~1/\\mathrm{kg}^{2}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-0.1 - i0.1)~1/\\mathrm{kg}^{2}");
    Quantity::setPrintMode(Quantity::LATEX_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(0.1 + i0.1)~\\frac{1}{\\mathrm{kg}^{2}}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-0.1 - i0.1)~\\frac{1}{\\mathrm{kg}^{2}}");

    a /= 10.0;
    Quantity::setPrintMode(Quantity::LATEX_ONELINE_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(0.01 + i0.01)~1/\\mathrm{kg}^{2}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-0.01 - i0.01)~1/\\mathrm{kg}^{2}");
    Quantity::setPrintMode(Quantity::LATEX_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(0.01 + i0.01)~\\frac{1}{\\mathrm{kg}^{2}}");
    BOOST_CHECK_EQUAL( (-1*a).toString(), "(-0.01 - i0.01)~\\frac{1}{\\mathrm{kg}^{2}}");

    a /= 10.0;
    Quantity::setPrintMode(Quantity::LATEX_ONELINE_PRINT);
    BOOST_CHECK_EQUAL(
      a.toString(),
      "(1 \\times 10^{-3} + i1 \\times 10^{-3})~1/\\mathrm{kg}^{2}");
    BOOST_CHECK_EQUAL(
      (-1*a).toString(),
      "(-1 \\times 10^{-3} - i1 \\times 10^{-3})~1/\\mathrm{kg}^{2}");
    Quantity::setPrintMode(Quantity::LATEX_PRINT);
    BOOST_CHECK_EQUAL(
      a.toString(),
      "(1 \\times 10^{-3} + i1 \\times 10^{-3})~\\frac{1}{\\mathrm{kg}^{2}}");
    BOOST_CHECK_EQUAL(
      (-1*a).toString(),
      "(-1 \\times 10^{-3} - i1 \\times 10^{-3})~\\frac{1}{\\mathrm{kg}^{2}}");

    a /= 10.0;
    Quantity::setPrintMode(Quantity::LATEX_ONELINE_PRINT);
    BOOST_CHECK_EQUAL(
      a.toString(),
      "(1 \\times 10^{-4} + i1 \\times 10^{-4})~1/\\mathrm{kg}^{2}");
    BOOST_CHECK_EQUAL(
      (-1*a).toString(),
      "(-1 \\times 10^{-4} - i1 \\times 10^{-4})~1/\\mathrm{kg}^{2}");
    Quantity::setPrintMode(Quantity::LATEX_PRINT);
    BOOST_CHECK_EQUAL(
      a.toString(),
      "(1 \\times 10^{-4} + i1 \\times 10^{-4})~\\frac{1}{\\mathrm{kg}^{2}}");
    BOOST_CHECK_EQUAL(
      (-1*a).toString(),
      "(-1 \\times 10^{-4} - i1 \\times 10^{-4})~\\frac{1}{\\mathrm{kg}^{2}}");
  }

  BOOST_AUTO_TEST_CASE( complex_test ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;

    Quantity a = std::complex<double>(1.0,1.0);
    a *= Quantity(1.0, units_pair("kg",1));

    Quantity::setPrintMode(Quantity::UGLY_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(1,1) * {(kg : 1), }");
  }

  BOOST_AUTO_TEST_CASE( quantity_pow ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;

    Quantity a = Quantity(std::complex<double>(2.,2.), units_pair("kg",1));

    a = pow(a, 3.0);

    Quantity::setPrintMode(Quantity::UGLY_PRINT);
    //std::cout << "a<complex>:  " << a << std::endl;
    BOOST_CHECK_EQUAL( a.toString(), "(-16,16) * {(kg : 3), }");
  }

  BOOST_AUTO_TEST_CASE( assertMatch ) {
    typedef runtime::physical::quantity< std::complex<double> > Quantity;
    using runtime::physical::units_pair;

    Quantity a = Quantity(std::complex<double>(2.,2.), units_pair("kg",1));
    Quantity b = Quantity(std::complex<double>(2.,2.), units_pair());

    a = pow(a, 5.0);
    b = pow(b, 5.0);

    Quantity::setPrintMode(Quantity::UGLY_PRINT);
    BOOST_CHECK_EQUAL( a.toString(), "(-128,-128) * {(kg : 5), }");
    BOOST_CHECK_EQUAL( b.toString(), "(-128,-128) * {}");

    BOOST_CHECK_NO_THROW(
      a.assertMatch(Quantity(std::complex<double>(1.,1.), units_pair("kg",5)))
    );
    BOOST_CHECK_NO_THROW(
      b.assertMatch(Quantity(std::complex<double>(1.,1.), units_pair()))
    );
    BOOST_CHECK_NO_THROW( b.assertUnitless() );

    BOOST_CHECK_THROW(
      a.assertMatch(Quantity(std::complex<double>(1.,1.), units_pair("kg",1))),
      std::runtime_error
    );
    BOOST_CHECK_THROW(
      b.assertMatch(Quantity(std::complex<double>(1.,1.), units_pair("kg",1))),
      std::runtime_error
    );
    BOOST_CHECK_THROW( a.assertUnitless(), std::runtime_error );
  }

BOOST_AUTO_TEST_SUITE_END();//}


#ifndef BOOST_PREPROCESSOR_MAX_HPP
#define BOOST_PREPROCESSOR_MAX_HPP

/* Copyright (C) 2001
 * Housemarque Oy
 * http://www.housemarque.com
 *
 * Permission to copy, use, modify, sell and distribute this software is
 * granted provided this copyright notice appears in all copies. This
 * software is provided "as is" without express or implied warranty, and
 * with no claim as to its suitability for any purpose.
 *
 * See http://www.boost.org for most recent version.
 */

#include <boost/preprocessor/comparison/less_equal.hpp>
#include <boost/preprocessor/if.hpp>

/**
Expands to the maximum of X and Y.

Both X and Y must expand to integer literals
in the range [0, BOOST_PP_LIMIT_MAG].

For example, BOOST_PP_MAX(5,7) expands to 7 (a
single token).

Uses

BOOST_PP_WHILE()

Test

arithmetic_test.cpp

*/
#define BOOST_PP_MAX(X,Y) BOOST_PP_MAX_D(0,X,Y)

/**
Can be used inside BOOST_PP_WHILE().
 */
#define BOOST_PP_MAX_D(D,X,Y) BOOST_PP_IF(BOOST_PP_LESS_EQUAL_D(D,X,Y),Y,X)

/*
Obsolete. Use BOOST_PP_MAX().
 */
#define BOOST_PREPROCESSOR_MAX(X,Y) BOOST_PP_MAX(X,Y)
#endif


#pragma once
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <list>
#include <string>
#include <iomanip>
#include <limits>
#include <vector>
#include "interpolator.hpp"
#include "system.hpp"
#include "rksolver.hpp"
#include "wkbsolver.hpp"

/** A class to store all information related to a numerical solution run.  */
class Solution
{
    private:
    double t, tf, rtol, atol, h0;
    std::complex<double> x, dx;
    int order;
    const char* fo;
    WKBSolver * wkbsolver;
    WKBSolver1 wkbsolver1;
    WKBSolver2 wkbsolver2;
    WKBSolver3 wkbsolver3;
    /** A \a de_system object to carry information about the ODE. */
    de_system *de_sys_;
    /** These define the event at which integration finishes (currently: when tf
     * is reached. */
    double fend, fnext;
    /** a boolean encoding the direction of integration: 1/True for forward. */
    bool sign;

    public:
    Solution(de_system &de_sys, std::complex<double> x0, std::complex<double>
    dx0, double t_i, double t_f, int o=3, double r_tol=1e-4, double a_tol=0.0,
    double h_0=1, const char* full_output="");

    template<typename X = double> Solution(de_system &de_sys,
    std::complex<double> x0, std::complex<double> dx0, double t_i, double t_f,
    const X &do_times, int o=3, double r_tol=1e-4, double a_tol=0.0,
    double h_0=1, const char* full_output="");

    void solve();

    /** Object to call RK steps */
    RKSolver rksolver;

    /** Successful, total attempted, and successful WKB steps the solver took,
     * respectively  */
    int ssteps,totsteps,wkbsteps;
    /** Lists to contain the solution and its derivative evaluated at internal
     * points taken by the solver (i.e. not dense output) after a run */
    std::list<std::complex<double>> sol, dsol;
    /** List to contain the timepoints at which the solution and derivative are
     * internally evaluated by the solver */
    std::list<double> times;
    /** List to contain the "type" of each step (RK/WKB) taken internally by the
     * solver after a run */
    std::list<bool> wkbs;
    /** Lists to contain the timepoints at which dense output was evaluated */
    std::list<double> dotimes, dotimes_rk;
    /** Lists to contain the dense output of the solution and its derivative */
    std::list<std::complex<double>> dosol, dodsol, dosol_rk, dodsol_rk;
    /** Iterator to iterate over the dense output timepoints, for when these
     * need to be written out to file */
    std::list<double>::iterator dotit;

};

/** Constructor for when dense output was not requested. Sets up solution of the
 * ODE.
 *
 * @param[in] de_sys de_system object carrying information about the ODE being
 * solved
 * @param[in] x0, dx0 initial conditions for the ODE, \f$ x(t) \f$, \f$ \frac{dx}{dt} \f$ evaluated at
 * the start of the integration range
 * @param[in] t_i start of integration range
 * @param[in] t_f end of integration range
 * @param[in] o order of WKB approximation to be used
 * @param[in] r_tol (local) relative tolerance
 * @param[in] a_tol (local) absolute tolerance
 * @param[in] h_0 initial stepsize to use
 * @param[in] full_output file name to write results to
 *
 */
Solution::Solution(de_system &de_sys, std::complex<double> x0,
std::complex<double> dx0, double t_i, double t_f, int o, double r_tol, double
a_tol, double h_0, const char* full_output){

    // Make underlying equation system accessible
    de_sys_ = &de_sys;

    // Set parameters for solver
    x = x0;
    dx = dx0;
    t = t_i;
    tf = t_f;
    order = o;
    rtol = r_tol;
    atol = a_tol;
    h0 = h_0;
    fo = full_output;
    rksolver = RKSolver(*de_sys_);

    // Determine direction of integration, fend>0 and integration ends when
    // it crosses zero
    if((t>=tf) and h0<0){
        // backwards
        fend = t-tf;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 0;
            de_sys_->Ginterp.sign_ = 0;
        }
        else
            sign = 0;

    }
    else if((t<=tf) and h0>0){
        // forward
        fend = tf-t;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 1;
            de_sys_->Ginterp.sign_ = 1;
        }
        else
            sign = 1;
    }
    else{
        throw "Direction of integration in conflict with direction of initial step, terminating. Please check your values for ti, tf, and h. ";
        return;
    }

    // No dense output desired if this constructor was called, so only output
    // answer at t_i and t_f
    dotimes.push_back(t_i);
    dotimes.push_back(t_f);
    dosol.push_back(x0);
    dodsol.push_back(dx0);
    dotit = dotimes.end();

    switch(order){
        case 1: wkbsolver1 = WKBSolver1(*de_sys_, order);
                wkbsolver = &wkbsolver1;
                break;
        case 2: wkbsolver2 = WKBSolver2(*de_sys_, order);
                wkbsolver = &wkbsolver2;
                break;
        case 3: wkbsolver3 = WKBSolver3(*de_sys_, order);
                wkbsolver = &wkbsolver3;
                break;
    };
};

/** Constructor for when dense output was requested. Sets up solution of the
 * ODE.
 *
 * @param[in] de_sys de_system object carrying information about the ODE being
 * solved
 * @param[in] x0, dx0 initial conditions for the ODE, \f$ x(t) \f$, \f$ \frac{dx}{dt} \f$ evaluated at
 * the start of the integration range
 * @param[in] t_i start of integration range
 * @param[in] t_f end of integration range
 * @param[in] do_times timepoints at which dense output is to be produced
 * @param[in] o order of WKB approximation to be used
 * @param[in] r_tol (local) relative tolerance
 * @param[in] a_tol (local) absolute tolerance
 * @param[in] h_0 initial stepsize to use
 * @param[in] full_output file name to write results to
 *
 */
template<typename X> Solution::Solution(de_system &de_sys, std::complex<double> x0,
std::complex<double> dx0, double t_i, double t_f, const X &do_times, int o, double r_tol, double
a_tol, double h_0, const char* full_output){

    // Make underlying equation system accessible
    de_sys_ = &de_sys;
    // Set parameters for solver
    x = x0;
    dx = dx0;
    t = t_i;
    tf = t_f;
    order = o;
    rtol = r_tol;
    atol = a_tol;
    h0 = h_0;
    fo = full_output;
    rksolver = RKSolver(*de_sys_);

    // Determine direction of integration, fend>0 and integration ends when
    // it crosses zero
    if((t>=tf) and h0<0){
        // backwards
        fend = t-tf;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 0;
            de_sys_->Ginterp.sign_ = 0;
        }
        else
            sign = 0;
    }
    else if((t<=tf) and h0>0){
        // forward
        fend = tf-t;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 1;
            de_sys_->Ginterp.sign_ = 1;
        }
        else
            sign = 1;
    }
    else{
        throw "Direction of integration in conflict with direction of initial step, terminating. Please check your values for ti, tf, and h. ";
        return;
    }

    // Dense output checks:
    int dosize = do_times.size();
    dotimes.resize(dosize);
    dosol.resize(dosize);
    dodsol.resize(dosize);
    int docount = 0;
    auto doit = do_times.begin();
    if(de_sys_->Winterp.sign_ == 1){
        for(auto it=dotimes.begin(); it!=dotimes.end(); it++){
            *it = *doit;
            docount++; doit++;
        }
    }
    else{
         for(auto it=dotimes.rbegin(); it!=dotimes.rend(); ++it){
            *it = *doit;
            docount++; ++doit;
        }
    }
    dotit = dotimes.begin();
    switch(order){
        case 1: wkbsolver1 = WKBSolver1(*de_sys_, order);
                wkbsolver = &wkbsolver1;
                break;
        case 2: wkbsolver2 = WKBSolver2(*de_sys_, order);
                wkbsolver = &wkbsolver2;
                break;
        case 3: wkbsolver3 = WKBSolver3(*de_sys_, order);
                wkbsolver = &wkbsolver3;
                break;
    };
};

/** \brief Function to solve the ODE \f$ \ddot{x} + 2\gamma(t)\dot{x} +
 * \omega^2(t)x = 0 \f$ for \f$ x(t), \frac{dx}{dt} \f$.
 *
 * While solving the ODE, this function will populate the \a Solution object
 * with the following results:
 *
 */
void Solution::solve(){

    int nrk, nwkb1, nwkb2;
    // Settings for MS
    nrk = 5;
    nwkb1 = 2;
    nwkb2 = 4;
    Eigen::Matrix<std::complex<double>,2,2> rkstep;
    Eigen::Matrix<std::complex<double>,3,2> wkbstep;
    Eigen::Matrix<std::complex<double>,1,2> rkx, wkbx;
    Eigen::Matrix<std::complex<double>,1,2> rkerr, wkberr, truncerr;
    Eigen::Matrix<double,1,2> errmeasure_rk;
    Eigen::Matrix<double,1,4> errmeasure_wkb;
    double tnext, hnext, h, hrk, hwkb;
    double wkbdelta, rkdelta;
    std::complex<double> xnext, dxnext;
    bool wkb = false;
    Eigen::Index maxindex_wkb, maxindex_rk;
    h = h0;
    tnext = t+h;
    // Initialise stats
    sol.push_back(x);
    dsol.push_back(dx);
    times.push_back(t);
    wkbs.push_back(false);
    ssteps = 0;
    totsteps = 0;
    wkbsteps = 0;
    // Dense output
    std::list<double> inner_dotimes;
    std::list<std::complex<double>> inner_dosols, inner_dodsols;
    auto it_dosol = dosol.begin();
    auto it_dodsol = dodsol.begin();
    Eigen::Matrix<std::complex<double>,1,2> y_dense_rk;
    std::complex<double> x_dense_rk, dx_dense_rk;

    while(fend > 0){
        // Check if we are reaching the end of integration
        if(fnext < 0){
            h = tf - t;
            tnext = tf;
        };

        // Keep updating stepsize until step is accepted
        while(true){
            // RK step
            rkstep = rksolver.step(x, dx, t, h);
            rkx << rkstep(0,0), rkstep(0,1);
            rkerr << rkstep(1,0), rkstep(1,1);
            // WKB step
            wkbstep = wkbsolver->step(x, dx, t, h, rksolver.ws, rksolver.gs, rksolver.ws5, rksolver.gs5);
            wkbx = wkbstep.row(0);
            wkberr = wkbstep.row(2);
            truncerr = wkbstep.row(1);
            // Safety feature for when all wkb steps are 0 (truncer=0), but not
            // necessarily in good WKB regime:
            truncerr(0) = std::max(1e-10,abs(truncerr(0)));
            truncerr(1) = std::max(1e-10,abs(truncerr(1)));
            // dominant error calculation
            // Error scale measures
            errmeasure_rk << std::abs(rkerr(0))/(std::abs(rkx(0))*rtol+atol), std::abs(rkerr(1))/(std::abs(rkx(1))*rtol+atol);
            errmeasure_wkb << std::abs(truncerr(0))/(std::abs(wkbx(0))*rtol+atol),
            std::abs(truncerr(1))/(std::abs(wkbx(1))*rtol+atol),
            std::abs(wkberr(0))/(std::abs(wkbx(0))*rtol+atol),
            std::abs(wkberr(1))/(std::abs(wkbx(1))*rtol+atol);
            rkdelta = std::max(1e-10, errmeasure_rk.maxCoeff(&maxindex_rk));
            if(std::isnan(errmeasure_wkb.maxCoeff())==false &&
               std::isinf(std::real(wkbx(0)))==false &&
               std::isinf(std::imag(wkbx(0)))==false &&
               std::isinf(std::real(wkbx(1)))==false &&
               std::isinf(std::imag(wkbx(1)))==false &&
               std::isnan(std::real(wkbx(0)))==false &&
               std::isnan(std::imag(wkbx(0)))==false &&
               std::isnan(std::real(wkbx(1)))==false &&
               std::isnan(std::imag(wkbx(1)))==false){
                wkbdelta = std::max(1e-10, errmeasure_wkb.maxCoeff(&maxindex_wkb));
            }
            else{
                wkbdelta = std::numeric_limits<double>::infinity();
            }

            // predict next stepsize
            hrk = h*std::pow((1.0/rkdelta),1.0/nrk);
            if(maxindex_wkb<=1)
                hwkb = h*std::pow(1.0/wkbdelta,1.0/nwkb1);
            else
                hwkb = h*std::pow(1.0/wkbdelta,1.0/nwkb2);
            // choose step with larger predicted stepsize
            if(std::abs(hwkb) >= std::abs(hrk)){
                wkb = true;
            }
            else{
                wkb = false;
            }
            if(wkb){
                xnext = wkbx(0);
                dxnext = wkbx(1);
                // if wkb step chosen, ignore truncation error in
                // stepsize-increase
                wkbdelta = std::max(1e-10, errmeasure_wkb.tail(2).maxCoeff());
                hnext = h*std::pow(1.0/wkbdelta,1.0/nwkb2);
            }
            else{
                xnext = rkx(0);
                dxnext = rkx(1);
                hnext = hrk;
            };
            totsteps += 1;
            // Checking for too many steps and low acceptance ratio:
            if(totsteps % 5000 == 0){
                std::cerr << "Warning: the solver took " << totsteps << " steps, and may take a while to converge." << std::endl;
                if(ssteps/totsteps < 0.05){
                    std::cerr << "Warning: the step acceptance ratio is below 5%, the solver may take a while to converge." << std::endl;
                }
            }

            // check if chosen step was successful
            if(std::abs(hnext)>=std::abs(h)){
                if(dotit!=dotimes.end()){
                    while((*dotit-t>=0 && tnext-*dotit>=0) or (*dotit-t<=0 && tnext-*dotit<=0)){
                        inner_dotimes.push_back(*dotit);
                        dotit++;
                    }
                    if(inner_dotimes.size() > 0){
                        inner_dosols.resize(inner_dotimes.size());
                        inner_dodsols.resize(inner_dotimes.size());
                        if(wkb){
                            // Dense output after successful WKB step
                            wkbsolver->dense_step(t,inner_dotimes,inner_dosols,inner_dodsols);
                        }
                        else{
                            // Dense output after successful RK step
                            for(auto it=inner_dotimes.begin(); it!=inner_dotimes.end(); it++)
                                rksolver.dense_step(t,h,x,dx,inner_dotimes,inner_dosols,inner_dodsols);
                        }
                    }
                }
                auto inner_it=inner_dosols.begin();
                auto inner_dit=inner_dodsols.begin();
                while(inner_it!=inner_dosols.end() && it_dosol!=dosol.end() && inner_dit!=inner_dodsols.end() && it_dodsol!=dodsol.end()){
                    *it_dosol = *inner_it;
                    *it_dodsol = *inner_dit;
                    it_dodsol++;
                    it_dosol++;
                    inner_it++;
                    inner_dit++;
                }
                inner_dotimes.resize(0);
                inner_dosols.resize(0);
                inner_dodsols.resize(0);

                // record type of step
                if(wkb){
                    wkbsteps +=1;
                    wkbs.push_back(true);
                }
                else{
                    wkbs.push_back(false);
                }
                sol.push_back(xnext);
                dsol.push_back(dxnext);
                times.push_back(tnext);
                tnext += hnext;
                x = xnext;
                dx = dxnext;
                t += h;
                h = hnext;
                if(h>0){
                    fend=tf-t;
                    fnext=tf-tnext;
                }
                else{
                    fend=t-tf;
                    fnext=tnext-tf;
                };
                ssteps +=1;
                // Update interpolation bounds
                if(de_sys_->is_interpolated == 1){
                    de_sys_->Winterp.update_interp_bounds();
                    de_sys_->Ginterp.update_interp_bounds();
                }

                break;
            }
            else{
                if(wkb){
                    if(maxindex_wkb<=1){
                        if(nwkb1 > 1)
                            hnext = h*std::pow(1.0/wkbdelta,1.0/(nwkb1-1));
                        else
                            hnext = 0.95*h*1.0/wkbdelta;
                    }
                    else
                        hnext = h*std::pow(1.0/wkbdelta,1.0/(nwkb2-1));
                }
                else
                    hnext = h*std::pow(1.0/rkdelta,1.0/(nrk-1));
                h = hnext;
                tnext = t + hnext;
                if(h>0){
                    fnext=tf-tnext;
                }
                else{
                    fnext=tnext-tf;
                };
            };
        };
    };

    // If integrating backwards, reverse dense output (because it will have been
    // reversed at the start)
    if(de_sys_->is_interpolated == 1){
        if(de_sys_->Winterp.sign_ == 0){
            dosol.reverse();
            dodsol.reverse();
        }
    }
    else{
        if(sign == 0){
            dosol.reverse();
            dodsol.reverse();
        }
    }

    // Write output to file if prompted
    if(not (*fo==0)){
        std::string output(fo);
        std::ofstream f;
        f.open(output);
        f << "# Summary:\n# total steps taken: " + std::to_string(totsteps) +
        "\n# of which successful: " + std::to_string(ssteps) + "\n# of which"+
        +" wkb: " + std::to_string(wkbsteps) + "\n# time, sol, dsol, wkb? (type)\n";
        auto it_t = times.begin();
        auto it_w = wkbs.begin();
        auto it_x = sol.begin();
        auto it_dx = dsol.begin();
        for(int i=0; i<=ssteps; i++){
            f << std::setprecision(15) << *it_t << ";" <<
            std::setprecision(15) << *it_x << ";" << std::setprecision(15) <<
            *it_dx << ";" << *it_w << "\n";
            ++it_t;
            ++it_x;
            ++it_dx;
            ++it_w;
        }
        // print all dense output to file
        int dosize = dosol.size();
        auto it_dosol = dosol.begin();
        auto it_dotimes = dotimes.begin();
        auto it_dodsol = dodsol.begin();
        for(int i=0; i<dosize; i++){
            f << std::setprecision(20) << *it_dotimes << ";" << std::setprecision(20) << *it_dosol << ";" << *it_dodsol << ";\n";
            ++it_dosol;
            ++it_dodsol;
            ++it_dotimes;
        }

        f.close();
    }

};


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This code will convert multiple cartesian state vectors
// into TLE and check where exactly the Atom is failing. The cartesian
// vectors are generated by converting randomly generated keplerian elements.
// The conversion is achieved through the pykep library of ESA.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>
#include <libsgp4/Globals.h>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include "CppProject/randomKepElem.hpp"
#include "CppProject/KepToCart.hpp"
#include "CppProject/KepToCartToTLE.hpp"
#include "CppProject/randomGen.hpp"

typedef double Real;
typedef std::vector< Real > Vector6;
typedef std::vector< Real > Vector3;
typedef std::vector< Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;

int main(void)
{

    // some constants values are defined here
    const double km2m = 1000; // conversion from km to m
    // earth radius
    const double EarthRadius = kXKMPER * km2m; // unit m
    const double EarthDiam = 2 * EarthRadius;
    // grav. parameter 'mu' of earth
    // const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
    const int bypass = true; // make this false to execute code with random orbital elements

    if(bypass == false){
        // initialize input parameters for the function generating random orbital elements. Description can be
        // found in randomKepElem.hpp for each of the following parameters.
        const Vector2 range_a      = { (EarthDiam+100000), (EarthDiam+1000000) };
        const Vector2 range_e      = { 0, 1 };
        const Vector2 range_i      = { 0, sml::convertDegreesToRadians( 180.0 ) };
        const Vector2 range_raan   = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_w      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_E      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const int limit            = 100;
        Vector2D randKep( limit, std::vector< Real >( 6 ) );

        // call the function to generate random keplerian orbital elements. Values are stored in randKepElem in a 2D
        // vector format. A single row represents one set of orbital elements, arranged in the same order as the
        // input argument order of the elements.
        randomKepElem::randomKepElem( range_a, range_e, range_i, range_raan, range_w, range_E, limit, randKep );

        // remove orbital elements where the radius of perigee is inside earth
        Vector2D randKepElem( limit, std::vector< Real >( 6 ) );
        int newLimit = limit; // whenever a row of cartesian (keplerian) elements is removed, the limit will have to be changed.
        Real radiusPerigee = 0; // variable storing the radius of Perigee for an orbit. this is used in our checking condition
        std::vector< int > rowsToDelete; // stores the row numbers which have to be deleted
        int insideCounter = 0; // to count the number of times radius of perigee is inside the Earth.
        int outsideCounter = 0; // to count the number of times the radius of perigee is beyond a cetain upper limit
        int newIndex = 0; // index counter for the second 2D vector
        for(int j = 0; j < newLimit; j++)
        {
            radiusPerigee = randKep[ j ][ 0 ] * (1 - randKep[ j ][ 1 ]);
            if(radiusPerigee <= EarthRadius )
            {
                insideCounter = insideCounter + 1; // counter increments everytime the condition is true
            }
            if(radiusPerigee >= (EarthRadius+2000000))
            {
                outsideCounter = outsideCounter + 1;
            }
            if(radiusPerigee > EarthRadius && radiusPerigee < (EarthRadius+2000000)) // this is obv not the most efficient way
            {
                randKepElem[ newIndex ] = randKep[ j ];
                newIndex++;
            }
        }
        randKepElem.erase( randKepElem.begin() + newIndex, randKepElem.end() ); // delete the left over rows in the final random keplerian element vector
        std::vector< std::vector < Real > > ().swap(randKep); // create an empty vector with no memory allocated to it ...
        // ... and swap it with the vector which you want to delete and deallocate the memory
        std::cout << "Inside counter value = " << insideCounter << std::endl;
        std::cout << "Outside counter value = " << outsideCounter << std::endl;
        std::cout << "Usefull sets left = " << limit - (insideCounter + outsideCounter) << std::endl;
        newLimit = randKepElem.size();
        std::cout << "Number of final rows in randKepElem 2D vector = " << newLimit << std::endl;
        KepToCartToTLE::KepToCartToTLE( newLimit, randKepElem );
    }
    else{
            const int newLimit = 2;
            Vector2D randKepElem( newLimit, std::vector < Real >( 6 ) );
            int indexer = 0;

            Vector2 range_a = { (EarthRadius + 100 * km2m), (EarthRadius + 1000 * km2m) }; // range for semi major axis
            std::vector< Real > semiAxis (newLimit, 0); // initialize vector of size newLimit with all zeros as elements
            randomGen::randomGen( range_a, newLimit, semiAxis); // random generator
            for(int i = 0; i < newLimit; i++)
            {
                randKepElem[ i ][ 0 ] = semiAxis[ i ]; // semi major axis
                randKepElem[ i ][ 1 ] = 0; // eccentricity
                randKepElem[ i ][ 2 ] = sml::convertDegreesToRadians( 0 ); // inclination
                randKepElem[ i ][ 3 ] = sml::convertDegreesToRadians( 0 ); // RAAN
                randKepElem[ i ][ 4 ] = sml::convertDegreesToRadians( 0 ); //AOP
                randKepElem[ i ][ 5 ] = sml::convertDegreesToRadians( 0 ); //EA
            }
            while( indexer < newLimit)
            {
                try
                {
                    indexer++;
                    KepToCartToTLE::KepToCartToTLE( 1, randKepElem );
                    std::cout << "indexer = " << indexer << std::endl;

                }
                catch(const std::exception& err)
                {
                    std::cout << "Error Caught = ";
                    std::cout << err.what() << std::endl;
                }
            }
        }
   return EXIT_SUCCESS;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_ros/blob/master/LICENSE

#ifndef CBR_ROS__MSG_UTILS_HPP_
#define CBR_ROS__MSG_UTILS_HPP_

#include <Eigen/Dense>

#include <builtin_interfaces/msg/time.hpp>

#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include <chrono>
#include <utility>

namespace cbr::msgs
{

namespace detail
{
// Traits to check if it's a chrono_duration
template<typename T>
struct is_chrono_duration : std::false_type {};
template<typename R, typename P>
struct is_chrono_duration<std::chrono::duration<R, P>>: std::true_type {};
}

/********************************************
    ROS types to Eigen/chrono types
*********************************************/

/**
 * Point/Vector3 message to Eigen Vector3d
 */
template<typename T>
inline std::enable_if_t<
  std::is_same_v<T, geometry_msgs::msg::Point>|| std::is_same_v<T, geometry_msgs::msg::Vector3>,
  Eigen::Vector3d
>
from_msg(const T & msg)
{
  return Eigen::Vector3d(msg.x, msg.y, msg.z);
}

/**
 * Twist message to Eigen Vector6d
 */
inline Eigen::Matrix<double, 6, 1> from_msg(const geometry_msgs::msg::Twist & msg)
{
  return (Eigen::Matrix<double, 6, 1>() << from_msg(msg.linear), from_msg(msg.angular)).finished();
}

/**
 * Quaternion message to Eigen quaternion
 */
inline Eigen::Quaterniond from_msg(const geometry_msgs::msg::Quaternion & msg)
{
  return Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
}

/**
 * Pose message to Eigen transform
 */

inline Eigen::Isometry3d from_msg(const geometry_msgs::msg::Pose & msg)
{
  return Eigen::Isometry3d(
    Eigen::Translation3d(from_msg(msg.position)) *
    from_msg(msg.orientation));
}

/**
 * Transform message to Eigen transform
 */
inline Eigen::Isometry3d from_msg(const geometry_msgs::msg::Transform & msg)
{
  return Eigen::Isometry3d(
    Eigen::Translation3d(from_msg(msg.translation)) *
    from_msg(msg.rotation));
}

/**
 * ROS duration to std::chrono duration
 */
template<typename T>
std::enable_if_t<detail::is_chrono_duration<T>::value, T>
from_msg(const builtin_interfaces::msg::Time & stamp)
{
  return std::chrono::duration_cast<T>(
    std::chrono::seconds(stamp.sec) + std::chrono::nanoseconds(stamp.nanosec)
  );
}

/********************************************
    Eigen/chrono types to ROS types
*********************************************/

/**
 * Eigen Vector3 to Point/Vector3/Lla message
 */
template<typename T = geometry_msgs::msg::Point, typename Derived>
std::enable_if_t<
  Derived::IsVectorAtCompileTime && Derived::SizeAtCompileTime == 3 &&
  (std::is_same_v<T, geometry_msgs::msg::Point>||
  std::is_same_v<T, geometry_msgs::msg::Vector3>),
  T
>
to_msg(const Eigen::MatrixBase<Derived> & p)
{
  T ret;
  ret.x = p.x();
  ret.y = p.y();
  ret.z = p.z();
  return ret;
}

/**
 * Eigen Vector6 to Twist message
 */
template<typename T = geometry_msgs::msg::Twist, typename Derived>
std::enable_if_t<
  Derived::IsVectorAtCompileTime && Derived::SizeAtCompileTime == 6 &&
  std::is_same_v<T, geometry_msgs::msg::Twist>,
  T
>
to_msg(const Eigen::MatrixBase<Derived> & p)
{
  geometry_msgs::msg::Twist ret;
  ret.linear = to_msg<geometry_msgs::msg::Vector3>(p.template head<3>());
  ret.angular = to_msg<geometry_msgs::msg::Vector3>(p.template tail<3>());
  return ret;
}

/**
 * Eigen Quaternion to Quaternion msg
 */
template<typename T = geometry_msgs::msg::Quaternion, typename Scalar>
std::enable_if_t<std::is_same_v<T, geometry_msgs::msg::Quaternion>, T>
to_msg(const Eigen::Quaternion<Scalar> & q)
{
  T ret;
  ret.w = q.w();
  ret.x = q.x();
  ret.y = q.y();
  ret.z = q.z();
  return ret;
}

/**
 * Eigen transform to Pose/Transform message
 */
template<typename T = geometry_msgs::msg::Pose, typename Scalar>
std::enable_if_t<
  std::is_same_v<T, geometry_msgs::msg::Pose>||
  std::is_same_v<T, geometry_msgs::msg::Transform>,
  T
>
to_msg(const Eigen::Transform<Scalar, 3, Eigen::Isometry> & pose)
{
  T ret;
  if constexpr (std::is_same_v<T, geometry_msgs::msg::Pose>) {
    ret.position = to_msg<geometry_msgs::msg::Point>(pose.translation());
    ret.orientation = to_msg(Eigen::Quaternion<Scalar>(pose.rotation()));
  }
  if constexpr (std::is_same_v<T, geometry_msgs::msg::Transform>)
  {
    ret.translation = to_msg<geometry_msgs::msg::Vector3>(pose.translation());
    ret.rotation = to_msg(Eigen::Quaternion<Scalar>(pose.rotation()));
  }
  return ret;
}

/**
 * std::chrono duration to ROS duration
 */
template<typename T = builtin_interfaces::msg::Time, typename S>
std::enable_if_t<detail::is_chrono_duration<S>::value, T>
to_msg(const S & t)
{
  T ret;
  auto t_sec = std::chrono::duration_cast<std::chrono::seconds>(t);
  ret.sec = static_cast<int>(t_sec.count());
  ret.nanosec = static_cast<unsigned int>(std::chrono::nanoseconds(
      std::chrono::duration_cast<std::chrono::nanoseconds>(t) - t_sec).count());
  return ret;
}

inline geometry_msgs::msg::Vector3 point_vec3(const geometry_msgs::msg::Point & p)
{
  geometry_msgs::msg::Vector3 out;
  out.x = p.x;
  out.y = p.y;
  out.z = p.z;
  return out;
}

inline geometry_msgs::msg::Point point_vec3(const geometry_msgs::msg::Vector3 & v)
{
  geometry_msgs::msg::Point out;
  out.x = v.x;
  out.y = v.y;
  out.z = v.z;
  return out;
}

}  // namespace cbr::msgs

#endif  // CBR_ROS__MSG_UTILS_HPP_


#ifndef SLIDE_POINT_HPP_
#define SLIDE_POINT_HPP_

#include <array>
#include <cstdlib>
#include <iostream>

#include <boost/assert.hpp>

#include "Direction.hpp"
#include "util/abs.hpp"
#include "util/define.hpp"

#ifdef __gcc__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif

namespace slide
{

class Point
{
public:
	int y;
	int x;

	Point() = default;

	constexpr Point(int y, int x) : y(y), x(x) {}

	constexpr explicit Point(uchar v, int w = MAX_DIVISION_NUM) : Point(v/w, v%w) {}

	constexpr uchar toInt(int w = MAX_DIVISION_NUM) const {
		return y * w + x;
	}

	constexpr int l1norm() const {
		return util::abs(y) + util::abs(x);
	}

	constexpr bool isIn(int height, int width) const {
		return isIn(0, 0, height, width);
	}

	constexpr bool isIn(int py, int px, int height, int width) const {
		return py <= y && y < py+height && px <= x && x < px+width;
	}

	constexpr Point operator+ (const Point& p) const {
		return Point(y+p.y, x+p.x);
	}

	constexpr Point operator- (const Point& p) const {
		return Point(y-p.y, x-p.x);
	}

	constexpr Point operator+ () const {
		return *this;
	}

	constexpr Point operator- () const {
		return Point(-y, -x);
	}

	Point& operator+=(const Point& p){
		y += p.y;
		x += p.x;
		return *this;
	}

	Point& operator-=(const Point& p){
		y -= p.y;
		x -= p.x;
		return *this;
	}

	constexpr Point operator* (const int v) const {
		return Point(y*v, x*v);
	}

	Point& operator*=(const int v) {
		y *= v;
		x *= v;
		return *this;
	}

	constexpr bool operator==(const Point& p) const {
		return y == p.y && x == p.x;
	}

	constexpr bool operator!=(const Point& p) const {
		return y != p.y || x != p.x;
	}

	bool operator< (const Point& p) const {
		return (x != p.x ? x < p.x : y < p.y);
	}

	static constexpr Point delta(Direction dir) {
		/*
		// I wanna use C++14!
		switch(dir){
			case Direction::Up:    return Point(-1,  0);
			case Direction::Right: return Point( 0,  1);
			case Direction::Down:  return Point( 1,  0);
			case Direction::Left:  return Point( 0, -1);
		}
		*/
		return
		dir == Direction::Up ?    Point(-1,  0) : (
		dir == Direction::Right ? Point( 0,  1) : (
		dir == Direction::Down ?  Point( 1,  0) :
								  Point( 0, -1)
		));
	}

	friend std::ostream& operator<<(std::ostream& out, const Point& p) {
		return out << '(' << p.x << ',' << p.y << ')';
	}

    /**************************************************************************
     * Rotation and flip
     *************************************************************************/

	constexpr Point rotateCW(int H) const {
		return Point(x, H-y-1);
	}

	constexpr Point rotateCCW(int W) const {
		return Point(W-x-1, y);
	}

	constexpr Point opposite(int H, int W) const {
		return Point(H-y-1, W-x-1);
	}

	constexpr Point flipY(int H) const {
		return Point(H-y-1, x);
	}

	constexpr Point flipX(int W) const {
		return Point(y, W-x-1);
	}

	Point rotateFlip(Direction topSide, bool yflip, bool xflip, int H, int W) const {
		Point ret = *this;
		if(yflip) ret = ret.flipY(H);
		if(xflip) ret = ret.flipX(W);
		if(topSide == Direction::Up)    return ret;
		if(topSide == Direction::Right) return rotateCCW(W);
		if(topSide == Direction::Down)  return opposite(H, W);
		return rotateCW(H);
	}
};

} // end of namespace slide

#ifdef __gcc__
#pragma GCC diagnostic pop
#endif

#endif


#include "spdlog/spdlog.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <limits>
#include <memory>
#include <mimkl/data_structures.hpp>
#include <mimkl/definitions.hpp>
#include <mimkl/io.hpp>
#include <mimkl/kernels.hpp>
#include <mimkl/kernels_handler.hpp>
#include <mimkl/linear_algebra.hpp>
#include <mimkl/models/easy_mkl.hpp>
#include <mimkl/solvers/komd.hpp>
#include <mimkl/utilities.hpp>
#include <numeric>
#include <spdlog/fmt/ostr.h>
#include <utility>

using dlib::mat;
using mimkl::data_structures::DataFrame;
using mimkl::data_structures::indexing_from_vector;
using mimkl::data_structures::range;
using mimkl::definitions::Indexing;
using mimkl::kernels_handler::KernelsHandler;
using mimkl::utilities::check_invocable;
using mimkl::utilities::print_type;

//#define SPDLOG_DEBUG_ON
//#define SPDLOG_TRACE_ON

// Compile time log levels
// define SPDLOG_DEBUG_ON or SPDLOG_TRACE_ON
// SPDLOG_TRACE(console, "Enabled only #ifdef SPDLOG_TRACE_ON..{} ,{}",
// 1, 3.23);  SPDLOG_DEBUG(console, "Enabled only #ifdef SPDLOG_DEBUG_ON.. {}
// ,{}", 1, 3.23);
typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &)>
InducedFunction;
typedef std::vector<InducedFunction> Function_vec;
typedef std::vector<MATRIX(double)> Matrix_vec;

void test_kernels_handler(const Function_vec function_vec,
                          const Matrix_vec matrix_vec,
                          const Matrix_vec matrix_vec_test,
                          const bool precompute,
                          const bool trace_normalization,
                          std::shared_ptr<spdlog::logger> console,
                          MATRIX(double) X,
                          MATRIX(double) Y,
                          COLUMN(double) w)
{
    console->trace("rows X,Y : {},{}", X.rows(), Y.rows());
    console->trace("sizes matric_vec : {},{}, sizes matric_vec_test : {},{}",
                   matrix_vec[0].rows(), matrix_vec[0].cols(),
                   matrix_vec_test[0].rows(), matrix_vec_test[0].cols());

    console->info("\n\n construction\n\n");
    MATRIX(double) outM, outF;

    KernelsHandler<double, InducedFunction> KHF(function_vec, precompute,
                                                trace_normalization);
    //	KHF.kernels->make_X_train_available(X);
    KHF.set_lhs(X);
    console->trace("KHF (train) sizes lhs,rhs : {},{}", KHF.get_lhs_size(),
                   KHF.get_rhs_size());
    KernelsHandler<double, InducedFunction> KHM(matrix_vec, precompute,
                                                trace_normalization);
    console->trace("KHM (train) sizes lhs,rhs : {},{}", KHM.get_lhs_size(),
                   KHF.get_rhs_size());

    console->info("\n\ntrain\n\n");

    console->info("[]");
    //	outF = KHF.kernels->operator [](0);
    //	outM = KHM.kernels->operator [](0);
    outF = KHF[0];
    outM = KHM[0];
    console->info("Functional access a kernel :\n{}", outF);
    console->info("Matricial access a kernel :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "[]");

    console->info("sum()");
    //	outF = KHF.kernels->sum();
    //	outM = KHM.kernels->sum();
    outF = KHF.sum();
    outM = KHM.sum();
    console->info("Functional sum() :\n{}", outF);
    console->info("Matricial sum() :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "sum()");

    console->info("weighted_sum()");
    //	outF = KHF.kernels->sum(w);
    //	outM = KHM.kernels->sum(w);
    outF = KHF.sum(w);
    outM = KHM.sum(w);
    console->info("Functional sum(weighted) :\n{}", outF);
    console->info("Matricial sum(weighted) :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "weighted_sum()");

    console->info("\n\ntest\n\n");
    KHF.set_rhs(Y);
    KHM.set_matrices(matrix_vec_test);
    console->trace("KHF (train) sizes lhs,rhs : {},{}", KHF.get_lhs_size(),
                   KHF.get_rhs_size());
    console->trace("KHM (train) sizes lhs,rhs : {},{}", KHM.get_lhs_size(),
                   KHF.get_rhs_size());

    console->info("[]");
    //	outF = KHF.kernels->operator [](0);
    //	outM = KHM.kernels->operator [](0);
    outF = KHF[0];
    outM = KHM[0];
    console->info("Functional access a kernel :\n{}", outF);
    console->info("Matricial access a kernel :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "[]");

    console->info("sum()");
    //	outF = KHF.kernels->sum();
    //	outM = KHM.kernels->sum();
    outF = KHF.sum();
    outM = KHM.sum();
    console->info("Functional sum() :\n{}", outF);
    console->info("Matricial sum() :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "sum()");

    console->info("weighted_sum()");
    //	outF = KHF.kernels->sum(w);
    //	outM = KHM.kernels->sum(w);
    outF = KHF.sum(w);
    outM = KHM.sum(w);
    console->info("Functional sum(weighted) :\n{}", outF);
    console->info("Matricial sum(weighted) :\n{}", outM);
    assert(((outF - outM).norm() == 0) && "weighted_sum()");
}

int main(int argc, char **argv)
{

    // Runtime log levels
    spdlog::set_level(spdlog::level::trace); // Set global log level to info
    auto console = spdlog::stdout_color_mt("console");

    const Index rows = 4; // 3 to reproduce single member in class  error
    const Index dims = 2;

    MATRIX(double) X(rows, 2);
    //	X << 1., 1., 3., 1., 1., 2.;
    X << 1., 1., 3., 1., 1., 4., 3., 2.;
    console->info("X\n{}", X);

    std::vector<std::string> labels;
    labels.reserve(rows);
    labels.push_back("a");
    labels.push_back("b");
    labels.push_back("a");
    labels.push_back("b");

    Eigen::SparseMatrix<double> L(dims, dims);
    mimkl::linear_algebra::fill_sparse_diagonal(L, 1.0);

    Eigen::SparseMatrix<double> L1(dims, dims);
    typedef Eigen::Triplet<double> TripletDouble; // (row,col,coef)
    std::vector<TripletDouble> triplet_list;
    triplet_list.reserve(4);
    triplet_list.push_back(TripletDouble(0, 1, 1.));
    //  triplet_list.push_back(TripletDouble(1, 2, 1.));
    triplet_list.push_back(TripletDouble(1, 0, 1.));
    //  triplet_list.push_back(TripletDouble(2, 1, 1.));
    L1.setFromTriplets(triplet_list.begin(), triplet_list.end());

    MATRIX(double)
    K_ref0 = mimkl::induction::induce_linear_kernel<MATRIX(double)>(X, X, L);
    console->info("a kernel:\n{}", K_ref0);
    //
    MATRIX(double)
    K_ref1 =
    mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(X, X, L1, 1, 0);
    console->info("another kernel:\n{}", K_ref1);

    std::vector<Eigen::SparseMatrix<double>> inducer_vec;
    inducer_vec.reserve(2);
    inducer_vec.push_back(L);
    inducer_vec.push_back(L1);

    typedef std::function<MATRIX(double)(const MATRIX(double) &, const MATRIX(double) &,
                                         const Eigen::SparseMatrix<double>)>
    InducerFunction;
    typedef std::function<MATRIX(double)(const MATRIX(double) &,
                                         const MATRIX(double) &)>
    InducedFunction;

    const double degree = 1.;
    const double offset = 0.;

    InducerFunction k_poly =
    [degree, offset](const MATRIX(double) & lhs, const MATRIX(double) & rhs,
                     const Eigen::SparseMatrix<double> inducer) {
        return mimkl::induction::induce_polynomial_kernel<MATRIX(double)>(
        lhs, rhs, inducer, degree, offset);
    };

    /////// here we go, preparing functional and matricial vectors
    std::vector<InducedFunction> function_vec =
    mimkl::induction::inducer_combination(k_poly, inducer_vec);

    MATRIX(double) K2 = function_vec[0](X, X);
    console->info("a kernel from function:\n{}", K2);

    MATRIX(double) K3 = function_vec[1](X, X);
    console->info("another kernel from function:\n{}", K3);

    std::vector<MATRIX(double)> matrix_vec(2);
    matrix_vec[0] = K2;
    matrix_vec[1] = K3;

    assert(((K_ref0 - K2).norm() == 0) && "Identity Inducer");
    assert(((K_ref1 - K3).norm() == 0) && "non-identity Inducer");

    console->info("Matricial vector invocable : {}",
                  check_invocable(matrix_vec[0]));
    console->info("Functional vector invocable : {}",
                  check_invocable(function_vec[0]));

    MATRIX(double) Y(6, 2);
    Y << 1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12.;
    console->info("Y\n{}", Y);
    std::vector<MATRIX(double)> matrix_vec_test(2);

    matrix_vec_test[0] = function_vec[0](X, Y);
    matrix_vec_test[1] = function_vec[1](X, Y);

    console->trace("some test kernel:\n{}", matrix_vec_test[0]);
    ////////
    MATRIX(double) *X_ptr = &X;
    MATRIX(double) *Y_ptr = &Y;
    console->trace("some test kernel from dereferenced pointers :\n{}",
                   function_vec[0](*X_ptr, *Y_ptr));

    ////////
    COLUMN(double) w(2);
    w << 1., 1.;
    MATRIX(double) out;

    //
    console->info("precomputed=false trace_normalization=false");
    test_kernels_handler(function_vec, matrix_vec, matrix_vec_test, false,
                         false, console, X, Y, w);

    console->info("precomputed=true trace_normalization=false");
    test_kernels_handler(function_vec, matrix_vec, matrix_vec_test, true, false,
                         console, X, Y, w);

    console->info("precomputed=false trace_normalization=true");
    test_kernels_handler(function_vec, matrix_vec, matrix_vec_test, false, true,
                         console, X, Y, w);

    console->info("default precomputed=true trace_normalization=true");
    test_kernels_handler(function_vec, matrix_vec, matrix_vec_test, true, true,
                         console, X, Y, w);
}


#pragma once

#include <Eigen/Dense>
#include "geometry/circular_section.hh"
#include "types.hh"

#include <vector>

namespace sonder {

struct IntersectionVotes {
  IntersectionVotes() = default;
  IntersectionVotes(const PointList &_points, const std::vector<std::size_t> &_votes) : points(_points), votes(_votes) {
  }

  inline void clear() {
    points.clear();
    votes.clear();
  }

  inline void add_point(const Eigen::Vector3f &point, const std::size_t point_votes = 1) {
    // constexpr float MIN_DIST_TO_ADD = 1e-2;
    constexpr float MIN_DIST_TO_ADD = 1e-1;

    for (std::size_t k = 0; k < points.size(); ++k) {
      if ((point - points[k]).norm() < MIN_DIST_TO_ADD) {
        votes[k] += point_votes;
        _max_vote = votes[k] > _max_vote ? votes[k] : _max_vote;
        return;
      }
    }
    points.push_back(point);
    votes.push_back(point_votes);
    _max_vote = point_votes > _max_vote ? point_votes : _max_vote;
  }

  inline float max_votes() const {
    return static_cast<float>(_max_vote);
  }

  PointList                points;
  std::vector<std::size_t> votes;

private:
  size_t _max_vote = 0;
};

// A gazillion potential optimizations
//
// Potential improvements:
// - Bias the search by already which arcs the points belong to (Does that make sense?)
// - Implement as kdtree
//
void add_points(const IntersectionVotes &new_votes, Out<IntersectionVotes> old_votes) {
  for (std::size_t new_index = 0; new_index < new_votes.points.size(); ++new_index) {
    const Eigen::Vector3f &pt = new_votes.points[new_index];
    old_votes->add_point(pt, new_votes.votes[new_index]);
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////
// Generic geometry
/////////////////////////////////////////////////////////////////////////////////////////////////

sonder::IntersectionVotes intersect_all_sections(const sonder::circular_section &             section,
                                                 const std::vector<sonder::circular_section> &other_sections) {
  sonder::IntersectionVotes all_intersections;

  //
  // Seek an intersection among `other_sections`
  //
  for (const auto &other_section : other_sections) {
    const auto intersections = section.intersect(other_section);
    for (const auto &pt : intersections) {
      all_intersections.add_point(pt);
    }
  }
  return all_intersections;
}
}


#include <UnitTest++/UnitTest++.h>

#include "../clean_and_register_subroutines.h"
#include "coela_core/src/convolution.h"
#include <iostream>
#include <sstream>
#include <boost/filesystem.hpp>

using namespace coela;
using namespace std;

SUITE(Clean_And_Register_Subroutines)
{
    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";

    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    struct camconf_fixture {
        camconf_fixture() {
            camconf.lens_inf.telescope_inner_diameter=0.5;
            camconf.lens_inf.telescope_outer_diameter=2.5;
            camconf.lens_inf.nominal_pixel_scale_in_mas=32.5;
            OpticalFilterInfo f;
            f.ccd_id=0;
            f.central_wavelength_in_metres=7.7e-7;
            f.name="i";
            camconf.filters.push_back(f);
        }
        CameraConfigInfo camconf;

    };

    TEST_FIXTURE(camconf_fixture, Airy_template) {
        double resample_factor=4;

        psf_models::ReferencePsf kernel =
            clean_and_register_subroutines::generate_airy_core_template(
                camconf,
                0,
                1.0/resample_factor
            );

        kernel.psf_image.write_to_file(test_suite_output_dir+"airy_core.fits");

        double fwhm_est = psf_characterisation::estimate_fwhm_in_image_pix(kernel.psf_image.pix,
                          kernel.exact_centre,
                          kernel.psf_image.pix.max_val(),
                          kernel.psf_image.pix.range().x_dim()/2.0);

        cout<<"Airy fwhm in pix:"<<fwhm_est / resample_factor << endl;

        CHECK_CLOSE(2.0, fwhm_est / resample_factor,
                    0.05); //FWHM approx 1.02 lambda / D, (aperture inner diameter=0)

        PixelArray2d<double> airy_autoconvolution =
            spatial_convolution::convolve_with_kernel(
                kernel.psf_image.pix,
                kernel.psf_image.pix,
                kernel.central_pixel);

        airy_autoconvolution.write_to_file(test_suite_output_dir+"airy_autoconv.fits");
        double autocon_fwhm_est = psf_characterisation::estimate_fwhm_in_image_pix(
                                      airy_autoconvolution,
                                      kernel.exact_centre,
                                      airy_autoconvolution.max_val(),
                                      airy_autoconvolution.range().x_dim()/2.0);
        cout<<"Autoconv fwhm: "<< autocon_fwhm_est / resample_factor<<endl;

    }

    TEST_FIXTURE(camconf_fixture, custom_template) {
        double resample_factor=4;
        psf_models::ReferencePsf kernel =
            clean_and_register_subroutines::generate_normalised_core_template(
                camconf,
                0,
                1.0/resample_factor
            );

        kernel.psf_image.write_to_file(
            test_suite_output_dir+"normalised_core.fits");

    }
}


// Copyright 2004, 2005 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP
#define BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP

#include <cmath>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <vector>
#include <list>
#include <algorithm> // for std::min, std::max, std::accumulate
#include <boost/graph/point_traits.hpp>
#include <functional>
#include <numeric>

#include <stdlib.h> // for drand48

namespace boost {

using boost::graph::point_traits;

struct square_distance_attractive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::edge_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return d * d / k;
  }
};

struct square_distance_repulsive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::vertex_descriptor,
             typename graph_traits<Graph>::vertex_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return k * k / d;
  }
};

template<typename T>
struct linear_cooling {
  typedef T result_type;

  linear_cooling(std::size_t iterations)
    : temp(T(iterations) / T(10)), step(0.1) { }

  linear_cooling(std::size_t iterations, T temp)
    : temp(temp), step(temp / T(iterations)) { }

  T operator()()
  {
    T old_temp = temp;
    temp -= step;
    if (temp < T(0)) temp = T(0);
    return old_temp;
  }

 private:
  T temp;
  T step;
};

struct all_force_pairs
{
  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    vertex_iterator v, end;
    for (tie(v, end) = vertices(g); v != end; ++v) {
      vertex_iterator u = v;
      for (++u; u != end; ++u) {
        apply_force(*u, *v);
        apply_force(*v, *u);
      }
    }
  }
};

namespace detail {
  template<typename Point>
  Point point_difference(const Point& p1, const Point& p2)
  {
    Point result;
    std::size_t dims = point_traits<Point>::dimensions(p1);
    for (std::size_t i = 0; i < dims; ++i)
      result[i] = p1[i] - p2[i];
    return result;
  }

  template<typename Point>
  typename point_traits<Point>::component_type
  point_norm(const Point& p)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    typename point_traits<Point>::component_type result(0);
    std::size_t dims = point_traits<Point>::dimensions(p);
    for (std::size_t i = 0; i < dims; ++i)
      result += p[i] * p[i];
    return sqrt(result);
  }

  template<typename Point>
  void
  maybe_jitter_point(Point& p1, const Point& p2, Point origin, Point extent)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
    using std::fabs;
    using std::rand;
#endif // BOOST_NO_STDC_NAMESPACE
    typedef typename point_traits<Point>::component_type Dim;
    std::size_t dims = point_traits<Point>::dimensions(p1);
    for (std::size_t i = 0; i < dims; ++i) {
      Dim too_close = extent[i] / Dim(10000);
      if (fabs(p1[i] - p2[i]) < too_close) {
        Dim dist_to_move = sqrt(extent[i]) / Dim(200);
        if (p1[i] - origin[i] < origin[i] + extent[i] - p1[i])
          p1[i] += dist_to_move * Dim(rand() % 100) / Dim(100);
        else
          p1[i] -= dist_to_move * Dim(rand() % 100) / Dim(100);
      }
    }
  }

  template<typename PositionMap, typename DisplacementMap,
           typename RepulsiveForce, typename Graph>
  struct fr_apply_force
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename property_traits<PositionMap>::value_type Point;
    typedef typename point_traits<Point>::component_type Dim;

    fr_apply_force(const PositionMap& position,
                   const DisplacementMap& displacement,
                   Point origin, Point extent,
                   RepulsiveForce repulsive_force, Dim k, const Graph& g)
      : position(position), displacement(displacement), origin(origin),
        extent(extent), repulsive_force(repulsive_force), k(k), g(g)
    {
      dims = point_traits<Point>::dimensions(origin);
    }

    void operator()(vertex_descriptor u, vertex_descriptor v)
    {
#ifndef BOOST_NO_STDC_NAMESPACE
      using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
      if (u != v) {
        // When the vertices land on top of each other, move the
        // first vertex away from the boundaries.
        maybe_jitter_point(position[u], position[v], origin, extent);

        // DPG TBD: Can we use the Topology concept's
        // distance/move_position_toward to handle this?
        Point delta = detail::point_difference(position[v], position[u]);
        Dim dist = detail::point_norm(delta);

        if (dist == Dim(0)) {
          for (std::size_t i = 0; i < dims; ++i)
            displacement[v][i] += 0.01;
        } else {
          Dim fr = repulsive_force(u, v, k, dist, g);

          for (std::size_t i = 0; i < dims; ++i)
            displacement[v][i] += delta[i] / dist * fr;
        }
      }
    }

  private:
    PositionMap position;
    DisplacementMap displacement;
    Point origin;
    Point extent;
    RepulsiveForce repulsive_force;
    Dim k;
    const Graph& g;
    std::size_t dims;
  };

} // end namespace detail

template<typename PositionMap>
struct grid_force_pairs
{
  typedef typename property_traits<PositionMap>::value_type Point;
  typedef typename point_traits<Point>::component_type Dim;

  template<typename Graph>
  explicit
  grid_force_pairs(const Point& origin, const Point& extent,
                   PositionMap position, const Graph& g)
    : origin(origin), extent(extent), position(position)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
    std::size_t dims = point_traits<Point>::dimensions(origin);
    two_k = Dim(2) * sqrt(std::accumulate(&extent[0], &extent[0] + dims,
                                          Dim(1), std::multiplies<Dim>()));

    num_buckets.resize(dims);
    for (std::size_t i = 0; i < dims; ++i)
      num_buckets[i] = std::size_t(extent[i] / two_k + Dim(1));
  }

  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef std::list<vertex_descriptor> bucket_t;
    typedef std::vector<bucket_t> buckets_t;

#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
    std::size_t dims = point_traits<Point>::dimensions(origin);

    buckets_t buckets(std::accumulate(&num_buckets[0], &num_buckets[0] + dims,
                                      std::size_t(1),
                                      std::multiplies<std::size_t>()));
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      std::vector<std::size_t> bucket(dims);
      for (std::size_t i = 0; i < dims; ++i) {
        bucket[i] = std::size_t((position[*v][i] + origin[i]) / two_k);
        if (bucket[i] >= num_buckets[i]) bucket[i] = num_buckets[i] - 1;
      }
      buckets[bucket_to_index(bucket)].push_back(*v);
    }

    std::vector<std::size_t> cell(2);
    do {
      bucket_t& bucket = buckets[bucket_to_index(cell)];

      std::vector<std::size_t> start_cell(dims);
      std::vector<std::size_t> end_cell(dims);

      for (std::size_t i = 0; i < dims; ++i) {
        start_cell[i] = cell[i] == 0? 0 : cell[i] - 1;
        end_cell[i] = cell[i] == num_buckets[i] - 1? cell[i] : cell[i] + 1;
      }

      // Repulse vertices in this bucket
      typedef typename bucket_t::iterator bucket_iterator;

      std::vector<std::size_t> adj_cell = start_cell;
      do {
        for (bucket_iterator u = bucket.begin(); u != bucket.end(); ++u) {
          // Repulse vertices in this bucket
          bucket_t& other_bucket = buckets[bucket_to_index(adj_cell)];
          for (bucket_iterator v = other_bucket.begin(); v != other_bucket.end(); ++v) {
            Point delta = detail::point_difference(position[*u],
                                                   position[*v]);
            Dim dist = detail::point_norm(delta);
            if (dist < two_k) apply_force(*u, *v);
          }
        }
      } while (next_bucket_in_subgrid(adj_cell, start_cell, end_cell));
    } while (next_bucket(cell));
  }

private:
  bool next_bucket_in_subgrid(std::vector<std::size_t>& bucket,
                              const std::vector<std::size_t>& start,
                              const std::vector<std::size_t>& end)
  {
    // Find the next bucket
    std::size_t index = bucket.size() - 1;
    do {
      if (bucket[index]++ >= end[index]) {
        bucket[index] = start[index];

        if (index == 0)
          return false;
        --index;
      } else {
        return true;
      }
    } while (true);
  }

  bool next_bucket(std::vector<std::size_t>& bucket)
  {
    // Find the next bucket
    std::size_t index = bucket.size() - 1;
    do {
      if (++bucket[index] >= num_buckets[index]) {
        bucket[index] = 0;

        if (index == 0)
          return false;
        --index;
      } else {
        return true;
      }
    } while (true);
  }

  std::size_t bucket_to_index(const std::vector<std::size_t>& bucket)
  {
    std::size_t multiplier = 1;
    std::size_t result = 0;

    std::size_t dims = num_buckets.size();
    for (std::size_t i = 0; i < dims; ++i) {
      result += bucket[i] * multiplier;
      multiplier *= num_buckets[i];
    }
    return result;
  }

  Point origin;
  Point extent;
  PositionMap position;
  Dim two_k;
  std::vector<std::size_t> num_buckets;
};

template<typename PositionMap, typename Graph>
inline grid_force_pairs<PositionMap>
make_grid_force_pairs
  (typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent,
   const PositionMap& position, const Graph& g)
{ return grid_force_pairs<PositionMap>(origin, extent, position, g); }

template<typename Graph, typename PositionMap,
         typename AttractiveForce, typename RepulsiveForce,
         typename ForcePairs, typename Cooling, typename DisplacementMap>
void
fruchterman_reingold_force_directed_layout
 (const Graph&    g,
  PositionMap     position,
  typename property_traits<PositionMap>::value_type const& origin,
  typename property_traits<PositionMap>::value_type const& extent,
  AttractiveForce attractive_force,
  RepulsiveForce  repulsive_force,
  ForcePairs      force_pairs,
  Cooling         cool,
  DisplacementMap displacement)
{
  typedef typename property_traits<PositionMap>::value_type Point;
  typedef typename point_traits<Point>::component_type Dim;
  typedef typename graph_traits<Graph>::vertex_iterator   vertex_iterator;
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::edge_iterator     edge_iterator;

#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

  std::size_t num_dimensions = point_traits<Point>::dimensions(origin);
  Dim volume = std::accumulate(&extent[0], &extent[0] + num_dimensions,
                               Dim(1), std::multiplies<Dim>());

  // assume positions are initialized randomly
  Dim k = sqrt(volume / num_vertices(g));

  detail::fr_apply_force<PositionMap, DisplacementMap,
                         RepulsiveForce, Graph>
    apply_force(position, displacement, origin, extent, repulsive_force, k, g);

  do {
    // Calculate repulsive forces
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v)
      displacement[*v] = Point();
    force_pairs(g, apply_force);

    // Calculate attractive forces
    edge_iterator e, e_end;
    for (tie(e, e_end) = edges(g); e != e_end; ++e) {
      vertex_descriptor v = source(*e, g);
      vertex_descriptor u = target(*e, g);

      if (u != v) {
        // When the vertices land on top of each other, move the
        // first vertex away from the boundaries.
        ::boost::detail::maybe_jitter_point(position[u], position[v],
                                            origin, extent);

        // DPG TBD: Can we use the Topology concept's
        // distance/move_position_toward to handle this?
        Point delta = detail::point_difference(position[v], position[u]);
        Dim dist = detail::point_norm(delta);
        Dim fa = attractive_force(*e, k, dist, g);

        for (std::size_t dim = 0; dim < num_dimensions; ++dim) {
          displacement[v][dim] -= delta[dim] / dist * fa;
          displacement[u][dim] += delta[dim] / dist * fa;
        }
      }
    }

    if (Dim temp = cool()) {
      // Update positions
      for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        Dim disp_size = detail::point_norm(displacement[*v]);

        for (std::size_t dim = 0; dim < num_dimensions; ++dim) {
          position[*v][dim] += displacement[*v][dim] / disp_size
                             * (min)(disp_size, temp);
          position[*v][dim] = (min)(origin[dim] + extent[dim],
                                    (max)(origin[dim], position[*v][dim]));
        }
      }
    } else {
      break;
    }
  } while (true);
}

namespace detail {
  template<typename DisplacementMap>
  struct fr_force_directed_layout
  {
    template<typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        typename property_traits<PositionMap>::value_type const& origin,
        typename property_traits<PositionMap>::value_type const& extent,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        DisplacementMap displacement,
        const bgl_named_params<Param, Tag, Rest>&)
    {
      fruchterman_reingold_force_directed_layout
        (g, position, origin, extent, attractive_force, repulsive_force,
         force_pairs, cool, displacement);
    }
  };

  template<>
  struct fr_force_directed_layout<error_property_not_found>
  {
    template<typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        typename property_traits<PositionMap>::value_type const& origin,
        typename property_traits<PositionMap>::value_type const& extent,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        error_property_not_found,
        const bgl_named_params<Param, Tag, Rest>& params)
    {
      typedef typename property_traits<PositionMap>::value_type Point;
      std::vector<Point> displacements(num_vertices(g));
      fruchterman_reingold_force_directed_layout
        (g, position, origin, extent, attractive_force, repulsive_force,
         force_pairs, cool,
         make_iterator_property_map
         (displacements.begin(),
          choose_const_pmap(get_param(params, vertex_index), g,
                            vertex_index),
          Point()));
    }
  };

} // end namespace detail

template<typename Graph, typename PositionMap, typename Param,
         typename Tag, typename Rest>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent,
   const bgl_named_params<Param, Tag, Rest>& params)
{
  typedef typename property_value<bgl_named_params<Param,Tag,Rest>,
                                  vertex_displacement_t>::type D;

  detail::fr_force_directed_layout<D>::run
    (g, position, origin, extent,
     choose_param(get_param(params, attractive_force_t()),
                  square_distance_attractive_force()),
     choose_param(get_param(params, repulsive_force_t()),
                  square_distance_repulsive_force()),
     choose_param(get_param(params, force_pairs_t()),
                  make_grid_force_pairs(origin, extent, position, g)),
     choose_param(get_param(params, cooling_t()),
                  linear_cooling<double>(100)),
     get_param(params, vertex_displacement_t()),
     params);
}

template<typename Graph, typename PositionMap>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent)
{
  fruchterman_reingold_force_directed_layout
    (g, position, origin, extent,
     attractive_force(square_distance_attractive_force()));
}

} // end namespace boost

#ifdef BOOST_GRAPH_IS_PARALLEL
#  include <boost/graph/distributed/fruchterman_reingold.hpp>
#endif // BOOST_GRAPH_IS_PARALLEL

#endif // BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP


#pragma once
#include <boost/multiprecision/cpp_int.hpp>

using uint256_t = boost::multiprecision::uint256_t;


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_UNIFORM_REAL_DISTRIBUTION_HPP
#define BOOST_COMPUTE_RANDOM_UNIFORM_REAL_DISTRIBUTION_HPP

#include <boost/compute/command_queue.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/detail/literal.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace boost {
namespace compute {

/// \class uniform_real_distribution
/// \brief Produces uniformily distributed random floating-point numbers.
///
/// The following example shows how to setup a uniform real distribution to
/// produce random \c float values between \c 1 and \c 100.
///
/// \snippet test/test_uniform_real_distribution.cpp generate
///
/// \see default_random_engine, normal_distribution
template<class RealType = float>
class uniform_real_distribution
{
public:
    typedef RealType result_type;

    /// Creates a new uniform distribution producing numbers in the range
    /// [\p a, \p b).
    uniform_real_distribution(RealType a = 0.f, RealType b = 1.f)
        : m_a(a),
          m_b(b)
    {
    }

    /// Destroys the uniform_real_distribution object.
    ~uniform_real_distribution()
    {
    }

    /// Returns the minimum value of the distribution.
    result_type a() const
    {
        return m_a;
    }

    /// Returns the maximum value of the distribution.
    result_type b() const
    {
        return m_b;
    }

    /// Generates uniformily distributed floating-point numbers and stores
    /// them to the range [\p first, \p last).
    template<class OutputIterator, class Generator>
    void generate(OutputIterator first,
                  OutputIterator last,
                  Generator &generator,
                  command_queue &queue)
    {
        BOOST_COMPUTE_FUNCTION(RealType, scale_random, (const uint_ x),
        {
            return LO + (convert_RealType(x) / MAX_RANDOM) * (HI - LO);
        });

        scale_random.define("LO", detail::make_literal(m_a));
        scale_random.define("HI", detail::make_literal(m_b));
        scale_random.define("MAX_RANDOM", "UINT_MAX");
        scale_random.define(
            "convert_RealType", std::string("convert_") + type_name<RealType>()
        );

        generator.generate(
            first, last, scale_random, queue
        );
    }

    /// \internal_ (deprecated)
    template<class OutputIterator, class Generator>
    void fill(OutputIterator first,
              OutputIterator last,
              Generator &g,
              command_queue &queue)
    {
        generate(first, last, g, queue);
    }

private:
    RealType m_a;
    RealType m_b;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_UNIFORM_REAL_DISTRIBUTION_HPP


#include <fastdtw_cpp/fastdtw.hpp>
#include <time.h>
#include <stdlib.h>
#include <opencv2/opencv.hpp>

#include <boost/assign.hpp>
#include <boost/date_time.hpp>

#include <stdio.h>

namespace {
void normalize(const std::vector<float> &sig, const float min,
               const float max, std::vector<float> &normed)
{
    float range = max - min;
    for(std::vector<float>::const_iterator it = sig.begin() ; it != sig.end();
        ++it) {
        normed.push_back((*it + min) / range);
    }
}
}

inline void initRandom()
{
    srand (time(NULL));
}

inline float getRandom(const float prev)
{
    return (prev + (rand() % 1000 - 500) * 0.0001f);
}

int main(int argc, char *argv[])
{
    unsigned int   size(1000);
    float max( 10.f);
    float min(-10.f);
    float norm(0.1f);
    std::deque<double>             a(size);
    std::deque<double>             b(size);
    std::map<std::string, double> timings = boost::assign::map_list_of
            ("DTW", 0.0)
            ("FAST_DTW", 0.0)
            ("DTW_NO_PATH", 0.0);

    initRandom();
    a.at(0) = 0.0f;
    b.at(0) = 0.0f;
    for(unsigned int i = 1 ; i < size ; ++i) {
        a.at(i) = std::min(max, std::max(min, getRandom(a.at(i - 1))));
        b.at(i) = std::min(max, std::max(min, getRandom(b.at(i - 1))));
    }

    int wait_time(50);
    double cycles(0.0);

    while(true) {
        cycles += 1;
        cv::Mat visual(size, size * 2, CV_8UC3, cv::Scalar::all(0));

        std::vector<double> signal_a;
        std::vector<double> signal_b;
        signal_a.assign(a.begin(), a.end());
        signal_b.assign(b.begin(), b.end());

        /// add new signal input
        a.push_back(std::min(max, std::max(min, getRandom(a.back()))));
        b.push_back(std::min(max, std::max(min, getRandom(b.back()))));
        a.pop_front();
        b.pop_front();

        /// DTW
        boost::posix_time::ptime start =
                boost::posix_time::microsec_clock::local_time();
        fastdtw_cpp::path::WarpPath<double> p_dtw;
        fastdtw_cpp::dtw::apply(signal_a, signal_b, p_dtw);
        boost::posix_time::ptime stop =
                boost::posix_time::microsec_clock::local_time();
        boost::posix_time::time_duration ms = (stop-start);
        timings.at("DTW") += ms.total_milliseconds();

        /// FASTDTW
        start = boost::posix_time::microsec_clock::local_time();
        fastdtw_cpp::path::WarpPath<double> p_fdtw;
        fastdtw_cpp::fastdtw::applyDyn(signal_a, signal_b, 15, p_fdtw);
        stop = boost::posix_time::microsec_clock::local_time();
        ms = (stop-start);
        timings.at("FAST_DTW") += ms.total_milliseconds();

        /// DTW_NO_PATH
        start = boost::posix_time::microsec_clock::local_time();
        double dtw_dist = 0.0;
        fastdtw_cpp::dtw::apply<double>(signal_a, signal_b, dtw_dist);
        stop = boost::posix_time::microsec_clock::local_time();
        ms = (stop-start);
        timings.at("DTW_NO_PATH") += ms.total_milliseconds();

        std::cout << " DTW         : "
                  << std::setw(10) << std::setprecision(8)
                  << (timings.at("DTW") / cycles) << "ms "
                  << std::setw(10) << std::setprecision(8)
                  << p_dtw.getDistance()
                  << std::endl;
        std::cout << " FAST_DTW    : "
                  << std::setw(10) << std::setprecision(8)
                  << (timings.at("FAST_DTW") / cycles) << "ms "
                  << std::setw(10) << std::setprecision(8)
                  << p_fdtw.getDistance()
                  << std::endl;
        std::cout << " DTW_NO_PATH : "
                  << std::setw(10) << std::setprecision(8)
                  << (timings.at("DTW_NO_PATH") / cycles) << "ms "
                  << std::setw(10) << std::setprecision(8)
                  << dtw_dist
                  << std::endl;
        std::cout << "---------------------------------------------" << std::endl;

        cv::Mat visual_dtw(visual, cv::Rect(0,0,size, size));
        for(unsigned int i = 0 ; i < p_dtw.size() ; ++i) {
            std::pair<unsigned int, unsigned int> pos = p_dtw.at(i);
            visual_dtw.at<cv::Vec3b>(pos.second, pos.first) = cv::Vec3b(255,0,0);
        }

        for(unsigned int i = 0 ; i < p_fdtw.size() ; ++i) {
            std::pair<unsigned int, unsigned int> pos = p_fdtw.at(i);
            visual_dtw.at<cv::Vec3b>(pos.second, pos.first) = cv::Vec3b(0,255,0);
        }

        //// signal and so on
        cv::Mat visual_signal_a(visual, cv::Rect(size, 0, size, size * 0.5));
        cv::Mat visual_signal_b(visual, cv::Rect(size, size * 0.5, size, size * 0.5));
        cv::line(visual_signal_a, cv::Point(0, size * 0.25), cv::Point(size, size * 0.25), cv::Scalar::all(255.0));
        cv::line(visual_signal_b, cv::Point(0, size * 0.25), cv::Point(size, size * 0.25), cv::Scalar::all(255.0));

        int max_idy = visual_signal_a.rows - 1;

        for(unsigned int i = 1 ; i < size ; ++i) {
            cv::Point a1(i-1, 0.5 * max_idy - 0.5 * max_idy * norm * signal_a.at(i-1));
            cv::Point a2(i,   0.5 * max_idy - 0.5 * max_idy * norm * signal_a.at(i));;
            cv::Point b1(i-1, 0.5 * max_idy - 0.5 * max_idy * norm * signal_b.at(i-1));
            cv::Point b2(i,   0.5 * max_idy - 0.5 * max_idy * norm * signal_b.at(i));;
            cv::line(visual_signal_a, a1, a2, cv::Scalar(0,80,255));
            cv::line(visual_signal_b, b1, b2, cv::Scalar(255,255,20));
        }

        //        cv::resize(visual,visual,cv::Size(1280, 640));
        cv::imshow("dtw", visual);
        int key = cv::waitKey(wait_time) & 0xFF;
        if(key == 27)
            return 0;
        if(key == 171)
            wait_time = std::max(wait_time - 10, 5);
        if(key == 173)
            wait_time = std::min(wait_time + 10, 1000);
    }

    return 0;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_GAMMA_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_GAMMA_RNG_HPP

#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_less_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/gamma_p.hpp>
#include <stan/math/prim/scal/fun/digamma.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/fun/grad_reg_inc_gamma.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    gamma_rng(double alpha,
              double beta,
              RNG& rng) {
      using boost::variate_generator;
      using boost::gamma_distribution;

      static const char* function("gamma_rng");
      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Inverse scale parameter", beta);

      /*
        the boost gamma distribution is defined by
        shape and scale, whereas the stan one is defined
        by shape and rate (inverse scale)
      */
      variate_generator<RNG&, gamma_distribution<> >
        gamma_rng(rng, gamma_distribution<>(alpha, 1.0 / beta));
      return gamma_rng();
    }

  }
}
#endif


#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "mnistrec.h"
#include <random>
#include <Eigen/Eigen>

Eigen::VectorXf values(784);

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    ui->table->setRowCount(28);
    ui->table->setColumnCount(28);
    ui->table->verticalHeader()->setVisible(false);
    ui->table->horizontalHeader()->setVisible(false);
    ui->table->setEditTriggers(QAbstractItemView::NoEditTriggers);
    ui->table->setSelectionMode(QAbstractItemView::NoSelection);
    for (int i = 0; i < 28; i++ ) {
       ui->table->setColumnWidth(i,20);
       ui->table->setRowHeight(i,20);
    }
    for (int i = 0; i < 28; i++ ) {
        for (int j = 0; j < 28; j++) {
             ui->table->setItem(i, j, new QTableWidgetItem(""));
             ui->table->item(i, j)->setBackground(QColor(255, 255,255  ,255));
             values[i*28+j] = 0;
          //   count++;
        }
    }
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::on_pushButton_clicked()
{
    using namespace MLP;
    std::random_device rd;
    std::mt19937 eng(rd());
    std::uniform_int_distribution<> distr(1, 10000);
    int rand = distr(eng);
    std::cout<<"Rand: "<<rand<<"\n";
    MLP::Result res = MLP::test(rand);
   ui->label->setText("Prediction: "+QString::number(res.predict) + "\n\n"+"R/A Score: \n" + QString::number(res.pred_pers)+"\nW/A Score: \n" + QString::number(-res.error));
   int count = 0;

   for (int i = 0; i < 28; i++ ) {
       for (int j = 0; j < 28; j++) {
            ui->table->setItem(i, j, new QTableWidgetItem(""));
            ui->table->item(i, j)->setBackground(QColor(0, 0, 0,(int)(255*res.image[count])));
            values[count] = res.image[count];
            count++;
       }
   }
}

void MainWindow::on_table_cellClicked(int row, int column)
{
    values[28*row+column] = 1;
    ui->table->item(row, column)->setBackground(QColor(0, 0, 0,255));
}

void MainWindow::on_table_cellPressed(int row, int column)
{

}

void MainWindow::on_pushButton_3_clicked()
{

    for (int i = 0; i < 28; i++ ) {
        for (int j = 0; j < 28; j++) {
             ui->table->setItem(i, j, new QTableWidgetItem(""));
             ui->table->item(i, j)->setBackground(QColor(255, 255,255  ,255));
             values[i*28+j] = 0;
          //   count++;
        }
    }

}

void MainWindow::on_pushButton_2_clicked()
{
    using namespace MLP;

    MLP::Result res = MLP::test_custom(values);
   ui->label->setText("Prediction: "+QString::number(res.predict) + "\n\n"+"R/A Score: \n" + QString::number(res.pred_pers)+"\nW/A Score: \n" + QString::number(-res.error));
   int count = 0;

   for (int i = 0; i < 28; i++ ) {
       for (int j = 0; j < 28; j++) {
            ui->table->setItem(i, j, new QTableWidgetItem(""));
            ui->table->item(i, j)->setBackground(QColor(0, 0, 0,(int)(255*res.image[count])));
            values[count] = res.image[count];
            count++;
       }
   }
}

void MainWindow::on_table_cellActivated(int row, int column)
{
    values[28*row+column] = 1;
    ui->table->item(row, column)->setBackground(QColor(0, 0, 0,255));

}

void MainWindow::on_table_cellEntered(int row, int column)
{
    values[28*row+column] = 1;
    ui->table->item(row, column)->setBackground(QColor(0, 0, 0,255));
}