#pragma once

#include "ArithmeticProgression.hpp"
#include "Misc.hpp"
#include "Sequences.hpp"
#include "VectorHelpers.hpp"
#include <boost/iterator/iterator_facade.hpp>

namespace discreture
{

////////////////////////////////////////////////////////////////////////////////////
/// \brief Class for iterating through all dyck (dyck) paths.
/// \param IntType must be a SIGNED integer type.
///
/// Dyck paths, also called Catalan Paths, are paths that go from \f$(0,0)\f$ to
/// \f$(0,2n)\f$, which never go below the \f$ y=0\f$ line, in which each step
/// is from \f$(x,y)\f$ to either \f$(x+1,y+1)\f$ or \f$(x+1,y-1)\f$ #Example
/// Usage:
///
///     dyck_paths X(3)
///     for (auto&& x : X)
///         cout << x << endl;
/// Prints out:
///     [ 1 1 1 -1 -1 -1 ]
///     [ 1 1 -1 1 -1 -1 ]
///     [ 1 -1 1 1 -1 -1 ]
///     [ 1 1 -1 -1 1 -1 ]
///     [ 1 -1 1 -1 1 -1 ]
///
/// # Example: Parenthesis
///
///     dyck_paths X(3)
///     for (auto&& x : X)
///         cout << dyck_paths::to_string(x, "()") << endl;
///
/// Prints out:
///     ((()))
///     (()())
///     ()(())
///     (())()
///     ()()()
///
/////////////////////////////////////////////////////////////////////////////////////
template <class IntType = int, class RAContainerInt = std::vector<IntType>>
class DyckPaths
{
public:
    static_assert(std::is_integral<IntType>::value,
                  "Template parameter IntType must be integral");
    static_assert(std::is_signed<IntType>::value,
                  "Template parameter IntType must be signed");
    using value_type = RAContainerInt;
    using dyck_path = value_type;
    using difference_type = std::ptrdiff_t;
    using size_type = difference_type;
    class iterator;
    using const_iterator = iterator;

    // **************** Begin static functions
    static void next_dyck_path(dyck_path& data)
    {
        size_t n_ = data.size()/2;

        if (n_ == 0)
            return;

        if (data[1] != -1)
        {
            size_t loc = 2;

            while (data[loc] != -1)
                ++loc;

            data[loc] = 1;
            data[loc - 1] = -1;

            return;
        }

        size_t verif = 0;
        size_t i = 1;

        while (i < n_ && verif == 0)
        {
            if (data[(2*i) + 1] == 1)
                verif = ((2*i) + 1);

            ++i;
        }

        //          for(nuint i=1; i<t; ++t)
        //          {
        //              if(data[(2*i)+1]==0)
        //                  verif=((2*i)+1);
        //          }

        if (verif == 0)
        {
            return;
        }

        size_t cont = 0;
        auto encontrar = verif + 1;

        for (size_t i = 0; i < verif; ++i)
        {
            if (data[i] == -1)
            {
                data[i] = 1;
                ++cont;
            }
        }

        while (data[encontrar] != -1)
            ++encontrar;

        data[encontrar] = 1;
        data[encontrar - 1] = -1;

        while (cont != 0)
        {
            data[encontrar - 1 - cont] = -1;
            --cont;
        }
    }

    static std::string to_string(const dyck_path& data,
                                 const std::string& delim = "()")
    {
        std::string toReturn;

        for (auto i : data)
        {
            auto j = 1 - (i + 1)/2;
            toReturn.push_back(delim[j]);
        }

        return toReturn;
    }

    // **************** End static functions

public:
    ////////////////////////////////////////////////////////////
    /// \brief Constructor
    ///
    /// \param n is an integer >= 0
    ///
    ////////////////////////////////////////////////////////////
    explicit DyckPaths(IntType n) : n_(n) {}

    ////////////////////////////////////////////////////////////
    /// \brief The total number of dyck_paths
    ///
    /// \return binomial(2n,n)/(n+1)
    ///
    ////////////////////////////////////////////////////////////
    size_type size() const { return catalan(n_); }

    IntType get_n() const { return n_; }

    ////////////////////////////////////////////////////////////
    /// \brief Forward iterator class.
    ////////////////////////////////////////////////////////////
    class iterator
        : public boost::iterator_facade<iterator, const dyck_path&, boost::forward_traversal_tag>
    {
    public:
        iterator() = default; // empty initializer
        explicit iterator(IntType n) : ID_(0), data_(2*n, 1)
        {
            for (size_t i = n; i < data_.size(); ++i)
                data_[i] = -1;
        }

        size_type ID() const { return ID_; }

        bool is_at_end(IntType n) const { return ID_ == catalan(n); }

        void reset(IntType n)
        {
            ID_ = 0;
            data_.resize(2*n);
            auto r = static_cast<size_t>(n);
            for (size_t i = 0; i < r; ++i)
                data_[i] = 1;

            for (size_t i = r; i < data_.size(); ++i)
                data_[i] = -1;
        }

        static const iterator make_invalid_with_id(size_type id)
        {
            iterator it;
            it.ID_ = id;
            return it;
        }

    private:
        void increment()
        {
            ++ID_;

            next_dyck_path(data_);
        }

        const dyck_path& dereference() const { return data_; }

        bool equal(const iterator& it) const { return it.ID() == ID(); }

    private:
        size_type ID_{0};
        dyck_path data_{};

        friend class boost::iterator_core_access;
    }; // end class iterator

    iterator begin() const { return iterator(n_); }

    const iterator end() const
    {
        return iterator::make_invalid_with_id(size());
    }

private:
    IntType n_;

}; // end class DyckPaths

using boost::container::static_vector;

using dyck_paths = DyckPaths<int>;
using dyck_paths_stack = DyckPaths<int, static_vector<int, 48>>;

} // namespace discreture


#ifndef FRACTIONAL_FIT_HH
#define FRACTIONAL_FIT_HH
#include <Eigen/Dense>
#include <vector>

/* this class intends to fit probability of processes to the data
 * with sim_1(x), sim_2(x), sim_3(x) samples, a,b,c probabilities, where a+b+c=1
 * and data(x) sample
 *   a*sim_1 + b*sim_2 + c*sim_3 = data
 * is achieved, via fitting the moments of the 1,2,3 distributions to the moments of the data
 * utilized by correlation_tensor_serie
 */

template<class T>
class fractional_fit{
typedef Eigen::LDLT< Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic > > decomposition;
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic > Matrix;
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vector;
 public:

  fractional_fit(Matrix const & A, Matrix const & B, Matrix const & D,
		 //Vector const & m, Vector const & n, Vector const & d );
		 std::vector<T> const &m, std::vector<T> const &n, std::vector<T> const &d);
  ~fractional_fit(){}
//  void solve();
  void analytic_solve();
  double operator()(double a);

  T a_min, chi2;
private:
  Matrix A, B, D;
  Vector m, n, d;

  Matrix Sigma; //depends on the 'a' fraction
  Vector l;

};

#endif //FRACTIONAL_FIT_HH


//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//  Observable functions for Kitaev Quantum Monte Carlo
//  written by: Tim Eschmann, June 2016
//  Modified version: February 2019
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

#define _USE_MATH_DEFINES

#include <iostream>
#include <cmath>
#include <complex>
#include <armadillo>

using namespace arma;

///////////////////////////////////////////////////////////////
// Calculate free energy for a given Z2 configuration:
///////////////////////////////////////////////////////////////

double free_en(vec ev, double b)
{
    int p; // running index
    double fe = 0; // free energy

    // Calculating:
    for (p = 0; p < size(ev)[0]/2; p++)
    {
        fe -= (1/b)*logl(2*coshl(b*ev[p]/2));
    }

    return fe;

}

///////////////////////////////////////////////////////////////
// Calculate internal energy of the Majorana fermion system:
///////////////////////////////////////////////////////////////

double en(vec ev, double b)
{
    int q; // running index;
    double en = 0; // internal energy

    for (q = 0; q < size(ev)[0]/2; q++)
    {
        en += ev[q]/2 * tanhl(-b * ev[q]/2);
    }

    return en;
}

///////////////////////////////////////////////////////////////
// Calculate derivative of E w.r.t. beta:
///////////////////////////////////////////////////////////////

double diffE(vec ev, double b)
{
    int qq; // running index
    double drv = 0;
    for (qq = 0; qq < size(ev)[0]/2; qq++)
    {
        //drv += ev[qq]*ev[qq]/4. * (1 - tanh(-beta_ * ev[qq]/2)*tanh(-beta_ * ev[qq]/2));
        drv += powl(ev[qq], 2)/4. / powl((coshl(-b * ev[qq]/2)),2);
    }

    return drv;
}

///////////////////////////////////////////////////////////////
// Calculate average flux per elementary plaquette:
///////////////////////////////////////////////////////////////

std::complex <double> flux(cx_mat ham, Mat<int> plaq)
{
    std::complex <double> flux;
    std::complex <double> av_flux = std::complex<double>(0.0, 0.0);
    int N = size(ham)[0];
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int coord1, coord2;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }
        av_flux += flux;
    }

    av_flux /= double(M1);

    return av_flux;

}

///////////////////////////////////////////////////////////////
// Calculate flux disorder ratio p:
///////////////////////////////////////////////////////////////

double get_p(cx_mat ham, Mat<int> plaq)
{
    std::complex <double> flux;
    double p;
    int N = size(ham)[0];
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int coord1, coord2;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }
        if ((M2 % 4 == 0) && (flux == std::complex<double>(-1, 0)))
            p += 1;
        else if ((M2 % 4 == 2) && (flux == std::complex<double>(1, 0)))
            p += 1;
        else if ((M2 % 2 != 0) && (flux == std::complex<double>(0, 1)))
            p += 1;
    }

    p /= double(M1);

    return p;

}

///////////////////////////////////////////////////////////////
// Give flux configurations as output:
///////////////////////////////////////////////////////////////
cx_vec flux_confs(cx_mat ham, Mat<int> plaq)
{
    int M1 = size(plaq)[0];
    int M2 = size(plaq)[1];
    int N = size(ham)[0];
    int coord1, coord2;
    cx_vec confs(M1);
    std::complex <double> flux;

    for (int i = 0; i < M1; i++)
    {
        flux = std::complex<double>(1.0, 0.0);
        for (int j = 0; j < M2; j++)
        {
            coord1 = plaq(i,j) / N;
            coord2 = plaq(i,j) % N;

            // Plaquette operator: W_p = prod_<i,j> (-i*u_ij)
            flux *= (-ham(coord1, coord2)) / std::abs(ham(coord1, coord2));
        }

        confs[i] = flux;
    }

    return confs;

}

///////////////////////////////////////////////////////////////
// Calculate spin-spin correlation:
// Make sure only to include one subset of bonds
///////////////////////////////////////////////////////////////

double correlation(cx_mat ham, std::vector<int> v_, double b)
{
    double value = 0.;
    std::complex<double> av;
    vec eigval;
    cx_mat eigvec;
    int N = size(ham)[0];
    int coord1, coord2;

    eig_sym(eigval, eigvec, ham);
    //eigvec = normalise(eigvec);

    for (int j = 0; j < v_.size(); j++)
    {
        coord1 = v_[j]/N;
        coord2 = v_[j]%N;

        for (int i = size(eigval)[0]/2; i < size(eigval)[0]; i++)
        {
            av = conj(eigvec(coord1,i))*((-ham(coord1, coord2)) / std::abs(ham(coord1, coord2)))*eigvec(coord2, i);
            av += conj(eigvec(coord2,i))*((-ham(coord2, coord1)) / std::abs(ham(coord2, coord1)))*eigvec(coord1, i);
            value -= real(av)*tanhl(b * eigval[i]/2.);
        }
    }

    value *= 2/double(N);
    return value;

}

