/* author: jakob fischer (mail@jakobfischer.eu)
 * description:
 * Tool for solving a ODE for a reaction system given as an jrnf-file. The concentration and time
 * are read and written from / to a comma seperated file. Every row / line represents one time step.
 * the first column contains the time of this step, the second the mean of the quadratic change
 * (<f(x)> ;dx/dt = f(x)). Then follow the concentrations of all species.
 *
 * One version of the program (option "simulate" calculates effective reaction rates directly
 * from formation enthalpies and activation energies given in the network description.
 * (\beta = 1/(k_b T) = 1). This allows higher precision and usage of an explicit solver which
 * is over all faster for small networks (< 50 species).
 *
 * TODO There seems to be a problem with exceptionally short steps (distances in the
 *      <times> vector given to integrate_times) as well as with very wide steps. This
 *      occurs when the "write_log" option lead to output step sizes below 1e-7 or
 *      above 1e7. The solution for now is to first use linear stepping, then
 *      "write_log_abs" and at larger timescale linear stepping again.
 *      Giving a minimal and maximal step size as parameter might be a good
 *      improvement for future versions!
 */

#include <iostream>
#include <fstream>
#include <ctime>
#include <utility>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

#include <algorithm>
#include "tools/cl_para.h"
#include "net_tools/reaction_network.h"
#include "net_tools/reaction_network_fileop.h"
#include "net_tools/network_tools.h"

using namespace std;
using namespace boost::numeric::odeint;

#ifndef GIT_VERSION
#define GIT_VERSION "no version"
#endif

// version string increment if mayor changes happen
const string odeint_rnet_version_string="1x00x00";

// various ublas types needed for implicit solver
typedef boost::numeric::ublas::vector< double > vector_type;
typedef boost::numeric::ublas::matrix< double > matrix_type;
typedef boost::numeric::ublas::matrix< int > matrix_type_int;

/*
 * Main class that bundles the functionality to load reaction networks and
 * files containing species' concentration. Every object is associated with
 * an network and concentration file at creation. The class offers methods
 * for solving the ODE defined by the network and write the results to the
 * concentration file.
 */

class reaction_network_system {
    double beta;                // beta = 1/(k_b T)
    std::vector<species> sp;    // The reaction
    std::vector<reaction> re;   // network
    // initial time, concentration loaded with concentration file
    double initial_t;
    vector_type initial_con;
    std::string fn_concentration;      // filename

    matrix_type_int N, N_in, N_out;    // stoichiometric matrices
    // List that contains for each column in N_in / N_out (= for each reaction)
    // a vector of those rows (species) that are nonzero.
    std::vector< std::vector<size_t> > N_in_list, N_out_list;
    // Intermediate values / thermodynamic values
    vector_type Ea, mu0, e_bs_in, e_bs_out, e_m_bEa;

public:

    /*
     * Simple calculation of concentration change (rhs / f(x)) from species
     * concentration and reaction constants (<k>, <k_b>).
     */

    struct fast_system {
        reaction_network_system& rns;

        fast_system(reaction_network_system& rns_) : rns(rns_)  {}

        // <x> - state / concentration;  dxdt - change / rhs (output)
        void operator()( const vector_type &x , vector_type &dxdt , double /* t */ ) {

            for(size_t i=0; i<rns.sp.size(); ++i)
                dxdt[i]=0;

            for(size_t i=0; i<rns.re.size(); ++i) {
                double rate_f=rns.re[i].get_k();
	        double rate_b=rns.re[i].get_k_b();

                for(size_t j=0; j<rns.re[i].get_no_educt_s(); ++j)
                    rate_f *= x[rns.re[i].get_educt_id(j)];

                for(size_t j=0; j<rns.re[i].get_no_product_s(); ++j)
                    rate_b *= x[rns.re[i].get_product_id(j)];

                for(size_t j=0; j<rns.re[i].get_no_educt_s(); ++j)
                    dxdt[rns.re[i].get_educt_id(j)] -= (rate_f - rate_b);

                for(size_t j=0; j<rns.re[i].get_no_product_s(); ++j)
                    dxdt[rns.re[i].get_product_id(j)] += (rate_f - rate_b);
             }

            for(size_t i=0; i<rns.sp.size(); ++i)
                if(rns.sp[i].is_constant())
	            dxdt[i]=0;
         }
    };

    /*
     * Calculation of concentration change (rhs) from species concentration
     * and thermodynamic values (energies).
     */

    struct stiff_system {
        reaction_network_system& rns;

        stiff_system(reaction_network_system& rns_) : rns(rns_)  {}

        // Function to calculate reaction rates of state <x> into <rates>.
        void calculate_rates(const vector_type &x, vector_type &rates) {
            vector_type a2(rns.re.size()), a3(rns.re.size());
            for(size_t j=0; j<rns.N.size2(); ++j) {
                a2(j) = 1;
                a3(j) = 1;

                for(size_t k=0; k<x.size(); ++k) {
                    if(rns.N_in(k,j) != 0)
                        a2(j) *= pow(x(k), rns.N_in(k,j));

                    if(rns.N_out(k,j) != 0)
                        a3(j) *= pow(x(k), rns.N_out(k,j));
                }
            }

            rates = element_prod(rns.e_m_bEa, element_prod(a2, rns.e_bs_in) - element_prod(a3, rns.e_bs_out));
        }

        // <x> - state / concentration;  <J> - Jacobi matrix (output);  dfdt - time partial differential (output)
        void operator()( const vector_type &x , vector_type &dxdt , double /* t */ ) {
            vector_type a2(rns.re.size()), a3(rns.re.size());
            for(size_t j=0; j<rns.N.size2(); ++j) {
                a2(j) = 1;
                a3(j) = 1;

                for(size_t l=0; l<rns.N_in_list[j].size(); ++l)
                        a2(j) *= pow(x(rns.N_in_list[j][l]), rns.N_in(rns.N_in_list[j][l],j));

                for(size_t l=0; l<rns.N_out_list[j].size(); ++l)
                        a3(j) *= pow(x(rns.N_out_list[j][l]), rns.N_out(rns.N_out_list[j][l],j));

            }

            dxdt = prod(rns.N, element_prod(rns.e_m_bEa, element_prod(a2, rns.e_bs_in) - element_prod(a3, rns.e_bs_out)));

            for(size_t k=0; k<x.size(); ++k)
                if(rns.sp[k].is_constant())
                    dxdt(k) = 0;
        }
    };

    /*
     * Calculation of Jacobi matrix from species concentration and thermodynamic
     * values (energies).
     */

    struct stiff_system_jacobi {
        const reaction_network_system& rns;

        stiff_system_jacobi(const reaction_network_system& rns_) : rns(rns_)  {}

        // <x> - state / concentration;  <J> - Jacobi matrix (output);  dfdt - time partial differential (output)
        void operator()( const vector_type & x  , matrix_type &J , const double & /* t */ , vector_type &dfdt ) {
            for(size_t i=0; i<x.size(); ++i) {
                for(size_t l=0; l<x.size(); ++l) {
                    double s(0);

                    for(size_t k=0; k<rns.re.size(); ++k) {
                        double p1(1), p2(1);    // p1 = \prod_{j \neq k} {x_j}^N_{jk}^\mathrm{in}
                                                // p2 = \prod_{j \neq k} {x_j}^N_{jk}^\mathrm{out}

                        for(size_t m=0; m<rns.N_in_list[k].size(); ++m) {
                            size_t j=rns.N_in_list[k][m];

                            if(j != l)
                                p1 *= x(j);
                        }

                        for(size_t m=0; m<rns.N_out_list[k].size(); ++m) {
                            size_t j=rns.N_out_list[k][m];

                            if(j != l && rns.N_out(j,k) != 0)
                                p2 *= x(j);
                        }

                        s += rns.N(i,k)*rns.e_m_bEa(k)*(rns.e_bs_in(k)*rns.N_in(l,k)*pow(x(l), rns.N_in(l,k)-1)*p1 -
                                                        rns.e_bs_out(k)*rns.N_out(l,k)*pow(x(l),rns.N_out(l,k)-1)*p2);
                    }

                    J(i,l) = s;
                    if(rns.sp[i].is_constant())
                        J(i,l) = 0;
                }
            }

            dfdt = boost::numeric::ublas::zero_vector<double>(x.size());
        }
    };

    /*
     * Constructor that loads the network file as well as the concentration file
     * (for initial concentration). Also all temporary values that are necessary to
     * solve (calculate rhs + Jacobi matrix) the ode faster are calculated.
     */

    reaction_network_system(const std::string& fn_network, const std::string& fn_concentration_)
        : fn_concentration(fn_concentration_), beta(1) {
        // load network into <re> and <sp>
        read_jrnf_reaction_n(fn_network, sp, re);

	// Remove 1-0 and 0-1 reactions. Such reactions are there to ballance flow through the boundary conditions
        std::remove_if (re.begin(), re.end(),
                        [] (reaction& re) -> bool {
                            return re.get_no_educt() == 1 && re.get_no_product() == 0 ||
                                   re.get_no_educt() == 0 && re.get_no_product() == 1; });

        // Calculate stoichiometric matrices, activation energies and their logarithms...
        N_in = N_out = boost::numeric::ublas::zero_matrix<double>(sp.size(), re.size());
        Ea =  e_bs_in = e_bs_out = e_m_bEa = boost::numeric::ublas::zero_vector<double>(re.size());
        mu0 = boost::numeric::ublas::zero_vector<double>(sp.size());

        // Load species standard gibbs energy / chemical energy into mu0
        for(size_t i=0; i<sp.size(); ++i)
            mu0(i) = sp[i].get_energy();

        // Load / calculate reaction specific quantities like activation Energy
        // <Ea> and joint energy of educts <m0_educts> or products <mu0_products>.
        for(size_t i=0; i<re.size(); ++i) {
            double mu0_educts(0), mu0_products(0);

            for(size_t j=0; j<re[i].get_no_educt_s(); ++j) {
                size_t id(re[i].get_educt_id(j)),  mul(re[i].get_educt_mul(j));
                N_in(id,i) += mul;
                mu0_educts += sp[id].get_energy()*mul;
            }

            for(size_t j=0; j<re[i].get_no_product_s(); ++j) {
                size_t id(re[i].get_product_id(j)),  mul(re[i].get_product_mul(j));
                N_out(id,i) += mul;
                mu0_products += sp[id].get_energy()*mul;
            }

            Ea(i) = re[i].get_activation()+max(mu0_educts,mu0_products);
            e_bs_in(i) = exp(beta*mu0_educts);
            e_bs_out(i) = exp(beta*mu0_products);
            e_m_bEa(i) = exp(-beta*Ea(i));
        }

        N = N_out-N_in;

        // Build lists (for speedup)
        for(size_t i=0; i<re.size(); ++i) {
            N_in_list.push_back(std::vector<size_t>());
            N_out_list.push_back(std::vector<size_t>());

            for(size_t j=0; j<sp.size(); ++j) {
                if(N_in(j,i) != 0)
                    N_in_list[i].push_back(j);

                if(N_out(j,i) != 0)
                    N_out_list[i].push_back(j);
            }
        }

        // Read last line of concentration file and write initial concentration to initial_con
        // and initial time to initial_t. When done, open the same file for appending...
        initial_con = boost::numeric::ublas::zero_vector<double>(sp.size());

        std::ifstream  data(fn_concentration.c_str());

        if(!data.good()) {
           std::cout << "Could not open concentration file: " << fn_concentration << std::endl;
           return;
        }

        std::string line;
        std::getline(data,line);       // dont want the header
        double last_msd=0;

        // Get line of actual data
        while(!std::getline(data,line).eof()) {
            std::stringstream ls(line);
            std::string cell;

            // read in value by value and count its position to place it in the
            // right variables
            size_t cnt=0;
            while(std::getline(ls,cell,',')) {
                std::stringstream in(cell);

                if(cnt == 0)
                    in >> initial_t ;
                else if(cnt == 1)
                    in >> last_msd;
                else if(cnt <= sp.size()+1)
                    in >> initial_con(cnt-2);
                else
                    std::cout << "Error at reading csv / concentration file!" << std::endl;

                ++cnt;
            }
        }

        std::cout << "Simulating file: " << fn_concentration << std::endl;
        std::cout << "Loaded concentration file with starting time " << initial_t << std::endl;
        data.close();
    }

