#include <memory>
#include <chrono>
#include <random>
#include <gflags/gflags.h>

#include <Eigen/Sparse>

#include "drake/solvers/mosek_solver.h"
#include "drake/solvers/gurobi_solver.h"
#include "drake/solvers/mathematical_program.h"

#include "examples/PlanarWalker/sgd_iter.h"
#include "systems/goldilocks_models/file_utils.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using std::string;
using drake::solvers::VectorXDecisionVariable;

namespace dairlib {
namespace goldilocks_models {

void runSGD() {
  std::random_device randgen;
  std::default_random_engine e1(randgen());
  std::uniform_real_distribution<> dist(0, 1);

  int n_batch = 5;

  // int n_weights = 43;
  int n_weights =  10;
  MatrixXd theta_0 = MatrixXd::Zero(2,n_weights);
  theta_0(0,0) = -0.1;
  theta_0(0,3) = 1.0;
  theta_0(1,0) = 0;
  theta_0(1,1) = 1;
  writeCSV("data/0_theta.csv", theta_0);

  double length = 0.3;
  double duration = 1;
  int snopt_iter = 200;
  string directory = "data/";
  string init_z = "z_save.csv";
  string weights = "0_theta.csv";
  string output_prefix = "0_0_";
  sgdIter(length, duration, snopt_iter, directory, init_z, weights, output_prefix);

  for (int iter = 1; iter <= 50; iter++) {
    int input_batch = iter == 1 ? 1 : n_batch;

    std::vector<MatrixXd> A_vec;
    std::vector<MatrixXd> B_vec;
    std::vector<MatrixXd> H_vec;
    std::vector<MatrixXd> A_active_vec;
    std::vector<MatrixXd> B_active_vec;
    std::vector<MatrixXd> lb_vec;
    std::vector<MatrixXd> ub_vec;
    std::vector<MatrixXd> y_vec;
    std::vector<MatrixXd> w_vec;
    std::vector<MatrixXd> z_vec;
    std::vector<MatrixXd> theta_vec;
    std::vector<double> nl_vec;
    std::vector<double> nz_vec;

    int nz=0,nt=0,nl=0;

    for (int batch = 0; batch < input_batch; batch++) {
      string batch_prefix = std::to_string(iter-1) + "_" + std::to_string(batch) + "_";
      string iter_prefix = std::to_string(iter-1) + "_";

      A_vec.push_back(readCSV(directory + batch_prefix + "A.csv"));
      B_vec.push_back(readCSV(directory + batch_prefix + "B.csv"));
      H_vec.push_back(readCSV(directory + batch_prefix + "H.csv"));
      lb_vec.push_back(readCSV(directory + batch_prefix + "lb.csv"));
      ub_vec.push_back(readCSV(directory + batch_prefix + "ub.csv"));
      y_vec.push_back(readCSV(directory + batch_prefix + "y.csv"));
      w_vec.push_back(readCSV(directory + batch_prefix + "w.csv"));
      z_vec.push_back(readCSV(directory + batch_prefix + "z.csv"));
      theta_vec.push_back(readCSV(directory + iter_prefix + "theta.csv"));

      DRAKE_ASSERT(w_vec[batch].cols() == 1);
      DRAKE_ASSERT(lb_vec[batch].cols() == 1);
      DRAKE_ASSERT(ub_vec[batch].cols() == 1);
      DRAKE_ASSERT(y_vec[batch].cols() == 1);
      DRAKE_ASSERT(w_vec[batch].cols() == 1);
      DRAKE_ASSERT(z_vec[batch].cols() == 1);

      int n_active = 0;
      double tol = 1e-4;
      for (int i = 0; i < y_vec[batch].rows(); i++) {
        if (y_vec[batch](i) >= ub_vec[batch](i) - tol || y_vec[batch](i) <= lb_vec[batch](i) + tol)
          n_active++;
      }

      int nz_i = A_vec[batch].cols();
      int nt_i = B_vec[batch].cols();

      MatrixXd A_active(n_active, nz_i);
      MatrixXd B_active(n_active, nt_i);
      MatrixXd AB_active(n_active, nz_i + nt_i);

      int nl_i = 0;
      for (int i = 0; i < y_vec[batch].rows(); i++) {
        if (y_vec[batch](i) >= ub_vec[batch](i) - tol || y_vec[batch](i) <= lb_vec[batch](i) + tol) {
          A_active.row(nl_i) = A_vec[batch].row(i);
          B_active.row(nl_i) = B_vec[batch].row(i);
          AB_active.row(nl_i) << A_vec[batch].row(i), B_vec[batch].row(i);
          nl_i++;
        }
      }

      A_active_vec.push_back(A_active);
      B_active_vec.push_back(B_active);
      nl_vec.push_back(nl_i);
      nz_vec.push_back(nz_i);

      nl += nl_i;
      nz += nz_i;
      if (batch == 0) {
        nt = nt_i;
      } else {
        DRAKE_ASSERT(nt == nt_i);
        DRAKE_ASSERT((theta_vec[0] - theta_vec[batch]).norm() == 0);
      }
    }

    //Join matricies
    // MatrixXd AB_active = MatrixXd::Zero(nl,nz+nt);
    // MatrixXd H_ext = MatrixXd::Zero(nz + nt, nz + nt);
    VectorXd w_ext = VectorXd::Zero(nz+nt,1);
    int nl_start = 0;
    int nz_start = 0;
    for (int batch = 0; batch < input_batch; batch++) {
      // AB_active.block(nl_start, nz_start, nl_vec[batch], nz_vec[batch]) = A_active_vec[batch];
      // AB_active.block(nl_start, nz, nl_vec[batch], nt) = B_active_vec[batch];

      // H_ext.block(nz_start,nz_start,nz_vec[batch],nz_vec[batch]) = H_vec[batch];
      w_ext.segment(nz_start,nz_vec[batch]) = w_vec[batch].col(0);

      nl_start += nl_vec[batch];
      nz_start += nz_vec[batch];
    }
    // H_ext.block(nz,nz,nt,nt) = 1e-2*MatrixXd::Identity(nt,nt);

    // Eigen::BDCSVD<MatrixXd> svd(AB_active,  Eigen::ComputeFullV);

    // MatrixXd N = svd.matrixV().rightCols(AB_active.cols() - svd.rank());

    // auto gradient = N*N.transpose()*w_ext;

    // double scale_num= gradient.dot(gradient);
    // double scale_den = gradient.dot(Ei_ext*gradient);

    // auto dtheta = -0.02*gradient.tail(nt)*scale_num/scale_den;

    nl_start = 0;
    nz_start = 0;
    std::vector<Eigen::Triplet<double>> tripletList;
    std::vector<Eigen::Triplet<double>> tripletList_H;
    for (int batch = 0; batch < input_batch; batch++) {
      for (int i = 0; i < nz_vec[batch]; i++) {
        for (int j = 0; j < nz_vec[batch]; j++) {
          tripletList.push_back(Eigen::Triplet<double>(nz_start + i, nz_start + j, H_vec[batch](i,j)));
          tripletList_H.push_back(Eigen::Triplet<double>(nz_start + i, nz_start + j, H_vec[batch](i,j)));
        }
      }
      for (int i = 0; i < nl_vec[batch]; i++) {
        for (int j = 0; j < nz_vec[batch]; j++) {
          int i_ind = nz + nt + nl_start + i;
          int j_ind = nz_start + j;
          tripletList.push_back(Eigen::Triplet<double>(i_ind, j_ind, A_active_vec[batch](i,j)));
          tripletList.push_back(Eigen::Triplet<double>(j_ind, i_ind, A_active_vec[batch](i,j)));
        }
        for (int j = 0; j < nt; j++) {
          int i_ind = nz + nt + nl_start + i;
          int j_ind = nz + j;
          tripletList.push_back(Eigen::Triplet<double>(i_ind, j_ind, B_active_vec[batch](i,j)));
          tripletList.push_back(Eigen::Triplet<double>(j_ind, i_ind, B_active_vec[batch](i,j)));
        }
      }
      nl_start += nl_vec[batch];
      nz_start += nz_vec[batch];
    }

    VectorXd b(nz + nt + nl);
    b << -w_ext, VectorXd::Zero(nl);

    Eigen::SparseMatrix<double> M(nz + nt + nl, nz + nt + nl);
    M.setFromTriplets(tripletList.begin(), tripletList.end());

    Eigen::SparseMatrix<double> H_ext(nz + nt, nz + nt);
    H_ext.setFromTriplets(tripletList_H.begin(), tripletList_H.end());
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> qr(M);
    auto gradient = qr.solve(b);

    // Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver;
    // // fill A and b;
    // // Compute the ordering permutation vector from the structural pattern of A
    // solver.analyzePattern(M);
    // // Compute the numerical factorization
    // solver.factorize(M);
    // std::cout << solver.info() << std::endl;
    // //Use the factors to solve the linear system
    // auto gradient = solver.solve(b);

    // Eigen::BiCGSTAB<Eigen::SparseMatrix<double> > solver;
    // solver.compute(M);
    // auto gradient = solver.solve(b);
    // std::cout << "#iterations:     " << solver.iterations() << std::endl;
    // std::cout << "estimated error: " << solver.error()      << std::endl;

    // std::cout << M*gradient - b << std::endl;

    // std::cout << gradient << std::endl;

    // //M = [H_ext A'; A 0]
    // MatrixXd M(nz + nt + nl, nz + nt + nl);
    // M.block(0, 0, nz + nt, nz + nt) = H_ext;
    // M.block(0, nz + nt, nz + nt, nl) = AB_active.transpose();
    // M.block(nz + nt, 0, nl, nz + nt) = AB_active;
    // M.block(nz + nt, nz + nt, nl, nl) = MatrixXd::Zero(nl,nl);
    // VectorXd b(nz + nt + nl);
    // b << -w_ext, VectorXd::Zero(nl);
    // auto gradient = M.colPivHouseholderQr().solve(b);

    auto zt = gradient.head(nz+nt);

    auto resid = M*gradient - b;

    std::cout << "residual-norm: "<< resid.tail(nl).norm() << std::endl;
    std::cout << "descent: "<< gradient.head(nz+nt).dot(w_ext) << std::endl;

    double scale = 1/sqrt(zt.dot(H_ext*zt));

    std::cout << "scale: "<< scale << std::endl;

    auto dtheta = -.01*scale*gradient.segment(nz, nt);

    std::cout << "found dtheta"<< std::endl;

    std::cout << std::endl<< "dtheta norm: " << dtheta.norm() << std::endl;
    std::cout << "***********Next iteration*************" << std::endl;

    // std::cout << "scale predict: " << scale_num/scale_den << std::endl;

    //reshape dtheta
    MatrixXd theta_mat(theta_vec[0].rows(), theta_vec[0].cols());
    for (int i = 0; i < theta_vec[0].rows(); i++) {
      theta_mat.row(i) = theta_vec[0].row(i) +
                         dtheta.segment(i*theta_vec[0].cols(),theta_vec[0].cols()).transpose();
    }
    if (iter == 1)
      writeCSV("data/" + std::to_string(iter) + "_theta.csv", theta_vec[0]);
    else
      writeCSV("data/" + std::to_string(iter) + "_theta.csv", theta_mat);

    // init_z = std::to_string(iter-1) + "_z.csv";
    init_z = "z_save.csv";
    weights = std::to_string(iter) +  "_theta.csv";
    output_prefix = std::to_string(iter) +  "_";

    for(int batch = 0; batch < n_batch; batch++) {
    //randomize distance on [0.3,0.5]
      // length = 0.2 + 0.3*dist(e1);
      length = 0.15 + 0.05*batch + 0.0*dist(e1);
      // duration =  length/0.5; //maintain constaint speed of 0.5 m/s
      duration = 1;

      int length_file_index = (int) ((0.5 - length) * 10);
      if (iter == 1)
        init_z = "init_length_" + std::to_string(length_file_index) + "_speed_0_z.csv";
      else
        init_z = std::to_string(iter-1) + "_" + std::to_string(batch) + "_z.csv";

      std::cout << std::endl << "Iter-Batch: " << iter << "-" << batch << std::endl;
      std::cout << "New length: " << length << std::endl;

      string batch_prefix = output_prefix + std::to_string(batch) + "_";

      sgdIter(length, duration, snopt_iter, directory, init_z, weights, batch_prefix);
    }
  }
}
}  // namespace goldilocks_models
}  // namespace dairlib

int main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);
  std::srand(time(0));  // Initialize random number generator.

  dairlib::goldilocks_models::runSGD();
}