/////////////////////////////////////////////////////////
// Set up simulation temperatures (w. different options):
/////////////////////////////////////////////////////////

double calc_temp(double T_min, double T_max, int me, int np, std::string dist)
{
    int i;
    double T;

    // a) Read temperature distribution from file:
    if (dist == "external")
    {
        double temperatures[np];
        std::ifstream tempfile("temp.saved", std::ifstream::in);
        if(tempfile.good())
        {
            for (i = 0; i < np; i++)
            {
                tempfile >> temperatures[i];
            }
        }

        T = temperatures[me - 1];
    }

    // b) Linear temperature distribution:
    else if (dist == "lin")
    {
        T = T_min + (T_max - T_min)*(me - 1)/float(np);
    }
    // c) Logarithmic temperature distribution:*/
    else if (dist == "log")
    {
        T = pow(10,log10(T_min)+((log10(T_max) - log10(T_min))*(me - 1)/double(np)));
    }
    // d) Double-logarithmic temperature distribution:
    else if (dist == "double_log")
    {
        T = pow(10,log10(T_min)+((log10(T_max) - log10(T_min))*pow(10, -(np - (me - 1))/double(np))));
    }

    return T;

}

//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//  Ensemble optimization functions
//  -> check if MC replica has been at T_min or T_max latest and assign
// "+1" or "-1" accordingly
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
// Record if replica has been at lowest or highest T latest ...      ////////
/////////////////////////////////////////////////////////////////////////////

int check_sign_i(int i, int np, int s_i, int s_i_plus_1)
{
    int sign;

    if (i != 1 && (i+1 != (np - 1)))
    {
        if (s_i == 1 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 0 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == 1 && s_i_plus_1 == -1)
            sign = -1;
        else if (s_i == -1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == 0 && s_i_plus_1 == -1)
            sign = -1;
        else if (s_i == -1 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 0 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == -1)
            sign = -1;
    }
    else if (i == 1)
        sign = 1;
    else if (i+1 == np - 1)
        sign = -1;
    else
        sign = 0;

    return sign;

}

int check_sign_i_plus_1(int i, int np, int s_i, int s_i_plus_1)
{
    int sign;

    if (i != 1 && (i+1 != (np - 1)))
    {
        if (s_i == 1 && s_i_plus_1 == 0)
            sign = 1;
        else if (s_i == 0 && s_i_plus_1 == 1)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == -1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == 1)
            sign = -1;
        else if (s_i == 0 && s_i_plus_1 == -1)
            sign = 0;
        else if (s_i == -1 && s_i_plus_1 == 0)
            sign = -1;
        else if (s_i == 0 && s_i_plus_1 == 0)
            sign = 0;
        else if (s_i == 1 && s_i_plus_1 == 1)
            sign = 1;
        else if (s_i == -1 && s_i_plus_1 == -1)
            sign = -1;
    }
    else if (i == 1)
        sign = 1;
    else if (i+1 == np - 1)
        sign = -1;
    else
        sign = 0;

    return sign;
}


#include "reconstruction.h"
#include "simulation.h"
#include <Eigen/SVD>
#include <Eigen/StdVector>
#include <igl/PI.h>
#include <igl/copyleft/marching_cubes.h>
#include <iostream>
#include <tbb/parallel_for.h>

void reconstruct(Eigen::MatrixXd &V, Eigen::MatrixXi &F, const Eigen::MatrixXd &P, const Eigen::Vector3i &res,
                 const Eigen::VectorXd &mass, const Eigen::VectorXd &density, double h, double isovalue) {
    auto W_k = 10. / (7. * igl::PI);
    auto P1 = [&](double r) {
        auto q = r;
        auto res = 0.0;
        if (q <= 1.0)
            res = (1 - 1.5 * q * q + 0.75 * q * q * q);
        else if (q < 2.0) {
            auto q2 = 2 - q;
            res = 0.25 * q2 * q2 * q2;
        }
        return res;
    };

    printf("P.rows() = %d\n", P.rows());
    using igl::copyleft::marching_cubes;
    // Eigen::Vector3d lower = Eigen::Vector3d::Zero() - Eigen::Vector3d::Constant(2 * h); // = P.colwise().minCoeff();
    // Eigen::Vector3d upper = Eigen::Vector3d::Ones() + Eigen::Vector3d::Constant(2 * h); // P.colwise().maxCoeff();
    Eigen::Vector3d lower = Eigen::Vector3d(P.colwise().minCoeff()) - Eigen::Vector3d::Constant(2 * h);
    Eigen::Vector3d upper = Eigen::Vector3d(P.colwise().maxCoeff()) + Eigen::Vector3d::Constant(2 * h);
    Eigen::Vector3d extent = upper - lower;
    Eigen::VectorXd S;
    Eigen::MatrixXd GV;
    S.resize(res.prod());
    GV.resize(res.prod(), 3);
    Eigen::Vector3d nn_cell_size(2 * h, 2 * h, 2 * h);
    Eigen::Vector3i nn_grid_size;
    nn_grid_size << extent[0] / nn_cell_size[0], extent[1] / nn_cell_size[1], extent[2] / nn_cell_size[2];
    Eigen::Vector3d grid_cell_size = extent.array() / (res - Eigen::Vector3i::Ones()).cast<double>().array();
    for (int i = 0; i < res.prod(); i++) {
        int x = i % res[0];
        int y = (i % (res[0] * res[1])) / res[0];
        int z = i / (res[0] * res[1]);
        Eigen::Vector3d p = Eigen::Vector3d(Eigen::Array3d(x, y, z) * grid_cell_size.array()) + lower;
        GV.row(i) = p;
    }
    // std::cout << GV << std::endl;
    struct Cell {
        std::vector<int> particles;
    };
    std::vector<Cell> grid(nn_grid_size.prod());
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> G(P.rows());
    std::vector<std::vector<int>> neighbors(P.rows());
    Eigen::MatrixXd X;
    X.resizeLike(P);
    // auto get_cell_index = [&](const Eigen::Vector3d &x) -> Eigen::Vector3i {
    //     Eigen::Vector3i ip =
    //         Eigen::Vector3i(((x - lower).array() / extent.array() * res.array().cast<double>()).cast<int>());
    //     for (int i = 0; i < 3; i++) {
    //         ip[i] = std::min(res[i] - 1, std::max(ip[i], 0));
    //     }
    //     return ip;
    // };
    // auto get_linear_index = [&](const Eigen::Vector3i &ip) { return ip[0] + ip[1] * res[0] + ip[2] * res[0] * res[1];
    // };
    auto get_nn_cell_index = [&](const Eigen::Vector3d &x) -> Eigen::Vector3i {
        Eigen::Vector3i ip = Eigen::Vector3i(((x - lower).array() / nn_cell_size.array()).cast<int>());
        for (int i = 0; i < 3; i++) {
            ip[i] = std::min(nn_grid_size[i] - 1, std::max(ip[i], 0));
        }
        return ip;
    };
    auto get_nn_linear_index = [&](const Eigen::Vector3i &ip) {
        return ip[0] + ip[1] * nn_grid_size[0] + ip[2] * nn_grid_size[0] * nn_grid_size[1];
    };
    for (int i = 0; i < P.rows(); i++) {
        Eigen::Vector3i ip = get_nn_cell_index(P.row(i));
        auto idx = get_nn_linear_index(ip);
        CHECK(idx < grid.size());
        // printf("%d\n", idx);
        grid[idx].particles.push_back(i);
    }
    for (int i = 0; i < P.rows(); i++) {

        Eigen::Vector3d p = P.row(i);
        Eigen::Vector3i ip = get_nn_cell_index(p);
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                for (int dz = -1; dz <= 1; dz++) {
                    Eigen::Vector3i cell_idx = ip + Eigen::Vector3i(dx, dy, dz);
                    if ((cell_idx.array() >= Eigen::Array3i::Zero()).all() &&
                        (cell_idx.array() < nn_grid_size.array()).all()) {
                        auto &cell = grid[get_nn_linear_index(cell_idx)];
                        for (auto &j : cell.particles) {
                            if (i == j)
                                continue;
                            Eigen::Vector3d q = P.row(j);
                            if ((p - q).norm() < 2 * h) {
                                neighbors[i].push_back(j);
                            }
                        }
                    }
                }
            }
        }
    }
    CHECK(neighbors.size() == P.rows());
    // for (int i = 0; i < P.rows(); i++) {
    tbb::parallel_for<int>(0, P.rows(), [&](int i) {
        Eigen::Vector3d xi = P.row(i);
        Eigen::Vector3d xiw(0, 0, 0);
        Eigen::Matrix3d C;
        Eigen::Vector3d x_bar(0.0, 0.0, 0.0);
        const auto lambda = 0.92;
        C.setZero();
        if (!neighbors.empty()) {
            double sum_w = 0.0;

            for (auto &j : neighbors[i]) {
                CHECK(j < P.rows());
                Eigen::Vector3d xj = P.row(j);
                const auto r = 2 * h;
                auto w = 0.0;
                if ((xi - xj).norm() <= r && (xi - xj).norm() > 0.0) {
                    w = 1.0 - std::pow((xi - xj).norm() / r, 3);
                }
                sum_w += w;
                xiw += w * xj;
            }
            xiw /= sum_w;
            x_bar = (1.0 - lambda) * xi + lambda * xiw;
            X.row(i) = x_bar;
        } else {
            X.row(i) = xi;
            xiw = xi;
        }
        {
            double sum_w = 0.0;
            for (auto &j : neighbors[i]) {
                Eigen::Vector3d xj = P.row(j);
                const auto r = 2 * h;
                auto w = 0.0;
                if ((xi - xj).norm() <= r && (xi - xj).norm() > 0.0) {
                    w = 1.0 - std::pow((xi - xj).norm() / r, 3);
                }
                sum_w += w;
                C += w * (xj - xiw) * (xj - xiw).transpose();
            }
            // std::cout << C << std::endl;
            C /= sum_w;
        }
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(C, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d R = svd.matrixU();
        Eigen::Vector3d sigmas = svd.singularValues();
        const auto kr = 4.0;
        const auto ks = 1400.0;
        const auto kn = 0.5;
        const int Ne = 25;
        double sigma1 = sigmas[0];
        for (int i = 0; i < 3; i++) {
            sigmas[i] = std::fmax(sigmas[i], sigma1 / kr);
        }
        auto N = (int)neighbors[i].size();
        // std::cout << "N: " << N << std::endl;
        Eigen::DiagonalMatrix<double, 3> Sigma;
        if (N < Ne) {
            Sigma = kn * Eigen::Vector3d::Ones().asDiagonal();
        } else {
            Sigma = ks * sigmas.asDiagonal();
        }
        G[i] = 1.0 / h * R * Sigma.inverse() * R.transpose();
        // G[i] = 1.0 / h * Eigen::Matrix3d::Identity();
        // std::cout << G[i] << std::endl;
    });
    for (auto &cell : grid) {
        cell.particles.clear();
    }
    for (int i = 0; i < P.rows(); i++) {
        Eigen::Vector3i ip = get_nn_cell_index(P.row(i));
        auto idx = get_nn_linear_index(ip);
        CHECK(idx < grid.size());
        grid[idx].particles.push_back(i);
    }
    neighbors.clear();
    neighbors.resize(GV.rows());
    // for (int i = 0; i < GV.rows(); i++) {
    tbb::parallel_for<int>(0, GV.rows(), [&](int i) {
        // int x = i % res[0];
        // int y = (i % (res[0] * res[1])) / res[0];
        // int z = i / (res[0] * res[1]);
        // Eigen::Vector3i gv_p(x, y, z);
        Eigen::Vector3d p = GV.row(i);
        Eigen::Vector3i ip = get_nn_cell_index(p);
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                for (int dz = -1; dz <= 1; dz++) {
                    Eigen::Vector3i cell_idx = ip + Eigen::Vector3i(dx, dy, dz);
                    if ((cell_idx.array() >= Eigen::Array3i::Zero()).all() &&
                        (cell_idx.array() < nn_grid_size.array()).all()) {
                        auto &cell = grid[get_nn_linear_index(cell_idx)];
                        for (auto &j : cell.particles) {
                            Eigen::Vector3d q = X.row(j);
                            if ((p - q).norm() < 2 * h) {
                                neighbors[i].push_back(j);
                            }
                        }
                    }
                }
            }
        }
    });

    // for (int i = 0; i < GV.rows(); i++) {
    tbb::parallel_for<int>(0, GV.rows(), [&](int i) {
        const double r = 2 * h;
        // Eigen::Vector3i ip;
        // {
        //     int x = i % res[0];
        //     int y = (i % (res[0] * res[1])) / res[0];
        //     int z = i / (res[0] * res[1]);
        //     ip = Eigen::Vector3i(x, y, z);
        // }
        Eigen::Vector3d x = GV.row(i);
        double s = 0.0;
        for (auto j : neighbors[i]) {
            Eigen::Vector3d xj = X.row(j);
            Eigen::Vector3d r = x - xj;
            auto W = W_k / (h * h) * G[j].norm() * P1((G[j] * r).norm());
            // std::cout << (G[j] * r).norm() << std::endl;
            // std::cout << (r.norm() / h) << std::endl;
            s += mass[j] / density[j] * W;
        }
        S[i] = s;

        // double s = 0.0;
        // for (auto j : neighbors[i]) {
        //     Eigen::Vector3d xj = P.row(j);
        //     Eigen::Vector3d r = x - xj;
        //     auto W = W_k / (h * h * h) * P1(r.norm() / h);
        //     s += mass[j] / density[j] * W;
        // }

        // S[i] = s;
    });
    // std::cout << S << std::endl;
    printf("S.mean() = %f\n", S.mean());

    marching_cubes(S, GV, res[0], res[1], res[2], isovalue, V, F);
}