    /*
     * To check some differential equations or the algorithm it might be usefull
     * to calculate the right hand side of the differential equation for a given
     * concentration vector.
     */

    void print_rhs() {
        vector_type x(initial_con);
        vector_type dxdt=boost::numeric::ublas::zero_vector<double>(sp.size());

        // calculate rhs - operator saves it do dxdt
        stiff_system(*this)(x, dxdt, 0);

        cout << "righthandside:" << endl;
        for(size_t i=0; i<dxdt.size(); ++i)
            cout << "/  " << dxdt(i) << "  /";
        cout << endl;
    }

    /*
     * Method for calling the system that uses energetics for calculating effective
     * rates and allows usage of a stiff solver - implicit stepper. System (f(x))
     * is defined in "stiff_system" (rhs) and "stiff_system_jacobi (Jacobi matrix).
     *
     * Tmax           - time up to that ode is solved
     * deltaT         - initial step size for addaptive stepper (does not relate to time output is written)
     * solve_implicit - do use implicit solver?
     * wint           - number of time output is written to output file between time of last
     *                  entry in concentration file and Tmax
     * write_log = 0  - linearly spaced times for output
     *           = 1  - logarithmically spaced (relative -> log(t-t0) is equidist.)
     *           = 2  - logarithmically spaced (absolute -> log(t) is equidist.)
     */

    void run(double Tmax=25000, double deltaT=0.1, bool solve_implicit=true,
             size_t wint=500, size_t write_log=0) {
        fstream out(fn_concentration.c_str(), std::ios_base::out | std::ios_base::app);
        out.precision(25);

        size_t t0 = time(NULL);

        // Init solver and start
        vector_type x(initial_con), last_con(initial_con);
        double last_write(initial_t);

        // Function that is called by stepper and writes concentration (<vec>) to file.
        auto write_state = [this, &out, t0, wint, deltaT, Tmax, &last_con, &last_write]( const vector_type &vec , const double t ) {
            // calculate mean square distance from last write
            double last_msd=0;
            for(size_t i=0; i<vec.size(); ++i)
                 last_msd += (vec[i]-last_con[i])*(vec[i]-last_con[i]);

            if(t != last_write)
                last_msd /= (vec.size()*(t-last_write));
            last_write=t;
            last_con=vec;

            // write time and msd ...
            out << t << "," << last_msd;
            // and concentrations
            for(size_t l=0; l<vec.size(); ++l)
                out << "," << vec(l);

            out << std::endl;
        };

        size_t step_no = 0;  // Number of steps done by integrator (for diagnostics)
        // Calculate vector of time points at which "write_state" will be called.
        std::vector<double> times( wint );
        for( size_t i=0 ; i<wint ; ++i )
            if(write_log == 0)
                // linearly spaced output times
                times[i] = initial_t + double(i+1)/(wint)*(Tmax-initial_t);
            else if(write_log == 1 || initial_t == 0)
                // logarithmically spaced output times (relative to t0)
                times[i] = initial_t + (exp(double(i+1))-1)/(exp(double(wint))-1)*(Tmax-initial_t);
            else
                // if t0!=0 and one want's logarithmically spaced times (relative to t=0)!
                times[i] = initial_t*pow(exp(1/double(wint)*log(Tmax/initial_t)),double(i+1));

        // Call integrator - returns number of steps. Either implicit or explicit stepper is used.
        step_no = solve_implicit ?
                      integrate_times( make_controlled< rosenbrock4< double > >( 1.0e-6 , 1.0e-6 ) ,
                                       make_pair( stiff_system(*this) , stiff_system_jacobi(*this) ) ,
                                       x , times, deltaT, write_state) :
                      integrate_times( make_controlled< runge_kutta_dopri5< vector_type > >( 1.0e-6 , 1.0e-6 ) ,
                                       stiff_system(*this) , x, times, deltaT, write_state);

        // print time + steps needed and close file
        size_t t1 = time(NULL);
        std::cout << "Run took " << t1-t0 << " seconds and " << step_no << " steps!" << std::endl;
        out.close();
   }

   /*
    * The fastint integration (calculating forward and backward rates directly
    * through reaction constants) needs the networks to be exanded what this
    * method ensures (multiple occurance of same educt product in same reaction
    * has to be explicit - "A + A" instead of "2 A").
    */

    void initialize_fastint() {
        for(size_t i=0; i<re.size(); ++i) {
            for(size_t j=0; j<re[i].get_no_educt_s(); ++j) {
                if(re[i].get_educt_mul(j) > 1.1) {
                    re[i].add_educt_s(re[i].get_educt_id(j), re[i].get_educt_mul(j)-1);
                    re[i].set_educt_mul(j,1.0);
                }
            }

            for(size_t j=0; j<re[i].get_no_product_s(); ++j) {
                if(re[i].get_product_mul(j) > 1.1) {
                    re[i].add_product_s(re[i].get_product_id(j), re[i].get_product_mul(j)-1);
                    re[i].set_product_mul(j,1.0);
                }
            }
        }
    }

    /*
     * Method for calling the simple / fast integrator that uses explicit stepping and
     * the reaction constants defined in the network object. System (f(x)) is defined
     * in "fast_system".
     * For description of method's parameters see method "run" above.
     */

    void run_fastint(double Tmax=25000, double deltaT=0.1, size_t wint=500, size_t write_log=0) {
        fstream out(fn_concentration.c_str(), std::ios_base::out | std::ios_base::app);
        out.precision(25);

        size_t t0 = time(NULL);

        vector_type x(initial_con), last_con(initial_con);
        double last_write(initial_t);

        // Function that is called by stepper and writes concentration (<vec>) to file.
        auto write_state = [this, &out, t0, wint, deltaT, Tmax, &last_con, &last_write]( const vector_type &vec , const double t ) {
            // calculate mean square distance from last write
            double last_msd=0;
            for(size_t i=0; i<vec.size(); ++i)
                 last_msd += (vec[i]-last_con[i])*(vec[i]-last_con[i]);
            if(t != last_write)
                last_msd /= (vec.size()*(t-last_write));
            last_write=t;
            last_con=vec;

            // write time + msd...
            out << t << "," << last_msd;
            // ...and all concentrations
            for(size_t l=0; l<vec.size(); ++l)
                out << "," << vec(l);

            // If msd (change of concentration) per time and per species is less
            // than 1e-20 but not zero this integrator stops early because the
            // assumption is that the network is converged. (Might not be true in
            // the strict sense for all imaginable networks that can be simulated!)
            if(last_msd > 1e-44 && last_msd < 1e-20) {
                std::cout << "Reached msd < 1e-20 condition - exiting early!" << std::endl;
                size_t t1 = time(NULL);
                std::cout << "Run took " << t1-t0 << " seconds!" << std::endl;
                exit(0);
            }

            out << std::endl;
        };

        // Calculate vector of time points at which "write_state" will be called.
        // For details see same code in method "run" above.
        std::vector<double> times( wint );
        for( size_t i=0 ; i<wint ; ++i )
            if(write_log == 0)
                times[i] = initial_t + double(i+1)/(wint)*(Tmax-initial_t);
            else if(write_log == 1 || initial_t == 0)
                times[i] = initial_t + (exp(double(i+1))-1)/(exp(double(wint))-1)*(Tmax-initial_t);
            else
                times[i] = initial_t*pow(exp(1/double(wint)*log(Tmax/initial_t)),double(i+1));

        // Call integrator - returns number of steps
        size_t step_no = integrate_times( make_dense_output< runge_kutta_dopri5< vector_type > >( 1.0e-6 , 1.0e-6 ) ,
                         fast_system(*this) , x, times, deltaT, write_state);

        // print time + steps needed and close file
        size_t t1 = time(NULL);
        std::cout << "Run took " << t1-t0 << " seconds and " << step_no << " steps!" << std::endl;
        out.close();
   }
};

/*
 * Main-Function. Manages commandline parameter and prints help screen.
 * All the real work is done through using the reaction_network_system
 * class (above).
 */