/* boost random/geometric_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP
#define BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>          // std::log
#include <iosfwd>
#include <ios>
#include <boost/assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

/**
 * An instantiation of the class template @c geometric_distribution models
 * a \random_distribution.  The distribution produces positive
 * integers which are the number of bernoulli trials
 * with probability @c p required to get one that fails.
 *
 * For the geometric distribution, \f$p(i) = p(1-p)^{i}\f$.
 *
 * @xmlwarning
 * This distribution has been updated to match the C++ standard.
 * Its behavior has changed from the original
 * boost::geometric_distribution.  A backwards compatible
 * wrapper is provided in namespace boost.
 * @endxmlwarning
 */
template<class IntType = int, class RealType = double>
class geometric_distribution
{
public:
    typedef RealType input_type;
    typedef IntType result_type;

    class param_type
    {
    public:

        typedef geometric_distribution distribution_type;

        /** Constructs the parameters with p. */
        explicit param_type(RealType p_arg = RealType(0.5))
          : _p(p_arg)
        {
            BOOST_ASSERT(RealType(0) < _p && _p < RealType(1));
        }

        /** Returns the p parameter of the distribution. */
        RealType p() const { return _p; }

        /** Writes the parameters to a std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._p;
            return os;
        }

        /** Reads the parameters from a std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            double p_in;
            if(is >> p_in) {
                if(p_in > RealType(0) && p_in < RealType(1)) {
                    parm._p = p_in;
                } else {
                    is.setstate(std::ios_base::failbit);
                }
            }
            return is;
        }

        /** Returns true if the two sets of parameters are equal. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._p == rhs._p; }

        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _p;
    };

    /**
     * Contructs a new geometric_distribution with the paramter @c p.
     *
     * Requires: 0 < p < 1
     */
    explicit geometric_distribution(const RealType& p_arg = RealType(0.5))
      : _p(p_arg)
    {
        BOOST_ASSERT(RealType(0) < _p && _p < RealType(1));
        init();
    }

    /** Constructs a new geometric_distribution from its parameters. */
    explicit geometric_distribution(const param_type& parm)
      : _p(parm.p())
    {
        init();
    }

    // compiler-generated copy ctor and assignment operator are fine

    /** Returns: the distribution parameter @c p  */
    RealType p() const { return _p; }

    /** Returns the smallest value that the distribution can produce. */
    IntType min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return IntType(0); }

    /** Returns the largest value that the distribution can produce. */
    IntType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return (std::numeric_limits<IntType>::max)(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_p); }

    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _p = parm.p();
        init();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

    /**
     * Returns a random variate distributed according to the
     * geometric_distribution.
     */
    template<class Engine>
    result_type operator()(Engine& eng) const
    {
        using std::log;
        using std::floor;
        RealType x = RealType(1) - boost::uniform_01<RealType>()(eng);
        return IntType(floor(log(x) / _log_1mp));
    }

    /**
     * Returns a random variate distributed according to the
     * geometric distribution with parameters specified by param.
     */
    template<class Engine>
    result_type operator()(Engine& eng, const param_type& parm) const
    { return geometric_distribution(parm)(eng); }

    /** Writes the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, geometric_distribution, gd)
    {
        os << gd._p;
        return os;
    }

    /** Reads the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, geometric_distribution, gd)
    {
        param_type parm;
        if(is >> parm) {
            gd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two distributions will produce identical
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(geometric_distribution, lhs, rhs)
    { return lhs._p == rhs._p; }

    /**
     * Returns true if the two distributions may produce different
     * sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(geometric_distribution)

private:

    /// \cond show_private

    void init()
    {
        using std::log;
        _log_1mp = log(1 - _p);
    }

    RealType _p;
    RealType _log_1mp;

    /// \endcond
};

} // namespace random

/// \cond show_deprecated

/**
 * Provided for backwards compatibility.  This class is
 * deprecated.  It provides the old behavior of geometric_distribution
 * with \f$p(i) = (1-p) p^{i-1}\f$.
 */
template<class IntType = int, class RealType = double>
class geometric_distribution
{
public:
    typedef RealType input_type;
    typedef IntType result_type;

    explicit geometric_distribution(RealType p_arg = RealType(0.5))
      : _impl(1 - p_arg) {}

    RealType p() const { return 1 - _impl.p(); }

    void reset() {}

    template<class Engine>
    IntType operator()(Engine& eng) const { return _impl(eng) + IntType(1); }

    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, geometric_distribution, gd)
    {
        os << gd.p();
        return os;
    }

    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, geometric_distribution, gd)
    {
        RealType val;
        if(is >> val) {
            typename impl_type::param_type impl_param(1 - val);
            gd._impl.param(impl_param);
        }
        return is;
    }

private:
    typedef random::geometric_distribution<IntType, RealType> impl_type;
    impl_type _impl;
};

/// \endcond

} // namespace boost

#endif // BOOST_RANDOM_GEOMETRIC_DISTRIBUTION_HPP


#ifndef BART_SRC_CONVERGENCE_MOMENTS_SINGLE_MOMENT_L1_NORM_H_
#define BART_SRC_CONVERGENCE_MOMENTS_SINGLE_MOMENT_L1_NORM_H_

#include "convergence/convergence_checker.hpp"

#include <deal.II/lac/vector.h>

//! Convergence checkers for problem vector moments
namespace bart::convergence::moments {

/*! \brief Checks for convergence between flux moments using the percentage
 * change in the L1 norm, compared to the current iteration (\f$i\f$):
 *
 * \f[
 *
 * \Delta_i = \frac{|\phi_i - \phi_{i-1}|_{1}}{|\phi_{i}|_{1}}
 *
 * \f]
 *
 * Convergence is achieved if \f$\Delta_i \leq \Delta_{\text{max}}\f$.
 * */

class ConvergenceCheckerL1Norm : public ConvergenceChecker<dealii::Vector<double>> {
 public:
  using Vector = dealii::Vector<double>;
  /*! \brief Default constructor, setting max delta to \f$10^{-6}\f$. */
  explicit ConvergenceCheckerL1Norm(const double max_delta = 1e-6);
  auto SetMaxDelta(const double& to_set) -> void override;
  auto IsConverged(const Vector& current_iteration, const Vector& previous_iteration) -> bool override;
};

} // namespace bart::convergence::moments

#endif // BART_SRC_CONVERGENCE_MOMENTS_SINGLE_MOMENT_L1_NORM_H_


#ifndef FIELD_MATH_H_
#define FIELD_MATH_H_

#include <Eigen/Core>
#include <Eigen/Dense>
#include <algorithm>
//#include <glm/glm.hpp>
#include <vector>
using namespace Eigen;

struct DEdge
{
    DEdge()
    : x(0), y(0)
    {}
    DEdge(int _x, int _y) {
        if (_x > _y)
            x = _y, y = _x;
        else
            x = _x, y = _y;
    }
    bool operator<(const DEdge& e) const {
        return (x < e.x) || (x == e.x && y < e.y);
    }
    bool operator==(const DEdge& e) const {
        return x == e.x && y == e.y;
    }
    bool operator!=(const DEdge& e) const {
        return x != e.x || y != e.y;
    }
    int x, y;
};

inline int get_parents(std::vector<std::pair<int, int>>& parents, int j) {
    if (j == parents[j].first) return j;
    int k = get_parents(parents, parents[j].first);
    parents[j].second = (parents[j].second + parents[parents[j].first].second) % 4;
    parents[j].first = k;
    return k;
}

inline int get_parents_orient(std::vector<std::pair<int, int>>& parents, int j) {
    if (j == parents[j].first) return parents[j].second;
    return (parents[j].second + get_parents_orient(parents, parents[j].first)) % 4;
}

inline double fast_acos(double x) {
    double negate = double(x < 0.0f);
    x = std::abs(x);
    double ret = -0.0187293f;
    ret *= x;
    ret = ret + 0.0742610f;
    ret *= x;
    ret = ret - 0.2121144f;
    ret *= x;
    ret = ret + 1.5707288f;
    ret = ret * std::sqrt(1.0f - x);
    ret = ret - 2.0f * negate * ret;
    return negate * (double)M_PI + ret;
}

inline double signum(double value) { return std::copysign((double)1, value); }

/// Always-positive modulo function (assumes b > 0)
inline int modulo(int a, int b) {
    int r = a % b;
    return (r < 0) ? r + b : r;
}

inline Vector3d rotate90_by(const Vector3d &q, const Vector3d &n, int amount) {
    return ((amount & 1) ? (n.cross(q)) : q) * (amount < 2 ? 1.0f : -1.0f);
}

inline Vector2i rshift90(Vector2i shift, int amount) {
    if (amount & 1) shift = Vector2i(-shift.y(), shift.x());
    if (amount >= 2) shift = -shift;
    return shift;
}

inline std::pair<int, int> compat_orientation_extrinsic_index_4(const Vector3d &q0,
                                                                const Vector3d &n0,
                                                                const Vector3d &q1,
                                                                const Vector3d &n1) {
    const Vector3d A[2] = {q0, n0.cross(q0)};
    const Vector3d B[2] = {q1, n1.cross(q1)};

    double best_score = -std::numeric_limits<double>::infinity();
    int best_a = 0, best_b = 0;

    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j < 2; ++j) {
            double score = std::abs(A[i].dot(B[j]));
            if (score > best_score) {
                best_a = i;
                best_b = j;
                best_score = score;
            }
        }
    }

    if (A[best_a].dot(B[best_b]) < 0) best_b += 2;

    return std::make_pair(best_a, best_b);
}

inline std::pair<Vector3d, Vector3d> compat_orientation_extrinsic_4(const Vector3d &q0,
                                                                    const Vector3d &n0,
                                                                    const Vector3d &q1,
                                                                    const Vector3d &n1) {
    const Vector3d A[2] = {q0, n0.cross(q0)};
    const Vector3d B[2] = {q1, n1.cross(q1)};

    double best_score = -std::numeric_limits<double>::infinity();
    int best_a = 0, best_b = 0;

    for (int i = 0; i < 2; ++i) {
        for (int j = 0; j < 2; ++j) {
            double score = std::abs(A[i].dot(B[j]));
            if (score > best_score + 1e-6) {
                best_a = i;
                best_b = j;
                best_score = score;
            }
        }
    }

    const double dp = A[best_a].dot(B[best_b]);
    return std::make_pair(A[best_a], B[best_b] * signum(dp));
}

inline Vector3d middle_point(const Vector3d &p0, const Vector3d &n0, const Vector3d &p1,
                             const Vector3d &n1) {
    /* How was this derived?
     *
     * Minimize \|x-p0\|^2 + \|x-p1\|^2, where
     * dot(n0, x) == dot(n0, p0)
     * dot(n1, x) == dot(n1, p1)
     *
     * -> Lagrange multipliers, set derivative = 0
     *  Use first 3 equalities to write x in terms of
     *  lambda_1 and lambda_2. Substitute that into the last
     *  two equations and solve for the lambdas. Finally,
     *  add a small epsilon term to avoid issues when n1=n2.
     */
    double n0p0 = n0.dot(p0), n0p1 = n0.dot(p1), n1p0 = n1.dot(p0), n1p1 = n1.dot(p1),
           n0n1 = n0.dot(n1), denom = 1.0f / (1.0f - n0n1 * n0n1 + 1e-4f),
           lambda_0 = 2.0f * (n0p1 - n0p0 - n0n1 * (n1p0 - n1p1)) * denom,
           lambda_1 = 2.0f * (n1p0 - n1p1 - n0n1 * (n0p1 - n0p0)) * denom;

    return 0.5f * (p0 + p1) - 0.25f * (n0 * lambda_0 + n1 * lambda_1);
}

inline Vector3d position_floor_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
                                 const Vector3d &p, double scale_x, double scale_y,
                                 double inv_scale_x, double inv_scale_y) {
    Vector3d t = n.cross(q);
    Vector3d d = p - o;
    return o + q * std::floor(q.dot(d) * inv_scale_x) * scale_x +
           t * std::floor(t.dot(d) * inv_scale_y) * scale_y;
}

inline std::pair<Vector3d, Vector3d> compat_position_extrinsic_4(
    const Vector3d &p0, const Vector3d &n0, const Vector3d &q0, const Vector3d &o0,
    const Vector3d &p1, const Vector3d &n1, const Vector3d &q1, const Vector3d &o1, double scale_x,
    double scale_y, double inv_scale_x, double inv_scale_y, double scale_x_1, double scale_y_1,
    double inv_scale_x_1, double inv_scale_y_1) {
    Vector3d t0 = n0.cross(q0), t1 = n1.cross(q1);
    Vector3d middle = middle_point(p0, n0, p1, n1);
    Vector3d o0p =
        position_floor_4(o0, q0, n0, middle, scale_x, scale_y, inv_scale_x, inv_scale_y);
    Vector3d o1p =
        position_floor_4(o1, q1, n1, middle, scale_x_1, scale_y_1, inv_scale_x_1, inv_scale_y_1);

    double best_cost = std::numeric_limits<double>::infinity();
    int best_i = -1, best_j = -1;

    for (int i = 0; i < 4; ++i) {
        Vector3d o0t = o0p + (q0 * (i & 1) * scale_x + t0 * ((i & 2) >> 1) * scale_y);
        for (int j = 0; j < 4; ++j) {
            Vector3d o1t = o1p + (q1 * (j & 1) * scale_x_1 + t1 * ((j & 2) >> 1) * scale_y_1);
            double cost = (o0t - o1t).squaredNorm();

            if (cost < best_cost) {
                best_i = i;
                best_j = j;
                best_cost = cost;
            }
        }
    }

    return std::make_pair(
        o0p + (q0 * (best_i & 1) * scale_x + t0 * ((best_i & 2) >> 1) * scale_y),
        o1p + (q1 * (best_j & 1) * scale_x_1 + t1 * ((best_j & 2) >> 1) * scale_y_1));
}