#include <iostream>
#include <armadillo>
#include <vector>
#include <cstdlib>
#include <fstream>

#include "world_rep.h"

int main(int argc, const char **argv)
{
    // Values for single neuron.
    int snn_id = 2;
    int n_inputs = 2;
    int n_lateral = 1;
    std::vector<double> init_d({2, 4});
    std::vector<double> init_w({4});
    double tau_m = 2;
    double u_rest = 0;
    double init_v = 20; // when testing math, make this large
    unsigned char t_rest = 2;
    double kappa_naugh = 3;
    double round_zero = 0.1;
    double u_max = 10;

    SpikeResponseModelNeuron single_neuron = SpikeResponseModelNeuron(
        snn_id, n_inputs, init_d, tau_m, u_rest,
        init_v, t_rest, kappa_naugh, round_zero, u_max);
    std::cout << "Created individual neuron." << std::endl;

    std::vector<std::vector<bool>> presynaptic_train = {
        {0, 0, 1, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}
    };

    std::ofstream exp_res;
    exp_res.open("experiment_results.log");
    exp_res << "[";
    for(unsigned int epoch = 0; epoch < presynaptic_train.at(0).size(); epoch++)
    {
        exp_res << single_neuron.membrane_potential() << ", ";
        // load dendrites with current value
        single_neuron.dendrite.at(0) = DelayedSpike(0, presynaptic_train.at(0).at(epoch));
        single_neuron.dendrite.at(1) = DelayedSpike(0, presynaptic_train.at(1).at(epoch));

        // Move network forward
        single_neuron.t_pulse();

    }
    exp_res << "]" << std::endl;

    printf("Testing FSTNs\n");
    double alpha = 1;
    FirstSpikeTimeNeuron fstn = FirstSpikeTimeNeuron(0, alpha);
    fstn.dendrite = 2;
    fstn.encode();

    unsigned char total_pulses = 10;

    exp_res << "First Spike Time Neuron spike after " << total_pulses << "pulses." << std::endl << "[";
    exp_res << fstn.axon.signal << ", ";
    for(unsigned char pulse= 0; pulse < total_pulses; pulse++)
    {
        fstn.t_pulse();
        exp_res << fstn.axon.signal << ", ";
    }
    exp_res << "]" << std::endl;

    exp_res.close();

    // Testing training algorthm
    unsigned int n_data = 2;
    tau_m = 0.8;
    u_rest = 0;
    init_v = 5;
    unsigned int t_reset = 3;
    double k_nought = 3;
    round_zero = 0.05;
    alpha = 1;
    // note that n_x * n_y = h_layer_size
    unsigned int n_x = 8;
    unsigned int n_y = 8;
    double delay_distance = 0.2;
    unsigned int distance_unit = 1;
    double sigma_neighbor = 1;
    double eta_d = 1.7;
    unsigned int t_max = 25;
    u_max = 10;

    printf("Create SNN\n");
    SNN model(n_data, tau_m, u_rest, init_v,
    t_reset, k_nought, round_zero, alpha, n_x, n_y, delay_distance,
    distance_unit, sigma_neighbor, eta_d, t_max, u_max);

    std::vector<std::vector<double>> data = {
        {0, 0, 0, 1, 2, 2, 3, 3, 3, 3, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 2, 2, 2, 3, 3, 4, 5, 6, 7, 7, 7, 7, 6, 5, 4, 3, 2, 1, 0}
    };
    std::vector<std::vector<double>> data_2 = {
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 2, 2, 1, 1, 1, 1},
    };

    printf("Create output file\n");

    // save delays into file:
    std::ofstream delay_file;
    delay_file.open("training_delays.txt");

    // print out delays
    delay_file << "Delays before training:" << std::endl;
    for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
    {
        delay_file << "[ ";
        for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
        {
            delay_file << model.snn->d_ji.at(i).at(j) << ", ";
        }
        delay_file << "]" << std::endl;
    }

    for(unsigned int p = 0; p < 1; p++)
        model.train(data);

    delay_file << std::endl << "Delays after training:"<< std::endl;
     for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
    {
        delay_file << "[ ";
        for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
        {
            delay_file << model.snn->d_ji.at(i).at(j) << ", ";
        }
        delay_file << "]" << std::endl;
    }

    for(unsigned int p = 0; p < 8; p++, sigma_neighbor -= 0.1)
    {
        model = SNN(n_data, tau_m, u_rest, init_v,
        t_reset, k_nought, round_zero, alpha, n_x, n_y, delay_distance,
        distance_unit, sigma_neighbor, eta_d, t_max, u_max);

        model.train(data);

        delay_file << std::endl << "Delays after training with sigma_neighbor = " << sigma_neighbor << ": \n" << std::endl;
        for(unsigned int i = 0; i < model.snn->d_ji.size(); i++)
        {
            delay_file << "[ ";
            for(unsigned int j = 0; j < model.snn->d_ji.at(i).size(); j++)
            {
                delay_file << model.snn->d_ji.at(i).at(j) << ", ";
            }
            delay_file << "]" << std::endl;
        }
    }

    printf("test world representation and planner.\n");

    sigma_neighbor = 0.3;

    std::ofstream planner_file;
    planner_file.open("world_representation.txt");

    double prune_dist = 0.1;

    WorldRep planner = WorldRep(k_nought, tau_m, init_v, round_zero,
    n_x, n_y, delay_distance, sigma_neighbor, eta_d, t_max, u_max,
    prune_dist);

    planner.train(data);

    planner_file << std::endl << "Delays after training:"<< std::endl;
    for(unsigned int i = 0; i < planner.snn.snn->d_ji.size(); i++)
    {
        planner_file << "[ ";
        for(unsigned int j = 0; j < planner.snn.snn->d_ji.at(i).size(); j++)
        {
            planner_file << planner.snn.snn->d_ji.at(i).at(j) << ", ";
        }
        planner_file << "]" << std::endl;
    }

    std::vector<std::vector<double>> world_rep = planner.get_map();

    planner_file << std::endl << "Delays after training with sigma_neighbor = " << sigma_neighbor << " and prunning with value:" << prune_dist << std::endl;

    for(unsigned int i = 0; i < world_rep.size(); i++)
    {
        planner_file << "[ ";
        for(unsigned int j = 0; j < world_rep.at(i).size(); j++)
        {
            planner_file << world_rep.at(i).at(j) << ", ";
        }
        planner_file << "]" << std::endl;
    }

    planner.dijkstra(28, 409);
    std::vector<double> src_coor, goal_coor;
    src_coor = std::vector<double>({0, 0});
    goal_coor = std::vector<double>({0, 7});
    std::vector<std::vector<double>> path = planner.get_path(WorldRep::PathAlgorithm::dijkstras, src_coor, goal_coor);
    printf("[");
    for(unsigned int x = 0; x < path.at(0).size(); x++)
    {
        printf("[%f, %f], ", path.at(0).at(x), path.at(1).at(x));
    }
    printf("]\n");
    planner_file.close();
}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <boost/math/special_functions/log1p.hpp>
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline ClaytonBicop::ClaytonBicop()
{
  family_ = BicopFamily::clayton;
  parameters_ = Eigen::VectorXd(1);
  parameters_lower_bounds_ = Eigen::VectorXd(1);
  parameters_upper_bounds_ = Eigen::VectorXd(1);
  parameters_ << 1e-10;
  parameters_lower_bounds_ << 1e-10;
  parameters_upper_bounds_ << 28;
}

inline double
ClaytonBicop::generator(const double& u)
{
  double theta = double(this->parameters_(0));
  return (std::pow(u, -theta) - 1) / theta;
}

inline double
ClaytonBicop::generator_inv(const double& u)
{
  double theta = double(this->parameters_(0));
  return std::pow(1 + theta * u, -1 / theta);
}

inline double
ClaytonBicop::generator_derivative(const double& u)
{
  return (-1) * std::pow(u, -1 - this->parameters_(0));
}

// inline double ClaytonBicop::generator_derivative2(const double &u)
//{
//    double theta = double(this->parameters_(0));
//    return (1 + theta) * std::pow(u, -2 - theta);
//}

inline Eigen::VectorXd
ClaytonBicop::pdf_raw(const Eigen::MatrixXd& u)
{
  double theta = static_cast<double>(parameters_(0));
  // avoid numerical issues when copula is too close to independence
  if (theta < 1e-10) {
    auto f = [](const double&, const double&) { return 1.0; };
    return tools_eigen::binaryExpr_or_nan(u, f);
  }

  auto f = [theta](const double& u1, const double& u2) {
    double temp = boost::math::log1p(theta) - (1.0 + theta) * std::log(u1 * u2);
    temp = temp - (2.0 + 1.0 / (theta)) *
                    std::log(std::pow(u1, -theta) + std::pow(u2, -theta) - 1.0);
    return std::exp(temp);
  };
  return tools_eigen::binaryExpr_or_nan(u, f);
}