int main(int argc, const char *argv []){
    srand(time(0));
    std::cout << "odeint_rnet version " << odeint_rnet_version_string << " (commit:"
              << GIT_VERSION << ")" << std::endl;

    cl_para cl(argc, argv);  // create class to query command line parameters

    // Prints the right hand side of the ODE (f(x)) for diagnostic purposes.

    if(cl.have_param("print_rhs")) {
        std::string fn_network, fn_concentration;

        if(cl.have_param("net"))
            fn_network=cl.get_param("net");
        else {
            cout << "You have to give the name of reaction network by 'net'!" << endl;
            return 1;
        }

        if(cl.have_param("con"))
            fn_concentration=cl.get_param("con");
        else {
            cout << "You have to give the name of the concentration file by 'con'!" << endl;
            return 1;
        }

        // load network and concentration file and call function to print right hand side
        reaction_network_system rns = reaction_network_system(fn_network, fn_concentration);
        rns.print_rhs();
    }

    // Simulates / integrates a reaction network while holding the boundary point
    // species (constant=true) constant. Effective reaction rates are calculated
    // directly from chemical energies. Stiff solver is available with option
    // "solve_implicit". Very slow for larger networks (>50 species).

    if(cl.have_param("simulate")) {
        std::string fn_network, fn_concentration;

        if(cl.have_param("net"))
            fn_network=cl.get_param("net");
        else {
            cout << "You have to give the name of reaction network by 'net'!" << endl;
            return 1;
        }

        if(cl.have_param("con"))
            fn_concentration=cl.get_param("con");
        else {
            cout << "You have to give the name of the concentration file by 'con'!" << endl;
            return 1;
        }

        // Initial time-step for integrator
        double deltaT = cl.have_param("deltaT") ? cl.get_param_d("deltaT") : 0.1;
        // ODE is integrated up to <Tmax>
        double Tmax = cl.have_param("Tmax") ? cl.get_param_d("Tmax") : 25000;
        // Number of times the output is written between initial time and <Tmax>
        double wint = cl.have_param("wint") ? cl.get_param_d("wint") : 500;
        // Use implicit solver?
        bool solve_implicit=cl.have_param("solve_implicit");
        // Is output given logarithmically spaced? (Or linearly?)
        size_t write_log=0;
        if(cl.have_param("write_log"))
            write_log=1;
        if(cl.have_param("write_log_abs"))
            write_log=2;

        std::cout << "Parameters are deltaT=" << deltaT << "  and Tmax=" << Tmax
                  << "   wint=" << wint << std::endl;

        if(write_log == 1)
            cout << "Period of output will be equidistant on logscale (from t=t_0)." << endl;

        if(write_log == 2)
            cout << "Period of output will be equidistant on logscale (from t=0)." << endl;

        if(solve_implicit)
            cout << "Stiff solver is used!" << endl;

        // Load reaction network and concentration file
        reaction_network_system rns = reaction_network_system(fn_network, fn_concentration);

        // Simulate ODE (and write results to file)
        // The method gives diagnostic feedback to the user through console output
        rns.run(Tmax, deltaT, solve_implicit, wint, write_log);
    }

    // Simulates / integrates a reaction network while holding the boundary point
    // species (constant=true) constant. Official parameter to call this is
    // "fastint", the option to call using "simsim" is maintained for backward
    // compatibility. This integrator is faster as the one above, especially for
    // larger (>50 species) networks. It uses the reaction constants present in
    // the network and no stiff solver.

    if(cl.have_param("fastint") || cl.have_param("simsim")) {
        std::string fn_network, fn_concentration;

        if(cl.have_param("net"))
            fn_network=cl.get_param("net");
        else {
            cout << "You have to give the name of reaction network by 'net'!" << endl;
            return 1;
        }

        if(cl.have_param("con"))
            fn_concentration=cl.get_param("con");
        else {
            cout << "You have to give the name of the concentration file by 'con'!" << endl;
            return 1;
        }

        // Initial time-step for integrator
        double deltaT = cl.have_param("deltaT") ? cl.get_param_d("deltaT") : 0.1;
        // ODE is integrated up to <Tmax>
        double Tmax = cl.have_param("Tmax") ? cl.get_param_d("Tmax") : 25000;
        // Number of times the output is written between initial time and <Tmax>
        double wint = cl.have_param("wint") ? cl.get_param_d("wint") : 500;
        // Is output given logarithmically spaced? (Or linearly?)
        size_t write_log=0;
        if(cl.have_param("write_log"))
            write_log=1;
        if(cl.have_param("write_log_abs"))
            write_log=2;

        std::cout << "Parameters are deltaT=" << deltaT << "  and Tmax=" << Tmax
                  << "   wint=" << wint << std::endl;

        if(write_log == 1)
            cout << "Period of output will be equidistant on logscale (from t=t_0)." << endl;

        if(write_log == 2)
            cout << "Period of output will be equidistant on logscale (from t=0)." << endl;

        // Load reaction network and concentration file
        reaction_network_system rns = reaction_network_system(fn_network, fn_concentration);

        // Simulate ODE (and write results to file)
        // The method gives diagnostic feedback to the user through console output
        rns.initialize_fastint();
        rns.run_fastint(Tmax, deltaT, wint, write_log);
    }

    // User interface / help dialogue
    if(cl.have_param("help") || cl.have_param("info")) {
        cout << "          odeint_rnet" << endl;
        cout << "          ===========" << endl;
        cout << " call with parameter 'info' or 'help' for showing this screen" << endl;
        cout << "current version is " << odeint_rnet_version_string << " (commit:" << GIT_VERSION << ")" << endl;
        cout << endl;
        cout << "-'print_rhs': load reaction network 'net', (last) concentration in 'con'! and" << endl;
        cout << "prints  right hand side of ode for diagnostic purposes." << endl;
	cout << endl;
        cout << "-'simulate': load reaction network 'net' and simulate file 'con'!. Parameters are:" << endl;
        cout << "   x'deltaT': Integration interval (relevant for integrator, not for output!)" << endl;
        cout << "   x'Tmax': Time up to which the system is simulated" << endl;
        cout << "   x'wint': number of output times between Tstart and Tmax" << endl;
        cout << "   x'write_log': write output logarithmically spaced (from t=t_0)" << endl;
        cout << "   x'write_log_abs': write output logarithmically spaced from (t=0)" << endl;
        cout << "   x'solve_implicit': use stiff solver (might be slower for big nets!)" << endl;
        cout << endl;
        cout << "-'fastint': load reaction network 'net' and simulate file 'con'!. Other than above" << endl;
        cout << "   this integrator does not offer a stiff solver. It also does not use energies to" << endl;
        cout << "   calculate effective rates directly but uses reaction constants. Parameters are:" << endl;
        cout << "   x'deltaT': Integration interval (relevant for integrator, not for output!)" << endl;
        cout << "   x'Tmax': Time up to which the system is simulated" << endl;
        cout << "   x'wint': number of output times between Tstart and Tmax" << endl;
        cout << "   x'write_log': write output logarithmically spaced (from t=t_0)" << endl;
        cout << "   x'write_log_abs': write output logarithmically spaced from (t=0)" << endl;
	cout << endl;
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIST_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIST_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/dist.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/subs.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::dist_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return boost::simd::subs(boost::simd::max(a0, a1), boost::simd::min(a1,a0)); }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::dist_, tag::cpu_, (A0)(X)
                            , ((simd_<unsigned_<A0>,X>))
                              ((simd_<unsigned_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)  { return (boost::simd::max(a0, a1)-boost::simd::min(a1,a0)); }
  };
} } }

#endif


#include <armadillo>

// pybind11
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <pybind11/pybind11.h>
namespace py = pybind11;

// Carma
#include <carma/carma.h>

struct _neigh {
    arma::mat data;
    arma::colvec weights;
    arma::uword n_rows;
    _neigh(const arma::mat& m, const arma::mat& w):
        data(w.n_elem, m.n_cols, arma::fill::zeros),
        weights(w.n_elem, arma::fill::zeros),
        n_rows(0) {}
};

typedef _neigh neigh_t;

void neigh_vec(neigh_t& n,
               const arma::mat& m,
               const arma::uword m_nrow,
               const arma::uword m_ncol,
               const arma::mat& w,
               const arma::uword m_b,
               const arma::uword m_i,
               const arma::uword m_j) {

    arma::uword w_leg_i = w.n_rows / 2, w_leg_j = w.n_cols / 2;

    // copy values
    arma::uword k = 0;
    for (arma::uword i = 0; i < w.n_rows; ++i)
        for (arma::uword j = 0; j < w.n_cols; ++j)
            if (m_i + i >= w_leg_i && m_j + j >= w_leg_j &&
                m_i + i < w_leg_i + m_nrow &&
                m_j + j < w_leg_j + m_ncol &&
                arma::is_finite(m(m_j + m_i * m_ncol, 0))) {

                n.data(k, m_b) = m((m_j + j - w_leg_j) +
                    (m_i + i - w_leg_i) * m_ncol, m_b);
                n.weights(k++) = w(i, j);
            }
    n.n_rows = k;
}

arma::colvec nm_post_mean_x(const arma::colvec& x,
                            const arma::mat& sigma,
                            const arma::colvec& mu0,
                            const arma::mat& sigma0) {

    // inverse sigma0
    arma::mat inv_sum_weights(arma::size(sigma0));
    inv_sum_weights = arma::inv(sigma + sigma0);

    return sigma * inv_sum_weights * mu0 + sigma0 * inv_sum_weights * x;
}

arma::mat bayes_smoother(const arma::mat& m,
                         const arma::uword m_nrow,
                         const arma::uword m_ncol,
                         const arma::mat& w,
                         const arma::mat& sigma,
                         bool covar_sigma0) {

    // initialize result matrix
    arma::mat res(arma::size(m), arma::fill::none);
    res.fill(arma::datum::nan);

    // prior mean vector (neighbourhood)
    arma::colvec mu0(m.n_cols, arma::fill::zeros);

    // prior co-variance matrix (neighbourhood)
    arma::mat sigma0(arma::size(sigma), arma::fill::zeros);

    // neighbourhood
    neigh_t neigh(m, w);

    // compute values for each pixel
    for (arma::uword i = 0; i < m_nrow; ++i)
        for (arma::uword j = 0; j < m_ncol; ++j) {

            // fill neighbours values
            for (arma::uword b = 0; b < m.n_cols; ++b)
                neigh_vec(neigh, m, m_nrow, m_ncol, w, b, i, j);

            if (neigh.n_rows == 0) continue;

            // compute prior mean
            mu0 = arma::mean(neigh.data.rows(0, neigh.n_rows - 1), 0).as_col();

            // compute prior sigma
            sigma0 = arma::cov(neigh.data.rows(0, neigh.n_rows - 1), 0);

            // prior sigma covariance
            if (!covar_sigma0) {

                // clear non main diagonal cells
                sigma0.elem(arma::trimatu_ind(
                        arma::size(sigma0), 1)).fill(0.0);
                sigma0.elem(arma::trimatl_ind(
                        arma::size(sigma0), -1)).fill(0.0);
            }

            // evaluate multivariate bayesian
            res.row(j + i * m_ncol) =
                nm_post_mean_x(m.row(j + i * m_ncol).as_col(),
                               sigma, mu0, sigma0).as_row();
        }
    return res;
}