inline Vector3d position_round_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
                                 const Vector3d &p, double scale_x, double scale_y,
                                 double inv_scale_x, double inv_scale_y) {
    Vector3d t = n.cross(q);
    Vector3d d = p - o;
    return o + q * std::round(q.dot(d) * inv_scale_x) * scale_x +
           t * std::round(t.dot(d) * inv_scale_y) * scale_y;
}

inline Vector2i position_floor_index_4(const Vector3d &o, const Vector3d &q, const Vector3d &n,
                                       const Vector3d &p, double /* unused */, double /* unused */,
                                       double inv_scale_x, double inv_scale_y) {
    Vector3d t = n.cross(q);
    Vector3d d = p - o;
    return Vector2i((int)std::floor(q.dot(d) * inv_scale_x),
                    (int)std::floor(t.dot(d) * inv_scale_y));
}

inline std::pair<Vector2i, Vector2i> compat_position_extrinsic_index_4(
    const Vector3d &p0, const Vector3d &n0, const Vector3d &q0, const Vector3d &o0,
    const Vector3d &p1, const Vector3d &n1, const Vector3d &q1, const Vector3d &o1, double scale_x,
    double scale_y, double inv_scale_x, double inv_scale_y, double scale_x_1, double scale_y_1,
    double inv_scale_x_1, double inv_scale_y_1, double *error) {
    Vector3d t0 = n0.cross(q0), t1 = n1.cross(q1);
    Vector3d middle = middle_point(p0, n0, p1, n1);
    Vector2i o0p =
        position_floor_index_4(o0, q0, n0, middle, scale_x, scale_y, inv_scale_x, inv_scale_y);
    Vector2i o1p = position_floor_index_4(o1, q1, n1, middle, scale_x_1, scale_y_1, inv_scale_x_1,
                                          inv_scale_y_1);

    double best_cost = std::numeric_limits<double>::infinity();
    int best_i = -1, best_j = -1;

    for (int i = 0; i < 4; ++i) {
        Vector3d o0t =
            o0 + (q0 * ((i & 1) + o0p[0]) * scale_x + t0 * (((i & 2) >> 1) + o0p[1]) * scale_y);
        for (int j = 0; j < 4; ++j) {
            Vector3d o1t = o1 + (q1 * ((j & 1) + o1p[0]) * scale_x_1 +
                                 t1 * (((j & 2) >> 1) + o1p[1]) * scale_y_1);
            double cost = (o0t - o1t).squaredNorm();

            if (cost < best_cost) {
                best_i = i;
                best_j = j;
                best_cost = cost;
            }
        }
    }
    if (error) *error = best_cost;

    return std::make_pair(Vector2i((best_i & 1) + o0p[0], ((best_i & 2) >> 1) + o0p[1]),
                          Vector2i((best_j & 1) + o1p[0], ((best_j & 2) >> 1) + o1p[1]));
}

inline void coordinate_system(const Vector3d &a, Vector3d &b, Vector3d &c) {
    if (std::abs(a.x()) > std::abs(a.y())) {
        double invLen = 1.0f / std::sqrt(a.x() * a.x() + a.z() * a.z());
        c = Vector3d(a.z() * invLen, 0.0f, -a.x() * invLen);
    } else {
        double invLen = 1.0f / std::sqrt(a.y() * a.y() + a.z() * a.z());
        c = Vector3d(0.0f, a.z() * invLen, -a.y() * invLen);
    }
    b = c.cross(a);
}

inline Vector3d rotate_vector_into_plane(Vector3d q, const Vector3d &source_normal,
                                         const Vector3d &target_normal) {
    const double cosTheta = source_normal.dot(target_normal);
    if (cosTheta < 0.9999f) {
        if (cosTheta < -0.9999f) return -q;
        Vector3d axis = source_normal.cross(target_normal);
        q = q * cosTheta + axis.cross(q) +
            axis * (axis.dot(q) * (1.0 - cosTheta) / axis.dot(axis));
    }
    return q;
}

inline Vector3d Travel(Vector3d p, const Vector3d &dir, double &len, int &f, VectorXi &E2E,
                       MatrixXd &V, MatrixXi &F, MatrixXd &NF,
                       std::vector<MatrixXd> &triangle_space, double *tx = 0, double *ty = 0) {
    Vector3d N = NF.col(f);
    Vector3d pt = (dir - dir.dot(N) * N).normalized();
    int prev_id = -1;
    int count = 0;
    while (len > 0) {
        count += 1;
        Vector3d t1 = V.col(F(1, f)) - V.col(F(0, f));
        Vector3d t2 = V.col(F(2, f)) - V.col(F(0, f));
        Vector3d N = NF.col(f);
        //		printf("point dis: %f\n", (p - V.col(F(1, f))).dot(N));
        int edge_id = f * 3;
        double max_len = 1e30;
        bool found = false;
        int next_id, next_f;
        Vector3d next_q;
        Matrix3d m, n;
        m.col(0) = t1;
        m.col(1) = t2;
        m.col(2) = N;
        n = m.inverse();
        MatrixXd &T = triangle_space[f];
        VectorXd coord = T * Vector3d(p - V.col(F(0, f)));
        VectorXd dirs = (T * pt);

        double lens[3];
        lens[0] = -coord.y() / dirs.y();
        lens[1] = (1 - coord.x() - coord.y()) / (dirs.x() + dirs.y());
        lens[2] = -coord.x() / dirs.x();
        for (int fid = 0; fid < 3; ++fid) {
            if (fid + edge_id == prev_id) continue;

            if (lens[fid] >= 0 && lens[fid] < max_len) {
                max_len = lens[fid];
                next_id = E2E[edge_id + fid];
                next_f = next_id;
                if (next_f != -1) next_f /= 3;
                found = true;
            }
        }
        if (!found) {
            printf("error...\n");
            exit(0);
        }
        //		printf("status: %f %f %d\n", len, max_len, f);
        if (max_len >= len) {
            if (tx && ty) {
                *tx = coord.x() + dirs.x() * len;
                *ty = coord.y() + dirs.y() * len;
            }
            p = p + len * pt;
            len = 0;
            return p;
        }
        p = V.col(F(0, f)) + t1 * (coord.x() + dirs.x() * max_len) +
            t2 * (coord.y() + dirs.y() * max_len);
        len -= max_len;
        if (next_f == -1) {
            if (tx && ty) {
                *tx = coord.x() + dirs.x() * max_len;
                *ty = coord.y() + dirs.y() * max_len;
            }
            return p;
        }
        pt = rotate_vector_into_plane(pt, NF.col(f), NF.col(next_f));
        f = next_f;
        prev_id = next_id;
    }
    return p;
}
inline Vector3d TravelField(Vector3d p, Vector3d &pt, double &len, int &f, VectorXi &E2E,
                            MatrixXd &V, MatrixXi &F, MatrixXd &NF, MatrixXd &QF, MatrixXd &QV,
                            MatrixXd &NV, std::vector<MatrixXd> &triangle_space, double *tx = 0,
                            double *ty = 0, Vector3d *dir_unfold = 0) {
    Vector3d N = NF.col(f);
    pt = (pt - pt.dot(N) * N).normalized();
    int prev_id = -1;
    int count = 0;
    std::vector<Vector3d> Ns;

    auto FaceQFromVertices = [&](int f, double tx, double ty) {
        const Vector3d &n = NF.col(f);
        const Vector3d &q_1 = QV.col(F(0, f)), &q_2 = QV.col(F(1, f)), &q_3 = QV.col(F(2, f));
        const Vector3d &n_1 = NV.col(F(0, f)), &n_2 = NV.col(F(1, f)), &n_3 = NV.col(F(2, f));
        Vector3d q_1n = rotate_vector_into_plane(q_1, n_1, n);
        Vector3d q_2n = rotate_vector_into_plane(q_2, n_2, n);
        Vector3d q_3n = rotate_vector_into_plane(q_3, n_3, n);
        auto orient = compat_orientation_extrinsic_4(q_1n, n, q_2n, n);
        Vector3d q = (orient.first * tx + orient.second * ty).normalized();
        orient = compat_orientation_extrinsic_4(q, n, q_3n, n);
        q = (orient.first * (tx + ty) + orient.second * (1 - tx - ty)).normalized();
        return q;
    };

    auto BestQFromGivenQ = [&](const Vector3d &n, const Vector3d &q, const Vector3d &given_q) {
        Vector3d q_1 = n.cross(q);
        double t1 = q.dot(given_q);
        double t2 = q_1.dot(given_q);
        if (fabs(t1) > fabs(t2)) {
            if (t1 > 0.0)
                return Vector3d(q);
            else
                return Vector3d(-q);
        } else {
            if (t2 > 0.0)
                return Vector3d(q_1);
            else
                return Vector3d(-q_1);
        }
    };

    while (len > 0) {
        count += 1;
        Vector3d t1 = V.col(F(1, f)) - V.col(F(0, f));
        Vector3d t2 = V.col(F(2, f)) - V.col(F(0, f));
        Vector3d N = NF.col(f);
        Ns.push_back(N);
        //		printf("point dis: %f\n", (p - V.col(F(1, f))).dot(N));
        int edge_id = f * 3;
        double max_len = 1e30;
        bool found = false;
        int next_id, next_f;
        Vector3d next_q;
        Matrix3d m, n;
        m.col(0) = t1;
        m.col(1) = t2;
        m.col(2) = N;
        n = m.inverse();
        MatrixXd &T = triangle_space[f];
        VectorXd coord = T * Vector3d(p - V.col(F(0, f)));
        VectorXd dirs = (T * pt);
        double lens[3];
        lens[0] = -coord.y() / dirs.y();
        lens[1] = (1 - coord.x() - coord.y()) / (dirs.x() + dirs.y());
        lens[2] = -coord.x() / dirs.x();
        for (int fid = 0; fid < 3; ++fid) {
            if (fid + edge_id == prev_id) continue;

            if (lens[fid] >= 0 && lens[fid] < max_len) {
                max_len = lens[fid];
                next_id = E2E[edge_id + fid];
                next_f = next_id;
                if (next_f != -1) next_f /= 3;
                found = true;
            }
        }
        double w1 = (coord.x() + dirs.x() * max_len);
        double w2 = (coord.y() + dirs.y() * max_len);
        if (w1 < 0) w1 = 0.0f;
        if (w2 < 0) w2 = 0.0f;
        if (w1 + w2 > 1) {
            double w = w1 + w2;
            w1 /= w;
            w2 /= w;
        }

        if (!found) {
            printf("error...\n");
            exit(0);
        }
        //		printf("status: %f %f %d\n", len, max_len, f);
        if (max_len >= len) {
            if (tx && ty) {
                *tx = w1;
                *ty = w2;
            }
            Vector3d ideal_q = FaceQFromVertices(f, *tx, *ty);
            *dir_unfold = BestQFromGivenQ(NF.col(f), ideal_q, *dir_unfold);
            for (int i = Ns.size() - 1; i > 0; --i) {
                *dir_unfold = rotate_vector_into_plane(*dir_unfold, Ns[i], Ns[i - 1]);
            }
            p = p + len * pt;
            len = 0;
            return p;
        }
        p = V.col(F(0, f)) + t1 * w1 + t2 * w2;
        len -= max_len;
        if (next_f == -1) {
            if (tx && ty) {
                *tx = w1;
                *ty = w2;
            }
            Vector3d ideal_q = FaceQFromVertices(f, *tx, *ty);
            *dir_unfold = BestQFromGivenQ(NF.col(f), ideal_q, *dir_unfold);
            for (int i = Ns.size() - 1; i > 0; --i) {
                *dir_unfold = rotate_vector_into_plane(*dir_unfold, Ns[i], Ns[i - 1]);
            }
            return p;
        }
        pt = rotate_vector_into_plane(pt, NF.col(f), NF.col(next_f));
        //		pt = BestQFromGivenQ(NF.col(next_f), QF.col(next_f), pt);
        if (dir_unfold) {
            *dir_unfold = BestQFromGivenQ(NF.col(next_f), QF.col(next_f), *dir_unfold);
        }
        f = next_f;
        prev_id = next_id;
    }

    return p;
}