inline Eigen::VectorXd
ClaytonBicop::hinv1_raw(const Eigen::MatrixXd& u)
{
  double theta = double(this->parameters_(0));
  Eigen::VectorXd hinv = u.col(0).array().pow(theta + 1.0);
  if (theta < 75) {
    hinv = u.col(1).cwiseProduct(hinv);
    hinv = hinv.array().pow(-theta / (theta + 1.0));
    Eigen::VectorXd x = u.col(0);
    x = x.array().pow(-theta);
    hinv = hinv - x + Eigen::VectorXd::Ones(x.size());
    hinv = hinv.array().pow(-1 / theta);
  } else {
    hinv = hinv1_num(u);
  }
  return hinv;
}

inline Eigen::MatrixXd
ClaytonBicop::tau_to_parameters(const double& tau)
{
  Eigen::VectorXd parameters(1);
  parameters(0) = 2 * std::fabs(tau) / (1 - std::fabs(tau));
  return parameters.cwiseMax(parameters_lower_bounds_)
    .cwiseMin(parameters_upper_bounds_);
}

inline double
ClaytonBicop::parameters_to_tau(const Eigen::MatrixXd& parameters)
{
  return parameters(0) / (2 + std::fabs(parameters(0)));
}

inline Eigen::VectorXd
ClaytonBicop::get_start_parameters(const double tau)
{
  Eigen::VectorXd par = tau_to_parameters(tau);
  par = par.cwiseMax(parameters_lower_bounds_);
  par = par.cwiseMin(parameters_upper_bounds_);
  return par;
}
}


#include <armadillo>

// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <utility>
namespace pt = boost::property_tree;

#include "ReflectionCoefficientsLocSlab.h"

// direct constructor
ReflectionCoefficientsLocSlab::ReflectionCoefficientsLocSlab(
    std::shared_ptr<Permittivity> permittivity, double thickness)
    : permittivity(std::move(permittivity)), thickness(thickness) {}

// constructor from .json file
ReflectionCoefficientsLocSlab::ReflectionCoefficientsLocSlab(
    const std::string &input_file) {
  // set permittivity
  // set parameters
  this->permittivity = PermittivityFactory::create(input_file);
  // Create a root
  pt::ptree root;

  // Load the json file in this ptree
  pt::read_json(input_file, root);

  // read parameters
  this->thickness = root.get<double>("ReflectionCoefficients.thickness");
}

// calculate the p-polarized reflection coefficient
void ReflectionCoefficientsLocSlab::calculate(double omega,
                                              std::complex<double> kappa,
                                              std::complex<double> &r_p,
                                              std::complex<double> &r_s) const {
  // absolute value of omega. r_p is always calculated for positive omega and if
  // needed complex conjugated after the calculation
  double omega_abs = std::abs(omega);
  std::complex<double> eps = this->permittivity->calculate(omega_abs);
  std::complex<double> eps_omega = this->permittivity->calculate_times_omega(omega_abs);
  std::complex<double> I(0., 1.);

  // kapppa as well as kappa_epsilon are defined to have either a purely
  // positive real part or purely negatively imaginary part
  std::complex<double> kappa_epsilon =
      sqrt(kappa * kappa - (eps - 1.) * omega_abs * omega_abs);
  kappa_epsilon = std::complex<double>(std::abs(kappa_epsilon.real()),
                                       -std::abs(kappa_epsilon.imag()));
  // Defining the reflection coefficients in transverse magnetice polarization
  // (p) and in transverse electric polarization (s)
  std::complex<double> r_p_bulk =
      (kappa * eps_omega - kappa_epsilon * omega_abs) / (kappa * eps_omega + kappa_epsilon * omega_abs);
  std::complex<double> r_s_bulk =
      (kappa - kappa_epsilon) / (kappa + kappa_epsilon);

  r_p = r_p_bulk * (1. - exp(-2. * kappa_epsilon * this->thickness)) /
        (1. - pow(r_p_bulk * exp(-kappa_epsilon * this->thickness), 2));
  r_s = r_s_bulk * (1. - exp(-2. * kappa_epsilon * this->thickness)) /
        (1. - pow(r_s_bulk * exp(-kappa_epsilon * this->thickness), 2));

  // Imposing crossing relation
  if (omega < 0.) {
    r_p = conj(r_p);
    r_s = conj(r_s);
  }
}
void ReflectionCoefficientsLocSlab::print_info(std::ostream &stream) const {
  stream << "# ReflectionCoefficientsLocSlab\n#\n"
         << "# thickness = " << thickness << "\n";
  permittivity->print_info(stream);
}


/*
 * PolynomialSplineContainer.hpp
 *
 *  Created on: Dec 8, 2014
 *      Author: C. Dario Bellicoso, Peter Fankhauser
 */

#pragma once

#include "curves/polynomial_splines.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <limits>

namespace curves {

class PolynomialSplineContainer {
 public:

  using SplineType = PolynomialSplineQuintic;
  using SplineList = std::vector<SplineType>;

  PolynomialSplineContainer();
  virtual ~PolynomialSplineContainer();

  bool advance(double dt);
  bool addSpline(const SplineType& spline);
  bool addSpline(SplineType&& spline);
  bool reset();
  bool resetTime();

  double getContainerDuration() const;

  double getPosition() const;
  double getVelocity() const;
  double getAcceleration() const;

  double getPositionAtTime(double t) const;
  double getVelocityAtTime(double t) const;
  double getAccelerationAtTime(double t) const;

  double getEndPosition() const;
  double getEndVelocity() const;
  double getEndAcceleration() const;

  double getContainerTime() const;

  int getActiveSplineIndex() const;
  int getActiveSplineIndexAtTime(double t, double& timeOffset) const;
  bool isEmpty() const;

  virtual void setData(const std::vector<double>& knotPositions,
                       const std::vector<double>& knotValues,
                       double initialVelocity,
                       double initialAcceleration,
                       double finalVelocity,
                       double finalAcceleration);

  SplineType* getSpline(int splineIndex);

  void setContainerTime(double t);

  const SplineList& getSplines() const;

  static constexpr double undefinedValue = std::numeric_limits<double>::quiet_NaN();

 protected:
  int getCoeffIndex(int splineIdx, int aIdx) const;
  int getSplineColumnIndex(int splineIdx) const;

  SplineList splines_;
  double timeOffset_;
  double containerTime_;
  double containerDuration_;
  int activeSplineIdx_;
};

} /* namespace */


//
// Implements iLQR (on a traditional chain) for nonlinear dynamics and cost.
//
// Arun Venkatraman (arunvenk@cs.cmu.edu)
// December 2016
//

#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>

#include <Eigen/Dense>

#include <tuple>
#include <vector>

namespace ilqr
{

class iLQR
{
public:
    // Stores linearization points x, u and the Taylor expansions of the dynamics and cost.
    using TaylorExpansion = std::tuple<Eigen::VectorXd, Eigen::VectorXd, ilqr::Dynamics, ilqr::Cost>;

    iLQR(const DynamicsFunc &dynamics, const CostFunc &cost,
            const std::vector<Eigen::VectorXd> &Xs,
            const std::vector<Eigen::VectorXd> &Us);

    void backwards_pass();

    void forward_pass(std::vector<double> &costs,
            std::vector<Eigen::VectorXd> &states,
            std::vector<Eigen::VectorXd> &controls,
            bool update_linearizations
            );

    std::vector<Eigen::VectorXd> states();
    std::vector<Eigen::VectorXd> controls();

private:
    int state_dim_ = -1;
    int control_dim_  = -1;
    int T_ = -1; // time horizon

    DynamicsFunc true_dynamics_;
    CostFunc true_cost_;

    // Taylor series expansion points and expanded dynamics, cost.
    std::vector<TaylorExpansion> expansions_;

    // Feedback control gains.
    std::vector<Eigen::MatrixXd> Ks_;
    std::vector<Eigen::VectorXd> ks_;
};

} // namespace lqr


#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>

//____________________________________________________________________________//

#include <cmath>

BOOST_AUTO_TEST_CASE( test )
{
    double res = std::sin( 45. );

    BOOST_WARN_MESSAGE( res > 1, "sin(45){" << res << "} is <= 1. Hmm.. Strange. " );
}

//____________________________________________________________________________//


/*
# Copyright 2018 HyphaROS Workshop.
# Developer: HaoChih, LIN (hypha.ros@gmail.com)
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
*/

#include "MPC_last.h"
//#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <Eigen/Core>

// The program use fragments of code from
// https://github.com/udacity/CarND-MPC-Quizzes

using CppAD::AD;

// =========================================
// FG_eval class definition implementation.
// =========================================
class FG_eval
{
    public:
        // Fitted polynomial coefficients
        Eigen::VectorXd coeffs;

        double _dt, _ref_cte, _ref_etheta, _ref_vel;
        double  _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, _w_accel_d;
        int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start;

        AD<double> cost_cte, cost_etheta, cost_vel;
        // Constructor
        FG_eval(Eigen::VectorXd coeffs)
        {
            this->coeffs = coeffs;

            // Set default value
            _dt = 0.1;  // in sec
            _ref_cte   = 0;
            _ref_etheta  = 0;
            _ref_vel   = 0.5; // m/s
            _w_cte     = 100;
            _w_etheta    = 100;
            _w_vel     = 1;
            _w_angvel   = 100;
            _w_accel   = 50;
            _w_angvel_d = 0;
            _w_accel_d = 0;

            _mpc_steps   = 40;
            _x_start     = 0;
            _y_start     = _x_start + _mpc_steps;
            _theta_start   = _y_start + _mpc_steps;
            _v_start     = _theta_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _etheta_start  = _cte_start + _mpc_steps;
            _angvel_start = _etheta_start + _mpc_steps;
            _a_start     = _angvel_start + _mpc_steps - 1;
        }

        // Load parameters for constraints
        void LoadParams(const std::map<string, double> &params)
        {
            _dt = params.find("DT") != params.end() ? params.at("DT") : _dt;
            _mpc_steps = params.find("STEPS") != params.end()    ? params.at("STEPS") : _mpc_steps;
            _ref_cte   = params.find("REF_CTE") != params.end()  ? params.at("REF_CTE") : _ref_cte;
            _ref_etheta  = params.find("REF_ETHETA") != params.end() ? params.at("REF_ETHETA") : _ref_etheta;
            _ref_vel   = params.find("REF_V") != params.end()    ? params.at("REF_V") : _ref_vel;

            _w_cte   = params.find("W_CTE") != params.end()   ? params.at("W_CTE") : _w_cte;
            _w_etheta  = params.find("W_EPSI") != params.end()  ? params.at("W_EPSI") : _w_etheta;
            _w_vel   = params.find("W_V") != params.end()     ? params.at("W_V") : _w_vel;
            _w_angvel = params.find("W_ANGVEL") != params.end() ? params.at("W_ANGVEL") : _w_angvel;
            _w_accel = params.find("W_A") != params.end()     ? params.at("W_A") : _w_accel;
            _w_angvel_d = params.find("W_DANGVEL") != params.end() ? params.at("W_DANGVEL") : _w_angvel_d;
            _w_accel_d = params.find("W_DA") != params.end()     ? params.at("W_DA") : _w_accel_d;

            _x_start     = 0;
            _y_start     = _x_start + _mpc_steps;
            _theta_start   = _y_start + _mpc_steps;
            _v_start     = _theta_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _etheta_start  = _cte_start + _mpc_steps;
            _angvel_start = _etheta_start + _mpc_steps;
            _a_start     = _angvel_start + _mpc_steps - 1;

            //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl;
        }