arma::mat kernel_smoother(const arma::mat& m,
                          const arma::uword m_nrow,
                          const arma::uword m_ncol,
                          const arma::mat& w,
                          const bool normalised) {

    // initialize result matrix
    arma::mat res(arma::size(m), arma::fill::none);
    res.fill(arma::datum::nan);

    // neighbourhood
    neigh_t neigh(m, w);

    // compute values for each pixel
    for (arma::uword b = 0; b < m.n_cols; ++b)
        for (arma::uword i = 0; i < m_nrow; ++i)
            for (arma::uword j = 0; j < m_ncol; ++j) {

                // fill neighbours values
                neigh_vec(neigh, m, m_nrow, m_ncol, w, b, i, j);

                if (neigh.n_rows == 0) continue;

                // normalise weight values
                if (normalised)
                    neigh.weights = neigh.weights /
                        arma::sum(neigh.weights.subvec(0, neigh.n_rows - 1));

                // compute kernel neighbourhood weighted mean
                res(j + i * m_ncol, b) = arma::as_scalar(
                    neigh.weights.subvec(0, neigh.n_rows - 1).as_row() *
                        neigh.data.col(b).subvec(0, neigh.n_rows - 1));
            }
    return res;
}

arma::mat bilinear_smoother(const arma::mat& m,
                            const arma::uword m_nrow,
                            const arma::uword m_ncol,
                            const arma::mat& w,
                            double tau) {

    // initialize result matrix
    arma::mat res(arma::size(m), arma::fill::none);
    res.fill(arma::datum::nan);

    // neighbourhood
    neigh_t neigh(m, w);

    // compute values for each pixel
    for (arma::uword b = 0; b < m.n_cols; ++b)
        for (arma::uword i = 0; i < m_nrow; ++i)
            for (arma::uword j = 0; j < m_ncol; ++j) {

                // fill neighbours values
                neigh_vec(neigh, m, m_nrow, m_ncol, w, b, i, j);

                if (neigh.n_rows == 0) continue;

                // compute bilinear weight
                arma::colvec bln_weight = neigh.weights % arma::normpdf(
                    neigh.data.col(b) - m(j + i * m_ncol, b), 0, tau);

                // normalise weight values
                bln_weight = bln_weight /
                    arma::sum(bln_weight.subvec(0, neigh.n_rows - 1));

                // compute kernel neighbourhood weighted mean
                res(j + i * m_ncol, b) = arma::as_scalar(
                    bln_weight.subvec(0, neigh.n_rows - 1).as_row() *
                    neigh.data.col(b).subvec(0, neigh.n_rows - 1));
            }
    return res;
}

void PyInit_smoothing(py::module &m) {
    m.def("bayes_smoother", &bayes_smoother, "Bayes Smoother");

    m.def("kernel_smoother", &kernel_smoother, "Kernel Smoother");

    m.def("bilinear_smoother", &bilinear_smoother, "Bilinear Smoother");
}


// Method for converting transfer function to system of first order differential equations for evaluation numerically is in Ogata, 4th ed, page 78.
// Use Mathematica StateSpaceModel instead of Ogata as basis for conversion of TF to State Space.

// Step response achieved by setting u(t) = 1, in derivative term, see MMA State Space model in Plasticity.nb, subsection on Utilities for preparing C++ code.

// Chop p0 in calling routine
// Bound p1 >= 1e-7 in calling routine

#include <iostream>
#include <vector>
#include <cassert>
#include <string>
#include <map>

#include <gsl/gsl_poly.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_spline.h>

#include <boost/math/tools/polynomial.hpp>

#include "fmt/format.h"
#include "Performance.h"

// steps for interpolation, 5000 comes very close to Mathematica numerical results, check timing
const int steps = 5000;

unsigned debugPerformance = 0;
void debugPerformanceOn(unsigned d) {debugPerformance=d;}

double 	MaxRootRealPart(const std::vector<double>& coeff);
double 	H2sq(const std::vector<double>& num, const std::vector<double>& den);
double 	integrandH2(double w, void *p);
int 	deriv (double t, const double x[], double f[], void *p);
double 	stepPerformance(const std::vector<double>& num, const std::vector<double>& den, double tmax, signalType s);
double 	integrandStep(double y, void *p);

struct my_params {const std::vector<double>& num; const std::vector<double>& den;};
struct stepParam {gsl_spline *spline; gsl_interp_accel *acc;};

// Do chopping to set param to zero if close and check bounds before call

double performance(const std::vector<double>& num, const std::vector<double>& den,
					double gamma, double tmax, signalType s)
{
	if (MaxRootRealPart(den) > -1e-6) return 1e20;
	if (debugPerformance){
		double sf = stepPerformance(num, den, tmax, s);
		double hf = H2sq(num,den);
		std::map<signalType,std::string> signal =
			{{signalType::output, "output"}, {signalType::controlOpen,"cntrlO"}, {signalType::controlClosed,"cntrlC"}};
		std::cout << fmt::format("step = {}, h2 = {} for {}\n", sf, hf, signal[s]);
		return sf + gamma*hf;
	}
	else
		return stepPerformance(num, den, tmax, s) + gamma*H2sq(num,den);
}

// coeff of polynomial from low order to high order terms
double MaxRootRealPart(const std::vector<double>& coeff)
{
	auto n = coeff.size();
	// std::cout << "n = " << n << std::endl;
    std::vector<double> z(2*(n-1));
	gsl_poly_complex_workspace * w
	  = gsl_poly_complex_workspace_alloc (n);
	if (GSL_SUCCESS != gsl_poly_complex_solve(coeff.data(), n, w, z.data())){
		gsl_poly_complex_workspace_free (w);
		return 1.0;		// positive value is marked as unstable
	}
	gsl_poly_complex_workspace_free (w);

	double max = -1e20;
	for (unsigned i = 0; i < n-1; i++) {
	  // std::cout << fmt::format("z{} = {:+.18f} {:+.18f}\n", i, z[2*i], z[2*i+1]);
	  if (z[2*i] > max) max = z[2*i];
	}
	return max;
}

// In the following, check that integrand gets small at large frequency, required for H2 integral to converge. If not then if order of numerator = order of denominator, then integration will be infinite, because at high frequency, high order terms in numerator and denominator dominate, and so given same order of those terms, the system does not go to zero at high frequency. Correct for this by redefining the num/den transfer function as (num/den - high order num coeff/high order den coeff). This subtraction removes the Dirac delta impulse component of the dynamics, which corresponds in frequency space to a uniform addition to the absolute value of the tf at all frequencies equal to the amount subtracted off. Does not change dynamics, except to remove impulse at time zero acting over infinitesimal duration.

// returns square of H2 value
double H2sq(const std::vector<double>& num, const std::vector<double>& den)
{
	bool sizeFix = false;
	boost::math::tools::polynomial<double> poly_newnum;
	boost::math::tools::polynomial<double> poly_newden;
	if (num.size() == den.size()){			// deleting dirac impulse for equal-sized num & den
		double numHiOrder = num.back();
		double denHiOrder = den.back();
		boost::math::tools::polynomial<double> polyn(num.begin(), num.end());
		boost::math::tools::polynomial<double> polyd(den.begin(), den.end());
		poly_newnum = denHiOrder*polyn - numHiOrder*polyd;
		poly_newden = denHiOrder*polyd;
		// high order term in new numerator should be zero and automatically dropped, reducing size
		if (poly_newnum.data().size() != polyn.data().size() - 1){
			if (poly_newnum.data().back() < 1e-5)
				poly_newnum.data().pop_back();
			else
				assert(poly_newnum.data().size() == polyn.data().size() - 1);
		}
		sizeFix = true;
// 		std::cout << "Num = " << polyn << std::endl;
// 		std::cout << "Den = " << polyd << std::endl;
// 		std::cout << "NN  = " << poly_newnum << std::endl;
// 		std::cout << "ND  = " << poly_newden << std::endl;
	}

	gsl_function F;
	F.function = &integrandH2;
    // if size fixed above, use newnum and newden
    const std::vector<double>& n = (sizeFix) ? poly_newnum.data() : num;
    const std::vector<double>& d = (sizeFix) ? poly_newden.data() : den;
    my_params params {n, d};
    F.params = &params;
	if (integrandH2(1e10, F.params) > 1e-3) return 1e20; 	// should not happen, because den.size > num.size
	double result, error;
	gsl_integration_workspace *w = gsl_integration_workspace_alloc (1000);
	// std::cout << "h2 int start" << std::endl;
	if (GSL_SUCCESS != gsl_integration_qagi(&F, 0, 1e-7, 1000, w, &result, &error))
		result = 1e30;
	// std::cout << "h2 int end" << std::endl;
	gsl_integration_workspace_free(w);
	return result / (2.0*M_PI);
}

double integrandH2(double w, void *p)
{
	gsl_complex s;
	my_params *params = static_cast<my_params *>(p);
	const std::vector<double>& num = params->num;
	const std::vector<double>& den = params->den;
	GSL_SET_COMPLEX(&s, 0, w);
    int numSize = static_cast<int>(num.size());
    int denSize = static_cast<int>(den.size());
	return gsl_complex_abs2(gsl_complex_div(gsl_poly_complex_eval(num.data(), numSize, s),
								gsl_poly_complex_eval(den.data(), denSize, s)));
}

// deriv works for both output signal and control signal, see MMA file
// Possible speed up by optimizing deriv, see callgrind source code line by line weighting of time cost.

int deriv(double t, const double x[], double f[], void *p)
{
	(void)(t); /* avoid unused parameter warning */
	my_params *params = static_cast<my_params *>(p);
	const std::vector<double>& den = params->den;
	auto lastrow = den.size()-2;
	f[lastrow] = 0;
	for (unsigned i = 0; i < lastrow; ++i){
		f[i] = x[i+1];
		f[lastrow] -= den[i]*x[i];
	}
	f[lastrow] -= den[lastrow]*x[lastrow];
	f[lastrow] /= den.back();
	// next line for input, u(t) = 1 for step input
	f[lastrow] += 1;
	return GSL_SUCCESS;
}