#endif


#define BOOST_TEST_MODULE LFPTest
#define BOOST_TEST_MAIN

#include <iostream>

#include <boost/test/unit_test.hpp>

#include "coreneuron/io/lfp.hpp"
#include "coreneuron/mpi/nrnmpi.h"

using namespace coreneuron;
using namespace coreneuron::lfputils;

template <typename F>
double integral(F f, double a, double b, int n) {
    double step = (b - a) / n;  // width of each small rectangle
    double area = 0.0;          // signed area
    for (int i = 0; i < n; i++) {
        area += f(a + (i + 0.5) * step) * step;  // sum up each small rectangle
    }
    return area;
}

BOOST_AUTO_TEST_CASE(LFP_PointSource_LineSource) {
#if NRNMPI
    nrnmpi_init(nullptr, nullptr);
#endif
    double segment_length{1.0e-6};
    double segment_start_val{1.0e-6};
    std::array<double, 3> segment_start = std::array<double, 3>{0.0, 0.0, segment_start_val};
    std::array<double, 3> segment_end =
        paxpy(segment_start, 1.0, std::array<double, 3>{0.0, 0.0, segment_length});
    double floor{1.0e-6};
    pi = 3.141592653589;

    std::array<double, 10> vals;
    double circling_radius{1.0e-6};
    std::array<double, 3> segment_middle{0.0, 0.0, 1.5e-6};
    double medium_resistivity_fac{1.0};
    for (auto k = 0; k < 10; k++) {
        std::array<double, 3> approaching_elec =
            paxpy(segment_middle, 1.0, std::array<double, 3>{0.0, 1.0e-5 - k * 1.0e-6, 0.0});
        std::array<double, 3> circling_elec =
            paxpy(segment_middle,
                  1.0,
                  std::array<double, 3>{0.0,
                                        circling_radius * std::cos(2.0 * pi * k / 10),
                                        circling_radius * std::sin(2.0 * pi * k / 10)});

        double analytic_approaching_lfp = line_source_lfp_factor(
            approaching_elec, segment_start, segment_end, floor, medium_resistivity_fac);
        double analytic_circling_lfp = line_source_lfp_factor(
            circling_elec, segment_start, segment_end, floor, medium_resistivity_fac);
        double numeric_circling_lfp = integral(
            [&](double x) {
                return 1.0 / std::max(floor,
                                      norm(paxpy(circling_elec,
                                                 -1.0,
                                                 paxpy(segment_end,
                                                       x,
                                                       paxpy(segment_start, -1.0, segment_end)))));
            },
            0.0,
            1.0,
            10000);
        // TEST of analytic vs numerical integration
        std::clog << "ANALYTIC line source " << analytic_circling_lfp
                  << " vs NUMERIC line source LFP " << numeric_circling_lfp << "\n";
        BOOST_REQUIRE_CLOSE(analytic_circling_lfp, numeric_circling_lfp, 1.0e-6);
        // TEST of LFP Flooring
        BOOST_REQUIRE((approaching_elec[1] < 0.866e-6) ? analytic_approaching_lfp == 1.0e6 : true);
        vals[k] = analytic_circling_lfp;
    }
    // TEST of SYMMETRY of LFP FORMULA
    for (size_t k = 0; k < 5; k++) {
        BOOST_REQUIRE(std::abs((vals[k] - vals[k + 5]) /
                               std::max(std::abs(vals[k]), std::abs(vals[k + 5]))) < 1.0e-12);
    }
    std::vector<std::array<double, 3>> segments_starts = {{0., 0., 1.},
                                                          {0., 0., 0.5},
                                                          {0.0, 0.0, 0.0},
                                                          {0.0, 0.0, -0.5}};
    std::vector<std::array<double, 3>> segments_ends = {{0., 0., 0.},
                                                        {0., 0., 1.},
                                                        {0., 0., 0.5},
                                                        {0.0, 0.0, 0.0}};
    std::vector<double> radii{0.1, 0.1, 0.1, 0.1};
    std::vector<std::array<double, 3>> electrodes = {{0.0, 0.3, 0.0}, {0.0, 0.7, 0.8}};
    std::vector<int> indices = {0, 1, 2, 3};
    LFPCalculator<LineSource> lfp(segments_starts, segments_ends, radii, indices, electrodes, 1.0);
    lfp.template lfp<std::vector<double>>({0.0, 1.0, 2.0, 3.0});
    std::vector<double> res_line_source = lfp.lfp_values();
    LFPCalculator<PointSource> lfpp(
        segments_starts, segments_ends, radii, indices, electrodes, 1.0);
    lfpp.template lfp<std::vector<double>>({0.0, 1.0, 2.0, 3.0});
    std::vector<double> res_point_source = lfpp.lfp_values();
    BOOST_REQUIRE_CLOSE(res_line_source[0], res_point_source[0], 1.0);
    BOOST_REQUIRE_CLOSE(res_line_source[1], res_point_source[1], 1.0);
#if NRNMPI
    nrnmpi_finalize();
#endif
}


// Build with:
// emcc --bind -I${EMSCRIPTEN}/system/include -I${QUANTLIB} -I${BOOST} -O3 -s MODULARIZE=1 -s "EXTRA_EXPORTED_RUNTIME_METHODS=['addOnPostRun']" -s EXPORT_NAME=QuantLib -s TOTAL_MEMORY=64MB -o quantlib-embind.js quantlib-embind.cpp ${QUANTLIB}/ql/.libs/libQuantLib.a

// https://www.quantlib.org/slides/dima-ql-intro-1.pdf

#include <math.h>
#include <iostream>
#include <malloc.h>
#include <ql/quantlib.hpp>
#include <emscripten/bind.h>
#include <boost/foreach.hpp>

using namespace std;
using namespace QuantLib;
using namespace emscripten;

namespace
{

    val emval_test_mallinfo()
    {
        const auto &i = mallinfo();
        unsigned long t =
            std::chrono::system_clock::now().time_since_epoch() /
            std::chrono::milliseconds(1);
        val rv(val::object());
        rv.set("arena", val(i.arena));
        rv.set("ordblks", val(i.ordblks));
        rv.set("smblks", val(i.smblks));
        rv.set("hblks", val(i.hblks));
        rv.set("hblkhd", val(i.hblkhd));
        rv.set("usmblks", val(i.usmblks));
        rv.set("fsmblks", val(i.fsmblks));
        rv.set("uordblks", val(i.uordblks));
        rv.set("fordblks", val(i.fordblks));
        rv.set("keepcost", val(i.keepcost));
        rv.set("time", val(t));
        return rv;
    }

    void setValuationDate(Date &date)
    {
        Settings::instance().evaluationDate() = date;
    }

    string timeUnitToString(Period &p)
    {
        stringstream stream;
        stream << p;
        return stream.str();
    }

    double swapNpv(VanillaSwap &swap)
    {
        return swap.NPV();
    }

    string calendarToISOString(Date &d)
    {
        stringstream stream;
        stream << d.year() << "-" << setfill('0') << setw(2) << (int)d.month() << "-" << setfill('0') << setw(2) << d.dayOfMonth();
        return stream.str();
    }

    string calendarToString(Date &d)
    {
        stringstream stream;
        stream << d;
        return stream.str();
    }

    string interestRateToString(InterestRate &r)
    {
        stringstream stream;
        stream.precision(4);
        stream << r;
        return stream.str();
    }

    Date calendarAdvance(Calendar &cal, Date &d, Integer n, TimeUnit unit, BusinessDayConvention c, bool endOfMonth)
    {
        return cal.advance(d, n, unit, c, endOfMonth);
    }

    Date *dateFromISOString(string s)
    {
        int y, m, d;
        sscanf(s.c_str(), "%d-%d-%d", &y, &m, &d);
        return new Date(d, (Month)m, y);
    }

    void swapSetPricingEngine(VanillaSwap &swap, Handle<YieldTermStructure> &discountingTermStructure)
    {
        boost::shared_ptr<PricingEngine> swapEngine(new DiscountingSwapEngine(discountingTermStructure));
        swap.setPricingEngine(swapEngine);
    }

    Handle<YieldTermStructure> *createLogLinearYieldTermStructure(vector<Date> &dates, vector<DiscountFactor> &discountFactor, DayCounter &dayCounter)
    {
        // auto curve = new InterpolatedDiscountCurve<LogLinear>(dates, discountFactor, dayCounter);
        // InterpolatedDiscountCurve<LogLinear> curve(InterpolatedDiscountCurve<LogLinear>(dates, discountFactor, dayCounter));
        // auto yts = boost::make_shared<InterpolatedDiscountCurve<LogLinear>>(*curve);
        boost::shared_ptr<YieldTermStructure> yts(new InterpolatedDiscountCurve<LogLinear>(dates, discountFactor, dayCounter));
        return new Handle<YieldTermStructure>(yts);
    }

    VanillaSwap *createVanillaSwap(VanillaSwap::Type type, Real nominal, const Schedule &fixedSchedule, Rate fixedRate, DayCounter &fixedDayCount,
                                   const Schedule &floatSchedule, IborIndex &iborIndex, Spread spread, DayCounter &floatingDayCount)
    {
        boost::shared_ptr<IborIndex> iborIndexPtr = boost::make_shared<IborIndex>(iborIndex);
        VanillaSwap *res = new VanillaSwap(type, nominal, fixedSchedule, fixedRate, fixedDayCount, floatSchedule, iborIndexPtr, spread, floatingDayCount);
        return res;
    }

    Handle<Quote> *createQuoteHandle(Rate rate)
    {
        boost::shared_ptr<SimpleQuote> ptrSimpleQuote = boost::make_shared<SimpleQuote>(rate);
        boost::shared_ptr<Quote> ptrQuote(ptrSimpleQuote);
        return new Handle<Quote>(ptrQuote);
    }

    Real handleQuoteValue(Handle<Quote> &quote)
    {
        return ((ext::shared_ptr<Quote> &)quote.currentLink())->value();
    }

    OISRateHelper *createOISRateHelper(Natural settlementDays, Period &tenor, Handle<Quote> &fixedRate, OvernightIndex &overnightIndex
                                       /*, Handle<YieldTermStructure> &discountingCurve, bool telescopicValueDates, Natural paymentLag,
                                   BusinessDayConvention paymentConvention, Frequency paymentFrequency, Calendar &paymentCalendar, Period &forwardStart,
                                   Spread overnightSpread */
    )
    {
        ext::shared_ptr<OvernightIndex> ptrOvernightIndex(&overnightIndex);
        return new OISRateHelper(settlementDays, tenor, fixedRate, ptrOvernightIndex /*, discountingCurve,
                                  telescopicValueDates, paymentLag, paymentConvention, paymentFrequency, paymentCalendar, forwardStart, overnightSpread*/
        );
    }

    DatedOISRateHelper *createDatedOISRateHelper(Date &startDate, Date &endDate, Handle<Quote> &fixedRate, OvernightIndex &overnightIndex)
    {
        ext::shared_ptr<OvernightIndex> ptrOvernightIndex(&overnightIndex);
        return new DatedOISRateHelper(startDate, endDate, fixedRate, ptrOvernightIndex);
    }

    FuturesRateHelper *createFuturesRateHelper(Handle<Quote> &price, Date &iborStartDate, Natural lengthInMonths, Calendar &calendar, BusinessDayConvention convention, bool endOfMonth, DayCounter &dayCounter)
    {
        return new FuturesRateHelper(price, iborStartDate, lengthInMonths, calendar, convention, endOfMonth, dayCounter);
    }

    SwapRateHelper *createSwapRateHelper(Handle<Quote> &rate, Period &tenor, Calendar &calendar, Frequency fixedFrequency,
                                         BusinessDayConvention fixedConvention, DayCounter &fixedDayCount, IborIndex &iborIndex)
    {
        boost::shared_ptr<IborIndex> ptrIborIndex = boost::make_shared<IborIndex>(iborIndex);
        // ext::shared_ptr<IborIndex> ptrIborIndex(&iborIndex);
        return new SwapRateHelper(rate, tenor, calendar, fixedFrequency, fixedConvention, fixedDayCount, ptrIborIndex);
    }

    Date *immNextDate(const Date &date, bool mainCycle)
    {
        return new Date(IMM::nextDate(date, mainCycle).serialNumber());
    }

    PiecewiseYieldCurve<Discount, Linear> *createPiecewiseYieldCurveDiscountLinear(Date &referenceDate, vector<RateHelper *> &instruments, DayCounter &dayCounter)
    {
        vector<boost::shared_ptr<RateHelper>> ptrs;
        for (int i = 0; i < instruments.size(); i++)
        {
            // boost::shared_ptr<RateHelper> ptr = boost::make_shared<RateHelper>(instruments[i]);
            // ext::shared_ptr<RateHelper> ptr(&instruments[i]);
            boost::shared_ptr<RateHelper> ptr(instruments[i]);
            ptrs.push_back(ptr);
        }
        return new PiecewiseYieldCurve<Discount, Linear>(referenceDate, ptrs, dayCounter);
    }

