#include "RadialSolver.hpp"
#include "Constants.hpp"

#include <cmath>
#include <algorithm>

#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

#include <iostream>

#include "dynarray.hpp"

#include <time.h>

using namespace std;

//----------------------------------------------------------------------------------------------------
// RadialSolver()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::RadialSolver(
        double minr,
        double maxr,
        double maxz,
        double mass,
        double T0,
        const double (&Li)[numr],
        const std::string& M_file,
        const std::string& N_file)
    :
        mMinr(minr),
        mMaxr(maxr),
        mMaxz(maxz),
        mMass(mass),
        mT0(T0),
        mVth(sqrt(2*BOLTZMANN_CONST*mT0/mMass)),
        mBD_matrix_compiled(false)
    {
        for(unsigned int nr = 0; nr < numr_bd; ++nr)
        {

            mSolN_BD_top[nr] = 0.5;
            mSolN_BD_top_tmp[nr] = 0.5;
        }

        for (unsigned int nz = 0; nz < numz_bd; ++nz)
        {
            mSolN_BD_outer[nz]=0.5;
            mSolN_BD_outer_tmp[nz]=0.5;
            mSolN_BD_inner[nz]=0.5;
            mSolN_BD_inner_tmp[nz]=0.5;
        }

        for(unsigned int nr = 0; nr < numr; ++nr)
        {

            mLi[nr] = Li[nr];

            for (unsigned int nz = 0; nz < numz; ++nz)
            {

                    mSolutionN[nr][nz] = 1;
                    mSolutionVr[nr][nz] = 0;
                    mSolutionVz[nr][nz] = 0;

            }
        }

        for (unsigned int i=0; i < num_bd; ++i)
        {
            for (unsigned int j=0; j < num_bd; ++j)
            {
                mBD_matrix[i][j] = 0;
            }
        }

        mM.load_text_file(M_file);
        mN.load_text_file(N_file);

        mDtheta = PI_CONST/(total_numv-1);
        mDphi = PI_CONST/(total_numv-1);

        mDr_bd = (mMaxr - mMinr)/(numr_bd-1);
        mDz_bd = mMaxz/(numz_bd-1);

        mDr = (mMaxr - mMinr)/(numr-1);

        mDz = mMaxz/(numz-1);

        for (unsigned int nz = 0; nz < numz; ++nz)
        {
            mZ[nz] = nz*mDz;
            //cerr << mZ[nz] << endl;
        }

    }

//----------------------------------------------------------------------------------------------------
// solutionN()
//----------------------------------------------------------------------------------------------------
template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solutionN(unsigned int nr, unsigned int nz)
    {
        return mSolutionN[nr][nz];
    }

//----------------------------------------------------------------------------------------------------
// solutionVr()
//----------------------------------------------------------------------------------------------------
template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solutionVr(unsigned int nr, unsigned int nz)
    {
        return mSolutionVr[nr][nz];
    }

//----------------------------------------------------------------------------------------------------
// solutionVz()
//----------------------------------------------------------------------------------------------------
template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solutionVz(unsigned int nr, unsigned int nz)
    {
        return mSolutionVz[nr][nz];
    }

//----------------------------------------------------------------------------------------------------
// boundaryN()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::boundaryN(
        unsigned int n,
        unsigned int bd)
    {
        // outer
        if (bd == 0)
        {
            return mSolN_BD_outer[n];
        }

        // inner
        if (bd == 1)
        {
            return mSolN_BD_inner[n];
        }

        // top
        if (bd == 2)
        {
            return mSolN_BD_top[n];
        }

        return -1;
    }

//----------------------------------------------------------------------------------------------------
// ionLevel()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::avg_ion_rate()
    {

        double sum=0;
        double weights=0;
        double r_weight = 0;

        for (unsigned int nr = 1; nr < numr-1; ++nr)
        {
            r_weight = nr*mDr+mMinr;

            sum += mLi[nr]*mSolutionN[nr][0]*r_weight;
            weights += mSolutionN[nr][0]*r_weight;

            for(unsigned int nz=1; nz < numz-1; ++nz)
            {
                sum += mLi[nr]*mSolutionN[nr][nz]*2*r_weight;
                weights += mSolutionN[nr][nz]*2*r_weight;
            }
        }

        double avg = sum/weights;

        return avg;

    }

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::avg_n()
    {

        double sum=0;
        double weights=0;
        double r_weight = 0;

        for (unsigned int nr = 1; nr < numr-1; ++nr)
        {
            r_weight = nr*mDr+mMinr;

            sum += mSolutionN[nr][0]*r_weight;
            weights += r_weight;

            for(unsigned int nz=1; nz < numz-1; ++nz)
            {
                sum += mSolutionN[nr][nz]*2*r_weight;
                weights += 2*r_weight;
            }
        }

        double avg = sum/weights;

        return avg;

    }

//----------------------------------------------------------------------------------------------------
// solve_bd()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solve_bd(
        double precision,
        unsigned int max_iterations,
        bool multithread)
    {

        if (!mBD_matrix_compiled)
        {
            cerr << "First time solving boundary: compiling boundary matrix." << endl;

            // must first compile boundary matrix

            // set boundary values to unity to compile matrix so that values can be scaled to solution
            for(unsigned int nr = 0; nr < numr_bd; ++nr)
            {
                mSolN_BD_top[nr] = 1;
            }

            for (unsigned int nz = 0; nz < numz_bd; ++nz)
            {
                mSolN_BD_outer[nz]=1;
                mSolN_BD_inner[nz]=1;
            }

            if (multithread)
            {
                // solve in parallel

                boost::thread_group calcThreads;

                for (unsigned int nz = 0; nz < numz_bd; ++nz)
                {
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::compile_bd, this, nz, 0));
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::compile_bd, this, nz, 1));
                }

                for (unsigned int nr = 0; nr < numr_bd; ++nr)
                {
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::compile_bd, this, nr, 2));
                }

                calcThreads.join_all();

            }else{

                // solve serially
                for (unsigned int nz = 0; nz < numz_bd; ++nz)
                {
                    compile_bd(nz, 0);
                    compile_bd(nz, 1);
                }

                for (unsigned int nr = 0; nr < numr_bd; ++nr)
                {
                    compile_bd(nr, 2);
                }
            }

            mBD_matrix_compiled = true;
            cerr << "Boundary matrix compiled." << endl;
        }

        unsigned int iterations = 0;

        double maxdev;

        // keep track of how long the solution takes to compute
        time_t t_begin = time(0);

        do{
            //cerr << "Solving boundary: ";

            for(unsigned int nr = 0; nr < numr_bd; ++nr)
            {
                mSolN_BD_top_tmp[nr] = mSolN_BD_top[nr];
            }

            for (unsigned int nz = 0; nz < numz_bd; ++nz)
            {
                mSolN_BD_outer_tmp[nz]=mSolN_BD_outer[nz];
                mSolN_BD_inner_tmp[nz]=mSolN_BD_inner[nz];
            }

            // solve boundary

            if (multithread)
            {
                // solve in parallel

                boost::thread_group calcThreads;

                for (unsigned int nz = 0; nz < numz_bd; ++nz)
                {
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::solve_bd, this, nz, 0));
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::solve_bd, this, nz, 1));
                }

                for (unsigned int nr = 0; nr < numr_bd; ++nr)
                {
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::solve_bd, this, nr, 2));
                }

                calcThreads.join_all();

            }else{

                // solve serially
                for (unsigned int nz = 0; nz < numz_bd; ++nz)
                {
                    solve_bd(nz, 0);
                    solve_bd(nz, 1);
                }

                for (unsigned int nr = 0; nr < numr_bd; ++nr)
                {
                    solve_bd(nr, 2);
                }
            }

            maxdev=0;

            for(unsigned int nr = 0; nr < numr_bd; ++nr)
            {
                double dev = 2*abs(mSolN_BD_top_tmp[nr] - mSolN_BD_top[nr])/(mSolN_BD_top_tmp[nr] + mSolN_BD_top[nr]);

                maxdev = max(maxdev, dev);
            }

            for (unsigned int nz = 0; nz < numz_bd; ++nz)
            {
                double dev = 2*abs(mSolN_BD_outer_tmp[nz] - mSolN_BD_outer[nz])/(mSolN_BD_outer_tmp[nz] + mSolN_BD_outer[nz]);

                maxdev = max(maxdev, dev);

                dev = 2*abs(mSolN_BD_inner_tmp[nz] - mSolN_BD_inner[nz])/(mSolN_BD_inner_tmp[nz] + mSolN_BD_inner[nz]);

                maxdev = max(maxdev, dev);

            }

            iterations++;

            //time_t t_cur = time(0);

            //cerr << maxdev << " deviation, iteration " << iterations << ", time = " << t_cur - t_begin << "s." << endl;

        }while(maxdev > precision && iterations < max_iterations);

        time_t t_end = time(0);

        if (iterations == max_iterations)
        {
            cerr << "Max iterations reached in attempt to solve boundary." << endl;
        }else{
            cerr << "Solution for boundary found to " << maxdev << " deviation in " << iterations << " steps and " << t_end - t_begin << "s." << endl;
        }

    }