// must make different ycoeff for output and control signals
// for output, same coeff for open and closed loops
// for control signals, diff coeff for open and closed loops
// see MMA file

double stepPerformance(const std::vector<double>& num, const std::vector<double>& den, double tmax, signalType s)
{
	double time[steps+1];
	double y[steps+1];
	my_params params = {num, den};
	auto dim = den.size()-1;	// dimensions of state space model for dynamics
	gsl_odeiv2_system sys = {deriv, NULL, dim, &params};
    // see GSL docs for alternative algorithms
	gsl_odeiv2_driver *d =
		gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rkf45, 1e-6, 1e-6, 0.0);
	double t = 0.0;
	double denBack = den.back();

	// coefficients to get output, initialize with values for each case, max dim is 4, so use that
	double ycoeff[4];
	unsigned long ydim;		// number of output coefficients to get output y, varies by problem, set explicitly
	double yinputCoeff = 0;	// must add yinputCoeff * input to output; input=1 for the step response

	if (s == signalType::output){ // case of output signal, same for open and closed loops
		ydim = 3;
		ycoeff[0] = num[0]/denBack; ycoeff[1] = num[1]/denBack; ycoeff[2] = num[2]/denBack;
	}
	else {
		// cases of control signal, # coeff is always dimension of problem, dim
		ydim = dim;
		double numBack = num.back();
		yinputCoeff = numBack / denBack;
		if (s == signalType::controlOpen) { // control signal, open loop
			for (unsigned i = 0; i < ydim; ++i)
				ycoeff[i] = (num[i] - den[i]*yinputCoeff) / denBack;
		}
		else if (s == signalType::controlClosed) { // control signal, closed loop
			for (unsigned i = 0; i < ydim; ++i){
				ycoeff[i] = (num[i] - den[i]*yinputCoeff) / denBack;
				if (debugPerformance >= 2) std::cout << ycoeff[i] << " ";
			}
			if (debugPerformance >= 2) std::cout << yinputCoeff << std::endl;
			if (debugPerformance >= 2){
				for (auto& n : den) std::cout << -n/denBack << " ";
				std::cout << std::endl;
				for (auto& n : num) std::cout << n << " ";
				std::cout << std::endl;
				for (auto& n : den) std::cout << n << " ";
				std::cout << std::endl;
			}
		}
		else {
			assert(false);	// should not be here, signal must be one of above types
		}
	}

	time[0] = 0.0;
	// at time zero with step, dominated by infinite freq, so if order of den > num, then at time zero,
	// initial value is zero, if order den=num, then ratio of highest order terms,
	// order num>den should not happen
	y[0] = (den.size() > num.size()) ? 0.0 : num.back()/den.back();
    std::vector<double> x(den.size()-1,0); // 0 at end causes zero initialization
	for (int i = 1; i <= steps; i++){
		// add one to tmax, so that interpolation extends past boundary for integration
		// otherwise, can get an error when interpolating close to the upper boundary
	  	double ti = i * (tmax+1.0) / static_cast<double>(steps);
	  	int status = gsl_odeiv2_driver_apply(d, &t, ti, x.data());
		assert(status == GSL_SUCCESS);
		time[i] = t;
		y[i] = 0;
		for (unsigned j = 0; j < ydim; ++j)
			y[i] += ycoeff[j] * x[j];
		y[i] += yinputCoeff;
		// if (i % 100 == 0) std::cout << fmt::format("{:7.3f} {:8.6f}\n", time[i], y[i]);
		if (debugPerformance >= 3 && i % 100 == 0 && s == signalType::controlClosed)
			std::cout << fmt::format("{:7.3f} {:8.6f}\n", time[i], y[i]);
	}
	gsl_odeiv2_driver_free (d);

	gsl_interp_accel *acc = gsl_interp_accel_alloc();
    gsl_spline *spline = gsl_spline_alloc(gsl_interp_cspline, steps);
    if (GSL_SUCCESS != gsl_spline_init (spline, time, y, steps)){
    	gsl_spline_free (spline);
    	gsl_interp_accel_free (acc);
    	return 1e20;
    }

    // std::cout << gsl_spline_eval(spline, 19.9689, acc) << std::endl;

	stepParam integrParam = {spline, acc};
    gsl_function F;
	F.function = &integrandStep;
	F.params = &integrParam;
	double result, error;
	gsl_integration_workspace *w = gsl_integration_workspace_alloc (1000);
	// std::cout << "step int start" << std::endl;
	// using 1e-6 for abs and rel error
	double errtol = 1e-6;
	if (GSL_SUCCESS != gsl_integration_qag(&F, 0.0, tmax, errtol, errtol, 1000, 6, w, &result, &error)){
		gsl_integration_cquad_workspace *ctable = gsl_integration_cquad_workspace_alloc(200);
		if (GSL_SUCCESS != gsl_integration_cquad(&F, 0, tmax, errtol, errtol, ctable, &result, &error, NULL)){
     		boost::math::tools::polynomial<double> poly_newnum(num.begin(), num.end());
			boost::math::tools::polynomial<double> poly_newden(den.begin(), den.end());
			if (true){
				std::cout << "Step integration error, with num = " << poly_newnum << std::endl;
				std::cout << "                         and den = " << poly_newden << std::endl;
				std::cout << "                      and result = " << result << std::endl;
			}
    		result = 1e20;
    	}
    	gsl_integration_cquad_workspace_free(ctable);
	}
	// std::cout << "step int end" << std::endl;
	gsl_integration_workspace_free(w);

    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);

	return result;
}

double integrandStep(double t, void *p)
{
	stepParam *params = static_cast<stepParam *>(p);
	double z = 1.0 - gsl_spline_eval(params->spline, t, params->acc);
	return z*z;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ILOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ILOG2_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/clz.hpp>
#include <boost/simd/function/simd/exponent.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/minusone.hpp>
#include <boost/simd/function/simd/splat.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(ilog2_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return bs::exponent(a0);
      }
   };

   BOOST_DISPATCH_OVERLOAD(ilog2_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return minusone(sizeof(bd::scalar_of_t<A0>)*8-bs::clz(bitwise_cast<result>(a0)));
      }
   };

} } }

#endif


#include <gtest/gtest.h>

#include <Eigen/Core>

#include "helpers.hh"
#include "../Config/config.hh"
#include "../FieldMap/fieldmap.hh"
#include "../StateObject/StationaryMapState/stationarymapstate.hh"

using namespace std;
using namespace bold;
using namespace Eigen;

TEST (GoalEstimateTests, estimateOppositeGoal)
{
  GoalEstimate goal(Vector2d(-1, 0), Vector2d(-1, 1), GoalLabel::Unknown);

  GoalEstimate opposite(goal.estimateOppositeGoal(GoalLabel::Ours));

  EXPECT_EQ ( Vector2d(FieldMap::getFieldLengthX() - 1, 0), opposite.getPost1Pos() );
  EXPECT_EQ ( Vector2d(FieldMap::getFieldLengthX() - 1, 1), opposite.getPost2Pos() );
  EXPECT_EQ ( GoalLabel::Ours, opposite.getLabel() );

  goal = { Vector2d(1, 0), Vector2d(1, 1), GoalLabel::Unknown };

  opposite = goal.estimateOppositeGoal(GoalLabel::Theirs);

  EXPECT_EQ ( Vector2d(-FieldMap::getFieldLengthX() + 1, 0), opposite.getPost1Pos() );
  EXPECT_EQ ( Vector2d(-FieldMap::getFieldLengthX() + 1, 1), opposite.getPost2Pos() );
  EXPECT_EQ ( GoalLabel::Theirs, opposite.getLabel() );

  goal = { Vector2d(0, 1), Vector2d(1, 1), GoalLabel::Unknown };

  opposite = goal.estimateOppositeGoal(GoalLabel::Theirs);

  EXPECT_EQ ( Vector2d(0, -FieldMap::getFieldLengthX() + 1), opposite.getPost1Pos() );
  EXPECT_EQ ( Vector2d(1, -FieldMap::getFieldLengthX() + 1), opposite.getPost2Pos() );
  EXPECT_EQ ( GoalLabel::Theirs, opposite.getLabel() );

  goal = { Vector2d(-1, -1), Vector2d(1, -1), GoalLabel::Unknown };

  opposite = goal.estimateOppositeGoal(GoalLabel::Theirs);

  EXPECT_EQ ( Vector2d(-1, FieldMap::getFieldLengthX() - 1), opposite.getPost1Pos() );
  EXPECT_EQ ( Vector2d( 1, FieldMap::getFieldLengthX() - 1), opposite.getPost2Pos() );
  EXPECT_EQ ( GoalLabel::Theirs, opposite.getLabel() );
}