    void handle_eptr(std::exception_ptr eptr) // passing by value is ok
    {
        try
        {
            if (eptr)
            {
                std::rethrow_exception(eptr);
            }
        }
        catch (const std::exception &e)
        {
            std::cout << "Caught exception \"" << e.what() << "\"\n";
        }
    }

    InterestRate *yieldTermStructureZeroRate(YieldTermStructure &yieldTermStructure, Date &d, DayCounter &resultDayCounter, Compounding comp, Frequency freq, bool extrapolate)
    {
        // try
        // {
        // std::cout << "Uno" << std::endl;
        // auto dates = yieldTermStructure.jumpDates();
        // std::cout << "Due" << std::endl;
        // for (int i=0; i<dates.size(); i++)
        // {
        //     std::cout << dates[i] << std::endl;
        // }
        // std::cout << "Tre" << std::endl;
        auto r = yieldTermStructure.zeroRate(d, resultDayCounter, comp, freq, extrapolate);
        // std::cout << "Quattro" << std::endl;
        return new InterestRate(r.rate(), r.dayCounter(), r.compounding(), r.frequency()); // Copies from stack to heap memory
        // }
        // catch (...)
        // {
        //     std::exception_ptr eptr = std::current_exception();
        //     handle_eptr(eptr);
        //     // std::cout << eptr->what() << std::endl;
        // }
        // return NULL;
    }

    DiscountFactor yieldTermStructureDiscount(YieldTermStructure &yieldTermStructure, Date &d, bool extrapolate)
    {
        return yieldTermStructure.discount(d, extrapolate);
    }

    InterestRate *yieldTermStructureForwardRate(YieldTermStructure &yieldTermStructure, Date &d1, Date &d2,
                                                DayCounter &resultDayCounter, Compounding comp, Frequency freq, bool extrapolate)
    {
        auto r = yieldTermStructure.forwardRate(d1, d2, resultDayCounter, comp, freq, extrapolate);
        return new InterestRate(r.rate(), r.dayCounter(), r.compounding(), r.frequency()); // Copies from stack to heap memory
    }

    EMSCRIPTEN_BINDINGS(quantlib)
    {
        emscripten::constant<string>("version", QL_VERSION);
        enum_<BusinessDayConvention>("BusinessDayConvention")
            .value("Following", Following)
            .value("ModifiedFollowing", ModifiedFollowing)
            .value("Preceding", Preceding)
            .value("ModifiedPreceding", ModifiedPreceding)
            .value("Unadjusted", Unadjusted)
            .value("HalfMonthModifiedFollowing", HalfMonthModifiedFollowing)
            .value("Nearest", Nearest);
        enum_<Month>("Month")
            .value("January", January)
            .value("February", February)
            .value("March", March)
            .value("April", April)
            .value("May", May)
            .value("June", June)
            .value("July", July)
            .value("August", August)
            .value("September", September)
            .value("October", October)
            .value("November", November)
            .value("December", December)
            .value("Jan", Jan)
            .value("Feb", Feb)
            .value("Mar", Mar)
            .value("Apr", Apr)
            .value("May", May)
            .value("Jun", Jun)
            .value("Jul", Jul)
            .value("Aug", Aug)
            .value("Sep", Sep)
            .value("October", Oct)
            .value("Nov", Nov)
            .value("Dec", Dec);
        enum_<TimeUnit>("TimeUnit")
            .value("D", Days)
            .value("W", Weeks)
            .value("M", Months)
            .value("Y", Years)
            .value("Days", Days)
            .value("Weeks", Weeks)
            .value("Months", Months)
            .value("Years", Years)
            .value("Hours", Hours)
            .value("Minutes", Minutes)
            .value("Seconds", Seconds)
            .value("Milliseconds", Milliseconds)
            .value("Microseconds", Microseconds);
        enum_<DateGeneration::Rule>("DateGenerationRule")
            .value("Backward", DateGeneration::Backward)
            .value("Forward", DateGeneration::Forward)
            .value("Zero", DateGeneration::Zero)
            .value("ThirdWednesday", DateGeneration::ThirdWednesday)
            .value("Twentieth", DateGeneration::Twentieth)
            .value("TwentiethIMM", DateGeneration::TwentiethIMM)
            .value("OldCDS", DateGeneration::OldCDS)
            .value("CDS", DateGeneration::CDS)
            .value("CDS2015", DateGeneration::CDS2015);
        enum_<Thirty360::Convention>("Thirty360Convention")
            .value("USA", Thirty360::USA)
            .value("BondBasis", Thirty360::BondBasis)
            .value("European", Thirty360::European)
            .value("EurobondBasis", Thirty360::EurobondBasis)
            .value("Italian", Thirty360::Italian)
            .value("German", Thirty360::German);
        enum_<ActualActual::Convention>("ActualActualConvention")
            .value("ISMA", ActualActual::ISMA)
            .value("Bond", ActualActual::Bond)
            .value("ISDA", ActualActual::ISDA)
            .value("Historical", ActualActual::Historical)
            .value("Actual365", ActualActual::Actual365)
            .value("AFB", ActualActual::AFB)
            .value("Euro", ActualActual::Euro);
        enum_<VanillaSwap::Type>("VanillaSwapType")
            .value("Payer", VanillaSwap::Payer)
            .value("Receiver", VanillaSwap::Receiver);
        enum_<Weekday>("Weekday")
            .value("Sunday", Sunday)
            .value("Monday", Monday)
            .value("Tuesday", Tuesday)
            .value("Wednesday", Wednesday)
            .value("Thursday", Thursday)
            .value("Friday", Friday)
            .value("Saturday", Saturday)
            .value("Sun", Sun)
            .value("Mon", Mon)
            .value("Tue", Tue)
            .value("Wed", Wed)
            .value("Thu", Thu)
            .value("Fri", Fri)
            .value("Sat", Sat);
        enum_<Pillar::Choice>("PillarChoice")
            .value("MaturityDate", Pillar::Choice::MaturityDate)
            .value("LastRelevantDate", Pillar::Choice::LastRelevantDate)
            .value("CustomDate", Pillar::Choice::CustomDate);
        enum_<UnitedStates::Market>("UnitedStatesMarket")
            .value("Settlement", UnitedStates::Market::Settlement)
            .value("NYSE", UnitedStates::Market::NYSE)
            .value("GovernmentBond", UnitedStates::Market::GovernmentBond)
            .value("NERC", UnitedStates::Market::NERC)
            .value("LiborImpact", UnitedStates::Market::LiborImpact)
            .value("FederalReserve", UnitedStates::Market::FederalReserve);
        enum_<UnitedKingdom::Market>("UnitedKingdomMarket")
            .value("Settlement", UnitedKingdom::Market::Settlement)
            .value("Exchange", UnitedKingdom::Market::Exchange)
            .value("Metals", UnitedKingdom::Market::Metals);
        enum_<JointCalendarRule>("JointCalendarRule")
            .value("JoinHolidays", JointCalendarRule::JoinHolidays)
            .value("JoinBusinessDays", JointCalendarRule::JoinBusinessDays);
        enum_<Frequency>("Frequency")
            .value("NoFrequency", NoFrequency)
            .value("Once", Once)
            .value("Annual", Annual)
            .value("Semiannual", Semiannual)
            .value("EveryFourthMonth", EveryFourthMonth)
            .value("Quarterly", Quarterly)
            .value("Bimonthly", Bimonthly)
            .value("Monthly", Monthly)
            .value("EveryFourthWeek", EveryFourthWeek)
            .value("Biweekly", Biweekly)
            .value("Weekly", Weekly)
            .value("Daily", Daily)
            .value("OtherFrequency", OtherFrequency);
        enum_<Compounding>("Compounding")
            .value("Simple", Simple)
            .value("Compounded", Compounded)
            .value("Continuous", Continuous)
            .value("SimpleThenCompounded", SimpleThenCompounded)
            .value("CompoundedThenSimple", CompoundedThenSimple);

        register_vector<int>("Vector<int>")
            .constructor<int>();
        register_vector<double>("Vector<double>")
            .constructor<int>();
        register_vector<Date>("Vector<Date>")
            .constructor<int>();
        register_vector<RateHelper *>("Vector<RateHelper>")
            .constructor<int>();

        class_<Date>("Date")
            .constructor<>()
            .constructor<int>()
            .constructor<int, Month, int>()
            .function("serialNumber", &Date::serialNumber)
            .function("weekday", &Date::weekday)
            .function("dayOfMonth", &Date::dayOfMonth)
            .function("dayOfYear", &Date::dayOfYear)
            .function("month", &Date::month)
            .function("year", &Date::year)
            .function("toISOString", &calendarToISOString)
            .function("toString", &calendarToString)
            .class_function("fromISOString", &dateFromISOString, allow_raw_pointers())
            .class_function("isLeap", &Date::isLeap);
        class_<Schedule>("Schedule")
            .constructor<vector<Date>>()
            .constructor<Date, Date, Period, Calendar, BusinessDayConvention, BusinessDayConvention, DateGeneration::Rule, bool, Date, Date>()
            .function("size", &Schedule::size)
            .function("dates", &Schedule::dates);

        /* Calendars */
        class_<Calendar>("Calendar")
            .function("name", &Calendar::name)
            .function("toString", &calendarToString)
            .function("isBusinessDay", &Calendar::isBusinessDay)
            .function("adjust", &Calendar::adjust)
            // .function<Calendar, Date, Integer, TimeUnit>("advance", &Calendar::advance)
            .function("advance", &calendarAdvance);
        class_<JointCalendar, base<Calendar>>("JointCalendar")
            .constructor<Calendar, Calendar, JointCalendarRule>()
            .constructor<Calendar, Calendar, Calendar, JointCalendarRule>()
            .constructor<Calendar, Calendar, Calendar, Calendar, JointCalendarRule>();
        class_<TARGET, base<Calendar>>("TARGET").constructor<>();
        class_<NullCalendar, base<Calendar>>("NullCalendar").constructor<>();
        class_<UnitedKingdom, base<Calendar>>("UnitedKingdom").constructor<UnitedKingdom::Market>();
        class_<UnitedStates, base<Calendar>>("UnitedStates").constructor().constructor<UnitedStates::Market>();
        class_<Sweden, base<Calendar>>("Sweden").constructor<>();

        /* Period */
        class_<Period>("Period")
            .constructor<int, TimeUnit>()
            .function("toString", &timeUnitToString);

        /* DayCounters */
        class_<DayCounter>("DayCounter")
            .function("name", &DayCounter::name)
            .function("dayCount", &DayCounter::dayCount)
            .function("yearFraction", &DayCounter::yearFraction);
        class_<Thirty360, base<DayCounter>>("Thirty360")
            .constructor<>()
            .constructor<Thirty360::Convention, bool>();
        class_<Actual360, base<DayCounter>>("Actual360")
            .constructor<>()
            .constructor<bool>();
        class_<Actual365Fixed, base<DayCounter>>("Actual365Fixed").constructor<>();
        class_<ActualActual, base<DayCounter>>("ActualActual")
            .constructor<>()
            .constructor<ActualActual::Convention, Schedule>();
        class_<Business252, base<DayCounter>>("Business252").constructor<>();
        class_<boost::optional<BusinessDayConvention>>("OptionalBusinessDayConvention");

        /* Misc */
        class_<VanillaSwap>("VanillaSwap")
            .constructor(&createVanillaSwap, allow_raw_pointers())
            // .constructor<VanillaSwap::Type, Rate, Schedule, Rate, DayCounter, Schedule, ext::shared_ptr<IborIndex>, Spread, DayCounter, boost::optional<BusinessDayConvention>>()
            .class_function("create", &createVanillaSwap, allow_raw_pointers())
            .function("setPricingEngine", &swapSetPricingEngine)
            .function("NPV", &swapNpv);
        class_<Handle<YieldTermStructure>>("Handle<YieldTermStructure>");
        emscripten::function("createVanillaSwap", &createVanillaSwap, allow_raw_pointers());
        emscripten::function("mallinfo", &emval_test_mallinfo);
        emscripten::function("setValuationDate", &setValuationDate);
        emscripten::function("createLogLinearYieldTermStructure", &createLogLinearYieldTermStructure, allow_raw_pointers());
        class_<InterestRate>("InterestRate")
            .function("rate", &InterestRate::rate)
            .function("toString", &interestRateToString);

        class_<Quote>("Quote");
        class_<SimpleQuote, base<Quote>>("SimpleQuote")
            .constructor<Real>();
        class_<Handle<Quote>>("QuoteHandle")
            .constructor(&createQuoteHandle, allow_raw_pointers())
            .function("value", &handleQuoteValue);
        class_<IMM>("IMM")
            .class_function("nextDate", &immNextDate, allow_raw_pointers());

        /* Indicies */
        class_<Index>("Index")
            .function("addFixing", &Index::addFixing);
        class_<InterestRateIndex, base<Index>>("InterestRateIndex")
            .function("fixingDate", &InterestRateIndex::fixingDate);
        class_<IborIndex, base<InterestRateIndex>>("IborIndex");
        class_<Euribor, base<IborIndex>>("Euribor")
            .constructor<Period, Handle<YieldTermStructure>>();
        class_<OvernightIndex, base<IborIndex>>("OvernightIndex");
        class_<Eonia, base<OvernightIndex>>("Eonia").constructor();
        class_<Libor, base<IborIndex>>("Libor");
        class_<USDLibor, base<Libor>>("USDLibor")
            .constructor<Period>()
            .constructor<Period, Handle<YieldTermStructure>>();

        /* Helpers */
        class_<RateHelper>("RateHelper")
            .function("maturityDate", &RateHelper::maturityDate)
            .function("quote", &RateHelper::quote);
        class_<DepositRateHelper, base<RateHelper>>("DepositRateHelper")
            .constructor<Handle<Quote>, Period, Natural, Calendar, BusinessDayConvention, bool, DayCounter>();
        class_<OISRateHelper, base<RateHelper>>("OISRateHelper").constructor(&createOISRateHelper, allow_raw_pointers());
        class_<DatedOISRateHelper, base<RateHelper>>("DatedOISRateHelper").constructor(&createDatedOISRateHelper, allow_raw_pointers());
        class_<FuturesRateHelper, base<RateHelper>>("FuturesRateHelper").constructor(&createFuturesRateHelper, allow_raw_pointers());
        class_<SwapRateHelper, base<RateHelper>>("SwapRateHelper").constructor(&createSwapRateHelper, allow_raw_pointers());

        class_<YieldTermStructure>("ZeroInflationTermStructure")
            .function("zeroRate", &yieldTermStructureZeroRate, allow_raw_pointers())
            .function("discount", &yieldTermStructureDiscount)
            .function("forwardRate", &yieldTermStructureForwardRate, allow_raw_pointers());
        class_<PiecewiseYieldCurve<Discount, Linear>, base<YieldTermStructure>>("PiecewiseYieldCurve<Discount,Linear>")
            .constructor(&createPiecewiseYieldCurveDiscountLinear, allow_raw_pointers());
    }

} // namespace