//----------------------------------------------------------------------------------------------------
// solve_bd()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solve_bd(
        unsigned int n,
        unsigned int bd)
    {

        unsigned int this_bd_index = bd_index(n, bd);

        double out_flux = 0;

        for(unsigned int nr = 0; nr < numr_bd; ++nr)
        {
            out_flux += mSolN_BD_top_tmp[nr]*mBD_matrix[this_bd_index][bd_index(nr, 2)];
        }

        for (unsigned int nz = 0; nz < numz_bd; ++nz)
        {
            out_flux += mSolN_BD_outer_tmp[nz]*mBD_matrix[this_bd_index][bd_index(nz, 0)];
            out_flux += mSolN_BD_inner_tmp[nz]*mBD_matrix[this_bd_index][bd_index(nz, 1)];
        }

        if (bd == 2)
        {
            double r = n*mDr_bd+mMinr;

            double source = pow(neTe_profile_function(r, 1.0, 0.4), 0.5)*neTe_profile_function(r, 1.1, 0.5)*r*2/(mMinr + mMaxr);

            if (n == 0)
            {
                mSolN_BD_top[0] = (out_flux*6*sqrt(PI_CONST)/mVth + 2*mSolN_BD_inner_tmp[numz_bd-1])/5 + source;

            }else if (n == numr_bd-1){

                mSolN_BD_top[numr_bd-1] = (out_flux*6*sqrt(PI_CONST)/mVth + 2*mSolN_BD_inner_tmp[numz_bd-1])/5 + source;
            }else{

                mSolN_BD_top[n] = out_flux*sqrt(PI_CONST)/mVth +  source;

            }
        }else if (bd == 1){

            if (n == numz_bd-1)
            {
                mSolN_BD_inner[numz_bd-1] = (out_flux*6*sqrt(PI_CONST)/mVth + 2*mSolN_BD_top_tmp[0])/5;
            }else{
                mSolN_BD_inner[n] = out_flux*sqrt(PI_CONST)/mVth;
            }

        }else{

            if (n == numz_bd-1)
            {
                mSolN_BD_outer[numz_bd-1] = (out_flux*6*sqrt(PI_CONST)/mVth + 2*mSolN_BD_top_tmp[numr_bd-1])/5;
            }else{
                mSolN_BD_outer[n] = out_flux*sqrt(PI_CONST)/mVth;
            }

        }

    }

//----------------------------------------------------------------------------------------------------
// compile_bd()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::compile_bd(
        unsigned int n,
        unsigned int bd)
    {

        double r, z;
        unsigned int min_theta, max_theta, min_phi, max_phi;
        double total_theta;
        bool need_vr_vz; // vr == true, vz == false
        double dot_product_sign;

        // outer
        if (bd == 0)
        {
            r = mMaxr;
            z = n*mDz_bd;

            need_vr_vz = true;

            min_theta = 0;
            max_theta = numv;
            total_theta = PI_CONST;

            dot_product_sign = 1;

            if (n == numz_bd-1)
            {
                // corner only 1/4 of distr.
                min_phi = 0;
                max_phi = numv;

            }else{
                min_phi = 0;
                max_phi = total_numv;
            }

        }

        // inner
        if (bd == 1)
        {

            r = mMinr;
            z = n*mDz_bd;

            need_vr_vz = true;

            min_theta = numv-1;
            max_theta = total_numv;

            total_theta = PI_CONST;

            dot_product_sign = -1;

            if (n == numz_bd-1)
            {
                // corner only 1/4 of distr.
                min_phi = 0;
                max_phi = numv;

            }else{
                min_phi = 0;
                max_phi = total_numv;
            }

        }

        // top
        if (bd == 2)
        {
            r = n*mDr_bd+mMinr;
            z = mMaxz;

            need_vr_vz = false;

            min_phi = 0;
            max_phi = numv;

            dot_product_sign = 1;

            if (n == 0)
            {
                // inner corner
                min_theta = numv-1;
                max_theta = total_numv;

                total_theta = PI_CONST;

            }else if (n == numr_bd-1){
                // outer corner
                min_theta = 0;
                max_theta = numv;

                total_theta = PI_CONST;

            }else{

                min_theta = 0;
                max_theta = total_numv;

                total_theta = TWOPI_CONST;

            }

        }

        double I1 = 0;
        double I2 = 0;
        double I = 0;
        bd_intersect intersect;

        double int_fact;
        double int_edge_fact;

        double V_factor = 2.0*mVth/pow(PI_CONST, 3.0/2.0);

        double theta, phi;

        unsigned int this_bd_index = bd_index(n, bd);

        for(unsigned int phi_i = min_phi; phi_i < max_phi; ++phi_i)
        {
            if (phi_i == 0)
            {
                int_fact = total_theta*(1-sin(mDphi)/mDphi);

                integrate(r, z, 0, 0, I1, I2, intersect);

                if (need_vr_vz)
                {
                    I = 0;
                }else{
                    I = dot_product_sign*int_fact*V_factor*I2;
                }

                mBD_matrix[this_bd_index][bd_index(intersect.n_1, intersect.bd_1)] += intersect.fraction_1*I;
                mBD_matrix[this_bd_index][bd_index(intersect.n_2, intersect.bd_2)] += intersect.fraction_2*I;

                if (intersect.bd_3 != -1)
                {
                    mBD_matrix[this_bd_index][bd_index(intersect.n_3, intersect.bd_3)] += intersect.fraction_3*I;
                }

            }else if (phi_i == total_numv-1){

                int_fact = total_theta*(1-sin(mDphi)/mDphi);

                integrate(r, z, 0, PI_CONST, I1, I2, intersect);

                if (need_vr_vz)
                {
                    I = 0;
                }else{
                    I = -dot_product_sign*int_fact*V_factor*I2;
                }

                mBD_matrix[this_bd_index][bd_index(intersect.n_1, intersect.bd_1)] += intersect.fraction_1*I;
                mBD_matrix[this_bd_index][bd_index(intersect.n_2, intersect.bd_2)] += intersect.fraction_2*I;

                if (intersect.bd_3 != -1)
                {
                    mBD_matrix[this_bd_index][bd_index(intersect.n_3, intersect.bd_3)] += intersect.fraction_3*I;
                }

            }else{

                phi = phi_i*mDphi;

                if (phi_i == min_phi)
                {
                    // if it got here then min_phi != 0, so integration factor is different

                    int_fact = mDtheta*(cos(phi) - (sin(phi+mDphi)-sin(phi))/mDphi);

                }else if (phi_i == max_phi){
                    // same deal except for other limit

                    int_fact = mDtheta*((sin(phi)-sin(phi-mDphi))/mDphi - cos(phi));

                }else{

                    // not on edge of phi integration
                    int_fact = 2*mDtheta*sin(phi)*(1-cos(mDphi))/mDphi;
                }

                for (unsigned int theta_i = min_theta; theta_i < max_theta; ++theta_i)
                {
                    if (theta_i == 0)
                    {
                        integrate(r, z, 0, phi, I1, I2, intersect);

                        if (need_vr_vz)
                        {
                            I = dot_product_sign*sin(phi)*int_fact*V_factor*I2;
                        }else{
                            I = dot_product_sign*cos(phi)*int_fact*V_factor*I2;
                        }

                        mBD_matrix[this_bd_index][bd_index(intersect.n_1, intersect.bd_1)] += intersect.fraction_1*I;
                        mBD_matrix[this_bd_index][bd_index(intersect.n_2, intersect.bd_2)] += intersect.fraction_2*I;

                        if (intersect.bd_3 != -1)
                        {
                            mBD_matrix[this_bd_index][bd_index(intersect.n_3, intersect.bd_3)] += intersect.fraction_3*I;
                        }

                    }else if (theta_i == total_numv-1){

                        integrate(r, z, PI_CONST, phi, I1, I2, intersect);

                        if (need_vr_vz)
                        {
                            I = -dot_product_sign*sin(phi)*int_fact*V_factor*I2;
                        }else{
                            I = dot_product_sign*cos(phi)*int_fact*V_factor*I2;
                        }

                        mBD_matrix[this_bd_index][bd_index(intersect.n_1, intersect.bd_1)] += intersect.fraction_1*I;
                        mBD_matrix[this_bd_index][bd_index(intersect.n_2, intersect.bd_2)] += intersect.fraction_2*I;

                        if (intersect.bd_3 != -1)
                        {
                            mBD_matrix[this_bd_index][bd_index(intersect.n_3, intersect.bd_3)] += intersect.fraction_3*I;
                        }

                    }else{

                        if (theta_i == min_theta || min_theta == max_theta)
                        {
                            // if limits are not 0 and pi, then need to alter integration factor
                            int_edge_fact = 1/2;
                        }else{
                            int_edge_fact = 1;
                        }

                        theta = theta_i*mDtheta;

                        integrate(r, z, theta, phi, I1, I2, intersect);

                        if (need_vr_vz)
                        {
                            I = 2*dot_product_sign*int_edge_fact*sin(phi)*cos(theta)*int_fact*V_factor*I2;
                        }else{
                            I = 2*dot_product_sign*int_edge_fact*cos(phi)*int_fact*V_factor*I2;
                        }

                        mBD_matrix[this_bd_index][bd_index(intersect.n_1, intersect.bd_1)] += intersect.fraction_1*I;
                        mBD_matrix[this_bd_index][bd_index(intersect.n_2, intersect.bd_2)] += intersect.fraction_2*I;

                        if (intersect.bd_3 != -1)
                        {
                            mBD_matrix[this_bd_index][bd_index(intersect.n_3, intersect.bd_3)] += intersect.fraction_3*I;
                        }
                    }

                }
            }

        }

    }

//----------------------------------------------------------------------------------------------------
// solve()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solve(bool multithread)
    {
        if (multithread)
        {
            // solve in parallel

            boost::thread_group calcThreads;

            for (unsigned int nr = 1; nr < numr-1; ++nr)
            {
                for (unsigned int nz = 0; nz < numz-1; ++nz)
                {
                    calcThreads.create_thread(boost::bind(&RadialSolver<numr,numz,numr_bd,numz_bd,numv>::solve, this, nr, nz));
                }

            }

            calcThreads.join_all();

        }else{
            // solve serially

            for (unsigned int nr = 1; nr < numr-1; ++nr)
            {
                for (unsigned int nz = 0; nz < numz-1; ++nz)
                {

                    solve(nr, nz);
                }

            }
        }
    }