        // MPC implementation (cost func & constraints)
        typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
        // fg: function that evaluates the objective and constraints using the syntax
        void operator()(ADvector& fg, const ADvector& vars)
        {
            // fg[0] for cost function
            fg[0] = 0;
            cost_cte =  0;
            cost_etheta = 0;
            cost_vel = 0;

            /*
            for (int i = 0; i < _mpc_steps; i++)
            {
                cout << i << endl;
                cout << "_x_start" << vars[_x_start + i] <<endl;
                cout << "_y_start" << vars[_y_start + i] <<endl;
                cout << "_theta_start" << vars[_theta_start + i] <<endl;
                cout << "_v_start" << vars[_v_start + i] <<endl;
                cout << "_cte_start" << vars[_cte_start + i] <<endl;
                cout << "_etheta_start" << vars[_etheta_start + i] <<endl;
            }*/

            for (int i = 0; i < _mpc_steps; i++)
            {
              fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error
              fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error
              fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error

              cost_cte +=  _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2);
              cost_etheta +=  (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2));
              cost_vel +=  (_w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2));
            }
            cout << "-----------------------------------------------" <<endl;
            cout << "cost_cte, etheta, velocity: " << cost_cte << ", " << cost_etheta  << ", " << cost_vel << endl;

            // Minimize the use of actuators.
            for (int i = 0; i < _mpc_steps - 1; i++) {
              fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2);
              fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2);
            }
            cout << "cost of actuators: " << fg[0] << endl;

            // Minimize the value gap between sequential actuations.
            for (int i = 0; i < _mpc_steps - 2; i++) {
              fg[0] += _w_angvel_d * CppAD::pow(vars[_angvel_start + i + 1] - vars[_angvel_start + i], 2);
              fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2);
            }
            cout << "cost of gap: " << fg[0] << endl;

            // fg[x] for constraints
            // Initial constraints
            fg[1 + _x_start] = vars[_x_start];
            fg[1 + _y_start] = vars[_y_start];
            fg[1 + _theta_start] = vars[_theta_start];
            fg[1 + _v_start] = vars[_v_start];
            fg[1 + _cte_start] = vars[_cte_start];
            fg[1 + _etheta_start] = vars[_etheta_start];

            // Add system dynamic model constraint
            for (int i = 0; i < _mpc_steps - 1; i++)
            {
                // The state at time t+1 .
                AD<double> x1 = vars[_x_start + i + 1];
                AD<double> y1 = vars[_y_start + i + 1];
                AD<double> theta1 = vars[_theta_start + i + 1];
                AD<double> v1 = vars[_v_start + i + 1];
                AD<double> cte1 = vars[_cte_start + i + 1];
                AD<double> etheta1 = vars[_etheta_start + i + 1];

                // The state at time t.
                AD<double> x0 = vars[_x_start + i];
                AD<double> y0 = vars[_y_start + i];
                AD<double> theta0 = vars[_theta_start + i];
                AD<double> v0 = vars[_v_start + i];
                AD<double> cte0 = vars[_cte_start + i];
                AD<double> etheta0 = vars[_etheta_start + i];

                // Only consider the actuation at time t.
                //AD<double> angvel0 = vars[_angvel_start + i];
                AD<double> w0 = vars[_angvel_start + i];
                AD<double> a0 = vars[_a_start + i];

                //AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
                AD<double> f0 = 0.0;
                for (int i = 0; i < coeffs.size(); i++)
                {
                    f0 += coeffs[i] * CppAD::pow(x0, i); //f(0) = y
                }

                //AD<double> trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
                AD<double> trj_grad0 = 0.0;
                for (int i = 1; i < coeffs.size(); i++)
                {
                    trj_grad0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) = f(1)/1
                }
                trj_grad0 = CppAD::atan(trj_grad0);

                // Here's `x` to get you started.
                // The idea here is to constraint this value to be 0.
                //
                // NOTE: The use of `AD<double>` and use of `CppAD`!
                // This is also CppAD can compute derivatives and pass
                // these to the solver.
                // TODO: Setup the rest of the model constraints
                fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt);
                fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt);
                fg[2 + _theta_start + i] = theta1 - (theta0 +  w0 * _dt);
                fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt);

                fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt));
                //fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt);//theta0-trj_grad0)->etheta : it can have more curvature prediction, but its gradient can be only adjust positive plan.
                fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt);
            }
        }
};

// ====================================
// MPC class definition implementation.
// ====================================
MPC::MPC()
{
    // Set default value
    _mpc_steps = 20;
    _max_angvel = 3.0; // Maximal angvel radian (~30 deg)
    _max_throttle = 1.0; // Maximal throttle accel
    _bound_value  = 1.0e3; // Bound value for other variables

    _x_start     = 0;
    _y_start     = _x_start + _mpc_steps;
    _theta_start   = _y_start + _mpc_steps;
    _v_start     = _theta_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _etheta_start  = _cte_start + _mpc_steps;
    _angvel_start = _etheta_start + _mpc_steps;
    _a_start     = _angvel_start + _mpc_steps - 1;

}

void MPC::LoadParams(const std::map<string, double> &params)
{
    _params = params;
    //Init parameters for MPC object
    _mpc_steps = _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps;
    _max_angvel = _params.find("ANGVEL") != _params.end() ? _params.at("ANGVEL") : _max_angvel;
    _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle;
    _bound_value  = _params.find("BOUND") != _params.end()  ? _params.at("BOUND") : _bound_value;

    _x_start     = 0;
    _y_start     = _x_start + _mpc_steps;
    _theta_start   = _y_start + _mpc_steps;
    _v_start     = _theta_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _etheta_start  = _cte_start + _mpc_steps;
    _angvel_start = _etheta_start + _mpc_steps;
    _a_start     = _angvel_start + _mpc_steps - 1;

    cout << "\n!! MPC Obj parameters updated !! " << endl;
}

vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs)
{
    bool ok = true;
    size_t i;
    typedef CPPAD_TESTVECTOR(double) Dvector;
    const double x = state[0];
    const double y = state[1];
    const double theta = state[2];
    const double v = state[3];
    const double cte = state[4];
    const double etheta = state[5];

    // Set the number of model variables (includes both states and inputs).
    // For example: If the state is a 4 element vector, the actuators is a 2
    // element vector and there are 10 timesteps. The number of variables is:
    // 4 * 10 + 2 * 9
    size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2;

    // Set the number of constraints
    size_t n_constraints = _mpc_steps * 6;

    // Initial value of the independent variables.
    // SHOULD BE 0 besides initial state.
    Dvector vars(n_vars);
    for (int i = 0; i < n_vars; i++)
    {
        vars[i] = 0;
    }

    // Set the initial variable values
    vars[_x_start] = x;
    vars[_y_start] = y;
    vars[_theta_start] = theta;
    vars[_v_start] = v;
    vars[_cte_start] = cte;
    vars[_etheta_start] = etheta;

    // Set lower and upper limits for variables.
    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);

    // Set all non-actuators upper and lowerlimits
    // to the max negative and positive values.
    for (int i = 0; i < _angvel_start; i++)
    {
        vars_lowerbound[i] = -_bound_value;
        vars_upperbound[i] = _bound_value;
    }
    // The upper and lower limits of angvel are set to -25 and 25
    // degrees (values in radians).
    for (int i = _angvel_start; i < _a_start; i++)
    {
        vars_lowerbound[i] = -_max_angvel;
        vars_upperbound[i] = _max_angvel;
    }
    // Acceleration/decceleration upper and lower limits
    for (int i = _a_start; i < n_vars; i++)
    {
        vars_lowerbound[i] = -_max_throttle;
        vars_upperbound[i] = _max_throttle;
    }

    // Lower and upper limits for the constraints
    // Should be 0 besides initial state.
    Dvector constraints_lowerbound(n_constraints);
    Dvector constraints_upperbound(n_constraints);
    for (int i = 0; i < n_constraints; i++)
    {
        constraints_lowerbound[i] = 0;
        constraints_upperbound[i] = 0;
    }
    constraints_lowerbound[_x_start] = x;
    constraints_lowerbound[_y_start] = y;
    constraints_lowerbound[_theta_start] = theta;
    constraints_lowerbound[_v_start] = v;
    constraints_lowerbound[_cte_start] = cte;
    constraints_lowerbound[_etheta_start] = etheta;
    constraints_upperbound[_x_start] = x;
    constraints_upperbound[_y_start] = y;
    constraints_upperbound[_theta_start] = theta;
    constraints_upperbound[_v_start] = v;
    constraints_upperbound[_cte_start] = cte;
    constraints_upperbound[_etheta_start] = etheta;

    // object that computes objective and constraints
    FG_eval fg_eval(coeffs);
    fg_eval.LoadParams(_params);

    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
      constraints_upperbound, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // Cost
    auto cost = solution.obj_value;
    std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl;
    cout << "-----------------------------------------------" <<endl;

    this->mpc_x = {};
    this->mpc_y = {};
    this->mpc_theta = {};
    for (int i = 0; i < _mpc_steps; i++)
    {
        this->mpc_x.push_back(solution.x[_x_start + i]);
        this->mpc_y.push_back(solution.x[_y_start + i]);
        this->mpc_theta.push_back(solution.x[_theta_start + i]);
    }

    vector<double> result;
    result.push_back(solution.x[_angvel_start]);
    result.push_back(solution.x[_a_start]);
    return result;
}


#include <iostream>
#include <boost/graph/adjacency_list.hpp>

using namespace std;
using namespace boost;
typedef boost::adjacency_list<listS, vecS, undirectedS> graphAL;

int main() {
  graphAL g;
  add_edge(0, 1, g);
  add_edge(0, 3, g);
  add_edge(1, 2, g);
  add_edge(2, 3, g);
  graphAL::vertex_iterator vertexIt, vertexEnd;
  graphAL::adjacency_iterator neighbourIt, neighbourEnd;
  tie(vertexIt, vertexEnd) = vertices(g);
  for (; vertexIt != vertexEnd; ++vertexIt) {
    cout << *vertexIt << " is connected with ";
    tie(neighbourIt, neighbourEnd) = adjacent_vertices(*vertexIt, g);
    for (; neighbourIt != neighbourEnd; ++neighbourIt) cout << *neighbourIt << " ";
    cout << "\n";
  }
}


/***************************************************************************
 *   Copyright (c) 2011 Konstantinos Poulios <logari81@gmail.com>          *
 *                                                                         *
 *   This file is part of the FreeCAD CAx development system.              *
 *                                                                         *
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Library General Public           *
 *   License as published by the Free Software Foundation; either          *
 *   version 2 of the License, or (at your option) any later version.      *
 *                                                                         *
 *   This library  is distributed in the hope that it will be useful,      *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU Library General Public License for more details.                  *
 *                                                                         *
 *   You should have received a copy of the GNU Library General Public     *
 *   License along with this library; see the file COPYING.LIB. If not,    *
 *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
 *   Suite 330, Boston, MA  02111-1307, USA                                *
 *                                                                         *
 ***************************************************************************/