// In [9]: eonia_curve_c = PiecewiseLogCubicDiscount(0, TARGET(),
// helpers, Actual365Fixed())
// eonia_curve_c.enableExtrapolation()


//
//  nDijkstra.hpp
//  evoNik
//
//  Created by Nikhil Joshi on 4/28/12.
//  Copyright (c) 2012 California Institute of Technology. All rights reserved.
//

#ifndef evoNik_nDijkstra_hpp
#define evoNik_nDijkstra_hpp

# include <cmath>
#include <iostream>

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "constants.hpp"

class nDijkstra_map2d{
public:
    std::map< std::pair< unsigned int, unsigned int > , int > sectorMap;
    int add(unsigned int x, unsigned int y);
    int get(unsigned int x, unsigned int y);
    void clear(void)                           {  sectorMap.clear();  }
};

class nDijkstra{
public:
    nDijkstra_map2d sectorMap;
    size_t height;
    size_t width;
    int old_goal_w;
    int old_goal_h;

    typedef boost::adjacency_list <boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, boost::property <boost::edge_weight_t, double> > graph_t;
    typedef boost::graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
    typedef boost::graph_traits < graph_t >::edge_descriptor edge_descriptor;

    graph_t test_graph;

    // constructor
    nDijkstra(){
        old_goal_w = old_goal_h = -1;
    }

    void buildGraph(const std::vector<std::vector<unsigned int> > & freeSpaceArray,
                    bool diag);
    void computeFitnessArray(std::vector<std::vector<double> > & fitnessArray,
                             int goal_w,
                             int goal_h,
                             bool cache=true);

};

#endif


#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include <laser_geometry/laser_geometry.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_listener.h>
#include <iostream>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/transforms.h>	//	pcl::transformPointCloud 用到这个头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

laser_geometry::LaserProjection projector_;

sensor_msgs::PointCloud2 cloud;
sensor_msgs::PointCloud2 cloud_2;
sensor_msgs::PointCloud2 cloud_all;
int statue=0;

   Eigen::Matrix4f transform_1;
   //Eigen::Matrix<float, 4, 4> matrix_44;
  // 定义一个旋转矩阵 (见 https://en.wikipedia.org/wiki/Rotation_matrix)
  float theta = 3.142; // 弧度角

  //transform_1 (0,1) = -sin(theta);
  //transform_1 (1,0) = sin (theta);
  //transform_1 (1,1) = cos (theta);
      	//(行, 列)

  // 在 X 轴上定义一个 2.5 米的平移.
  //transform_1(0,3) = -0.240;

  //transform_1(2,3) = -0.106;

 //Eigen::Affine3f transform= Eigen::Affine3f::Identity();
 //Eigen::Affine3f transform_2;

void callback(const sensor_msgs::LaserScan::ConstPtr& scan_msg,const sensor_msgs::LaserScan::ConstPtr& scan_2_msg)
{

        transform_1 (0,0) = cos (theta);
        transform_1 (0,1) = -sin(theta);
        transform_1 (1,0) = sin (theta);
        transform_1 (1,1) = cos (theta);
        transform_1 (0,3) = -0.240;
        transform_1 (2,3) = -0.106;

        pcl::PointCloud<pcl::PointXYZI>::Ptr scan_pcl(new pcl::PointCloud<pcl::PointXYZI>());
        pcl::PointCloud<pcl::PointXYZI>::Ptr scan_pcl_2(new pcl::PointCloud<pcl::PointXYZI>());
        projector_.projectLaser(*scan_msg, cloud);
        pcl::fromROSMsg(cloud, *scan_pcl);
        //std::cout<<"transfrom done"<<std::endl;
        projector_.projectLaser(*scan_2_msg, cloud_2);
        pcl::fromROSMsg(cloud_2, *scan_pcl_2);
        //matrix_44 << cos (theta),-sin(theta),0,-0.240,sin (theta),cos (theta),0,0,0,0,1,-0.106,0,0,0,1;
        //transform.translation() << -0.240, 0.0, -0.106;
        //transform.rotate (Eigen::AngleAxisf (3.142, Eigen::Vector3f::UnitZ()));

        pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
        pcl::transformPointCloud (*scan_pcl_2, *transformed_cloud, transform_1);
        /*for(std::size_t i = 0; i < transformed_cloud->size(); ++i)
        {
          transformed_cloud->points[i].intensity = 64;
        }
        for(std::size_t i = 0; i < scan_pcl->size(); ++i)
        {
          transformed_cloud->points[i].intensity = 128;
        }*/
        pcl::PointCloud<pcl::PointXYZI>::Ptr scan_all_pcl(new pcl::PointCloud<pcl::PointXYZI>());
        *scan_all_pcl =   *scan_pcl+*transformed_cloud;
        pcl::toROSMsg(*scan_all_pcl, cloud_all);
        //point_cloud_publisher_.publish(cloud_all);
        //publishCloudI(&point_cloud_publisher_, *scan_all_pcl);
        statue=1;
        std::cout<<statue<<std::endl;

}

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

  ros::init(argc, argv, "scan_all");
  ros::NodeHandle n_;

  //ros::Publisher scan_all_pub = n.advertise<sensor_msgs::LaserScan>("scan_all", 1000);
  /*ros::Rate loop_rate(10);
  ros::Subscriber sub = n.subscribe("scan", 1000, Callback);
  ros::Subscriber sub_2 = .subscribe("scan_2", 1000, Callback);*/
  ros::Publisher point_cloud_publisher_;
  point_cloud_publisher_ = n_.advertise<sensor_msgs::PointCloud2> ("/cloud", 10000, false);
  message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub(n_, "scan", 1);
  message_filters::Subscriber<sensor_msgs::LaserScan> scan_2_sub(n_, "scan_2_filtered", 1);
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::LaserScan> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), scan_sub, scan_2_sub);
  //message_filters::TimeSynchronizer<sensor_msgs::LaserScan, sensor_msgs::LaserScan> sync(scan_sub, scan_2_sub, 10);
  //sync.registerCallback(boost::bind(&callback, _1, _2));
  ros::Rate loop_rate(30);
  //SubscribeAndPublish SAPObject;
  while (ros::ok())
  {
    sync.registerCallback(boost::bind(&callback, _1, _2));
    std::cout<<"123"<<std::endl;
    point_cloud_publisher_.publish(cloud_all);
    ros::spinOnce();
    loop_rate.sleep();
 }
  //ros::spin();
  return 0;
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifdef BOOST_QVM_TEST_SINGLE_HEADER
#   include BOOST_QVM_TEST_SINGLE_HEADER
#else
#   include <boost/qvm/math.hpp>
#endif

#include <boost/core/lightweight_test.hpp>
#include <stdlib.h>

namespace
    {
    template <class T>
    void
    test1( T (*f1)(T), T (*f2)(T) )
        {
        for( int i=0; i!=100; ++i )
            {
            T a = T(rand()) / T(RAND_MAX);
            BOOST_TEST_EQ(f1(a), f2(a));
            }
        }
    template <class T,class U>
    void
    test2( T (*f1)(T,U), T (*f2)(T,U) )
        {
        for( int i=0; i!=100; ++i )
            {
            T a = T(rand()) / T(RAND_MAX);
            T b = T(rand()) / T(RAND_MAX);
            BOOST_TEST_EQ(f1(a,b), f2(a,b));
            }
        }
    }

int
main()
    {
    test1<float>(&boost::qvm::acos<float>, &::acosf);
    test1<float>(&boost::qvm::asin<float>, &::asinf);
    test1<float>(&boost::qvm::atan<float>, &::atanf);
    test2<float,float>(&boost::qvm::atan2<float>, &::atan2f);
    test1<float>(&boost::qvm::cos<float>, &::cosf);
    test1<float>(&boost::qvm::sin<float>, &::sinf);
    test1<float>(&boost::qvm::tan<float>, &::tanf);
    test1<float>(&boost::qvm::cosh<float>, &::coshf);
    test1<float>(&boost::qvm::sinh<float>, &::sinhf);
    test1<float>(&boost::qvm::tanh<float>, &::tanhf);
    test1<float>(&boost::qvm::exp<float>, &::expf);
    test1<float>(&boost::qvm::log<float>, &::logf);
    test1<float>(&boost::qvm::log10<float>, &::log10f);
    test2<float,float>(&boost::qvm::mod<float>, &::fmodf);
    test2<float,float>(&boost::qvm::pow<float>, &::powf);
    test1<float>(&boost::qvm::sqrt<float>, &::sqrtf);
    test1<float>(&boost::qvm::ceil<float>, &::ceilf);
    test1<float>(&boost::qvm::abs<float>, &::fabsf);
    test1<float>(&boost::qvm::floor<float>, &::floorf);
    test2<float, int>(&boost::qvm::ldexp<float>, &::ldexpf);

    test1<double>(&boost::qvm::acos<double>, &::acos);
    test1<double>(&boost::qvm::asin<double>, &::asin);
    test1<double>(&boost::qvm::atan<double>, &::atan);
    test2<double,double>(&boost::qvm::atan2<double>, &::atan2);
    test1<double>(&boost::qvm::cos<double>, &::cos);
    test1<double>(&boost::qvm::sin<double>, &::sin);
    test1<double>(&boost::qvm::tan<double>, &::tan);
    test1<double>(&boost::qvm::cosh<double>, &::cosh);
    test1<double>(&boost::qvm::sinh<double>, &::sinh);
    test1<double>(&boost::qvm::tanh<double>, &::tanh);
    test1<double>(&boost::qvm::exp<double>, &::exp);
    test1<double>(&boost::qvm::log<double>, &::log);
    test1<double>(&boost::qvm::log10<double>, &::log10);
    test2<double,double>(&boost::qvm::mod<double>, &::fmod);
    test2<double,double>(&boost::qvm::pow<double>, &::pow);
    test1<double>(&boost::qvm::sqrt<double>, &::sqrt);
    test1<double>(&boost::qvm::ceil<double>, &::ceil);
    test1<double>(&boost::qvm::abs<double>, &::fabs);
    test1<double>(&boost::qvm::floor<double>, &::floor);
    test2<double, int>(&boost::qvm::ldexp<double>, &::ldexp);

    test1<long double>(&boost::qvm::acos<long double>, &::acosl);
    test1<long double>(&boost::qvm::asin<long double>, &::asinl);
    test1<long double>(&boost::qvm::atan<long double>, &::atanl);
    test2<long double,long double>(&boost::qvm::atan2<long double>, &::atan2l);
    test1<long double>(&boost::qvm::cos<long double>, &::cosl);
    test1<long double>(&boost::qvm::sin<long double>, &::sinl);
    test1<long double>(&boost::qvm::tan<long double>, &::tanl);
    test1<long double>(&boost::qvm::cosh<long double>, &::coshl);
    test1<long double>(&boost::qvm::sinh<long double>, &::sinhl);
    test1<long double>(&boost::qvm::tanh<long double>, &::tanhl);
    test1<long double>(&boost::qvm::exp<long double>, &::expl);
    test1<long double>(&boost::qvm::log<long double>, &::logl);
    test1<long double>(&boost::qvm::log10<long double>, &::log10l);
    test2<long double,long double>(&boost::qvm::mod<long double>, &::fmodl);
    test2<long double,long double>(&boost::qvm::pow<long double>, &::powl);
    test1<long double>(&boost::qvm::sqrt<long double>, &::sqrtl);
    test1<long double>(&boost::qvm::ceil<long double>, &::ceill);
    test1<long double>(&boost::qvm::abs<long double>, &::fabsl);
    test1<long double>(&boost::qvm::floor<long double>, &::floorl);
    test2<long double, int>(&boost::qvm::ldexp<long double>, &::ldexpl);

    return boost::report_errors();
    }