//----------------------------------------------------------------------------------------------------
// solve()
//----------------------------------------------------------------------------------------------------

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::solve(
        unsigned int nr,
        unsigned int nz)
    {

        double r = nr*mDr+mMinr;
        double z = mZ[nz];

        // reset to zero

        mSolutionN[nr][nz] = 0;
        mSolutionVr[nr][nz] = 0;
        mSolutionVz[nr][nz] = 0;

        double I1 = 0;
        double I2 = 0;
        bd_intersect intersect;

        double int_fact;
        double N_factor = 2.0/pow(PI_CONST, 3.0/2.0);
        double V_factor = 2.0*mVth/pow(PI_CONST, 3.0/2.0);
        double theta, phi;

        for(unsigned int phi_i = 0; phi_i < total_numv; ++phi_i)
        {
            if (phi_i == 0)
            {
                int_fact = TWOPI_CONST*(1-sin(mDphi)/mDphi);

                integrate(r, z, 0, 0, I1, I2, intersect);

                mSolutionN[nr][nz] += int_fact*N_factor*I1;
                mSolutionVz[nr][nz] += int_fact*V_factor*I2;

            }else if (phi_i == total_numv-1){

                int_fact = TWOPI_CONST*(1-sin(mDphi)/mDphi);

                integrate(r, z, 0, PI_CONST, I1, I2, intersect);

                mSolutionN[nr][nz] += int_fact*N_factor*I1;
                mSolutionVz[nr][nz] += -int_fact*V_factor*I2;

            }else{

                phi = phi_i*mDphi;

                int_fact = 2*mDtheta*sin(phi)*(1-cos(mDphi))/mDphi;

                for (unsigned int theta_i = 0; theta_i < total_numv; ++theta_i)
                {
                    if (theta_i == 0)
                    {
                        integrate(r, z, 0, phi, I1, I2, intersect);

                        mSolutionN[nr][nz] += int_fact*N_factor*I1;
                        mSolutionVr[nr][nz] += sin(phi)*int_fact*V_factor*I2;
                        mSolutionVz[nr][nz] += cos(phi)*int_fact*V_factor*I2;

                    }else if (theta_i == total_numv-1){

                        integrate(r, z, PI_CONST, phi, I1, I2, intersect);

                        mSolutionN[nr][nz] += int_fact*N_factor*I1;
                        mSolutionVr[nr][nz] += -sin(phi)*int_fact*V_factor*I2;
                        mSolutionVz[nr][nz] += cos(phi)*int_fact*V_factor*I2;

                    }else{

                        theta = theta_i*mDtheta;

                        integrate(r, z, theta, phi, I1, I2, intersect);

                        mSolutionN[nr][nz] += 2*int_fact*N_factor*I1;
                        mSolutionVr[nr][nz] += 2*sin(phi)*cos(theta)*int_fact*V_factor*I2;
                        mSolutionVz[nr][nz] += 2*cos(phi)*int_fact*V_factor*I2;
                    }

                }
            }

        }

    }

//----------------------------------------------------------------------------------------------------
// integrate()
//----------------------------------------------------------------------------------------------------
template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
void RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::integrate(
        double r,
        double z,
        double theta,
        double phi,
        double& nM,
        double& nN,
        bd_intersect& intersect)
    {
        // setup variables defining integral path

        double sintheta = sin(theta);
        double costheta = cos(theta);

        double sinphi = sin(phi);
        double cosphi = cos(phi);

        double theta_transition = asin(mMinr/r);

        double L_perp = -1;

        if (theta <= theta_transition)
        {
            //inner wall
            L_perp = r*costheta - sqrt(mMinr*mMinr-r*r*sintheta*sintheta);

        }else{
            // outer wall
            L_perp = r*costheta + sqrt(mMaxr*mMaxr-r*r*sintheta*sintheta);
        }

        double h1 = mMaxz+z;
        double h2 = mMaxz-z;

        double phi1 = atan(L_perp/h1);
        double phi2 = atan(h2/L_perp) + HALFPI_CONST;

        double L = -1;

        if (z == mMaxz && phi == HALFPI_CONST)
        {
            L = L_perp;

        }else if ((r == mMinr || r == mMaxr) && phi == 0){

            L = h1;

        }else if ((r == mMinr || r == mMaxr) && phi == PI_CONST){

            L = h2;

        }else{

            if (phi < phi1)
            {
                // floor
                L = h1/cosphi;

            }else if (phi < phi2){
                // wall
                L = L_perp/sinphi;

            }else{
                // ceiling
                L = -h2/cosphi;
            }
        }

        // get value at wall using linear interpolation
        double n0_wall = 0;

        double r_intersect = sqrt(r*r + L*L*sinphi*sinphi - 2*r*L*sinphi*costheta);
        double delta_r = r - r_intersect;
        double delta_r2 = delta_r*delta_r;
        double delta_z = -L*cosphi;
        double delta_z2 = delta_z*delta_z;
        double z_intersect = abs(z + delta_z); // is symmetric about +/- z, so just use positive z
        //int sign_z = z - L*cosphi > 0 ? 1 : -1;

        double frac1, frac2, fracA, fracB;
        unsigned int nri1, nri2, nzi1, nzi2;

        if (phi < phi1 || phi > phi2)
        {
            // floor or ceiling

            nri1 = floor((r_intersect - mMinr)/mDr_bd);
            nri2 = nri1+1;
            frac2 = (r_intersect - (mDr_bd*nri1 + mMinr))/mDr_bd;
            frac1 = 1-frac2;

            if (nri1 == 0)
            {
                //includes corner
                fracA = frac1*delta_z2/(delta_r2 + delta_z2);
                fracB = frac1*delta_r2/(delta_r2 + delta_z2);

                n0_wall = fracA*mSolN_BD_top[nri1] + fracB*mSolN_BD_inner[numz_bd-1] + frac2*mSolN_BD_top[nri2];

                intersect.bd_1 = 2;
                intersect.bd_2 = 2;
                intersect.bd_3 = 1;
                intersect.n_1 = nri1;
                intersect.n_2 = nri2;
                intersect.n_3 = numz_bd-1;
                intersect.fraction_1 = fracA;
                intersect.fraction_2 = frac2;
                intersect.fraction_3 = fracB;

            }else if (nri2 == numr_bd-1){

                //includes corner
                fracA = frac2*delta_z2/(delta_r2 + delta_z2);
                fracB = frac2*delta_r2/(delta_r2 + delta_z2);

                n0_wall = frac1*mSolN_BD_top[nri1] + fracA*mSolN_BD_top[nri2] + fracB*mSolN_BD_outer[numz_bd-1];

                intersect.bd_1 = 2;
                intersect.bd_2 = 2;
                intersect.bd_3 = 0;
                intersect.n_1 = nri1;
                intersect.n_2 = nri2;
                intersect.n_3 = numz_bd-1;
                intersect.fraction_1 = frac1;
                intersect.fraction_2 = fracA;
                intersect.fraction_3 = fracB;

            }else{
                // flat part

                n0_wall = frac1*mSolN_BD_top[nri1] + frac2*mSolN_BD_top[nri2];

                intersect.bd_1 = 2;
                intersect.bd_2 = 2;
                intersect.bd_3 = -1;
                intersect.n_1 = nri1;
                intersect.n_2 = nri2;
                intersect.fraction_1 = frac1;
                intersect.fraction_2 = frac2;

            }

        }else{
            // wall
            nzi1 = floor(z_intersect/mDz_bd);
            nzi2 = nzi1+1;
            frac2 = (z_intersect - mDz_bd*nzi1)/mDz_bd;
            frac1 = 1-frac2;

            if (theta <= theta_transition)
            {
                //inner wall

                if (nzi2 == numz_bd-1)
                {
                    fracA = frac2*delta_r2/(delta_r2 + delta_z2);
                    fracB = frac2*delta_z2/(delta_r2 + delta_z2);

                    // includes corner which is different

                    n0_wall = frac1*mSolN_BD_inner[nzi1] + fracA*mSolN_BD_inner[nzi2] + fracB*mSolN_BD_top[0];

                    intersect.bd_1 = 1;
                    intersect.bd_2 = 1;
                    intersect.bd_3 = 2;
                    intersect.n_1 = nzi1;
                    intersect.n_2 = nzi2;
                    intersect.n_3 = 0;
                    intersect.fraction_1 = frac1;
                    intersect.fraction_2 = fracA;
                    intersect.fraction_3 = fracB;

                }else{
                    // flat part
                    n0_wall = frac1*mSolN_BD_inner[nzi1] + frac2*mSolN_BD_inner[nzi2];

                    intersect.bd_1 = 1;
                    intersect.bd_2 = 1;
                    intersect.bd_3 = -1;
                    intersect.n_1 = nzi1;
                    intersect.n_2 = nzi2;
                    intersect.fraction_1 = frac1;
                    intersect.fraction_2 = frac2;
                }

            }else{

                // outer wall

                if (nzi2 == numz_bd-1)
                {
                    fracA = frac2*delta_r2/(delta_r2 + delta_z2);
                    fracB = frac2*delta_z2/(delta_r2 + delta_z2);

                    // includes corner which is different

                    n0_wall = frac1*mSolN_BD_outer[nzi1] + fracA*mSolN_BD_outer[nzi2] + fracB*mSolN_BD_top[numr_bd-1];

                    intersect.bd_1 = 0;
                    intersect.bd_2 = 0;
                    intersect.bd_3 = 2;
                    intersect.n_1 = nzi1;
                    intersect.n_2 = nzi2;
                    intersect.n_3 = numr_bd-1;
                    intersect.fraction_1 = frac1;
                    intersect.fraction_2 = fracA;
                    intersect.fraction_3 = fracB;

                }else{
                    // flat part
                    n0_wall = frac1*mSolN_BD_outer[nzi1] + frac2*mSolN_BD_outer[nzi2];

                    intersect.bd_1 = 0;
                    intersect.bd_2 = 0;
                    intersect.bd_3 = -1;
                    intersect.n_1 = nzi1;
                    intersect.n_2 = nzi2;
                    intersect.fraction_1 = frac1;
                    intersect.fraction_2 = frac2;
                }
            }

        }

        if (n0_wall < 0)
        {
            cerr << "n0_wall not defined" << endl;
        }

        unsigned int N = 200;
        double dl = L/((double) N-1);
        double l = 0;
        double ri;
        double x;

        if (L == 0.0)
        {
            cerr << "L = 0" << endl;
        }

        dynarray<double> ion_rate_function(N, 0.0, L);

        for (unsigned int i=0; i < N; ++i)
        {
            // calculate element indecies
            l = i*dl;
            x = l - L;

            ri = sqrt(r*r + x*x*sinphi*sinphi + 2*r*x*sinphi*costheta);

            if (ri > mMaxr)
            {
                //cerr << "r > " << mMaxr << ": " << ri << endl;

                ri = mMaxr;
            }

            if (ri < mMinr)
            {
                //cerr << "r < " << mMinr << ": " << ri << endl;

                ri = mMinr;
            }

            nri1 = floor((ri - mMinr)/mDr);
            nri2 = nri1+1;
            frac2 = (ri - (mDr*nri1 + mMinr))/mDr;
            frac1 = 1-frac2;

            ion_rate_function(i) = frac1*mLi[nri1] + frac2*mLi[nri2];

            //ion_rate_function(i) = 0;

        }

        double vk = ion_rate_function.integral_total();

        double alpha = vk/mVth;

        if (alpha > mM.xmax())
        {
            nM = 0.0;
            nN = 0.0;
        }else{
            nM = n0_wall*mM.interp(alpha);
            nN = n0_wall*mN.interp(alpha);
        }

    }

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
unsigned int RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::
    bd_index(
        unsigned int n,
        unsigned int bd)
    {
        if (bd == 0)
        {
            return n;
        }

        if (bd == 1)
        {
            return n + numz_bd;
        }

        if (bd == 2)
        {
            return n + 2*numz_bd;
        }

        throw -1;
    }