#ifdef _MSC_VER
#pragma warning(disable : 4244)
#endif

#include <iostream>
#include <Eigen/QR>

using namespace Eigen;

// minimizes ( 0.5 * x^T * H * x + g^T * x ) under the condition ( A*x + c = 0 )
// it returns the solution in x, the row-space of A in Y, and the null space of A in Z
int qp_eq(MatrixXd &H, VectorXd &g, MatrixXd &A, VectorXd &c,
          VectorXd &x, MatrixXd &Y, MatrixXd &Z)
{
    FullPivHouseholderQR<MatrixXd> qrAT(A.transpose());
    MatrixXd Q = qrAT.matrixQ ();

    size_t params_num = qrAT.rows();
    size_t constr_num = qrAT.cols();
    size_t rank = qrAT.rank();

    if (rank != constr_num || constr_num > params_num)
        return -1;

    // A^T = Q*R*P^T = Q1*R1*P^T
    // Q = [Q1,Q2], R=[R1;0]
    // Y = Q1 * inv(R^T) * P^T
    // Z = Q2
    Y = qrAT.matrixQR().topRows(constr_num)
                       .triangularView<Upper>()
                       .transpose()
                       .solve<OnTheRight>(Q.leftCols(rank))
        * qrAT.colsPermutation().transpose();
    if (params_num == rank)
        x = - Y * c;
    else {
        Z = Q.rightCols(params_num-rank);

        MatrixXd ZTHZ = Z.transpose() * H * Z;
        VectorXd rhs = Z.transpose() * (H * Y * c - g);

        VectorXd y = ZTHZ.colPivHouseholderQr().solve(rhs);

        x = - Y * c + Z * y;
    }

    return 0;
}


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_FRAC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FRAC_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/if_allbits_else.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(frac_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0&)const
      {
        return Zero<A0>();
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(frac_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return if_allbits_else(is_invalid(a0), a0-bs::trunc(a0));
      }
   };

} } }
#endif


//
// Created by cheyulin on 7/10/16.
//

#define some_int 1

struct some_type;
//编写元函数就像是编写一个普通的运行函数，但形式上确是一个模板类
template<typename arg1, typename arg2> // 元函数参数列表
struct meta_function {
    typedef some_type type;  //type 是 元数据  值为 some_type
    //using type = some_type
    static int const value = some_int;
};

#include <boost/config.hpp>
#include <iostream>

template<int N, int M>  //两个整数元数据
struct meta_function1 {
    BOOST_STATIC_CONSTANT(int, value = N + M); //编译器计算整数之和
};

int main() {
    using namespace std;
    cout << meta_function1<10, 10>::value << endl;
}


#include <NTL/quad_float.h>
#include <NTL/RR.h>

#include <cfloat>

NTL_START_IMPL

#if (NTL_BITS_PER_LONG >= NTL_DOUBLE_PRECISION)

quad_float to_quad_float(long n)
{
   double xhi, xlo;

   xhi = TrueDouble(n);

   // Because we are assuming 2's compliment integer
   // arithmetic, the following prevents long(xhi) from overflowing.

   if (n > 0)
      xlo = TrueDouble(n+long(-xhi));
   else
      xlo = TrueDouble(n-long(xhi));

   // renormalize...just to be safe

   quad_float z;
   quad_float_normalize(z, xhi, xlo);
   return z;
}

quad_float to_quad_float(unsigned long n)
{
   double xhi, xlo, t;

   const double bnd = double(1L << (NTL_BITS_PER_LONG-2))*4.0;

   xhi = TrueDouble(n);

   if (xhi >= bnd)
      t = xhi - bnd;
   else
      t = xhi;

   // we use the "to_long" function here to be as portable as possible.
   long llo = to_long(n - (unsigned long)(t));
   xlo = TrueDouble(llo);

   quad_float z;
   quad_float_normalize(z, xhi, xlo);
   return z;
}
#endif

NTL_CHEAP_THREAD_LOCAL
long quad_float::oprec = 10;

void quad_float::SetOutputPrecision(long p)
{
   if (p < 1) p = 1;

   if (NTL_OVERFLOW(p, 1, 0))
      ResourceError("quad_float: output precision too big");

   oprec = p;
}

void power(quad_float& z, const quad_float& a, long e)
{
   quad_float res, u;
   unsigned long k;

   if (e < 0)
      k = -((unsigned long) e);
   else
      k = e;

   res = 1.0;
   u = a;

   while (k) {
      if (k & 1)
         res = res * u;

      k = k >> 1;
      if (k)
         u = u * u;
   }

   if (e < 0)
      z = 1.0/res;
   else
      z = res;
}

void power2(quad_float& z, long e)
{
   z.hi = _ntl_ldexp(1.0, e);
   z.lo = 0;
}

long to_long(const quad_float& x)
{
   double fhi, flo;

   fhi = floor(x.hi);

   if (fhi == x.hi)
      flo = floor(x.lo);
   else
      flo = 0;

   // the following code helps to prevent unnecessary integer overflow,
   // and guarantees that to_long(to_quad_float(a)) == a, for all long a,
   // provided long's are not too wide.

   if (fhi > 0)
      return long(flo) - long(-fhi);
   else
      return long(fhi) + long(flo);
}

// This version of ZZ to quad_float coversion relies on the
// precise rounding rules implemented by the ZZ to double conversion.

void conv(quad_float& z, const ZZ& a)
{
   double xhi, xlo;

   conv(xhi, a);

   if (!IsFinite(&xhi)) {
      z.hi = xhi;
      z.lo = 0;
      return;
   }

   NTL_ZZRegister(t);

   conv(t, xhi);
   sub(t, a, t);

   conv(xlo, t);

   quad_float_normalize(z, xhi, xlo);
}

void conv(ZZ& z, const quad_float& x)
{
   NTL_ZZRegister(t1);
   NTL_ZZRegister(t2);
   NTL_ZZRegister(t3);

   double fhi, flo;

   fhi = floor(x.hi);

   if (fhi == x.hi) {
      flo = floor(x.lo);

      conv(t1, fhi);
      conv(t2, flo);

      add(z, t1, t2);
   }
   else
      conv(z, fhi);
}

ostream& operator<<(ostream& s, const quad_float& a)
{
   quad_float aa = a;

   if (!IsFinite(&aa)) {
      s << "NaN";
      return s;
   }

   RRPush push;
   RROutputPush opush;

   RR::SetPrecision(long(3.33*quad_float::oprec) + 10);
   RR::SetOutputPrecision(quad_float::oprec);

   NTL_TLS_LOCAL(RR, t);

   conv(t, a);
   s << t;

   return s;
}

istream& operator>>(istream& s, quad_float& x)
{
   RRPush push;
   RR::SetPrecision(4*NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   NTL_INPUT_CHECK_RET(s, s >> t);
   conv(x, t);

   return s;
}

void random(quad_float& x)
{
   RRPush push;
   RR::SetPrecision(4*NTL_DOUBLE_PRECISION);

   NTL_TLS_LOCAL(RR, t);
   random(t);
   conv(x, t);
}

quad_float random_quad_float()
{
   quad_float x;
   random(x);
   return x;
}

long IsFinite(quad_float *x)
{
   return IsFinite(&x->hi) && IsFinite(&x->lo);
}

quad_float floor(const quad_float& x)
{
   double fhi = floor(x.hi);

   if (fhi != x.hi)
      return quad_float(fhi, 0.0);
   else {
      double flo = floor(x.lo);
      quad_float z;
      quad_float_normalize(z, fhi, flo);
      return z;
   }
}

quad_float ceil(const quad_float& x) {
  return -floor(-x);
}

quad_float trunc(const quad_float& x) {
  if (x>=0.0) return floor(x); else return -floor(-x);
}

long compare(const quad_float& x, const quad_float& y)
{
   if (x.hi > y.hi)
      return 1;
   else if (x.hi < y.hi)
      return -1;
   else if (x.lo > y.lo)
      return 1;
   else if (x.lo < y.lo)
      return -1;
   else
      return 0;
}

quad_float fabs(const quad_float& x)
{ if (x.hi>=0.0) return x; else return -x; }

quad_float ldexp(const quad_float& x, long exp) { // x*2^exp
   double xhi, xlo;
   quad_float z;

   xhi = _ntl_ldexp(x.hi, exp);
   xlo = _ntl_ldexp(x.lo, exp);

   quad_float_normalize(z, xhi, xlo);
   return z;
}

quad_float exp(const quad_float& x) { // New version 97 Aug 05
/*
!  Calculate a quadruple-precision exponential
!  Method:
!   x    x.log2(e)    nint[x.log2(e)] + frac[x.log2(e)]
!  e  = 2          = 2
!
!                     iy    fy
!                  = 2   . 2
!  Then
!   fy    y.loge(2)
!  2   = e
!
!  Now y.loge(2) will be less than 0.3466 in absolute value.
!  This is halved and a Pade aproximation is used to approximate e^x over
!  the region (-0.1733, +0.1733).   This approximation is then squared.
*/
  if (x.hi<DBL_MIN_10_EXP*2.302585092994045684017991)
    return to_quad_float(0.0);
  if (x.hi>DBL_MAX_10_EXP*2.302585092994045684017991) {
    ResourceError("exp(quad_float): overflow");
  }

  static const quad_float Log2 = to_quad_float("0.6931471805599453094172321214581765680755");
  // GLOBAL (assumes C++11 thread-safe init)

  quad_float y,temp,ysq,sum1,sum2;
  long iy;
  y=x/Log2;
  temp = floor(y+0.5);
  iy = to_long(temp);
  y=(y-temp)*Log2;
  y=ldexp(y,-1L);
  ysq=y*y;
  sum1=y*((((ysq+3960.0)*ysq+2162160.0)*ysq+302702400.0)*ysq+8821612800.0);
  sum2=(((90.0*ysq+110880.0)*ysq+30270240.0)*ysq+2075673600.0)*ysq+17643225600.0;
/*
!                     sum2 + sum1         2.sum1
! Now approximation = ----------- = 1 + ----------- = 1 + 2.temp
!                     sum2 - sum1       sum2 - sum1
!
! Then (1 + 2.temp)^2 = 4.temp.(1 + temp) + 1
*/
  temp=sum1/(sum2-sum1);
  y=temp*(temp+1);
  y=ldexp(y,2L);
  return ldexp(y+1,iy);
}

quad_float log(const quad_float& t) { // Newton method. See Bailey, MPFUN
  if (t.hi <= 0.0) {
    ArithmeticError("log(quad_float): argument must be positive");
  }

  quad_float s = to_quad_float(log(t.hi));
  // NOTE: in case log yields excess precision, this assumes
  // that to_quad_float removes it

  quad_float e = exp(s);
  return s+(t-e)/e;  // Newton step
}

quad_float sqrt(const quad_float& y)
{
  if (y.hi < 0.0)
    ArithmeticError("quad_float: square root of negative number");
  if (y.hi == 0.0) return quad_float(0.0,0.0);

  double c = TrueDouble(sqrt(y.hi));
  // NOTE: we call TrueDouble, just in case sqrt yields excess precision

  quad_float yy = y;
  quad_float_in_place_sqrt(yy, c);
  return yy;
}

long operator> (const quad_float& x, const quad_float& y) {
   return (x.hi> y.hi) || (x.hi==y.hi && x.lo> y.lo); }
long operator>=(const quad_float& x, const quad_float& y) {
   return (x.hi>y.hi) || (x.hi==y.hi && x.lo>=y.lo); }
long operator< (const quad_float& x, const quad_float& y) {
   return (x.hi< y.hi) || (x.hi==y.hi && x.lo< y.lo); }
long operator<=(const quad_float& x, const quad_float& y) {
   return (x.hi<y.hi) || (x.hi==y.hi && x.lo<=y.lo); }
long operator==(const quad_float& x, const quad_float& y)
   { return x.hi==y.hi && x.lo==y.lo; }
long operator!=(const quad_float& x, const quad_float& y)
   { return x.hi!=y.hi || x.lo!=y.lo; }

NTL_END_IMPL


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    // 3x3 C-style row major storage, base zero
    Array<int,2> A(3, 3);

    // 3x3 Fortran-style column major storage, base one
    Array<int,2> B(3, 3, ColumnMajorArray<2>());

    // A custom storage format:
    // Indices have range 0..3, 0..3
    // Column major ordering
    // Rows are stored ascending, columns stored descending
    GeneralArrayStorage<2> storage;
    storage.ordering() = firstRank, secondRank;
    storage.base() = 0, 0;
    storage.ascendingFlag() = true, false;

    //    Array<int,2> C(3, 3, storage);
    Array<int,2> C;
    C.setStorage(storage);
    C.resize(3,3);

    // Set each array equal to
    // [ 1 2 3 ]
    // [ 4 5 6 ]
    // [ 7 8 9 ]

    A = 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

    // Comma-delimited lists initialize in memory-storage order only.
    // Hence we list the values in column-major order to initialize B:

    B = 1, 4, 7, 2, 5, 8, 3, 6, 9;

    BZTEST(all(A == B));
    BZTEST(B(2,1) == 8);
    BZTEST(B(0,2) == 3);

    // Array C is stored in column major, plus the columns are stored
    // in descending order

    C = 3, 6, 9, 2, 5, 8, 1, 4, 7;
    BZTEST(all(A == C));
    BZTEST(count(A==C) == 9);

    Array<int,2> D(3,3);
    D = A + B + C;

    Array<int,2> E(3,3);
    E = 3 * A;
    BZTEST(all(D == E));
    BZTEST(!all(D == A));

    return 0;
}