#include "kmeans_clustering.h"
#include "ocv_kmeans_wrapper.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>

using namespace Eigen;
void kmeans_clustering(VectorXi& idx, int clusters, std::vector<VectorXi>& ibones, std::vector<VectorXi>& imuscle, MatrixXd& mG, SparseMatrix<double>& mC, SparseMatrix<double>& mA, VectorXd& mx0){
        MatrixXd Centroids;
        idx.resize(mC.rows()/12);
        idx.setZero();
        std::cout<<"    kmeans0 un "<<std::endl;

        for(int b=0; b<ibones.size(); b++){
            for(int i=0; i<ibones[b].size(); i++){
                idx[ibones[b][i]] = clusters - 1 - b;
            }
        }
        clusters = clusters - ibones.size();

        std::cout<<"     kmeans1 un"<<std::endl;
        VectorXd CAx0 = mC*mA*mx0;

        int clusters_per_muscle = clusters/imuscle.size();

        for(int m=0; m<imuscle.size(); m++){
            std::cout<<"     kmeans2 un"<<std::endl;
            VectorXi labels;
            MatrixXd Data = MatrixXd::Zero(imuscle[m].size(), 3);

            for(int i=0; i<Data.rows(); i++){
                Data.row(i) = RowVector3d(CAx0[12*imuscle[m][i]+0],CAx0[12*imuscle[m][i]+1],CAx0[12*imuscle[m][i]+2]);
            }

            std::cout<<"     kmeans3 do clustering"<<std::endl;
            if(m==imuscle.size()-1){
                //deal with remainder clusters
                ocv_kmeans(Data, clusters, 1000, Centroids, labels);
            }else{
                ocv_kmeans(Data, clusters_per_muscle, 1000, Centroids, labels);
            }

            std::cout<<"     kmeans4 create element_cluster_map"<<std::endl;
            for(int q=0; q<imuscle[m].size(); q++){
                idx[imuscle[m][q]] = clusters_per_muscle*m + labels[q];
            }
            clusters = clusters - clusters_per_muscle;

        }
        std::cout<<"    kmeans5 un"<<std::endl;

        assert(clusters==0);
        return;
    }


// Example of using the GeographicLib::NearestNeighbor class.  WARNING: this
// creates a file, vptree.xml or vptree.bin, in the current directory.

#include <iostream>

#include <vector>
#include <cstdlib>              // For srand, rand
#include <cmath>                // For asin
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>            // For sort
#include <GeographicLib/NearestNeighbor.hpp>
#include <GeographicLib/Geodesic.hpp>

#if !defined(GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION)
#define GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION 0
#endif

#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
// If Boost serialization is available, use it.
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#endif

using namespace std;
using namespace GeographicLib;

// A structure to hold a geographic coordinate.  Also included is a field for a
// "name".  This is unused in this example.
struct pos {
  double lat, lon;
  string name;
  pos(double lat = 0, double lon = 0, const string& name = "")
    : lat(lat), lon(lon), name(name) {}
};

pos randompos() {
  double r, lat, lon;
  r = 2 * (rand() + 0.5) / (RAND_MAX + 1.0) - 1;
  lat = asin(r) / Math::degree();
  r = 2 * (rand() + 0.5) / (RAND_MAX + 1.0) - 1;
  lon = 180 * r;
  return pos(lat, lon);
}

// A class to compute the distance between 2 positions.
class DistanceCalculator {
private:
  Geodesic _geod;
public:
  explicit DistanceCalculator(const Geodesic& geod)
    : _geod(geod) {}
  double operator() (const pos& a, const pos& b) const {
    double s12;
    _geod.Inverse(a.lat, a.lon, b.lat, b.lon, s12);
    return s12;
  }
};

typedef NearestNeighbor<double, pos, DistanceCalculator> GeodesicNeighbor;

// Pick 10000 points on the ellipsoid and determine which ones are more than
// 350 km from all the others.

// In this example the NearestNeighbor object is saved to an external file and
// read back in.  This is unnecessary in this simple application, but is useful
// if many different applications need to query the same dataset.

int main() {
  try {
    // Define a distance function object
    DistanceCalculator distance(Geodesic::WGS84());
    srand(0);
    vector<pos> pts;
    int num = 10000;
    // Sample the points
    for (int i = 0; i < num; ++i) pts.push_back(randompos());
    {
      // Illustrate saving and restoring the GeodesicNeighbor
      // construct it
      GeodesicNeighbor posset(pts, distance);
      // and save it
#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      ofstream f("vptree.xml");
      boost::archive::xml_oarchive oa(f);
      oa << BOOST_SERIALIZATION_NVP(posset);
#else
      ofstream ofs("vptree.txt");
      ofs << posset << "\n";
#endif
    }
    // Construct an empty GeodesicNeighbor
    GeodesicNeighbor posset;
    // restore it from the file
    {
#if GEOGRAPHICLIB_HAVE_BOOST_SERIALIZATION
      ifstream f("vptree.xml");
      boost::archive::xml_iarchive ia(f);
      ia >> BOOST_SERIALIZATION_NVP(posset);
#else
      ifstream ifs("vptree.txt");
      ifs >> posset;
#endif
    }
    // Now use it
    vector<int> ind;
    int cnt = 0;
    double thresh = 325000;
    cout << "Points more than " << thresh/1000 << "km from their neighbors\n"
         << "latitude longitude distance\n";
    for (int i = 0; i < num; ++i) {
      // Call search with distance limits = (0, thresh].  Set exhaustive = false
      // so that the search ends as some as a neighbor is found.
      posset.Search(pts, distance, pts[i], ind, 1, thresh, 0, false);
      if (ind.size() == 0) {
        // If no neighbors in (0, thresh], search again with no upper limit and
        // with exhaustive = true (the default).
        double d = posset.Search(pts, distance, pts[i], ind, 1,
                                 numeric_limits<double>::max(), 0);
        cout << pts[i].lat << " " << pts[i].lon << " " << d << "\n";
        ++cnt;
      }
    }
    int setupcost, numsearches, searchcost, mincost, maxcost;
    double mean, sd;
    posset.Statistics(setupcost, numsearches, searchcost, mincost, maxcost,
                      mean, sd);
    int totcost = setupcost + searchcost, exhaustivecost = num * (num - 1) / 2;
    cout
      << "Number of distance calculations = " << totcost << "\n"
      << "With an exhaustive search = " << exhaustivecost << "\n"
      << "Ratio = " << double(totcost) / exhaustivecost << "\n"
      << "Efficiency improvement = "
      << 100 * (1 - double(totcost) / exhaustivecost) << "%\n";
  }
  catch (const exception& e) {
    cerr << "Caught exception: " << e.what() << "\n";
    return 1;
  }
}


#include <Eigen/Core>
#include "densecrf.h"
#include "densecrf_wrapper.h"

DenseCRFWrapper::DenseCRFWrapper(int npixels, int nlabels)
: m_npixels(npixels), m_nlabels(nlabels) {
	m_crf = new DenseCRF(npixels, nlabels);
}

DenseCRFWrapper::~DenseCRFWrapper() {
	delete m_crf;
}

int DenseCRFWrapper::npixels() { return m_npixels; }
int DenseCRFWrapper::nlabels() { return m_nlabels; }

void DenseCRFWrapper::add_pairwise_energy(float* pairwise_costs_ptr, float* features_ptr, int nfeatures) {
	m_crf->addPairwiseEnergy(
		Eigen::Map<const Eigen::MatrixXf>(features_ptr, nfeatures, m_npixels),
		new MatrixCompatibility(
			Eigen::Map<const Eigen::MatrixXf>(pairwise_costs_ptr, m_nlabels, m_nlabels)
		),
		DIAG_KERNEL,
		NORMALIZE_SYMMETRIC
	);
}

void DenseCRFWrapper::set_unary_energy(float* unary_costs_ptr) {
	m_crf->setUnaryEnergy(
		Eigen::Map<const Eigen::MatrixXf>(
			unary_costs_ptr, m_nlabels, m_npixels)
	);
}