template<
    unsigned int numr,
    unsigned int numz,
    unsigned int numr_bd,
    unsigned int numz_bd,
    unsigned int numv>
double RadialSolver<
    numr,
    numz,
    numr_bd,
    numz_bd,
    numv>::
    neTe_profile_function(
        double r,
        double rp,
        double gamma)
    {
        double xp = (rp - mMinr)/(mMaxr - mMinr);
        double x = (r - mMinr)/(mMaxr - mMinr);

        double a = (0.5 - xp)/(xp*xp - xp);
        double y = a*x*x + (1-a)*x;

        return (gamma/2)*(1-cos(PI_CONST*y)) + (1-gamma/2)*pow(sin(PI_CONST*y), 3);
    }


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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_IEEE_IEEE_HPP_INCLUDED
#define BOOST_SIMD_IEEE_IEEE_HPP_INCLUDED

//<include> please don't modify between these tags
#include <boost/simd/ieee/include/functions/negate.hpp>
#include <boost/simd/ieee/include/functions/sign.hpp>
#include <boost/simd/ieee/include/functions/negation.hpp>
#include <boost/simd/ieee/include/functions/sbits.hpp>
#include <boost/simd/ieee/include/functions/signnz.hpp>
#include <boost/simd/ieee/include/functions/prev.hpp>
#include <boost/simd/ieee/include/functions/next.hpp>
#include <boost/simd/ieee/include/functions/ulpdist.hpp>
#include <boost/simd/ieee/include/functions/ulp.hpp>
#include <boost/simd/ieee/include/functions/toint.hpp>
#include <boost/simd/ieee/include/functions/tofloat.hpp>
#include <boost/simd/ieee/include/functions/successor.hpp>
#include <boost/simd/ieee/include/functions/predecessor.hpp>
#include <boost/simd/ieee/include/functions/nextpow2.hpp>
#include <boost/simd/ieee/include/functions/nextafter.hpp>
#include <boost/simd/ieee/include/functions/modf.hpp>
#include <boost/simd/ieee/include/functions/minnummag.hpp>
#include <boost/simd/ieee/include/functions/minnum.hpp>
#include <boost/simd/ieee/include/functions/minmag.hpp>
#include <boost/simd/ieee/include/functions/maxnummag.hpp>
#include <boost/simd/ieee/include/functions/maxnum.hpp>
#include <boost/simd/ieee/include/functions/maxmag.hpp>
#include <boost/simd/ieee/include/functions/mantissa.hpp>
#include <boost/simd/ieee/include/functions/ldexp.hpp>
#include <boost/simd/ieee/include/functions/ilogb.hpp>
#include <boost/simd/ieee/include/functions/frexp.hpp>
#include <boost/simd/ieee/include/functions/frac.hpp>
#include <boost/simd/ieee/include/functions/fast_ldexp.hpp>
#include <boost/simd/ieee/include/functions/fast_frexp.hpp>
#include <boost/simd/ieee/include/functions/exponentbits.hpp>
#include <boost/simd/ieee/include/functions/exponent.hpp>
#include <boost/simd/ieee/include/functions/eps.hpp>
#include <boost/simd/ieee/include/functions/copysign.hpp>
#include <boost/simd/ieee/include/functions/bitofsign.hpp>
#include <boost/simd/ieee/include/functions/bitinteger.hpp>
#include <boost/simd/ieee/include/functions/bitfloating.hpp>
//<\include>

#endif


// include statements
#include <climits>
#include <iostream>
#include <cstddef>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <cmath>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include "matplotlibcpp.h"

//Obstcale Points- Change points here to edit obstacles
int obst_array[][2] = {{2, 7}, {7, 7}, {6, 4}, {4, 4}, {4, 6}, {2, 6}};

//typedefs to define geometries in boost geometry
typedef boost::geometry::model::d2::point_xy<double> point_xy;
typedef boost::geometry::model::polygon<point_xy> polygon_type;
typedef boost::geometry::model::multi_polygon<polygon_type> multi_polygon_type;
typedef boost::geometry::model::linestring<point_xy> linestring_t;

//variable to keep track of goal sampling and goal sampling rate
int g_sampling;
int a;

// Node tructure definiton
struct Node
{
    // x and y define the 2D coordinates of the node
    float x;
    float y;
    // Parent Node to the current Node
    Node *parent;
};

//array of all nodes
Node node_list[500];

// counter to keep track of no. of elements in node_list
int count = 0;

//array to store the path in
Node path[100];

//variable to track elements in path
int track = 0;

//sampler variables- used to set the sampling range
//Change these to use a different search space
int Offset = 0;
int Range = 13;

//Search space is defined from (0,0), (0,12), (12,0) and (12,12)
// function to sample random points in search space and also sample the goal at regular intervals defined by g_sampling
Node sampler(int goal_x, int goal_y)
{
    /*
        Args:
            goal_x(int): x-coordinate of the goal
            goal_y(int): y-coordinate of the goal

        Retuns:
            sample(Node object): The sampled node
    */
    Node sample = {};
    //samples random points
    if (count < a)
    {
        sample.x = Offset + (rand() % Range);
        sample.y = Offset + (rand() % Range);
        sample.parent = NULL;
    }
    //samples goal as every g_sampling th point
    else
    {
        a += g_sampling;
        sample.x = goal_x;
        sample.y = goal_y;
        sample.parent = NULL;
    }

    return sample;
}

//finds distance between two Node's
float distance(Node node1, Node node2)
{
    /*
        Args:
            node1(Node object): first node for distance calculation
            node2(Node object): second node for distance calculation

        Retuns:
            dist(float): the distance between the nodes
    */
    float dist = sqrt(((node1.x - node2.x) * (node1.x - node2.x)) + ((node1.y - node2.y) * (node1.y - node2.y)));
    return dist;
}

//function to find the nearest node to the passed node
int nearest_node(Node current)
{
    /*
        Args:
            current(Node object):node for which nearest node is to be found

        Retuns:
            near_i(int): the index of the nearest node wrt node_list
    */
    int near_i;
    float min_dist = 20;
    for (int i = 0; i < count; i++)
    {
        float dist = distance(current, node_list[i]);
        if (dist < min_dist)
        {
            min_dist = dist;
            near_i = i;
        }
    }

    return near_i;
}

//checks intersection between obstacles and the line segment (rand,near) using boost-geometry
bool check_intersection(Node end1, Node end2)
{
    /*
        Args:
            end1(Node object): first node of the line segment
            end2(Node object): second node of the line segment

        Retuns:
            status(bool): 1 if it collides with obstacles, 0 otherwise
    */

    //define polygons and multi-polygons
    polygon_type obstacle;
    multi_polygon_type obstacles;
    linestring_t line;

    //Adding the line
    // Store points in vector so we can assign them to a polygon
    std::vector<point_xy> pointList;

    point_xy point1(end1.x, end1.y);
    pointList.push_back(point1);

    point_xy point2(end2.x, end2.y);
    pointList.push_back(point2);

    // assign points to polygon line
    boost::geometry::assign_points(line, pointList);

    //Adding the obstacle
    // Store points in vector so we can assign them to a polygon
    std::vector<point_xy> ObstaclePointList;

    for (int i = 0; i < (sizeof(obst_array) / sizeof(obst_array[0])); i++)
    {
        point_xy point(obst_array[i][0], obst_array[i][1]);
        ObstaclePointList.push_back(point);
    }

    // assign points to polygon
    boost::geometry::assign_points(obstacle, ObstaclePointList);

    // Add polygon to multi-polygon
    obstacles.push_back(obstacle);

    //checks if line intersects with obstacles
    bool status = boost::geometry::intersects(line, obstacles);

    return status;
}

//checks if a point is inside an obstacle using boost-geometry
bool is_inside_obstacle(float a, float b)
{
    /*
        Args:
            a(float): x-coordinate of the point to be checked
            b(float): y-coordinate of the point to be checked

        Retuns:
            status(bool): 1 if it is within obstacles, 0 otherwise
    */
    //define polygons and multi-polygons
    polygon_type obstacle;
    multi_polygon_type obstacles;

    //Adding the point
    point_xy point(a, b);

    //Adding the obstacle
    // Store points in vector so we can assign them to a polygon
    std::vector<point_xy> ObstaclePointList;

    for (int i = 0; i < (sizeof(obst_array) / sizeof(obst_array[0])); i++)
    {
        point_xy point(obst_array[i][0], obst_array[i][1]);
        ObstaclePointList.push_back(point);
    }

    // assign points to polygon
    boost::geometry::assign_points(obstacle, ObstaclePointList);

    // Add polygon to multi-polygon
    obstacles.push_back(obstacle);

    //checks if point is within obstacles
    bool status = boost::geometry::within(point, obstacles);

    return status;
}

//function to backtrack path from goal to start
void backtrack(Node goal, Node start)
{
    /*
        Args:
            goal(Node object): goal
            start(Node object): start
    */
    //initialize current node as the goal
    Node *current = &goal;
    while (current->parent != NULL)
    {
        path[track] = *current;
        track++;
        //assign current node as its parent
        current = current->parent;
    }

    //Add start to the path
    path[track] = start;
    track++;
}