#pragma once
#include <Eigen/Sparse>

namespace edp
{
namespace internal
{
	inline uint32_t ipow(uint32_t base, uint32_t exp)
	{
		uint32_t result = 1U;
		while (exp)
		{
			if (exp & 1U)
				result *= base;
			exp >>= 1U;
			base *= base;
		}

		return result;
	}
}

template<typename T>
struct TwoSiteTerm
{
	std::pair<uint32_t, uint32_t> sites;
	Eigen::SparseMatrix<T> m;

	TwoSiteTerm(const std::pair<uint32_t, uint32_t>& p1, const Eigen::SparseMatrix<T>& p2)
		: sites(p1), m(p2)
	{
	}
	TwoSiteTerm(std::pair<uint32_t, uint32_t>&& p1, Eigen::SparseMatrix<T>&& p2)
		: sites(p1), m(p2)
	{
	}
};

template<typename T>
struct OneSiteTerm
{
	uint32_t site;
	Eigen::SparseMatrix<T> m;

	OneSiteTerm(uint32_t p1, const Eigen::SparseMatrix<T>& p2)
		: site(p1), m(p2)
	{
	}

	OneSiteTerm(uint32_t p1, Eigen::SparseMatrix<T>&& p2)
		: site(p1), m(p2)
	{
	}
};

template<typename T>
class LocalHamiltonian
{
private:
	uint32_t numSites_;
	uint64_t d_; //local Hibert dimension

	std::vector<TwoSiteTerm<T> > twoSiteTerms_;
	std::vector<OneSiteTerm<T> > oneSiteTerms_;

	uint32_t swapBaseD(uint32_t idx, uint32_t pos, uint32_t val) const
	{
		uint32_t b = internal::ipow(d_, pos);
		uint32_t upper = (idx / (b*d_))*d_ + val;
		return upper*b + (idx % b);
	}
public:
	LocalHamiltonian(uint32_t numSites, uint32_t d)
		: numSites_{numSites}, d_{d}
	{
	}
	LocalHamiltonian(const LocalHamiltonian&) = default;
	LocalHamiltonian(LocalHamiltonian&&) = default;

	LocalHamiltonian& operator=(const LocalHamiltonian&) = default;
	LocalHamiltonian& operator=(LocalHamiltonian&&) = default;

	void clearTerms()
	{
		std::vector<TwoSiteTerm<T> >().swap(twoSiteTerms_);
		std::vector<OneSiteTerm<T> >().swap(oneSiteTerms_);
	}
	uint32_t getNumSites() const
	{
		return numSites_;
	}

	std::map<uint32_t, T> getCol(uint32_t n) const;
	inline std::map<uint32_t, T> operator()(uint32_t n) const
	{
		return getCol(n);
	}

	void addTwoSiteTerm(const std::pair<int,int>& site, Eigen::SparseMatrix<T> m)
	{
		m.makeCompressed();
		twoSiteTerms_.emplace_back(site, std::move(m));
	}
	void addOneSiteTerm(uint32_t site, Eigen::SparseMatrix<T> m)
	{
		m.makeCompressed();
		oneSiteTerms_.emplace_back(site, std::move(m));
	}
};

}

template<typename T>
std::map<uint32_t, T> edp::LocalHamiltonian<T>::getCol(uint32_t n) const
{
	using internal::ipow;
	using Eigen::SparseMatrix;

	std::map<uint32_t, T> m;

	for(auto& twoSiteTerm : twoSiteTerms_)
	{
		auto a = (n/ipow(d_,twoSiteTerm.sites.first))%d_;
		auto b = (n/ipow(d_,twoSiteTerm.sites.second))%d_;
		//auto col = twoSiteTerm.m.col(a*d_ + b);
		auto col = b*d_ + a;
		for(typename SparseMatrix<T>::InnerIterator it(twoSiteTerm.m, col); it; ++it)
		{
			uint32_t r = it.row();
			uint32_t t = n;
			t = swapBaseD(t, twoSiteTerm.sites.first, r%d_);
			t = swapBaseD(t, twoSiteTerm.sites.second, r/d_);
			m[t] += it.value();
		}
	}
	for(auto& oneSiteTerm: oneSiteTerms_)
	{
		uint32_t a = (n/ipow(d_,oneSiteTerm.site))%d_;
		//auto col = oneSiteTerm.m.col(a);
		auto col = a;

		for(typename SparseMatrix<T>::InnerIterator it(oneSiteTerm.m, col); it; ++it)
		{
			uint32_t r = it.row();
			uint32_t t = n;
			t = swapBaseD(t, oneSiteTerm.site, r);
			m[t] += it.value();
		}
	}
	return m;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Corrected Third Printing, Springer, 2005.
 *      Bate R. Fundamentals of Astrodynamics, Courier Dover Publications, 1971.
 *
 */

#include <Eigen/Core>
#include <cmath>

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"

namespace tudat
{

namespace mission_geometry
{

//! Compute whether an orbit is retrograde based on inclination.
bool isOrbitRetrograde( const double inclination )
{
    bool isRetrograde = false;

    // Check which range inclination is in and return value accordingly.
    if ( inclination < 0.0 || inclination > mathematical_constants::PI )
    {
        throw std::runtime_error(
                    "The inclination is in the wrong range when determining retrogradeness" );
    }
    else if ( inclination <= mathematical_constants::PI / 2.0 )
    {
        isRetrograde = false;
    }
    else if ( inclination > mathematical_constants::PI / 2.0 )
    {
        isRetrograde = true;
    }

    return isRetrograde;
}

//! Compute whether an orbit is retrograde based on Keplerian state.
bool isOrbitRetrograde( const Eigen::Vector6d& keplerElements )
{
    // Get inclination from vector and call overloaded function.
    return isOrbitRetrograde(
                keplerElements(
                    orbital_element_conversions::inclinationIndex ) );
}

//! Compute the shadow function.
double computeShadowFunction( const Eigen::Vector3d& occultedBodyPosition,
                              const double occultedBodyRadius,
                              const Eigen::Vector3d& occultingBodyPosition,
                              const double occultingBodyRadius,
                              const Eigen::Vector3d& satellitePosition )
{
    // Calculate coordinates of the spacecraft with respect to the occulting body.
    const Eigen::Vector3d satellitePositionRelativeToOccultingBody = satellitePosition
            - occultingBodyPosition;

    // Calculate apparent radius of occulted body.
    const double occultedBodyApparentRadius
            = std::asin( occultedBodyRadius
                         / ( occultedBodyPosition - satellitePosition ).norm( ) );

    // Calculate apparent radius of occulting body.
    const double occultingBodyApparentRadius =
            std::asin( occultingBodyRadius / satellitePositionRelativeToOccultingBody.norm( ) );

    // Calculate apparent separation of the center of both bodies.
    const double apparentSeparationPartOne = -satellitePositionRelativeToOccultingBody.transpose( )
            * ( occultedBodyPosition - satellitePosition );
    const double apparentSeparationPartTwo = satellitePositionRelativeToOccultingBody.norm( )
            * ( occultedBodyPosition - satellitePosition ).norm( );
    const double apparentSeparation = std::acos( apparentSeparationPartOne
                                                 / apparentSeparationPartTwo );

    // Set initial value for the shadow function.
    double shadowFunction = 1.0;

    // Check if partial occultation takes place
    if ( std::fabs( occultedBodyApparentRadius - occultingBodyApparentRadius ) < apparentSeparation
         && apparentSeparation < occultedBodyApparentRadius + occultingBodyApparentRadius )
    {
        // Pre-compute values for optimal computations.
        const double apparentSeparationSquared = apparentSeparation * apparentSeparation;
        const double occultedBodyApparentRadiusSquared = occultedBodyApparentRadius
                * occultedBodyApparentRadius;
        const double occultingBodyApparentRadiusSquared = occultingBodyApparentRadius
                * occultingBodyApparentRadius;

        // Partial occultation takes place, calculate the occulted area.
        const double occultedAreaPartOne
                = ( apparentSeparationSquared + occultedBodyApparentRadiusSquared
                    - occultingBodyApparentRadiusSquared ) / ( 2.0 * apparentSeparation );
        const double occultedAreaPartTwo = std::sqrt( occultedBodyApparentRadiusSquared
                                                      - occultedAreaPartOne * occultedAreaPartOne );
        const double occultedArea = occultedBodyApparentRadiusSquared
                * std::acos( occultedAreaPartOne / occultedBodyApparentRadius )
                + occultingBodyApparentRadiusSquared
                * std::acos( ( apparentSeparation - occultedAreaPartOne )
                             / occultingBodyApparentRadius )
                - apparentSeparation * occultedAreaPartTwo;
        shadowFunction = 1.0 - occultedArea / ( mathematical_constants::PI *
                                                occultedBodyApparentRadiusSquared );
    }

    else
    {
        // Full or no occultation takes place.
        // Check for type of occultation.
        if ( apparentSeparation < occultingBodyApparentRadius - occultedBodyApparentRadius &&
             occultedBodyApparentRadius < occultingBodyApparentRadius )
        {
            // Total occultation.
            shadowFunction = 0.0;
        }

        else if ( apparentSeparation < occultedBodyApparentRadius - occultingBodyApparentRadius &&
                  occultedBodyApparentRadius > occultingBodyApparentRadius )
        {
            // Maximum partial occultation.
            shadowFunction = 0.0;
        }

        else if ( occultedBodyApparentRadius + occultingBodyApparentRadius <= apparentSeparation )
        {
            // No occultation
            shadowFunction = 1.0;
        }
    }

    // Return the shadow function
    return shadowFunction;
}

double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double ratioOfOrbitingToCentralBodyMass )
{
    // Return the radius of the sphere of influence.
    return distanceToCentralBody * std::pow( ratioOfOrbitingToCentralBodyMass, 0.4 );
}

//! Compute the sphere of influence.
double computeSphereOfInfluence( const double distanceToCentralBody,
                                 const double massOrbitingBody,
                                 const double massCentralBody )
{
    // Return the radius of the sphere of influence.
    return computeSphereOfInfluence( distanceToCentralBody, massOrbitingBody / massCentralBody );
}

} // namespace mission_geometry

} // namespace tudat


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {

{

//! [ap]
BOOST_HANA_CONSTEXPR_CHECK(
    ap(make<Tuple>(_+_), make<Tuple>(1, 2), make<Tuple>(3, 4, 5))
        ==
    make<Tuple>(
        1 + 3,      1 + 4,      1 + 5,
        2 + 3,      2 + 4,      2 + 5
    )
);

BOOST_HANA_CONSTEXPR_LAMBDA auto g = [](auto a, auto b, auto c) {
    return a * b * c;
};
BOOST_HANA_CONSTEXPR_CHECK(
    ap(just(g), just(1), just(2), just(3)) == just(1 * 2 * 3)
);
BOOST_HANA_CONSTANT_CHECK(
    ap(just(g), just(1), nothing, just(3)) == nothing
);
//! [ap]

}{

//! [lift]
BOOST_HANA_CONSTEXPR_CHECK(lift<Tuple>('x') == make<Tuple>('x'));
BOOST_HANA_CONSTEXPR_CHECK(lift<Maybe>('x') == just('x'));
//! [lift]

}

}