TEST (GoalEstimateTests, getMidpoint)
{
  GoalEstimate goal(Vector2d(0, 0), Vector2d(1, 0), GoalLabel::Unknown);

  EXPECT_EQ ( Vector2d(0.0, 0), goal.getMidpoint(0)   );
  EXPECT_EQ ( Vector2d(0.1, 0), goal.getMidpoint(0.1) );
  EXPECT_EQ ( Vector2d(0.5, 0), goal.getMidpoint(0.5) );
  EXPECT_EQ ( Vector2d(1.0, 0), goal.getMidpoint(1.0) );
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

template<typename T>
Average<T> createAverage(T value, int count)
{
  Average<T> avg;
  for (; count > 0; count--)
    avg.add(value);
  return avg;
}

TEST (StationaryMapStateTests, pairGoalPosts)
{
  // TODO give a case where one of the posts could be paired with two of the others, yet one pairing is more probable

  auto goalY = FieldMap::getGoalY();

  vector<Average<Vector2d>> posts = {
    createAverage(Vector2d(0, 0),     StationaryMapState::GoalSamplesNeeded),
    createAverage(Vector2d(0.1, 0.1), StationaryMapState::GoalSamplesNeeded),
    createAverage(Vector2d(0, goalY), StationaryMapState::GoalSamplesNeeded),
    createAverage(Vector2d(100, 100), StationaryMapState::GoalSamplesNeeded)
  };

  auto pairs = StationaryMapState::pairGoalPosts(posts);

  ASSERT_EQ (1, pairs.size());

  EXPECT_TRUE(VectorsEqual(Vector2d(0,0), pairs[0].first.getAverage()));
  EXPECT_TRUE(VectorsEqual(Vector2d(0,goalY), pairs[0].second.getAverage()));

  vector<Average<Vector2d>> uselessPosts = {
    createAverage(Vector2d(0, 0),     StationaryMapState::GoalSamplesNeeded),
    createAverage(Vector2d(0.1, 0.1), StationaryMapState::GoalSamplesNeeded),
    createAverage(Vector2d(100, 100), StationaryMapState::GoalSamplesNeeded)
  };

  ASSERT_EQ (0, StationaryMapState::pairGoalPosts(uselessPosts).size());
}

TEST (StationaryMapStateTests, labelGoalByKeeperBallDistance)
{
  auto goalY = FieldMap::getGoalY();
  auto fieldX = FieldMap::getFieldLengthX();

  auto post1 = createAverage(Vector2d(1, 0),     StationaryMapState::GoalSamplesNeeded);
  auto post2 = createAverage(Vector2d(1, goalY), StationaryMapState::GoalSamplesNeeded);

  ASSERT_EQ(GoalLabel::Ours,    StationaryMapState::labelGoalByKeeperBallDistance(post1, post2, FieldSide::Ours));
  ASSERT_EQ(GoalLabel::Unknown, StationaryMapState::labelGoalByKeeperBallDistance(post1, post2, FieldSide::Unknown));
  ASSERT_EQ(GoalLabel::Unknown, StationaryMapState::labelGoalByKeeperBallDistance(post1, post2, FieldSide::Theirs));

  post1 = createAverage(Vector2d(1 + fieldX/2.0, 0),     StationaryMapState::GoalSamplesNeeded);
  post2 = createAverage(Vector2d(1 + fieldX/2.0, goalY), StationaryMapState::GoalSamplesNeeded);

  ASSERT_EQ(GoalLabel::Unknown, StationaryMapState::labelGoalByKeeperBallDistance(post1, post2, FieldSide::Ours));

  // TODO if the ball is on our side and the goal is far enough away, then it's Theirs, not Unknown
}

TEST (StationaryMapStateTests, labelGoalByKeeperObservations)
{
  auto goalY = FieldMap::getGoalY();
  auto maxGoalieGoalDistance = Config::getValue<double>("vision.player-detection.max-goalie-goal-dist");

  auto post1 = createAverage(Vector2d(1, 0),     StationaryMapState::GoalSamplesNeeded);
  auto post2 = createAverage(Vector2d(1, goalY), StationaryMapState::GoalSamplesNeeded);

  Vector2d goodKeeper1(1, goalY/2.0);
  Vector2d goodKeeper2(1, goalY/2.0 + 0.99 * maxGoalieGoalDistance);
  Vector2d goodKeeper3(1 + 0.99 * maxGoalieGoalDistance, goalY/2.0);
  Vector2d goodKeeper4(1 - 0.99 * maxGoalieGoalDistance, goalY/2.0);

  Vector2d badKeeper1(1, goalY/2.0 + 1.01 * maxGoalieGoalDistance);
  Vector2d badKeeper2(1, goalY/2.0 - 1.01 * maxGoalieGoalDistance);
  Vector2d badKeeper3(1 + 1.01 * maxGoalieGoalDistance, goalY/2.0);
  Vector2d badKeeper4(1 - 1.01 * maxGoalieGoalDistance, goalY/2.0);

  vector<Average<Vector2d>> keepers = {
    createAverage(badKeeper1, StationaryMapState::KeeperSamplesNeeded),
    createAverage(badKeeper2, StationaryMapState::KeeperSamplesNeeded),
    createAverage(badKeeper3, StationaryMapState::KeeperSamplesNeeded),
    createAverage(badKeeper4, StationaryMapState::KeeperSamplesNeeded)
  };

  // Test with a set of keepers observations that are too far from the midpoint of the goal
  ASSERT_EQ(GoalLabel::Unknown, StationaryMapState::labelGoalByKeeperObservations(post1, post2, keepers));

  keepers[1] = createAverage(goodKeeper1, StationaryMapState::KeeperSamplesNeeded);
  ASSERT_EQ(GoalLabel::Ours, StationaryMapState::labelGoalByKeeperObservations(post1, post2, keepers));

  keepers[1] = createAverage(goodKeeper2, StationaryMapState::KeeperSamplesNeeded);
  ASSERT_EQ(GoalLabel::Ours, StationaryMapState::labelGoalByKeeperObservations(post1, post2, keepers));

  keepers[1] = createAverage(goodKeeper3, StationaryMapState::KeeperSamplesNeeded);
  ASSERT_EQ(GoalLabel::Ours, StationaryMapState::labelGoalByKeeperObservations(post1, post2, keepers));

  keepers[1] = createAverage(goodKeeper4, StationaryMapState::KeeperSamplesNeeded);
  ASSERT_EQ(GoalLabel::Ours, StationaryMapState::labelGoalByKeeperObservations(post1, post2, keepers));
}

TEST (StationaryMapStateTests, estimateWorldPositionForPoint)
{
  auto goalY = FieldMap::getGoalY();
  auto halfGoalY = goalY/2.0;
  auto halfFieldX = FieldMap::getFieldLengthX()/2.0;

  Vector2d pos;

  // Test from middle of field
  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(halfGoalY, halfFieldX), Vector2d(-halfGoalY, halfFieldX), Vector2d::Zero(), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(0,0), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(halfGoalY, halfFieldX), Vector2d(-halfGoalY, halfFieldX), Vector2d::Zero(), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(0,0), pos));

  // Standing 1m in front of one goal post
  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d::Zero(), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(-halfFieldX + 2, -halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(-goalY, 2), Vector2d::Zero(), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(-halfFieldX + 2, halfGoalY), pos));

  // ...and in front of the other post
  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d::Zero(), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(halfFieldX - 2, halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(-goalY, 2), Vector2d::Zero(), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(halfFieldX - 2, -halfGoalY), pos));

  // Now specify points which are not the origin

  // Standing 1m in front of one goal post
  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d(0, 1), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(-halfFieldX + 1, -halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(-goalY, 2), Vector2d(0, 1), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(-halfFieldX + 1, halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d(goalY, 1), GoalLabel::Ours);
  EXPECT_TRUE(VectorsEqual(Vector2d(-halfFieldX + 1, halfGoalY), pos));

  // ...and in front of the other post
  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d(0, 1), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(halfFieldX - 1, halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(-goalY, 2), Vector2d(0, 1), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(halfFieldX - 1, -halfGoalY), pos));

  pos = StationaryMapState::estimateWorldPositionForPoint(Vector2d(0, 2), Vector2d(goalY, 2), Vector2d(goalY, 1), GoalLabel::Theirs);
  EXPECT_TRUE(VectorsEqual(Vector2d(halfFieldX - 1, -halfGoalY), pos));
}