//function to visualise path using matplotlib-cpp
void visualise(bool is_path)
{
    /*
        Args:
            is_path(bool): status of whether path exists or not
    */
    namespace plt = matplotlibcpp;
    //plot obstacle
    std::vector<double> x = {};
    std::vector<double> y = {};

    for (int i = 0; i < (sizeof(obst_array) / sizeof(obst_array[0])); i++)
    {
        x.push_back(obst_array[i][0]);
        y.push_back(obst_array[i][1]);
    }
    x.push_back(obst_array[0][0]);
    y.push_back(obst_array[0][1]);

    plt::plot(x, y);

    //scatter plot if all nodes
    std::vector<double> scatter_x = {};
    std::vector<double> scatter_y = {};

    for (int i = 0; i < count; i++)
    {

        scatter_x.push_back(node_list[i].x);
        scatter_y.push_back(node_list[i].y);
    }

    plt::plot(scatter_x, scatter_y, ".r");

    //plot tree
    for (int i = 0; i < count; i++)
    {
        if (node_list[i].parent != NULL)
        {
            std::vector<double> node_x = {};
            std::vector<double> node_y = {};
            node_x.push_back(node_list[i].x);
            node_y.push_back(node_list[i].y);
            node_x.push_back(node_list[i].parent->x);
            node_y.push_back(node_list[i].parent->y);
            plt::plot(node_x, node_y, "r");
        }
    }

    if (is_path == 1)
    {
        //plot path
        for (int i = 0; i < track; i++)
        {
            if (path[i].parent != NULL)
            {
                std::vector<double> node_x = {};
                std::vector<double> node_y = {};
                node_x.push_back(path[i].x);
                node_y.push_back(path[i].y);
                node_x.push_back(path[i].parent->x);
                node_y.push_back(path[i].parent->y);
                plt::plot(node_x, node_y, ".b-");
            }
        }
    }

    plt::show();
}

// Start of main RRT function
//returns 0 if path not found and 1 if path is found
bool rrt(float start_x, float start_y, float goal_x, float goal_y, int K, float step)
{
    /*
        Args:
            start_x(float):x-coordinate of the start point
            start_y(float):y-coordinate of the start point
            goal_x(float):x-coordinate of the goal point
            goal_y(float):y-coordinate of the goal point
            K(int):maximum number of nodes in the tree
            step(float):step-size for RRT

        Returns:
            1, if path is found
            0, otherwise
    */
    //variable to track if goal has been reached
    bool not_reached;

    // defining start, goal and rand as a Node
    Node start = {start_x, start_y};
    Node goal = {goal_x, goal_y};
    Node rand = {};

    //variable to store index of the nearest neighbour
    int near_i;

    //add start to the node_list
    node_list[count] = start;
    count++;

    for (int j = 0; j < K;)
    {
        //samples a random point
        rand = sampler(goal_x, goal_y);

        // finds the index of the nearest node to the sampled point
        near_i = nearest_node(rand);

        //computing the angle b/w near and rand wrt x-axis
        float theta = atan2(rand.y - node_list[near_i].y, rand.x - node_list[near_i].x);

        //uses step-size to modify rand to a point in its direction, at a step distance from near
        rand.x = node_list[near_i].x + step * cos(theta);
        rand.y = node_list[near_i].y + step * sin(theta);

        //obstacle checking
        bool status = check_intersection(rand, node_list[near_i]);
        if (status == 1)
        {
            //if the line intersects any obstacle
            continue;
        }
        else
        {
            //adds line to node_list if it is collision free
            rand.parent = &node_list[near_i];
            node_list[count] = rand;
            count++;
            j++;
        }

        //variable to track if a direct connection to the goal is possible
        not_reached = check_intersection(rand, goal);

        //variable to store distance between current node and goal
        float goal_dist = distance(rand, goal);

        //adds goal to the node_list and breaks if goal is reachable or justa step-size distance away
        if (not_reached == 0 || goal_dist <= step)
        {
            goal.parent = &rand;
            node_list[count] = goal;
            count++;
            //Goal Reached
            break;
        }
    }
    //returns 0 if path not found
    if (not_reached == 1)
    {
        return 0;
    }

    //calls function to traverse from goal to start using their parent fields
    backtrack(goal, start);

    //returns 1 as path is founs succesfully
    return 1;
}

int main()
{
    // Providing a seed value for rand in sampler function
    srand((unsigned)time(NULL));

    //variables of rrt
    float start_x, start_y, goal_x, goal_y, K, step;

    //Print search space and obstacle end points
    std::cout << "\033[2J\033[1;1H";
    std::cout << "Rapidly Exploring Random Trees" << std::endl;
    std::cout << "The Search space is defined by points (0,0), (0,12), (12,0) and (12,12)" << std::endl;
    std::cout << "The Obstacle is a polygon defined by points (2, 7), (7, 7), (6, 4), (4, 4), (4, 6) and (2, 6)" << std::endl;

    //Taking user input for the rrt variables
    while (1)
    {
        std::cout << "Enter the x-coordinate of start(0-12):";
        std::cin >> start_x;

        std::cout << "Enter the y-coordinate of start(0-12):";
        std::cin >> start_y;

        if (start_x < 0 || start_x > 12 || start_y < 0 || start_y > 12)
        {
            std::cout << "The point is outside the Search space! Retry" << std::endl;
        }
        else if (is_inside_obstacle(start_x, start_y))
        {
            std::cout << "Point inside obstacle! Retry" << std::endl;
        }
        else
        {
            break;
        }
    }

    std::cout << "\033[2J\033[1;1H";
    std::cout << "Rapidly Exploring Random Trees" << std::endl;
    std::cout << "The Search space is defined by points (0,0), (0,12), (12,0) and (12,12)" << std::endl;
    std::cout << "The Obstacle is a polygon defined by points (2, 7), (7, 7), (6, 4), (4, 4), (4, 6) and (2, 6)" << std::endl;

    while (1)
    {
        std::cout << "Enter the x-coordinate of goal(0-12):";
        std::cin >> goal_x;

        std::cout << "Enter the y-coordinate of goal(0-12):";
        std::cin >> goal_y;

        if (goal_x < 0 || goal_x > 12 || goal_y < 0 || goal_y > 12)
        {
            std::cout << "The point is outside the Search space! Retry" << std::endl;
        }
        else if (is_inside_obstacle(goal_x, goal_y))
        {
            std::cout << "Point inside obstacle! Retry" << std::endl;
        }
        else
        {
            break;
        }
    }

    std::cout << "\033[2J\033[1;1H";
    std::cout << "Rapidly Exploring Random Trees" << std::endl;
    std::cout << "The Search space is defined by points (0,0), (0,12), (12,0) and (12,12)" << std::endl;
    std::cout << "The Obstacle is a polygon defined by points (2, 7), (7, 7), (6, 4), (4, 4), (4, 6) and (2, 6)" << std::endl;

    std::cout << "Enter the maximum number of nodes in the tree(0-499):";
    std::cin >> K;

    std::cout << "Enter the step-size for RRT(Preferably between 0.1-2):";
    std::cin >> step;

    std::cout << "Enter the rate at which the goal should be sampled(Enter 0 to generate an unbiased tree)" << std::endl
              << "(Enter 25 if you want every 25th sample to be the goal):";
    std::cin >> g_sampling;

    if (g_sampling == 0)
    {
        g_sampling = K + 1;
    }

    //variable assignment to ensure goal sampling rate is followed
    a = g_sampling;

    std::cout << "\033[2J\033[1;1H";
    std::cout << "Rapidly Exploring Random Trees" << std::endl;
    std::cout << "Finding a path!! Please Wait!" << std::endl;

    //call rrt function- start point is assumed to be the origin
    bool is_path = rrt(start_x, start_y, goal_x, goal_y, K, step);

    //plots the path and the tree
    visualise(is_path);

    if(is_path==0)
    {
        std::cout<<"Sorry,Path Not Found!!";
    }

    //print nodelist
    // for(int i=0;i<count;i++)
    // {
    //     std::cout<<node_list[i].x<<" "<<node_list[i].y<<std::endl;
    // }

    //print path
    // for(int i=0;i<track;i++)
    //     {
    //         std::cout<<path[i].x<<" "<<path[i].y<<std::endl;
    //     }

    return 0;
}


// signbit.hpp

#ifndef BOOST_SPIRIT_MATH_SIGNBIT_HPP
#define BOOST_SPIRIT_MATH_SIGNBIT_HPP

// Copyright (c) 2006 Johan Rade

// 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)

#if defined(_MSC_VER)
#pragma once
#endif

#include <boost/spirit/home/support/detail/math/detail/fp_traits.hpp>

namespace boost {
namespace spirit {
namespace math {

//------------------------------------------------------------------------------

template<class T> bool (signbit)(T x)
{
    typedef BOOST_DEDUCED_TYPENAME detail::fp_traits<T>::type traits;
    traits::init();

    BOOST_DEDUCED_TYPENAME traits::bits a;
    traits::get_bits(x,a);
    a &= traits::sign;
    return a != 0;
}

//------------------------------------------------------------------------------

namespace detail {

    template<class T> T copysign_impl(T x, T y)
    {
        typedef BOOST_DEDUCED_TYPENAME fp_traits<T>::type traits;
        traits::init();

        BOOST_DEDUCED_TYPENAME traits::bits a;
        traits::get_bits(x,a);
        a &= ~traits::sign;

        BOOST_DEDUCED_TYPENAME traits::bits b;
        traits::get_bits(y,b);
        b &= traits::sign;

        traits::set_bits(x,a|b);
        return x;
    }
}

inline float (copysign)(float x, float y)      // magnitude of x and sign of y
{
    return detail::copysign_impl(x,y);
}

inline double (copysign)(double x, double y)
{
    return detail::copysign_impl(x,y);
}

inline long double (copysign)(long double x, long double y)
{
    return detail::copysign_impl(x,y);
}

//------------------------------------------------------------------------------

template<class T> T (changesign)(T x)
{
    typedef BOOST_DEDUCED_TYPENAME detail::fp_traits<T>::type traits;
    traits::init();

    BOOST_DEDUCED_TYPENAME traits::bits a;
    traits::get_bits(x,a);
    a ^= traits::sign;
    traits::set_bits(x,a);
    return x;
}

//------------------------------------------------------------------------------

}   // namespace math
}   // namespace spirit
}   // namespace boost

#endif