//=======================================================================
// Copyright 2007 Aaron Windsor - Ben Sisson
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>

using namespace boost;

//a class to hold the coordinates of the straight line embedding
struct coord_t{
  std::size_t x;
  std::size_t y;
};

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

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  graph g(7);
  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,0,g);
  add_edge(0,4,g);
  add_edge(1,5,g);
  add_edge(2,6,g);
  add_edge(3,7,g);
  add_edge(4,5,g);
  add_edge(5,6,g);
  add_edge(6,7,g);
  add_edge(7,4,g);

  //Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity; compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  make_biconnected_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  //Test for planarity again; compute the planar embedding as a side-effect
  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  make_maximal_planar(g, &embedding[0]);

  // Re-initialize the edge index, since we just added a few edges
  edge_count = 0;
  for(boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   );

  // Find a canonical ordering
  std::vector<graph_traits<graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, &embedding[0], std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector< coord_t > straight_line_drawing_storage_t;
  typedef boost::iterator_property_map
    < straight_line_drawing_storage_t::iterator,
      property_map<graph, vertex_index_t>::type
    >
    straight_line_drawing_t;

  straight_line_drawing_storage_t straight_line_drawing_storage
    (num_vertices(g));
  straight_line_drawing_t straight_line_drawing
    (straight_line_drawing_storage.begin(),
     get(vertex_index,g)
     );

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g,
                                      embedding,
                                      ordering.begin(),
                                      ordering.end(),
                                      straight_line_drawing
                                      );

  std::cout << "The straight line drawing is: " << std::endl;
  graph_traits<graph>::vertex_iterator vi, vi_end;
  for(boost::tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    {
      coord_t coord(get(straight_line_drawing,*vi));
      std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                << std::endl;
    }

  // Verify that the drawing is actually a plane drawing
  if (is_straight_line_drawing(g, straight_line_drawing))
    std::cout << "Is a plane drawing." << std::endl;
  else
    std::cout << "Is not a plane drawing." << std::endl;

  return 0;
}


#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

boost::shared_ptr<pcl::visualization::PCLVisualizer>
simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
  //viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}

int
main(int argc, char** argv)
{
  // initialize PointClouds
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);

  // populate our PointCloud with points
  cloud->width    = 500;
  cloud->height   = 1;
  cloud->is_dense = false;
  cloud->points.resize (cloud->width * cloud->height);
  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    {
      cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if (i % 5 == 0)
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else if(i % 2 == 0)
        cloud->points[i].z =  sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                      - (cloud->points[i].y * cloud->points[i].y));
      else
        cloud->points[i].z =  - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
                                        - (cloud->points[i].y * cloud->points[i].y));
    }
    else
    {
      cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
      cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
      if( i % 2 == 0)
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
      else
        cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
    }
  }

  std::vector<int> inliers;

  // created RandomSampleConsensus object and compute the appropriated model
  pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
    model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
  if(pcl::console::find_argument (argc, argv, "-f") >= 0)
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }
  else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
  {
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
    ransac.setDistanceThreshold (.01);
    ransac.computeModel();
    ransac.getInliers(inliers);
  }

  // copies all inliers of the model computed to another PointCloud
  pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);

  // creates the visualization object and adds either our orignial cloud or all of the inliers
  // depending on the command line arguments specified.
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
    viewer = simpleVis(final);
  else
    viewer = simpleVis(cloud);
  while (!viewer->wasStopped ())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }
  return 0;
 }


/** A simple cartesian space controller.
    It receives a destination, a duration and publishes the trajectory
    in cartesian space at 200Hz
    Note: the robot is required to be calibrated to the zero configuration
    as used in the kdl chain model.
**/
#include "ros/ros.h"
#include <boost/lexical_cast.hpp>
#include <boost/thread/thread.hpp>
#include <trajectory_msgs/JointTrajectory.h>
#include "bosch_arm_srvs/GetJointAngles.h"
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>
#include <stdio.h>
#include <iostream>
#include <vector>
using namespace Eigen;

class BoschArmKinematicModel
{
  public:
    double L0,L3,L4;
    Vector3f getTipPosition(Vector4f q)
    {
      Vector3f tip;
      double q1=q(0);
      double q2=q(1);
      double q3=q(2);
      double q4=q(3);
      tip(0)= - L4*(sin(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) + cos(q2)*cos(q4)*sin(q1)) - L3*cos(q2)*sin(q1);
      tip(1)=  - L4*(cos(q4)*sin(q2) + cos(q2)*sin(q3)*sin(q4)) - L3*sin(q2);
      tip(2)=L0 - L4*(sin(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) - cos(q1)*cos(q2)*cos(q4)) + L3*cos(q1)*cos(q2);
      return tip;
    }
    Matrix3f getJacobianLockJoint3(Vector4f qlock)
    {
      Matrix3f jacob;
      double q1=qlock(0);
      double q2=qlock(1);
      double q3=qlock(2);
      double q4=qlock(3);
      jacob(0,0)=L4*(sin(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) - cos(q1)*cos(q2)*cos(q4)) - L3*cos(q1)*cos(q2);
      jacob(0,1)=L4*(cos(q4)*sin(q1)*sin(q2) + cos(q2)*sin(q1)*sin(q3)*sin(q4)) + L3*sin(q1)*sin(q2);
      jacob(0,2)=-L4*(cos(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) - cos(q2)*sin(q1)*sin(q4));
      jacob(1,0)=0;
      jacob(1,1)=-L4*(cos(q2)*cos(q4) - sin(q2)*sin(q3)*sin(q4)) - L3*cos(q2);
      jacob(1,2)=L4*(sin(q2)*sin(q4) - cos(q2)*cos(q4)*sin(q3));
      jacob(2,0)=- L4*(sin(q4)*(cos(q1)*cos(q3) - sin(q1)*sin(q2)*sin(q3)) + cos(q2)*cos(q4)*sin(q1)) - L3*cos(q2)*sin(q1);
      jacob(2,1)=- L4*(cos(q1)*cos(q4)*sin(q2) + cos(q1)*cos(q2)*sin(q3)*sin(q4)) - L3*cos(q1)*sin(q2);
      jacob(2,2)=-L4*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*sin(q2)*sin(q3)) + cos(q1)*cos(q2)*sin(q4));
      return jacob;
    }

};
int main ( int argc, char **argv )
{
  ros::init ( argc, argv, "traj_gen" );
  ros::NodeHandle n;
  ros::Publisher traj_pub =  n.advertise<trajectory_msgs::JointTrajectory> ( "/traj_cmd",1 );
  //boost::thread t2 = boost::thread::thread ( boost::bind ( &pubTrajectory ) );
  ros::ServiceClient client = n.serviceClient<bosch_arm_srvs::GetJointAngles> ( "get_joint_angles" );
  bosch_arm_srvs::GetJointAngles srv;
  client.call ( srv );

  //read destination and duration
  KDL::Vector des;
  for ( int i=0;i<3;i++ )
    des[i]=boost::lexical_cast<double> ( argv[i+1] );
  double t=boost::lexical_cast<double> ( argv[4] );

  //TODO convert destination to joint angles

  Segment seg0=Segment ( Joint ( Joint::None ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.27 ) ) );
  Segment seg1=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
  Segment seg2=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
  Segment seg3=Segment ( Joint ( Joint::None ),
                         Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.50 ) ) );
  Segment seg4=Segment ( Joint ( Joint::RotZ ),
                         Frame ( Vector ( 0.48,0,0 ) ) );
  Chain chain;
  chain.addSegment ( seg0 );
  chain.addSegment ( seg1 );
  chain.addSegment ( seg2 );
  chain.addSegment ( seg3 );
  chain.addSegment ( seg4 );
  ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive ( chain );

  //linear interpolation in joint space
  trajectory_msgs::JointTrajectory traj;
  traj.header.stamp=ros::Time::now();
  traj.header.frame_id="";
  traj.header.seq=0;
  traj.points.resize ( npts+1 );
  traj.joint_names.resize ( 4 );
  traj.joint_names[0]="joint1";
  traj.joint_names[1]="joint2";
  traj.joint_names[2]="joint3";
  traj.joint_names[3]="joint4";
  traj.points[i].positions.resize ( 4 );

  int npts=ceil ( t*200 );
  KDL::JntArray jointpositions = JntArray (3);
  KDL::Frame cartpos;
  double q3;
  bool kinematics_status;

  for ( int i=0;i<=npts;i++ )
  {
    //get the current joint position
    client.call ( srv );
    //compute forward kinematics.

    jointpositions (0) =srv.response.joint_angles[0];
    jointpositions (1) =srv.response.joint_angles[1];
    q3                 =srv.response.joint_angles[2];
    jointpositions (2) =srv.response.joint_angles[3];
    kinematics_status = fksolver.JntToCart ( jointpositions,cartpos );
    if ( kinematics_status>=0 )
    {
      std::cout << cartpos.p <<std::endl;
    }
    else
    {
      printf ( "%s \n","Error: could not calculate forward kinematics :(" );
    }
    double steps_to_go=npts-i;
    Vector step_length_cart= (des-cartpos.p)/steps_to_go;

    for ( int j=0;j<4;j++ )
      traj.points[i].positions[j]=srv.response.joint_angles[j]+i*dq[j];
    traj.points[i].time_from_start=ros::Duration ( i*t/npts );
  }
  //The first message is always lost
  for ( int i=0;i<2;i++ )
  {
    traj_pub.publish ( traj );
    sleep ( 1 );
  }
  ros::spin();
  return 0;
}