void DenseCRFWrapper::map(int n_iters, int* labels) {
	VectorXs labels_vec = m_crf->map(n_iters);
	for (int i = 0; i < m_npixels; i ++)
		labels[i] = labels_vec(i);
}


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

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

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

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

 /*!
    @ingroup group-trigonometric
    Function object implementing sincpi capabilities

    Computes the sinpi cardinal  value of its parameter that is sin(Pi*x)/(Pi*x).
    @par Semantic:

    For every parameter of floating type T

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

    is similar to:

    @code
    T r = x ? sinpi(x)/(Pi<T>()*x) : One;
    @endcode

    @see sin, sinc, sinhc

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

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

#endif


#include "drake/multibody/contact_solvers/sparse_linear_operator.h"

#include <memory>

#include <Eigen/SparseCore>
#include <gtest/gtest.h>

namespace drake {
namespace multibody {
namespace contact_solvers {
namespace internal {
namespace {

using SparseMatrixd = Eigen::SparseMatrix<double>;
using SparseVectord = Eigen::SparseVector<double>;
using Eigen::VectorXd;
using Triplet = Eigen::Triplet<double>;

// This method makes a sparse matrix that emulates the contact Jacobian that we
// would have in an application in which a subset of the vertices of a mesh is
// in contact. More specifically, if nc is the number of vertices in contact,
// vc ∈ ℝ³ˣⁿᶜ is the vector that concatenates the 3D contact velocities of all
// nc contact points and, v is the vector that concatenates the 3D velocities of
// all nv vertices in the mesh, then the contact Jacobian Jc is defined such
// that vc = Jc⋅v. Jc is of size 3nc x 3nv.
//
// N.B. Though inspired in a real application, this is only meant for test
// purposes while in a real application with meshes the computation of this
// Jacobian is more complex. Our goal is to generate a sparse matrix so that we
// can attach a LinearOperator interface to it for testing purposes.
//
// num_vertices:
//   The number of vertices nv in a hypothetical mesh. The number of
//   generalized velocities in this case equals 3 * num_vertices.
// vertices_in_contact:
//   set of the vertices that are in "contact". The size of vertices_in_contact
//   is nc, the number of contact points.
//
// For each contact point ic, its velocity simply equals the velocity for that
// vertex iv. Therefore the Jacobian contains an identity matrix at 3x3 block
// (ic, iv). All other entries are zero. Thus the Jacobian is very sparse.
SparseMatrixd MakeMeshInContactJacobian(
    int num_vertices, const std::vector<int>& vertices_in_contact) {
  const int num_contacts = static_cast<int>(vertices_in_contact.size());
  const int nnz = 3 * num_contacts;
  std::vector<Triplet> triplets;
  triplets.reserve(nnz);
  for (int ic = 0; ic < num_contacts; ++ic) {
    const int v = vertices_in_contact[ic];
    DRAKE_DEMAND(v < num_vertices);
    triplets.emplace_back(3 * ic, 3 * v, 1.0);
    triplets.emplace_back(3 * ic + 1, 3 * v + 1, 1.0);
    triplets.emplace_back(3 * ic + 2, 3 * v + 2, 1.0);
  }
  SparseMatrixd J(3 * num_contacts, 3 * num_vertices);
  J.setFromTriplets(triplets.begin(), triplets.end());
  return J;
}

class ContactJacobianTest : public ::testing::Test {
 protected:
  void SetUp() override {
    J_ = MakeMeshInContactJacobian(kNumVertices_, contact_set_);
    Jop_ = std::make_unique<SparseLinearOperator<double>>("Jc", &J_);
    num_rows_ = 3 * static_cast<int>(contact_set_.size());
  }

  const int kNumVertices_ = 300;
  const std::vector<int> contact_set_{12, 3, 75, 100, 99, 233, 7};
  int num_rows_{};
  int kCols_{3 * kNumVertices_};
  SparseMatrixd J_;
  std::unique_ptr<SparseLinearOperator<double>> Jop_;
};

TEST_F(ContactJacobianTest, Construction) {
  EXPECT_EQ(Jop_->name(), "Jc");
  EXPECT_EQ(Jop_->rows(), num_rows_);
  EXPECT_EQ(Jop_->cols(), kCols_);
}

TEST_F(ContactJacobianTest, MultiplyDense) {
  VectorXd y(num_rows_);
  const VectorXd x = VectorXd::LinSpaced(kCols_, 0.0, 1.0);
  Jop_->Multiply(x, &y);

  VectorXd y_expected = J_ * x;
  // y's values should equal those in y_expected bit by bit.
  EXPECT_EQ(y, y_expected);
}

TEST_F(ContactJacobianTest, MultiplyByTransposeDense) {
  VectorXd y(kCols_);
  const VectorXd x = VectorXd::LinSpaced(num_rows_, 0.0, 1.0);
  Jop_->MultiplyByTranspose(x, &y);

  VectorXd y_expected = J_.transpose() * x;
  // y's values should equal those in y_expected bit by bit.
  EXPECT_EQ(y, y_expected);
}

TEST_F(ContactJacobianTest, MultiplySparse) {
  SparseVectord y(num_rows_);
  const SparseVectord x = VectorXd::LinSpaced(kCols_, 0.0, 1.0).sparseView();
  Jop_->Multiply(x, &y);

  VectorXd y_expected = J_ * x;
  // y's values should equal those in y_expected bit by bit.
  EXPECT_EQ(VectorXd(y), y_expected);
}

TEST_F(ContactJacobianTest, MultiplyByTransposeSparse) {
  SparseVectord y(kCols_);
  const SparseVectord x = VectorXd::LinSpaced(num_rows_, 0.0, 1.0).sparseView();
  Jop_->MultiplyByTranspose(x, &y);

  VectorXd y_expected = J_.transpose() * x;
  // y's values should equal those in y_expected bit by bit.
  EXPECT_EQ(VectorXd(y), y_expected);
}

TEST_F(ContactJacobianTest, AssembleMatrix) {
  SparseMatrixd Jcopy(Jop_->rows(), Jop_->cols());
  Jop_->AssembleMatrix(&Jcopy);

  // Required before we access their data pointers.
  Jcopy.makeCompressed();
  J_.makeCompressed();

  // We verify the Jcopy is an exact bit by bit copy of J_.
  // Eigen does not offer SparseMatrix::operator==() and therefore we compare
  // the results by explicitly comparing the individual components of the CCS
  // format.
  Eigen::Map<VectorX<double>> Jcopy_values(Jcopy.valuePtr(), Jcopy.nonZeros());
  Eigen::Map<VectorX<double>> J_values(J_.valuePtr(), J_.nonZeros());
  EXPECT_EQ(Jcopy_values, J_values);

  Eigen::Map<VectorX<int>> Jcopy_inner(Jcopy.innerIndexPtr(),
                                       Jcopy.innerSize());
  Eigen::Map<VectorX<int>> J_inner(J_.innerIndexPtr(), J_.innerSize());
  EXPECT_EQ(Jcopy_inner, J_inner);

  Eigen::Map<VectorX<int>> Jcopy_outer(Jcopy.outerIndexPtr(),
                                       Jcopy.outerSize());
  Eigen::Map<VectorX<int>> J_outer(J_.outerIndexPtr(), J_.outerSize());
  EXPECT_EQ(Jcopy_outer, J_outer);
}

}  // namespace
}  // namespace internal
}  // namespace contact_solvers
}  // namespace multibody
}  // namespace drake


#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sensor_msgs/LaserScan.h>
#include <stdio.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <cv.h>
#include <opencv2/opencv.hpp>
#include <vector>
#include <map>
#include <math.h>
#include <ros/package.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/utils.h>
#include <boost/shared_ptr.hpp>

//TODO: add theta shift as per pose
using namespace cv;

int scan_beams;
double dist_resolution, fov, scan_resolution, max_invalid_range, max_range;
std::string frame_id;
bool test_with_mouse_click;
long int seq_id = 0;
std::map<uint16_t, float> scan;
Mat input_image,image;
void mouse_click_callback();
boost::shared_ptr<sensor_msgs::LaserScan> get_scan_from_image(int col, int row, double pose_theta);
ros::Publisher scan_pub;
std::vector<double> distances;
geometry_msgs::TransformStamped image_to_map;
sensor_msgs::LaserScan laser_scan;
inline double get_theta_image(int col_a, int row_a, int col_b, int row_b)
{
   return atan2(-1*(row_b - row_a), col_b - col_a); // representing row as a true Y axis

}

geometry_msgs::TransformStamped get_transform(std::vector<double> map_origin, Mat &image)
{
    geometry_msgs::Pose pose_msg;
    pose_msg.position.x = map_origin[0];
    pose_msg.position.y = map_origin[1];
    pose_msg.position.z = 0;
    tf2::Quaternion q;
    q.setRPY(0, 0, map_origin[2]);
    pose_msg.orientation = tf2::toMsg(q);
    tf2::Transform map_image_origin_to_map, map_to_map_image_origin, image_to_map_image_origin;
    tf2::fromMsg(pose_msg, map_to_map_image_origin);
    map_image_origin_to_map = map_to_map_image_origin.inverse();
    pose_msg.position.x = 0;
    pose_msg.position.y = -1 * image.rows * dist_resolution;
    pose_msg.position.z = 0;
    q.setRPY(0, 0, 0);
    pose_msg.orientation = tf2::toMsg(q);
    tf2::fromMsg(pose_msg, image_to_map_image_origin);
    tf2::Transform image_to_map_origin;
    image_to_map_origin = image_to_map_image_origin*map_image_origin_to_map;
    image_to_map.transform = tf2::toMsg(image_to_map_origin);
    image_to_map.header.seq = 1;
    image_to_map.header.stamp = ros::Time::now();
    image_to_map.header.frame_id = 'image';
    image_to_map.child_frame_id = 'map';
    ROS_INFO("transform:%f, %f,",image_to_map.transform.translation.x, image_to_map.transform.translation.y);

    return image_to_map;
}

void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{

    if  ( event == EVENT_LBUTTONDOWN )
    {
        for (uint16_t i = 0; i < scan_beams; ++i)
        {
            scan[i] = max_invalid_range;
        }
        scan_pub.publish(get_scan_from_image(x,y,-0.0));

    }

}

boost::shared_ptr<sensor_msgs::LaserScan> get_scan_from_image(int col, int row, double pose_theta)
{

    float r,theta;
    int pix_max_range = max_range/dist_resolution;
    int start_row, start_col, end_row, end_col;
    int crop_x,crop_y;
    start_col =  std::max(0, col - pix_max_range);
    end_col = std::min(image.cols, col + pix_max_range);
    start_row = std::max(0, row - pix_max_range);
    end_row = std::min(image.rows, row + pix_max_range);
    // ROS_INFO("ROI:%d,%d,%d,%d   ,%f  total:%d", start_col, end_col, start_row, end_row, dist_resolution,(end_col-start_col)*(end_row-start_row) );

    for (uint16_t i = start_col; i < end_col; i++)
    {
        for (uint16_t j = start_row; j < end_row; j++)
        {
            if(image.at<uchar>(j,i)<150)
            {
                r = sqrt(pow((col - i),2)+pow((row - j),2)) * dist_resolution;
                if (r>max_range)
                {
                    continue;
                }
                theta = get_theta_image(col, row, i, j) - pose_theta;
                theta = atan2(sin(theta), cos(theta));
                int index = (theta - laser_scan.angle_min)/scan_resolution;
                if (index < 0 || index >=scan_beams)
                    continue;

                int skip_checks = dist_resolution/(r * scan_resolution);//width beam in denominator
                for (uint16_t k = std::max(0,index - skip_checks/2); k <= std::min(index + skip_checks/2, scan_beams); ++k)
                {
                    if(r < scan[k])
                    {
                        scan[k] = r;
                    }
                }

            }

        }

    }

    for (uint16_t i = 0; i < scan_beams; ++i)
    {
        laser_scan.ranges[i] = scan[i];
    }
    laser_scan.header.stamp = ros::Time::now();
    boost::shared_ptr<sensor_msgs::LaserScan> ptr_scan;
    ptr_scan.reset(new sensor_msgs::LaserScan(laser_scan));
    return (ptr_scan);
}

void pose_callback(const geometry_msgs::PoseStamped msg)
{
    geometry_msgs::PoseStamped image_pose;
    tf2::doTransform(msg, image_pose, image_to_map);
    int col_pix = image_pose.pose.position.x / dist_resolution;
    int row_pix = -1 * image_pose.pose.position.y / dist_resolution;// row corresponds to negative y axis
    for (uint16_t i = 0; i < scan_beams; ++i)
    {
        scan[i] = max_invalid_range;
    }
    double pose_theta = tf2::getYaw(image_pose.pose.orientation);
    // ROS_INFO("pixel:%f ,%f ,%f",image_pose.pose.position.x,image_pose.pose.position.x, pose_theta);
    scan_pub.publish(get_scan_from_image(col_pix,row_pix,pose_theta));

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle nh;
    std::string map_name;
    scan_pub = nh.advertise<sensor_msgs::LaserScan>("/scan",1);
    ros::Subscriber pose_sub = nh.subscribe("/pose_stamped", 1, &pose_callback);

    //Loading laser scan information
    nh.param("/scan_params/fov", fov, 2*M_PI );
    nh.param("/scan_params/max_range", max_range, 30.0);
    nh.param("/scan_params/max_invalid_range", max_invalid_range, 1048.0);
    nh.param("/scan_params/scan_beams", scan_beams, 628);
    nh.param("/scan_params/test_with_mouse_click", test_with_mouse_click, true);
    nh.param("/scan_params/frame_id", frame_id, std::string("laser"));
    scan_resolution= (fov)/scan_beams ;
    std::vector<double> origin;
    if(!nh.getParam("/map_data/origin",origin))
    {
        ROS_ERROR("cant find the parameters of the map");
        return 0;
    }
    if (!nh.getParam("/map_data/resolution",dist_resolution))
    {
        ROS_ERROR("cant find the parameters of the map");
        return 0;
    }
    if (!nh.getParam("/map_data/image",map_name))
    {
        ROS_ERROR("cant find the parameters of the map");
        return 0;
    }

    std::string image_location = ros::package::getPath("scan_from_image");
    image_location = image_location + "/launch/"+map_name;
    ROS_INFO("image_location is:%s", image_location.c_str());
    input_image = imread(image_location.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
    image = input_image.clone();
     ROS_INFO("transform:%f, %f,",origin[0], origin[1]);
    image_to_map = get_transform(origin, input_image);

    for (uint16_t i = 0; i < scan_beams; ++i)
    {
        scan[i]=max_invalid_range;
    }

    ros::Rate loop(5);
    if(test_with_mouse_click)
    {
        int disp_image_size = 800;
        Size size(disp_image_size,disp_image_size);//the dst image size,e.g.100x100
        float scale_image = static_cast<float>(input_image.rows)/disp_image_size;
        dist_resolution = scale_image*dist_resolution;
        resize(input_image, input_image,size);//resize image
        namedWindow("My Window", 1);
    }
    image_to_map = get_transform(origin, input_image);
    image = input_image;

    laser_scan.header.seq = seq_id;
    laser_scan.header.stamp = ros::Time::now();
    laser_scan.header.frame_id = frame_id;
    laser_scan.angle_min = -fov/2;
    laser_scan.angle_max = fov/2;
    laser_scan.angle_increment = (laser_scan.angle_max - laser_scan.angle_min)/scan_beams;
    laser_scan.range_min = 0.0;
    laser_scan.range_max = max_range;
    laser_scan.scan_time = 1.0 / 5.0;
    laser_scan.time_increment = (1.0 / 5.0) / scan_beams;
    laser_scan.ranges.resize(scan_beams);
    laser_scan.intensities.resize(scan_beams);

    while(nh.ok())
    {
        if (test_with_mouse_click)
        {
            mouse_click_callback();
            imshow("My Window", image);
            waitKey(0);
        }

        loop.sleep();
        ros::spinOnce();
    }
    destroyAllWindows();
    return 0;
}

void mouse_click_callback()
{

    setMouseCallback("My Window", CallBackFunc, NULL);

}