/*
 * Copyright (c) 2018, Sunanda Bose (Neel Basu) (neel.basu.z@gmail.com)
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 * DAMAGE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "mathematica++ Unit Test (Module::STL)"
#include <boost/test/unit_test.hpp>
#include <string>
#include <cmath>
#include <complex>
#include <map>
#include <vector>
#include <iostream>
#include <boost/format.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include "boost/tuple/tuple.hpp"
#include "mathematica++/connector.h"
#include "mathematica++/m.h"
#include "mathematica++/io.h"
#include "mathematica++/declares.h"
#include "mathematica++/operators.h"
#include "mathematica++/rules.h"

#include "mathematica++/association.h"
using namespace mathematica;

// MATHEMATICA_DECLARE(FactorInteger)
MATHEMATICA_DECLARE(Flatten)

BOOST_AUTO_TEST_SUITE(stl)

BOOST_AUTO_TEST_CASE(stl_vector){
    connector shell;
    BOOST_CHECK(shell.connected());
    {
        value result;
        std::vector<int> list = {1,2,3,4,5};
        shell << Total(list);
        shell >> result;
        // std::cout << result << std::endl; // Prints 15
        BOOST_CHECK(*result == 15);
    }
    {
        value result;
        typedef std::vector<std::vector<int>> ivv_type;
        shell << FactorInteger(2434500);
        shell >> result;
        ivv_type prime_powers = mathematica::cast<ivv_type>(result);
        int sbase = 0 , spow = 0;
        for(auto pp: prime_powers){
            // std::cout << pp[0] << " ^ " << pp[1] << std::endl;
            sbase += pp[0];
            spow  += pp[1];
        }
        BOOST_CHECK(sbase == 551);
        BOOST_CHECK(spow == 8);
    }
    {
        value result;
        typedef std::vector<std::pair<int, int>>    ipv_type;
        typedef std::vector<boost::tuple<int, int>> itv_type;
        shell << FactorInteger(2434500);
        shell >> result;

        int sbase = 0 , spow = 0;

        ipv_type prime_powers_pair = mathematica::cast<ipv_type>(result);
        for(auto pp: prime_powers_pair){
            // std::cout << pp.first << " ^ " << pp.second << std::endl;
            sbase += pp.first;
            spow  += pp.second;
        }
        BOOST_CHECK(sbase == 551);
        BOOST_CHECK(spow == 8);

        sbase = 0, spow = 0;
        itv_type prime_powers_tuple = mathematica::cast<itv_type>(result);
        for(auto pp: prime_powers_tuple){
            // std::cout << pp << std::endl;
            sbase += boost::tuples::get<0>(pp);
            spow  += boost::tuples::get<1>(pp);
        }
        BOOST_CHECK(sbase == 551);
        BOOST_CHECK(spow == 8);
    }
    {
        typedef std::vector<std::vector<std::vector<int>>> ivv_type; // 2 x 2 x 3
        ivv_type matrix;
        std::vector<int> row11 = {111, 112, 113};
        std::vector<int> row12 = {121, 122, 123};

        std::vector<int> row21 = {211, 212, 213};
        std::vector<int> row22 = {221, 222, 223};

        std::vector<std::vector<int>> row1;
        std::vector<std::vector<int>> row2;

        row1.push_back(row11);
        row1.push_back(row12);

        row2.push_back(row21);
        row2.push_back(row22);

        matrix.push_back(row1);
        matrix.push_back(row2);

        value res;

        shell.enable(mathematica::bulk_io);
        shell << Total(Flatten(matrix));
        shell >> res;

        BOOST_CHECK(*res == 2004);
    }
    {
        typedef std::vector<std::vector<std::vector<int>>> ivv_type; // 2 x 2 x 3
        ivv_type matrix;
        std::vector<int> row11 = {111, 112, 113};
        std::vector<int> row12 = {121, 122, 123};

        std::vector<int> row21 = {211, 212, 213};
        std::vector<int> row22 = {221, 222};

        std::vector<std::vector<int>> row1;
        std::vector<std::vector<int>> row2;

        row1.push_back(row11);
        row1.push_back(row12);

        row2.push_back(row21);
        row2.push_back(row22);

        matrix.push_back(row1);
        matrix.push_back(row2);

        value res;

        shell.enable(mathematica::bulk_io);
        shell << Total(Flatten(matrix));
        shell >> res;

        BOOST_CHECK(*res == 1781);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef UQ_STRUCTURE_BASE_HPP
#define UQ_STRUCTURE_BASE_HPP

#include "pcells_hash_table.hpp"
#include <Eigen/Dense>

class StructureBase
{
public:
    explicit StructureBase() {}

    //! Evaluates the structure function on structured grid
    Eigen::VectorXd eval(const Eigen::MatrixXd& U,
                         Eigen::VectorXi stencil, int p);

    //! Evaluates the structure function for a point/cell
    //! of the structured grid
    double evalAtPoint(const Eigen::MatrixXd& U,
                       int i, int j,
                       int stencil, int p);

    //! Evaluates the structure function for a cell
    //! in the uniform grid hash table
    Eigen::VectorXd evalInCell
    (std::shared_ptr<CellsHashTable>& cellsHashTable,
     int i, int j, Eigen::VectorXd offsets, int p);

    //! Evaluates the structure function
    //! on unstructured meshes
    //! using uniform grid hash table
    Eigen::VectorXd eval
    (std::shared_ptr<CellsHashTable>& cellsHashTable,
     Eigen::VectorXd offsets, int p);

    //! Evaluates the structure function
    //! on unstructured meshes
    //! using uniform grid hash table
    Eigen::VectorXd eval
    (std::shared_ptr<ParCellsHashTable>& cellsHashTable,
     Eigen::VectorXd offsets, int p);

private:
    //! Evaluates functional
    inline double functional(const Eigen::Vector2d& Uij,
                             const Eigen::Vector2d& Ukl,
                             int p);

    //! Returns solution U(k,l)
    inline Eigen::Vector2d getSol(const Eigen::MatrixXd& U,
                                  int k, int l);

    //! Checks if the neighbour lies within the shell offset
    inline bool neighbour_in_shell
    (Eigen::Vector2d& coords, Eigen::Vector2d& nbCoords,
     double minOffset, double maxOffset);

    //! Checks if the neighbour lies within the shells
    std::pair<bool, int> neighbour_in_shells
    (Eigen::Vector2d& coords, Eigen::Vector2d& nbCoords,
     Eigen::VectorXd& offsets);

    double m_tol=1E-12;
};

#endif /// UQ_STRUCTURE_BASE_HPP


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>
#include <locale>

using namespace boost::gregorian;

int main()
{
  date d{2014, 5, 12};
  date_facet *df = new date_facet{"%A, %d %B %Y"};
  std::cout.imbue(std::locale{std::cout.getloc(), df});
  std::cout << d << '\n';
}


#ifndef EFANNA_HAMMING_INDEX_H_
#define EFANNA_HAMMING_INDEX_H_

#include "algorithm/base_index.hpp"
#include <boost/dynamic_bitset.hpp>
#include <time.h>
//for Debug
#include <iostream>
#include <fstream>
#include <string>
#include <unordered_map>
#include <map>
#include <sstream>
#include <set>

namespace efanna{

struct HAMMINGIndexParams : public IndexParams
{
	HAMMINGIndexParams(int codelen, char*& BaseCodeFile, char*& QueryCodeFile)
	{
		init_index_type = HAMMING;
		ValueType len;
		len.int_val = codelen;
		extra_params.insert(std::make_pair("codelen",len));
		ValueType bcf;
		bcf.str_pt = BaseCodeFile;
		extra_params.insert(std::make_pair("bcfile",bcf));
		ValueType qcf;
		qcf.str_pt = QueryCodeFile;
		extra_params.insert(std::make_pair("qcfile",qcf));
	}
};

template <typename DataType>
class HAMMINGIndex : public InitIndex<DataType>
{
public:

	typedef InitIndex<DataType> BaseClass;

	typedef std::vector<unsigned int> Code;
	typedef std::vector<Code> Codes;

	HAMMINGIndex(const Matrix<DataType>& dataset, const Distance<DataType>* d, const IndexParams& params = HAMMINGIndexParams(0,NULL,NULL)) :
		BaseClass(dataset,d,params)
	{
		std::cout<<"HASHING initial" <<std::endl;

		ExtraParamsMap::const_iterator it = params_.extra_params.find("codelen");
		if(it != params_.extra_params.end()){
			codelength = (it->second).int_val;
		}
		else{
			std::cout << "error: no code length setting" << std::endl;
		}

		it = params_.extra_params.find("bcfile");
		if(it != params_.extra_params.end()){
			char* fpath = (it->second).str_pt;
			std::string str(fpath);
			std::cout << "Loading base code from " << str << std::endl;

			LoadCode(fpath, BaseCode);
		}
		else{
			std::cout << "error: no base code file" << std::endl;
		}

		it = params_.extra_params.find("qcfile");
		if(it != params_.extra_params.end()){
			char* fpath = (it->second).str_pt;
			std::string str(fpath);
			std::cout << "Loading query code from " << str << std::endl;

			LoadCode(fpath, QueryCode);

		}
		else{
			std::cout << "error: no query code file" << std::endl;
		}

		std::cout << "Search begin..." << std::endl;
	}

	void LoadCode(char* filename, Codes& base){

		unsigned tableNum = codelength / 32;
		unsigned lastLen = codelength % 32;

		std::stringstream ss;
		unsigned j;
		for(j = 0; j < tableNum; j++){

			ss << filename << "_" << j+1 ;
			std::string codeFile;
			ss >> codeFile;
			ss.clear();

			std::ifstream in(codeFile.c_str(), std::ios::binary);
			if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

			int codeNum;
			in.read((char*)&codeNum,4);
			if (codeNum != 1){
				std::cout<<"codefile  "<< j << " error!"<<std::endl;
			}

			unsigned len;
			in.read((char*)&len,4);
			unsigned num;
			in.read((char*)&num,4);

			Code b;
			for(unsigned i = 0; i < num; i++){
				unsigned int codetmp;
				in.read((char*)&codetmp,4);
				b.push_back(codetmp);
			}
			base.push_back(b);
			in.close();
		}

		if(lastLen > 0){
			int shift = 32 - lastLen;
			ss << filename << "_" << j+1 ;
			std::string codeFile;
			ss >> codeFile;
			ss.clear();

			std::ifstream in(codeFile.c_str(), std::ios::binary);
			if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

			int codeNum;
			in.read((char*)&codeNum,4);
			if (codeNum != 1){
				std::cout<<"codefile  "<< j << " error!"<<std::endl;
			}

			unsigned len;
			in.read((char*)&len,4);
			unsigned num;
			in.read((char*)&num,4);

			Code b;
			for(unsigned i = 0; i < num; i++){
				unsigned int codetmp;
				in.read((char*)&codetmp,4);
				codetmp = codetmp >> shift;
				b.push_back(codetmp);
			}
			base.push_back(b);
			in.close();
		}

	}

	void buildIndexImpl(){}

	void locateNeighbors(const Matrix<DataType>& query){

		size_t pNum = BaseCode[0].size();
		size_t nTable = BaseCode.size();

		/*HammingDistNum.clear();
		HammingDistNum.resize(codelength+1);
		std::fill(HammingDistNum.begin(), HammingDistNum.end(), 0);*/

		for (size_t cur = 0; cur < query.get_rows(); cur++) {
			std::vector<int>hammingDistance(pNum);
			std::fill(hammingDistance.begin(), hammingDistance.end(), 0);

			for (size_t j = 0; j < nTable; j++) {
				for (size_t i = 0; i < pNum; i++) {
					hammingDistance[i] += parallel_popcnt32(BaseCode[j][i] ^ QueryCode[j][cur]);
				}
			}

			// initialize index
			std::vector<size_t> hammingDistanceIndex(pNum);
			for (size_t i = 0; i < hammingDistanceIndex.size(); i++) {
				hammingDistanceIndex[i] = i;
			}
			// compare function
			auto compare_func = [&hammingDistance](const size_t a, const size_t b)->bool
					{
				return hammingDistance[a] < hammingDistance[b];
					};
			// sort the index by its value
			std::partial_sort(hammingDistanceIndex.begin(), hammingDistanceIndex.begin() + SP.search_init_num, hammingDistanceIndex.end(),  compare_func);

			/*for (size_t i = 0; i < (unsigned) SP.search_init_num; i++) {
				HammingDistNum[hammingDistance[hammingDistanceIndex[i]]]++;
			}*/

		}

	}

	void getNeighbors(size_t K, const Matrix<DataType>& query){

		size_t pNum = BaseCode[0].size();
		size_t nTable = BaseCode.size();

		nn_results.clear();
		for (size_t cur = 0; cur < query.get_rows(); cur++) {
			std::vector<int>hammingDistance(pNum);
			std::fill(hammingDistance.begin(), hammingDistance.end(), 0);

			for (size_t j = 0; j < nTable; j++) {
				for (size_t i = 0; i < pNum; i++) {
					hammingDistance[i] += parallel_popcnt32(BaseCode[j][i] ^ QueryCode[j][cur]);
				}
			}

			// initialize index
			std::vector<size_t> hammingDistanceIndex(pNum);
			for (size_t i = 0; i < hammingDistanceIndex.size(); i++) {
				hammingDistanceIndex[i] = i;
			}
			// compare function
			auto compare_func = [&hammingDistance](const size_t a, const size_t b)->bool
					{
				return hammingDistance[a] < hammingDistance[b];
					};
			// sort the index by its value
			std::partial_sort(hammingDistanceIndex.begin(), hammingDistanceIndex.begin() + SP.search_init_num, hammingDistanceIndex.end(),  compare_func);

			std::vector<int> res;
			if ((unsigned) SP.search_init_num <= K){
				for (unsigned int j = 0; j < (unsigned) SP.search_init_num; j++) res.push_back(hammingDistanceIndex[j]);
			}else{
				std::vector<std::pair<float, size_t>> Distance;
				for (size_t i = 0; i < (unsigned) SP.search_init_num; i++) {
					Distance.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(hammingDistanceIndex[i]), features_.get_cols()), hammingDistanceIndex[i]));
				}
				std::partial_sort(Distance.begin(), Distance.begin() + K, Distance.end());

				for (unsigned int j = 0; j < K; j++) res.push_back(Distance[j].second);
			}
			nn_results.push_back(res);
		}
	}

	void outputVisitBucketNum(){
		for (size_t i = 0; i < HammingDistNum.size(); i++){
			if(HammingDistNum[i]>0){
				std::cout <<"hamming dist "<< i <<": "<< HammingDistNum[i]<<std::endl;
			}
		}
	}

	void loadGraph(char* filename){
		std::ifstream in(filename,std::ios::binary);
		in.seekg(0,std::ios::end);
		std::ios::pos_type ss = in.tellg();
		size_t fsize = (size_t)ss;
		int dim;
		in.seekg(0,std::ios::beg);
		in.read((char*)&dim, sizeof(int));
		size_t num = fsize / (dim+1) / 4;
		std::cout<<"load g "<<num<<" "<<dim<< std::endl;
		in.seekg(0,std::ios::beg);
		gs.clear();
		for(size_t i = 0; i < num; i++){
			std::vector<unsigned> heap;
			in.read((char*)&dim, sizeof(int));
			for(int j =0; j < dim; j++){
				unsigned id;
				in.read((char*)&id, sizeof(int));
				heap.push_back(id);
			}
			gs.push_back(heap);
		}
		in.close();
	}