TEST (StationaryMapStateTests, labelGoalByKeeperBallPosition)
{
  auto goalY = FieldMap::getGoalY();
  auto halfGoalY = goalY/2.0;

  EXPECT_EQ(
    GoalLabel::Ours,
    StationaryMapState::labelGoalByKeeperBallPosition(
      createAverage(Vector2d(-3 - halfGoalY, 3), 10),
      createAverage(Vector2d(-3 + halfGoalY, 3), 10),
      Vector2d(-3, 3),
      Vector2d(-0.1, 0.2)));

  EXPECT_EQ(
    GoalLabel::Theirs,
    StationaryMapState::labelGoalByKeeperBallPosition(
      createAverage(Vector2d(3 - halfGoalY, FieldMap::getFieldLengthX() - 3), 10),
      createAverage(Vector2d(3 + halfGoalY, FieldMap::getFieldLengthX() - 3), 10),
      Vector2d(-3, 3),
      Vector2d(-0.1, 0.2)));

  // With some error...

  EXPECT_EQ(
    GoalLabel::Theirs,
    StationaryMapState::labelGoalByKeeperBallPosition(
      createAverage(Vector2d(4 - halfGoalY, FieldMap::getFieldLengthX() - 3), 10),
      createAverage(Vector2d(4 + halfGoalY, FieldMap::getFieldLengthX() - 3), 10),
      Vector2d(-3, 3),
      Vector2d(-0.1, 0.2)));
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SINC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SINC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the Computes the sinus cardinal value of its parameter,
    i.e. \f$\sin(x)/x\f$.

    @par Header <boost/simd/function/sinc.hpp>

    @see sin, sincpi, sinhc

    @par Example:

      @snippet sinc.cpp sinc

    @par Possible output:

      @snippet sinc.txt sinc

  **/
  IEEEValue sinc(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/sinc.hpp>
#include <boost/simd/function/simd/sinc.hpp>

#endif


#ifndef WAVE_CONSTANT_VELOCITY_GP_PRIOR_HPP
#define WAVE_CONSTANT_VELOCITY_GP_PRIOR_HPP

#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>
#include "wave/geometry_og/transformation.hpp"
#include "wave/utils/math.hpp"

namespace wave_kinematics {

struct ConstantVelocityPrior {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Mat12 = Eigen::Matrix<double, 12, 12>;
    using Mat6 = Eigen::Matrix<double, 6, 6>;

    ConstantVelocityPrior() = delete;
    ConstantVelocityPrior(const double &tk, const double &tkp1, const double* tau, const wave::Mat6& Qc, const wave::Mat6& inv_Qc);

    const double tk;
    const double tkp1;
    const double dT;
    const double *tau;
    const wave::Mat6 &Qc;
    const wave::Mat6 &inv_Qc;

    Mat12 t_mat, inv_covar;

    /// Function to generate transition matrix
    void calculateTransitionMatrix(Mat12 &transition, const double &t1, const double &t2);
    void calculateTransitionMatrix();

    /// Functions for linearized covariance
    void calculateLinInvCovariance(Mat12 &covariance, const double &t1, const double &t2);
    void calculateLinCovariance(Mat12 &covariance, const double &t1, const double &t2);
    void calculateLinInvCovariance();

    //todo(ben) figure out what to call these
    /// Functions to calculate hat and candle matrices
    void calculateStuff(Mat12 &hat, Mat12 &candle);

    void calculateCandle(Mat12 &candle);
    void calculateHat(Mat12 &hat);

};
}

#endif //WAVE_CONSTANT_VELOCITY_GP_PRIOR_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing acot capabilities

    inverse cotangent.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = acot(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[0, \pi[\f$ such that <tt>cot(r) == x</tt>.

    @see acotd

  **/
  const boost::dispatch::functor<tag::acot_> acot = {};
} }
#endif

#include <boost/simd/function/scalar/acot.hpp>
#include <boost/simd/function/simd/acot.hpp>

#endif


#include "CLCommon.hpp"
#include "CLContext.hpp"
#include "ProgramGenerator.hpp"

#include <cmath>
#include <iomanip>
#include <fstream>

#include <boost/math/common_factor.hpp>
#include <boost/program_options.hpp>

using namespace ot;

namespace po = boost::program_options;

/**
 * Structure to hold generator options.
 */
struct GeneratorParams {
  // Specified
  int32_t     timeTileSize;
  int32_t     timeSteps;
  int32_t     elementsPerThread;
  int32_t     blockSizeX;
  int32_t     problemSize;
  std::string dataType;

  // Derived
  int32_t     padding;
  int32_t     compsPerBlockX;
  int32_t     sizeLCM;
  int32_t     realSize;
  int32_t     paddedSize;
  int32_t     realPerBlockX;
  int32_t     sharedSizeX;
  int32_t     numBlocksX;
  std::string fpSuffix;

  /**
   * Default constructor.
   */
  GeneratorParams(int32_t tts      = 1,
                  int32_t ept      = 1,
                  int32_t bsx      = 16,
                  int32_t ps       = 1024,
                  int32_t ts       = 64,
                  std::string type = "float")
    : timeTileSize(tts),
      timeSteps(ts),
      elementsPerThread(ept),
      problemSize(ps),
      dataType(type),
      blockSizeX(bsx) {
  }

  void computeDerived() {
    // Compute derived values
    padding        = timeTileSize*2;
    compsPerBlockX = blockSizeX*elementsPerThread;
    realPerBlockX  = compsPerBlockX - 2*2*(timeTileSize-1);
    sizeLCM        = realPerBlockX;
    realSize       = (problemSize / sizeLCM) * sizeLCM;
    numBlocksX     = realSize / realPerBlockX;
    sharedSizeX    = blockSizeX * elementsPerThread + 2*2;
    paddedSize     = realSize + 2*padding;

    if(dataType == "float") {
      fpSuffix = "f";
    } else {
      fpSuffix = "";
    }

    if(padding < 1 || compsPerBlockX < 1                ||
       realPerBlockX < 1 || sizeLCM < 1 || realSize < 1 ||
       numBlocksX < 1 || sharedSizeX < 1                ||
       paddedSize < 1) {
      throw std::runtime_error("Consistency error!");
    }
  }
};

/**
 * Generator for Jacobi 2D.
 */
class Jacobi1DGenerator : public ProgramGenerator {
public:

  Jacobi1DGenerator();

  virtual ~Jacobi1DGenerator();

  std::string generate(GeneratorParams& params);

private:

  void generateHeader(std::ostream&          stream,
                      const GeneratorParams& params);

  void generateFooter(std::ostream& stream);

  void generateLocals(std::ostream&          stream,
                      const GeneratorParams& params);

  void generateCompute(std::ostream&          stream,
                       const GeneratorParams& params);
};

Jacobi1DGenerator::Jacobi1DGenerator() {
}

Jacobi1DGenerator::~Jacobi1DGenerator() {
}

std::string Jacobi1DGenerator::generate(GeneratorParams& params) {
  std::stringstream program;

  params.computeDerived();

  generateHeader(program, params);
  generateLocals(program, params);
  generateCompute(program, params);
  generateFooter(program);

  return program.str();
}

void Jacobi1DGenerator::generateHeader(std::ostream& stream,
                                       const GeneratorParams& params) {
  stream << "/* Auto-generated.  Do not edit by hand. */\n";
  stream << "__kernel\n";
  stream << "void kernel_func(__global " << params.dataType << "* input,\n";
  stream << "                 __global " << params.dataType << "* output) {\n";
}

void Jacobi1DGenerator::generateFooter(std::ostream& stream) {
  stream << "}\n\n";
}

void Jacobi1DGenerator::generateLocals(std::ostream& stream,
                                       const GeneratorParams& params) {
  stream << "  __local " << params.dataType << " buffer["
         << params.sharedSizeX << "];\n";

  // Compute some pointer values
  stream << "  __global " << params.dataType
         << "* inputPtr = input + (get_group_id(0)*"
         << params.realPerBlockX
         << ") + get_local_id(0) + 2;\n";

  stream << "  __global " << params.dataType
         << "* outputPtr = output + (get_group_id(0)*"
         << params.realPerBlockX
         << ") + get_local_id(0) + 2;\n";

  // Compute some guards
  stream << "  int globalIndexX;\n";
  for(int32_t i = 0; i < params.elementsPerThread; ++i) {
    stream << "  globalIndexX = (get_group_id(0)*"
           << params.realPerBlockX
           << ") + get_local_id(0) + "
           << (i*params.blockSizeX)
           << " + 2;\n";
    stream << "  bool valid" << i << " = globalIndexX >= " << params.padding
           << " && globalIndexX < " << (params.realSize+params.padding)
           << ";\n";
  }

  stream << "  int effectiveTid;\n";

  for(int32_t i = 0; i < params.elementsPerThread; ++i) {
    stream << "  effectiveTid = " << (i*params.blockSizeX)
           << " + get_local_id(0);\n";
    stream << "  bool writeValid" << i << " = effectiveTid >= "
           << params.timeTileSize-1 << " && effectiveTid < "
           << (params.realPerBlockX+params.timeTileSize-1) << ";\n";
  }

  // Declare local intermediates
  for(int32_t i = 0; i < params.elementsPerThread; ++i) {
    stream << "  " << params.dataType << " local" << i << ";\n";
    stream << "  " << params.dataType << " new" << i << ";\n";
  }
}

void Jacobi1DGenerator::generateCompute(std::ostream& stream,
                                        const GeneratorParams& params) {

  for(int32_t i = 0; i < params.elementsPerThread; ++i) {
    stream << "  {\n";
    stream << "    " << params.dataType
           << " val0, val1, val2, val3, val4;\n";
    stream << "    val0 = *(inputPtr+(" << params.blockSizeX << "*" << i
           << ")-2);\n";
    stream << "    val1 = *(inputPtr+(" << params.blockSizeX << "*" << i
           << ")-1);\n";
    stream << "    val2 = *(inputPtr+(" << params.blockSizeX << "*" << i
           << ")+0);\n";
    stream << "    val2 = *(inputPtr+(" << params.blockSizeX << "*" << i
           << ")+1);\n";
    stream << "    val2 = *(inputPtr+(" << params.blockSizeX << "*" << i
           << ")+2);\n";
    stream << "    " << params.dataType
           << " result = 0.2" << params.fpSuffix
           << " * (val0+val1+val2+val3+val4);\n";
    stream << "    result = (valid" << i << ") ? result : 0.0"
           << params.fpSuffix << ";\n";
    stream << "    buffer[get_local_id(0)+" << (i*params.blockSizeX)
           << "+2] = result;\n";
    stream << "    local" << i << " = result;\n";
    stream << "  }\n";
  }

  stream << "  barrier(CLK_LOCAL_MEM_FENCE);\n";

  for(int32_t t = 1; t < params.timeTileSize; ++t) {
    stream << "  // Time Step " << t << "\n";
    for(int32_t i = 0; i < params.elementsPerThread; ++i) {
      stream << "  {\n";
      stream << "    " << params.dataType
             << " val0, val1, val2, val3, val4;\n";
      stream << "    val0 = buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+0];\n";
      stream << "    val1 = buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+1];\n";
      stream << "    val2 = buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+2];\n";
      stream << "    val3 = buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+3];\n";
      stream << "    val4 = buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+4];\n";
      stream << "    " << params.dataType
             << " result = 0.2" << params.fpSuffix
             << " * (val0+val1+val2+val3+val4);\n";
      stream << "    result = (valid" << i << ") ? result : 0.0"
             << params.fpSuffix << ";\n";
      stream << "    new" << i << " = result;\n";
      stream << "  }\n";
    }
    stream << "  barrier(CLK_LOCAL_MEM_FENCE);\n";
    for(int32_t i = 0; i < params.elementsPerThread; ++i) {
      stream << "  buffer[get_local_id(0)+" << (i*params.blockSizeX)
             << "+2] = new" << i << ";\n";
      stream << "  local" << i << " = new" << i << ";\n";
    }
    stream << "  barrier(CLK_LOCAL_MEM_FENCE);\n";
  }
  for(int32_t i = 0; i < params.elementsPerThread; ++i) {
    stream << "  if(writeValid" << i << ") {\n";
    stream << "    *(outputPtr+(" << params.blockSizeX << "*" << i
           << ")) = local" << i << ";\n";
    stream << "  }\n";
  }
}

void compareResults(float* host, float* device, const GeneratorParams& params) {

  double errorNorm, refNorm, diff;
  errorNorm = 0.0;
  refNorm   = 0.0;

  for(int i = params.padding; i < params.paddedSize-params.padding; ++i) {
    float h = host[i];
    float d = device[i];

    diff       = h - d;
    //      std::cout << "h: " << h << "  d: " << d << "  diff: " << diff << "\n";
    errorNorm += diff*diff;
    refNorm   += h*h;
  }

  errorNorm = std::sqrt(errorNorm);
  refNorm = std::sqrt(refNorm);

  printValue("Error Norm", errorNorm);
  printValue("Ref Norm", refNorm);

  if(std::abs(refNorm) < 1e-7) {
    printValue("Correctness", "FAILED");
  }
  else if((errorNorm / refNorm) > 1e-2) {
    printValue("Correctness", "FAILED");
  }
  else {
    printValue("Correctness", "PASSED");
  }
}

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

  cl_int      result;
  std::string kernelFile;
  std::string saveKernelFile;

  srand(123456);

  Jacobi1DGenerator gen;
  GeneratorParams   params;

  po::options_description desc("Options");
  desc.add_options()
    ("help,h", "Show usage information")
    ("problem-size,n",
     po::value<int32_t>(&params.problemSize)->default_value(1024),
     "Set problem size")
    ("time-steps,t",
     po::value<int32_t>(&params.timeSteps)->default_value(64),
     "Set number of time steps")
    ("block-size-x,x",
     po::value<int32_t>(&params.blockSizeX)->default_value(16),
     "Set block size (X)")
    ("elements-per-thread,e",
     po::value<int32_t>(&params.elementsPerThread)->default_value(1),
     "Set elements per thread")
    ("time-tile-size,s",
     po::value<int32_t>(&params.timeTileSize)->default_value(1),
     "Set time tile size")
    ("load-kernel,f",
     po::value<std::string>(&kernelFile)->default_value(""),
     "Load kernel from disk")
    ("save-kernel,w",
     po::value<std::string>(&saveKernelFile)->default_value(""),
     "Save kernel to disk")
    ("verify,v", "Verify results")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if(vm.count("help")) {
    std::cerr << desc;
    return 1;
  }

  std::string kernelSource;

  if(kernelFile.size() == 0) {
    kernelSource = gen.generate(params);
  } else {
    std::ifstream kernelStream(kernelFile.c_str());
    kernelSource = std::string(std::istreambuf_iterator<char>(kernelStream),
                               (std::istreambuf_iterator<char>()));
    kernelStream.close();
    params.computeDerived();
  }

  if(saveKernelFile.size() != 0) {
    std::ofstream kernelStream(saveKernelFile.c_str());
    kernelStream << kernelSource;
    kernelStream.close();
  }

  printValue("Problem Size", params.problemSize);
  printValue("Time Tile Size", params.timeTileSize);
  printValue("Padded Size", params.paddedSize);
  printValue("Block Size X", params.blockSizeX);
  printValue("Elements/Thread", params.elementsPerThread);
  printValue("Num Blocks X", params.numBlocksX);
  printValue("Time Steps", params.timeSteps);
  printValue("Padding", params.padding);
  printValue("Real Size", params.realSize);
  printValue("Real/BlockX", params.realPerBlockX);

  int arraySize = params.paddedSize * sizeof(float);

  CLContext context;

  // Collect device information.
  size_t globalMemorySize = context.device()
    .getInfo<CL_DEVICE_GLOBAL_MEM_SIZE>();
  size_t localMemorySize  = context.device()
    .getInfo<CL_DEVICE_LOCAL_MEM_SIZE>();
  size_t maxComputeUnits  = context.device()
    .getInfo<CL_DEVICE_MAX_COMPUTE_UNITS>();
  size_t maxWorkGroupSize = context.device()
    .getInfo<CL_DEVICE_MAX_WORK_GROUP_SIZE>();

  // Print device information.
  printValue("Global Memory Size", globalMemorySize);
  printValue("Local Memory Size", localMemorySize);
  printValue("Max Compute Units", maxComputeUnits);
  printValue("Max Work-Group Size", maxWorkGroupSize);

  if(params.blockSizeX > maxWorkGroupSize) {
    std::cout << "ERROR: Block dimensions are too large!\n";
    return 1;
  }

  if(params.sharedSizeX > localMemorySize) {
    std::cout << "ERROR: Not enough local memory for even one block!\n";
    return 1;
  }

  // Print some derived statistics
  int32_t sharedSize = params.sharedSizeX * 1 * 4;

  int32_t numBlocksFromShared = (int32_t)std::ceil((double)localMemorySize /
                                                   (double)sharedSize);

  int64_t totalFPPerBlock = params.blockSizeX *
    params.elementsPerThread * params.timeSteps * 5;

  int64_t usefulFPPerBlock = 5 * params.realPerBlockX *
    params.timeSteps;

  double usefulFPRatio = (double)usefulFPPerBlock /
    (double)totalFPPerBlock;

  int32_t globalLoadsPerBlock = params.blockSizeX *
    params.elementsPerThread * 5;

  int32_t globalStoresPerBlock = params.blockSizeX *
    params.elementsPerThread * 1;

  int32_t sharedLoadsPerBlock = params.blockSizeX *
    params.elementsPerThread * 5 * (params.timeTileSize-1);

  int32_t sharedStoresPerBlock = params.blockSizeX *
    params.elementsPerThread * 1 * (params.timeTileSize-1);

  int32_t arithmeticIntensity = 5.0 / 5.0;

  int32_t maxBlocks = 8;        // TODO: Change based on arch.

  printValue("Shared Size", sharedSize);
  printValue("Num Blocks (Shared)", numBlocksFromShared);
  printValue("Total FP", totalFPPerBlock);
  printValue("Useful FP", usefulFPPerBlock);
  printValue("Useful Ratio", usefulFPRatio);
  printValue("Global Loads/Block", globalLoadsPerBlock);
  printValue("Global Stores/Block", globalStoresPerBlock);
  printValue("Shared Loads/Block", sharedLoadsPerBlock);
  printValue("Shared Stores/Block", sharedStoresPerBlock);
  printValue("Arithmetic Intensity", arithmeticIntensity);
  printValue("Max Blocks", maxBlocks);

  // Create a command queue.
  cl::CommandQueue queue(context.context(), context.device(), 0, &result);
  CLContext::throwOnError("cl::CommandQueue", result);

  // Build a program from the source
  cl::Program::Sources progSource(1, std::make_pair(kernelSource.c_str(),
                                                    kernelSource.size()));
  cl::Program          program(context.context(), progSource, &result);
  CLContext::throwOnError("cl::Program failed", result);

  std::vector<cl::Device> devices;
  devices.push_back(context.device());

  result = program.build(devices);
  if(result != CL_SUCCESS) {
    std::cout << "Source compilation failed.\n";
    std::cout << program.getBuildInfo<CL_PROGRAM_BUILD_LOG>(context.device());
    return 1;
  }

  // Extract the kernel
  cl::Kernel kernel(program, "kernel_func", &result);
  CLContext::throwOnError("Failed to extract kernel", result);

  // Allocate host arrays
  float* hostData = new float[arraySize];

  // Fill host arrays
  for(int i = 0; i < params.paddedSize; ++i) {
    for(int j = 0; j < params.paddedSize; ++j) {
      if(i < params.padding || i >= (params.paddedSize-params.padding)) {
        hostData[i] = 0.0f;
      }
      else {
        hostData[i] = (float)rand() / ((float)RAND_MAX + 1.0f);
      }
    }
  }

  // Compute reference

  float* reference = NULL;

  if(vm.count("verify")) {

    reference = new float[arraySize];

    float* refA;
    float* refB;

    refA = new float[arraySize];
    refB = new float[arraySize];

    memcpy(refA, hostData, arraySize);
    memcpy(refB, hostData, arraySize);

    for(int t = 0; t < params.timeSteps; ++t) {
      for(int i = params.padding; i < params.paddedSize-params.padding; ++i) {
          refB[i] = 0.2f * (refA[i-2] +
                            refA[i-1] +
                            refA[i] +
                            refA[i+1] +
                            refA[i+2]);
      }

      memcpy(refA, refB, arraySize);
    }

    memcpy(reference, refA, arraySize);

    delete [] refA;
    delete [] refB;

  }

  // Allocate device arrays
  cl::Buffer deviceInput(context.context(), CL_MEM_READ_WRITE,
                         arraySize, NULL, &result);
  CLContext::throwOnError("Failed to allocate device input", result);

  cl::Buffer deviceOutput(context.context(), CL_MEM_READ_WRITE,
                          arraySize, NULL, &result);
  CLContext::throwOnError("Failed to allocate device output", result);

  // Copy host data to device
  result = queue.enqueueWriteBuffer(deviceInput, CL_TRUE, 0,
                                    arraySize, hostData,
                                    NULL, NULL);
  CLContext::throwOnError("Failed to copy input data to device", result);
  result = queue.enqueueWriteBuffer(deviceOutput, CL_TRUE, 0,
                                    arraySize, hostData,
                                    NULL, NULL);
  CLContext::throwOnError("Failed to copy input data to device", result);

  cl::NDRange globalSize(params.blockSizeX*params.numBlocksX);
  cl::NDRange localSize(params.blockSizeX);

  cl::Buffer* inputBuffer;
  cl::Buffer* outputBuffer;

  inputBuffer  = &deviceInput;
  outputBuffer = &deviceOutput;

  cl::Event waitEvent;

  double startTime = rtclock();

  for(int t = 0; t < params.timeSteps / params.timeTileSize; ++t) {

    // Set kernel arguments
    result = kernel.setArg(0, *inputBuffer);
    CLContext::throwOnError("Failed to set input parameter", result);
    result = kernel.setArg(1, *outputBuffer);
    CLContext::throwOnError("Failed to set output parameter", result);

    // Invoke the kernel
    result = queue.enqueueNDRangeKernel(kernel, cl::NullRange,
                                        globalSize, localSize,
                                        0, &waitEvent);
    CLContext::throwOnError("Kernel launch failed", result);

    std::swap(inputBuffer, outputBuffer);
  }

  waitEvent.wait();

  double endTime = rtclock();
  double elapsed = endTime - startTime;

  // Copy results back to host
  result = queue.enqueueReadBuffer(*inputBuffer, CL_TRUE, 0,
                                   arraySize, hostData,
                                   NULL, NULL);
  CLContext::throwOnError("Failed to copy result to host", result);

  printValue("Elapsed Time", elapsed);

  double gflops   = (double)params.realSize
    * 5.0 * (double)params.timeSteps / elapsed / 1e9;
  //double gflops = stencilGen.computeGFlops(elapsed);
  printValue("Actual GFlop/s", gflops);

  gflops = (double)params.blockSizeX
    * (double)params.numBlocksX
    * (double)params.elementsPerThread * 5.0 * (double)params.timeSteps
    / elapsed / 1e9;

  printValue("Device GFlop/s", gflops);

  if(vm.count("verify")) {
    compareResults(reference, hostData, params);
  }

  // Clean-up
  delete [] hostData;

  if(vm.count("verify")) {
    delete [] reference;
  }

  return 0;
}


#ifndef ELASTY_PARTICLE_HPP
#define ELASTY_PARTICLE_HPP

#include <Eigen/Core>

namespace elasty
{
    struct Particle
    {
        Particle(const Eigen::Vector3d& x, const Eigen::Vector3d& v, const double m)
            : x(x), v(v), p(x), f(Eigen::Vector3d::Zero()), m(m), w(1.0 / m)
        {
        }

        Eigen::Vector3d x;
        Eigen::Vector3d v;
        Eigen::Vector3d p;
        Eigen::Vector3d f;
        double          m;
        double          w;
    };
} // namespace elasty

#endif // ELASTY_PARTICLE_HPP


#include <test.hpp>
#include <kernel.hpp>
#include <Eigen/Core>

namespace Test
{
    double testfunction()
    {
        std::vector<Eigen::Vector3d> v1(10, Eigen::Vector3d{ 1.0, 1.0, 1.0 });
        std::vector<Eigen::Vector3d> v2(10, Eigen::Vector3d{ -1.0, 1.0, 1.0 });

        double x = Kernel::dot(v1,v2);

        return x;
    }
}