protected:
	USING_BASECLASS_SYMBOLS

	int codelength;
	Codes BaseCode;
	Codes QueryCode;

	// for statistic info
	std::vector<unsigned> HammingDistNum;

};
}
#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP
#define BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP

#include <algorithm>
#include <string>

#include <boost/concept_check.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/strategies/side_info.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

namespace boost { namespace geometry
{

namespace policies { namespace relate
{

template <typename S1, typename S2, typename ReturnType, typename CalculationType = void>
struct segments_intersection_points
{
    typedef ReturnType return_type;
    typedef S1 segment_type1;
    typedef S2 segment_type2;
    typedef typename select_calculation_type
        <
            S1, S2, CalculationType
        >::type coordinate_type;

    // Get the same type, but at least a double
    typedef typename select_most_precise<coordinate_type, double>::type rtype;

    static inline return_type segments_intersect(side_info const&,
                    coordinate_type const& dx1, coordinate_type const& dy1,
                    coordinate_type const& dx2, coordinate_type const& dy2,
                    S1 const& s1, S2 const& s2)
    {
        return_type result;
        typedef typename geometry::coordinate_type
            <
                typename return_type::point_type
            >::type coordinate_type;

        // Get the same type, but at least a double (also used for divisions
        typedef typename select_most_precise
            <
                coordinate_type, double
            >::type promoted_type;

        promoted_type const s1x = get<0, 0>(s1);
        promoted_type const s1y = get<0, 1>(s1);

        // Calculate other determinants - Cramers rule
        promoted_type const wx = get<0, 0>(s1) - get<0, 0>(s2);
        promoted_type const wy = get<0, 1>(s1) - get<0, 1>(s2);
        promoted_type const d = (dy2 * dx1) - (dx2 * dy1);
        promoted_type const da = (promoted_type(dx2) * wy) - (promoted_type(dy2) * wx);

        // r: ratio 0-1 where intersection divides A/B
        promoted_type const r = da / d;

        result.count = 1;
        set<0>(result.intersections[0],
            boost::numeric_cast<coordinate_type>(s1x + r * promoted_type(dx1)));
        set<1>(result.intersections[0],
            boost::numeric_cast<coordinate_type>(s1y + r * promoted_type(dy1)));

        return result;
    }

    static inline return_type collinear_touch(coordinate_type const& x,
                coordinate_type const& y, int, int)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], x);
        set<1>(result.intersections[0], y);
        return result;
    }

    template <typename S>
    static inline return_type collinear_inside(S const& s, int index1 = 0, int index2 = 1)
    {
        return_type result;
        result.count = 2;
        set<0>(result.intersections[index1], get<0, 0>(s));
        set<1>(result.intersections[index1], get<0, 1>(s));
        set<0>(result.intersections[index2], get<1, 0>(s));
        set<1>(result.intersections[index2], get<1, 1>(s));
        return result;
    }

    template <typename S>
    static inline return_type collinear_interior_boundary_intersect(S const& s, bool a_in_b,
                int, int, bool opposite)
    {
        int index1 = opposite && ! a_in_b ? 1 : 0;
        return collinear_inside(s, index1, 1 - index1);
    }

    static inline return_type collinear_a_in_b(S1 const& s, bool opposite)
    {
        return collinear_inside(s);
    }
    static inline return_type collinear_b_in_a(S2 const& s, bool opposite)
    {
        int index1 = opposite ? 1 : 0;
        return collinear_inside(s, index1, 1 - index1);
    }

    static inline return_type collinear_overlaps(
                coordinate_type const& x1, coordinate_type const& y1,
                coordinate_type const& x2, coordinate_type const& y2,
                int, int, bool)
    {
        return_type result;
        result.count = 2;
        set<0>(result.intersections[0], x1);
        set<1>(result.intersections[0], y1);
        set<0>(result.intersections[1], x2);
        set<1>(result.intersections[1], y2);
        return result;
    }

    static inline return_type segment_equal(S1 const& s, bool opposite)
    {
        return_type result;
        result.count = 2;
        // TODO: order of IP's
        set<0>(result.intersections[0], get<0, 0>(s));
        set<1>(result.intersections[0], get<0, 1>(s));
        set<0>(result.intersections[1], get<1, 0>(s));
        set<1>(result.intersections[1], get<1, 1>(s));
        return result;
    }

    static inline return_type disjoint()
    {
        return return_type();
    }
    static inline return_type error(std::string const& msg)
    {
        return return_type();
    }

    static inline return_type collinear_disjoint()
    {
        return return_type();
    }
    static inline return_type parallel()
    {
        return return_type();
    }
    static inline return_type degenerate(S1 const& s, bool)
    {
        return_type result;
        result.count = 1;
        set<0>(result.intersections[0], get<0, 0>(s));
        set<1>(result.intersections[0], get<0, 1>(s));
        return result;
    }
};

}} // namespace policies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRY_POLICIES_RELATE_INTERSECTION_POINTS_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <boost/simd/swar/include/functions/group.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/swar/include/functions/group.hpp>
#include <boost/simd/include/functions/enumerate.hpp>
#include <boost/simd/include/functions/divides.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/type_traits/is_floating_point.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( demote, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::enumerate;
  using boost::simd::group;
  using boost::simd::tag::group_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;
  typedef typename boost::simd::meta::vector_of< typename boost::dispatch::meta::downgrade<T>::type, vT::static_size >::type dvT;
  typedef typename boost::dispatch::meta::call<group_(vT)>::type r_t;
  NT2_TEST_TYPE_IS(r_t, dvT);

  NT2_TEST_EQUAL( group(boost::simd::enumerate<vT>(T(-vT::static_size/2)))
                , boost::simd::enumerate<dvT>(T(-vT::static_size/2))
                );
}

NT2_TEST_CASE_TPL_MPL( group_groupable__2_0, NT2_TEST_SEQ_MPL_FILTER(BOOST_SIMD_SIMD_GROUPABLE_TYPES, not_< boost::is_floating_point<_> >) )
{
  using boost::simd::group;
  using boost::simd::tag::group_;
  using boost::simd::native;
  using boost::simd::Two;
  using boost::simd::Valmax;
  using boost::simd::Valmin;
  using boost::simd::One;
  using boost::simd::Zero;
  using boost::simd::Inf;
  using boost::simd::Minf;
  using boost::simd::Allbits;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::call<group_(vT,vT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;

  // specific values tests
  NT2_TEST_EQUAL(group(One<vT>(),  One<vT>())[0],  One<sr_t>());
  NT2_TEST_EQUAL(group(Zero<vT>(), Zero<vT>())[0], Zero<sr_t>());
  NT2_TEST_EQUAL(group(Valmax<vT>()/Two<vT>(), Valmax<vT>()/Two<vT>())[0], Allbits<sr_t>());
  NT2_TEST_EQUAL(group(Valmin<vT>()/Two<vT>(), Valmin<vT>()/Two<vT>())[0], Zero<sr_t>());
  NT2_TEST_EQUAL(group(Valmax<vT>(), Valmax<vT>())[0], Allbits<sr_t>());
  NT2_TEST_EQUAL(group(Valmin<vT>(), Valmin<vT>())[0], Zero<sr_t>());
} // end of test for groupable_

NT2_TEST_CASE_TPL_MPL( group_groupable__2_1, NT2_TEST_SEQ_MPL_FILTER(BOOST_SIMD_SIMD_GROUPABLE_TYPES, boost::is_floating_point<_>) )
{
  using boost::simd::group;
  using boost::simd::tag::group_;
  using boost::simd::native;
  using boost::simd::Two;
  using boost::simd::Valmax;
  using boost::simd::Valmin;
  using boost::simd::One;
  using boost::simd::Zero;
  using boost::simd::Inf;
  using boost::simd::Minf;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;
  typedef typename boost::dispatch::meta::call<group_(vT,vT)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;

  // specific values tests
  NT2_TEST_EQUAL(group(One<vT>(),  One<vT>())[1],  One<sr_t>());
  NT2_TEST_EQUAL(group(Zero<vT>(), Zero<vT>())[1], Zero<sr_t>());
  NT2_TEST_EQUAL(group(Valmax<vT>()/Two<vT>(), Valmax<vT>()/Two<vT>())[1], Inf<sr_t>());
  NT2_TEST_EQUAL(group(Valmin<vT>()/Two<vT>(), Valmin<vT>()/Two<vT>())[1], Minf<sr_t>());
  NT2_TEST_EQUAL(group(Valmax<vT>(), Valmax<vT>())[1], Inf<sr_t>());
  NT2_TEST_EQUAL(group(Valmin<vT>(), Valmin<vT>())[1], Minf<sr_t>());
} // end of test for groupable_

NT2_TEST_CASE_TPL ( group_groupable__3_1,  BOOST_SIMD_SIMD_GROUPABLE_TYPES)
{
  using boost::simd::group;
  using boost::simd::tag::group_;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  using boost::simd::Two;
  using boost::simd::Valmax;
  using boost::simd::Valmin;
  using boost::simd::One;
  using boost::simd::Zero;
  using boost::simd::Inf;
  using boost::simd::Minf;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;

  // specific values tests

  size_t n = cardinal_of<vT>::value;
  for(size_t i=0; i < cardinal_of<vT>::value; i++)
  {
    NT2_TEST_EQUAL(group(boost::simd::enumerate<vT>(T(0)),  boost::simd::enumerate<vT>(T(n)))[i],  T(i));
  }
} // end of test for groupable_

NT2_TEST_CASE_TPL ( group_groupable__4_1,  (int32_t))
{
  using boost::simd::group;
  using boost::simd::tag::group_;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  using boost::simd::Two;
  using boost::simd::Valmax;
  using boost::simd::Valmin;
  using boost::simd::One;
  using boost::simd::Zero;
  using boost::simd::Inf;
  using boost::simd::Minf;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                        n_t;
  typedef n_t                                     vT;

  // specific values tests

  size_t n = cardinal_of<vT>::value;
  int32_t s = 32767 - int32_t(n);
  for(size_t i=0; i < cardinal_of<vT>::value; i++)
  {
    NT2_TEST_EQUAL(group( boost::simd::enumerate<vT>(T(s))
                        , boost::simd::enumerate<vT>(T(s+n))
                        )[i]
                  ,  T(s+i)
                  );
  }
} // end of test for groupable_


#include <boost/program_options/options_description.hpp>
#include <boost/program_options.hpp>

#include <frovedis/ml/lda/lda_cgs.hpp>

using namespace frovedis;

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

    use_frovedis use(argc, argv);

    using namespace boost::program_options;

    options_description opt("option");
    opt.add_options()
        ("help,h", "print help message")
        ("algorithm,g", value<std::string>(), "sampling algorithm: \n\"original\": original cgs, "
            "\n\"dp\": doc_proposal, \n\"wp\": word_proposal, \n\"cp\":cycle_proposal, \n\"sparse\": sparse_lda")
        ("input,i", value<std::string>(), "path for input corpus of crs matrix")
        ("output,o", value<std::string>(), "path for output model of LDA")
        ("topics,p", value<int>(), "number of topics.")
        ("train_iter", value<int>(), "number of iterations.")
        ("explore_iter", value<int>(), "number of iteration to explore optimal hyperparams")
        // ("delay_update", "update model after sampling for all tokens")
        ("eval_cycle", value<int>(), "evaluate during training: 0 means not to evaluate")
        ("trace", "set log level to TRACE")
        ("debug", "set log level to DEBUG")
        ("binary", "save/load file with binary")
        ("alpha", value<double>(), "alpha")
        ("beta" , value<double>(), "beta");

    variables_map argmap;
    store(command_line_parser(argc,argv).options(opt).allow_unregistered().run(), argmap);
    notify(argmap);

    std::string algorithm;
    std::string input_path;
    std::string output_path;
    double alpha = 0.1;
    double beta  = 0.01;
    int topics = 1000;
    int train_iter = 30;
    int explore_iter = 0;
    bool vectorized  = true;
    int eval_cycle = 0;
    // bool delay_update = false;
    set_loglevel(INFO);
    bool binary = false;

#if !(defined(_SX) || defined(__ve__))
    vectorized = false;
#endif

    if(argmap.count("help")){
        std::cerr << opt << std::endl;
        finalizefrovedis(0);
    }
    if(argmap.count("algorithm")){
        algorithm = argmap["algorithm"].as<std::string>();
        if(algorithm!="original" && algorithm!="dp" && algorithm!="wp" && algorithm!="cp" && algorithm!="sparse"){
            std::cerr<<"ERROR: Algorithm "<<algorithm<<" doesn't exist!"<<std::endl;
            std::cerr << opt << std::endl;
            finalizefrovedis(1);
        }
    } else {
      std::cerr << "algorithm is not specified" << std::endl;
      std::cerr << opt << std::endl;
      finalizefrovedis(1);
    }
    if(argmap.count("input")){
      input_path = argmap["input"].as<std::string>();
    } else {
      std::cerr << "input path for corpus is not specified" << std::endl;
      std::cerr << opt << std::endl;
      finalizefrovedis(1);
    }
    if(argmap.count("output")){
      output_path = argmap["output"].as<std::string>();
    } else {
      std::cerr << "output path for model is not specified" << std::endl;
      std::cerr << opt << std::endl;
      finalizefrovedis(1);
    }
    if(argmap.count("alpha")) alpha = argmap["alpha"].as<double>();
    if(argmap.count("beta"))  beta = argmap["beta"].as<double>();
    if(argmap.count("topics")){
        topics = argmap["topics"].as<int>();
        // alpha = 50/(double)topics;   // XXX
    }
    if(argmap.count("train_iter")) { train_iter = argmap["train_iter"].as<int>(); }
    if(argmap.count("explore_iter")) { explore_iter = argmap["explore_iter"].as<int>(); }
    // if(argmap.count("delay_update")) { delay_update = true; }
    if(argmap.count("eval_cycle")) { eval_cycle = argmap["eval_cycle"].as<int>(); }
    if (argmap.count("debug")) { set_loglevel(DEBUG); }
    if (argmap.count("trace")) { set_loglevel(TRACE); }
    if (argmap.count("binary")) { binary = true; }

    // if(delay_update && algorithm!="dp" && algorithm!="wp" && algorithm!="cp" && algorithm!="original"){
    //     std::cerr<<"WARNING: delay update for algorithm *" << algorithm << "* has not been implemented!" << " ";
    //     std::cerr<<"The model will be updated after per-token sampling!" << std::endl;
    //     delay_update = false;
    // }
    if(vectorized && algorithm!="dp" && algorithm!="wp" && algorithm!="cp" && algorithm!="original") {
        std::cerr<<"WARNING: vectorized sampling for algorithm *" << algorithm << "* has not been implemented!" << " ";
        std::cerr<<"Running original CGS" << std::endl;
        algorithm = "original";
    }

    rowmajor_matrix<int32_t> doc_topic_count;
    auto data_train = binary ? make_crs_matrix_loadbinary<int32_t>(input_path) : make_crs_matrix_load<int32_t>(input_path);
    auto model = lda_train<int32_t>(data_train,alpha,beta,topics,train_iter,algorithm,explore_iter,
                                    eval_cycle, doc_topic_count);
    model.save_as(output_path, binary);
    binary ? doc_topic_count.savebinary(output_path + "/doc_topic")
           : doc_topic_count.save(output_path + "/doc_topic");

}