//
// Created by huangkun on 2020/2/28.
//

#ifndef OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP
#define OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP

#include <Eigen/Eigen>
#include <ceres/rotation.h>

#include <opengv2/bundle_adjustment/BundleAdjustmentBase.hpp>
#include <opengv2/spline/BsplineReal.hpp>

namespace opengv2 {
    class SplineBundleAdjustmentV2 : public BundleAdjustmentBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        SplineBundleAdjustmentV2(bool fixTsb, bool robust);

        void
        run(const std::map<double, Bodyframe::Ptr> &keyframes,
            const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        template<class T>
        static inline void
        derToRotMatrix(const Eigen::Matrix<T, 3, 1> &unitY, const T &theta, Eigen::Matrix<T, 3, 3> &Rbw) {
            Rbw.row(1) = unitY.transpose();
            Eigen::Matrix<T, 3, 1> n = unitY.cross(Eigen::Vector3d(0, 0, 1));
            n.normalize();
            Eigen::Matrix<T, 3, 1> z = n.cross(unitY);

            Rbw.row(2) = (ceres::cos(theta) * z + ceres::sin(theta) * n).transpose();
            Rbw.row(2) /= Rbw.row(2).norm();
            Rbw.row(0) = Rbw.row(1).cross(Rbw.row(2)); // X = Y x Z
        }

        static inline void rotMatrixToDer(const Eigen::Matrix3d &Rbw, Eigen::Vector3d &unitY, double &theta) {
            unitY = Rbw.row(1).transpose();

            Eigen::Vector3d n = unitY.cross(Eigen::Vector3d(0, 0, 1));
            n /= n.norm();
            Eigen::Vector3d z = n.cross(unitY);
            z /= z.norm();

            double tmp = Rbw.row(2) * z;
            theta = std::acos(tmp > 1.0 ? 1.0 : tmp);// [0, pi]

            if (Rbw.row(2) * n < 0)
                theta = -theta;
        }

        struct ReprojectionError {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            ReprojectionError(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                              std::shared_ptr<std::vector<std::vector<double>>> basis,
                              bool isMono)
                    : isMono_(isMono), bearingVector_(bearingVector), basis_(std::move(basis)) {}

            template<typename T>
            bool operator()(const T *const cp0, const T *const cp1, const T *const cp2, const T *const cp3,
                            const T *const lm, const T *const Tcb, T *residuals) const {
                // Map to Eigen type
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_0(cp0);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_1(cp1);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_2(cp2);
                Eigen::Map<Eigen::Matrix<T, 4, 1> const> const cp_3(cp3);
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const lm_e(lm);
                Eigen::Quaternion<T> Qcb(Tcb);
                Qcb.normalize();
                Eigen::Map<Eigen::Matrix<T, 3, 1> const> const tcb(Tcb + 4);
                Eigen::Map<Eigen::Matrix<T, 3, 1>> res(residuals);

                // spline evaluation
                Eigen::Matrix<T, 4, 1> twb_theta = basis_->at(0)[0] * cp_0 + basis_->at(0)[1] * cp_1 +
                                                   basis_->at(0)[2] * cp_2 + basis_->at(0)[3] * cp_3;
                Eigen::Matrix<T, 3, 1> y = basis_->at(1)[0] * cp_0.head(3) + basis_->at(1)[1] * cp_1.head(3) +
                                           basis_->at(1)[2] * cp_2.head(3) + basis_->at(1)[3] * cp_3.head(3);
                y.normalize();

                Eigen::Matrix<T, 3, 3> Rbw;
                derToRotMatrix(y, twb_theta[3], Rbw);

                Eigen::Matrix<T, 3, 1> Xb = Rbw * (lm_e - twb_theta.head(3));
                Eigen::Matrix<T, 3, 1> Xc = Qcb * Xb + tcb;
                if (isMono_)
                    Xc.normalize();

                res = bearingVector_ - Xc;
                return true;
            }

            static ceres::CostFunction *
            Create(const Eigen::Ref<const Eigen::Vector3d> &bearingVector,
                   const std::shared_ptr<std::vector<std::vector<double>>> &basis, bool isMono) {
                return (new ceres::AutoDiffCostFunction<ReprojectionError, 3, 4, 4, 4, 4, 3, 7>(
                        new ReprojectionError(bearingVector, basis, isMono)));
            }

            bool isMono_;
            Eigen::Vector3d bearingVector_;

            // for spline evaluation
            std::shared_ptr<std::vector<std::vector<double>>> basis_;
        };

    protected:
        void optimize(ceres::Problem &problem,
                      const std::map<double, Bodyframe::Ptr> &keyframes,
                      const std::map<int, LandmarkBase::Ptr> &landmarks) override;

        bool fixTsb_;
        bool robust_;

        BsplineReal<4> Tspline_; // twb[3] + angle, by cubic spline
    };
}

#endif //OPENGV2_SPLINEBUNDLEADJUSTMENTV2_HPP


#include <iostream>
#include <string>
#include <cmath>
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <boost/range/numeric.hpp>
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Tools/Subdivider/Uniform/LoopT.hh>
#include <tuple>

typedef OpenMesh::TriMesh_ArrayKernelT<>  MyMesh;

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

const double sigma = 1.1;
const double beta = .4;
const double delta = .1;
const double phi = .4;

typedef boost::array< double , 2 > state_type;

void ODE( const state_type &x , state_type &dxdt , double t )
{
    dxdt[0] = (sigma * x[0]) - (beta*x[0]*x[1]);
    dxdt[1] = (delta * x[0] * x[1]) - (phi * x[1]);
}

double prey_count[100];
double pred_count[100];
int i = 0;
tuple<double, double> write_ODE( const state_type &x , const double t )
{

    prey_count[i] = x[0];
    pred_count[i] = x[1];

    cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << prey_count[i] << endl;

    i++;
    return std::make_tuple(pred_count[i], prey_count[i]);

}

int main()
{
  MyMesh mesh;

  //Create Vertices
  MyMesh::VertexHandle vhandle[3];
  vhandle[0] = mesh.add_vertex(MyMesh::Point( 0, 0 ,  0));
  vhandle[1] = mesh.add_vertex(MyMesh::Point( 1, 0 ,  0));
  vhandle[2] = mesh.add_vertex(MyMesh::Point( 1, 1 ,  0));
  vhandle[3] = mesh.add_vertex(MyMesh::Point( 0, 1 ,  0));

  //Create Faces
  std::vector<MyMesh::VertexHandle>  face_vhandles;
  face_vhandles.clear();
  face_vhandles.push_back(vhandle[0]);
  face_vhandles.push_back(vhandle[1]);
  face_vhandles.push_back(vhandle[2]);
  face_vhandles.push_back(vhandle[3]);
  mesh.add_face(face_vhandles);

 // Initialize Subdivider Tool
 OpenMesh::Subdivider::Uniform::LoopT<MyMesh> Loop;

 // Execute 5 subdivision steps
 Loop.attach(mesh);
 Loop( 3 );
 Loop.detach();

 // Solve ODE
 state_type x = { 30 , 30}; // initial conditions
 runge_kutta4< state_type > stepper;
 integrate(ODE, x, 0.0, 100.0, 0.01, write_ODE);
  for (int k = 0; k<i; k++)
  { cout << prey_count[k] << endl; }

// integrate_n_steps( stepper , ODE , x , 0.0, 1.0 , 50, write_ODE );

string Output;

OpenMesh::IO::Options wopt;
wopt = OpenMesh::IO::Options::FaceColor;

 mesh.request_face_colors();

for (int count = 0; count<i; count++)
{
	int count_prey = prey_count[count];
	int count_pred = pred_count[count];
	int total_count = count_prey + count_pred;
	int z = 0;
	int y = 0;

	for (MyMesh::FaceIter f_it=mesh.faces_begin();  f_it !=mesh.faces_end(); ++f_it)
	{

		if ( z < count_prey)
			{
		    		mesh.set_color(*f_it,MyMesh::Color(0,0,255));
			}

		else if ( (y > count_prey) && (y < total_count) )
			{
				mesh.set_color(*f_it,MyMesh::Color(255,0,0));

			}

		else
			{
		       	        mesh.set_color(*f_it,MyMesh::Color(0,0,0));
			}

			z++;
			y++;
	}

Output = std::to_string(count) + "output.off";
OpenMesh::IO::write_mesh(mesh, Output, wopt);

}

return 0;

}


#ifndef CPP_TENSOR_LINE_DEFINITIONS_HH
#define CPP_TENSOR_LINE_DEFINITIONS_HH

#include <Eigen/Core>

namespace tl
{

using Vec3d = Eigen::Vector3d;
using Mat3d = Eigen::Matrix3d;

/**
 * Rank/order of an eigenvalue of a 3x3 matrix
 */
enum class ERank : int
{
    First = 0,
    Second = 1,
    Third = 2
};

/**
 * Tensor line solution point.
 */
struct TLPoint
{
    Vec3d pos; ///< position
    ERank s_rank; ///< rank of eigenvector of tensor field S
    ERank t_rank; ///< rank of eigenvector of tensor field T
    Vec3d eivec; ///< Eigenvector direction
    double s_eival; ///< Eigenvalue for tensor field S
    double t_eival; ///< Eigenvalue for tensor field T
    bool s_has_imaginary; ///< S has any imaginary eigenvalues at the position
    bool t_has_imaginary; ///< T has any imaginary eigenvalues at the position
    std::size_t cluster_size; ///< Number of candidate points that contributed
    double pos_uncertainty; ///< Size of the last subdivision cell in
                            /// position space
    double dir_uncertainty; ///< Size of the last subdivision cell in
                            /// direction space
    double line_stability; ///< Measure of numeric stability of the solution
};

} // namespace tl

#endif


//
// Copyright (c) 2017-2020 CNRS INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_fd(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Zero(model.nv));
  VectorXd a(VectorXd::Zero(model.nv));

  /// Check againt non-derivative algo
  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
  computeGeneralizedGravityDerivatives(model,data,q,g_partial_dq);

  VectorXd g0 = computeGeneralizedGravity(model,data_fd,q);
  BOOST_CHECK(data.g.isApprox(g0));

  MatrixXd g_partial_dq_fd(model.nv,model.nv); g_partial_dq_fd.setZero();

  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd g_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    g_plus = computeGeneralizedGravity(model,data_fd,q_plus);

    g_partial_dq_fd.col(k) = (g_plus - g0)/alpha;
    v_eps[k] -= alpha;
  }

  BOOST_CHECK(g_partial_dq.isApprox(g_partial_dq_fd,sqrt(alpha)));
}

BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_fd(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  VectorXd q = randomConfiguration(model);

  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();

  // Check againt non-derivative algo
  MatrixXd static_vec_partial_dq(model.nv,model.nv); static_vec_partial_dq.setZero();
  computeStaticTorqueDerivatives(model,data,q,fext,static_vec_partial_dq);

  VectorXd tau0 = computeStaticTorque(model,data_fd,q,fext);
  BOOST_CHECK(data.tau.isApprox(tau0));

  std::cout << "data.tau: " << data.tau.transpose() << std::endl;
  std::cout << "tau0: " << tau0.transpose() << std::endl;

  MatrixXd static_vec_partial_dq_fd(model.nv,model.nv);

  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = computeStaticTorque(model,data_fd,q_plus,fext);

    static_vec_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] = 0.;
  }

  BOOST_CHECK(static_vec_partial_dq.isApprox(static_vec_partial_dq_fd,sqrt(alpha)));
}

BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_fd(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  rnea(model,data_ref,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k])));
  }

  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
  computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq);

  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
  BOOST_CHECK(rnea_partial_dq.isApprox(g_partial_dq));
  BOOST_CHECK(data.tau.isApprox(data_ref.g));

  VectorXd tau0 = rnea(model,data_fd,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();

  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double alpha = 1e-8;
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));

    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));

  // Check with q and a non zero
  tau0 = rnea(model,data_fd,q,0*v,a);
  rnea_partial_dq_fd.setZero();

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),a);

    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }

  rnea_partial_dq.setZero();
  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  forwardKinematics(model,data_ref,q,VectorXd::Zero(model.nv),a);

  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.oh[k].isApprox(Force::Zero()));
  }

  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));

  // Check with q and v non zero
  const Motion gravity(model.gravity);
  model.gravity.setZero();
  tau0 = rnea(model,data_fd,q,v,VectorXd::Zero(model.nv));
  rnea_partial_dq_fd.setZero();

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,v,VectorXd::Zero(model.nv));

    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }

  VectorXd v_plus(v);
  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();

  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    tau_plus = rnea(model,data_fd,q,v_plus,VectorXd::Zero(model.nv));

    rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
    v_plus[k] -= alpha;
  }

  rnea_partial_dq.setZero();
  rnea_partial_dv.setZero();
  computeRNEADerivatives(model,data,q,v,VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  forwardKinematics(model,data_ref,q,v,VectorXd::Zero(model.nv));

  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
  }

  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));

//    std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.block<10,10>(0,0) << std::endl;
//    std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.block<10,10>(0,0) << std::endl;
//    std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.topRows<10>() << std::endl;
//    std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.topRows<10>() << std::endl;
  // Check with q, v and a non zero
  model.gravity = gravity;
  v_plus = v;
  tau0 = rnea(model,data_fd,q,v,a);
  rnea_partial_dq_fd.setZero();

  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] += alpha;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,v,a);

    rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
    v_eps[k] -= alpha;
  }

  rnea_partial_dv_fd.setZero();
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += alpha;
    tau_plus = rnea(model,data_fd,q,v_plus,a);

    rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
    v_plus[k] -= alpha;
  }

  rnea_partial_dq.setZero();
  rnea_partial_dv.setZero();
  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  forwardKinematics(model,data_ref,q,v,a);

  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
  }

  computeJointJacobiansTimeVariation(model,data_ref,q,v);
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  crba(model,data_ref,q);

  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
  data_ref.M.triangularView<Eigen::StrictlyLower>()
  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));

  BOOST_CHECK(data.tau.isApprox(tau0));
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));

  Data data2(model);
  computeRNEADerivatives(model,data2,q,v,a);
  data2.M.triangularView<Eigen::StrictlyLower>()
  = data2.M.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(rnea_partial_dq.isApprox(data2.dtau_dq));
  BOOST_CHECK(rnea_partial_dv.isApprox(data2.dtau_dv));
  BOOST_CHECK(rnea_partial_da.isApprox(data2.M));

}

BOOST_AUTO_TEST_CASE(test_rnea_derivatives_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);
  typedef Model::Force Force;

  Data data(model), data_fd(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();

  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();

  computeRNEADerivatives(model,data,q,v,a,fext,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  rnea(model,data_ref,q,v,a,fext);

  BOOST_CHECK(data.tau.isApprox(data_ref.tau));

  computeRNEADerivatives(model,data_ref,q,v,a);
  BOOST_CHECK(rnea_partial_dv.isApprox(data_ref.dtau_dv));
  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));

  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();
  MatrixXd rnea_partial_da_fd(model.nv,model.nv); rnea_partial_da_fd.setZero();

  VectorXd v_eps(VectorXd::Zero(model.nv));
  VectorXd q_plus(model.nq);
  VectorXd tau_plus(model.nv);
  const double eps = 1e-8;

  const VectorXd tau_ref = rnea(model,data_ref,q,v,a,fext);
  for(int k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = integrate(model,q,v_eps);
    tau_plus = rnea(model,data_fd,q_plus,v,a,fext);

    rnea_partial_dq_fd.col(k) = (tau_plus - tau_ref) / eps;

    v_eps[k] = 0.;
  }
  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(eps)));

  VectorXd v_plus(v);
  for(int k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;

    tau_plus = rnea(model,data_fd,q,v_plus,a,fext);

    rnea_partial_dv_fd.col(k) = (tau_plus - tau_ref) / eps;

    v_plus[k] -= eps;
  }
  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(eps)));

  VectorXd a_plus(a);
  for(int k = 0; k < model.nv; ++k)
  {
    a_plus[k] += eps;

    tau_plus = rnea(model,data_fd,q,v,a_plus,fext);

    rnea_partial_da_fd.col(k) = (tau_plus - tau_ref) / eps;

    a_plus[k] -= eps;
  }

  rnea_partial_da.triangularView<Eigen::Lower>() = rnea_partial_da.transpose().triangularView<Eigen::Lower>();
  BOOST_CHECK(rnea_partial_da.isApprox(rnea_partial_da_fd,sqrt(eps)));

  // test the shortcut
  Data data_shortcut(model);
  computeRNEADerivatives(model,data_shortcut,q,v,a,fext);
  BOOST_CHECK(data_shortcut.dtau_dq.isApprox(rnea_partial_dq));
  BOOST_CHECK(data_shortcut.dtau_dv.isApprox(rnea_partial_dv));
  data_shortcut.M.triangularView<Eigen::Lower>() = data_shortcut.M.transpose().triangularView<Eigen::Lower>();
  BOOST_CHECK(data_shortcut.M.isApprox(rnea_partial_da));
}

BOOST_AUTO_TEST_CASE(test_rnea_derivatives_vs_kinematics_derivatives)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  /// Check againt computeGeneralizedGravityDerivatives
  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();

  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));

  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
    BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
  }
}

BOOST_AUTO_TEST_CASE(test_multiple_calls)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  Data data1(model), data2(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill(1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v(VectorXd::Random(model.nv));
  VectorXd a(VectorXd::Random(model.nv));

  computeRNEADerivatives(model,data1,q,v,a);
  data2 = data1;

  for(int k = 0; k < 20; ++k)
  {
    computeRNEADerivatives(model,data1,q,v,a);
  }

  BOOST_CHECK(data1.J.isApprox(data2.J));
  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));

  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
  BOOST_CHECK(data1.dFda.isApprox(data2.dFda));

  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
  BOOST_CHECK(data1.M.isApprox(data2.M));
}

BOOST_AUTO_TEST_CASE(test_get_coriolis)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_ref(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd tau = VectorXd::Random(model.nv);

  computeCoriolisMatrix(model,data_ref,q,v);

  computeRNEADerivatives(model,data,q,v,tau);
  getCoriolisMatrix(model,data);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  for(JointIndex k = 1; k < model.joints.size(); ++k)
  {
    BOOST_CHECK(data.B[k].isApprox(data_ref.B[k]));
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
  }

  BOOST_CHECK(data.C.isApprox(data_ref.C));
}

BOOST_AUTO_TEST_SUITE_END()


#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ras_follower/moveTo.h>
#include <ras_follower/Rotate.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <math.h>
#include <boost/math/special_functions/round.hpp>

#define ZERO 1E-1
/*
 * States: 0 idle
 *         1 Following
 *         -1 Error following
 *         2 Externally aborted
 *         8 rotating
 */
class PathFollower
{
public:
  ros::NodeHandle nH;
  ros::Subscriber point_subscriber;
  ros::Subscriber grid_subscriber;
  ros::Publisher twist_publisher;
  ros::Publisher marker_publisher;
  ros::Publisher state_publisher;
  ros::ServiceServer service;
  ros::ServiceServer abort_follow_service;
  ros::ServiceServer rotate_Service;
  ros::ServiceServer relative_rotate_Service;
  std_msgs::Int32 mStateMsg;
  nav_msgs::OccupancyGrid gridMap;
  tf::Vector3 mTarget;
  geometry_msgs::Pose mTargetPose;

  float windowSize;
  double a;
  double b;
  int l;
  float alpha; //rad
  int T;
  int s_max;
  int targetSector;
  int k_n;
  int k_f;
  float V_max;
  float h_m;
  float P_angle;
  float robotWidth;
  float mLengthFromCenterToFront;
  bool mErrorFollowing;
  int mMaxSearchWidth;
  double mTolerance;
  bool mShouldKillMotorsOnTarget;
  double mTargetAngle;

  int mState;
  const static int RUNNING = 1;
  const static int ROTATING_TO_POSE = 2;
  const static int DONE_ROTATING = 3;
  const static int ABORTED = 4;
  const static int IDLE = 5;
  const static int DONE = 6;

  std::vector<double> smothHist;

  tf::Stamped<tf::Pose> robotPoseInWorld;
  tf::Vector3 robotPointInGrid;
  tf::Transformer transFormer;

  PathFollower() :
                  nH("~"),
                  targetSector(18),
                  k_n(18),
                  k_f(18),
                  mErrorFollowing(false)
    {
    windowSize = 2;
    a = 2;
    b = 2;
    alpha =2*M_PI/72;
    l = 5;
    T = 100;
    s_max = 15;
    h_m = T*0.9;
    V_max = 0.2;
    P_angle = 1;
    robotWidth = 0.4;
    mLengthFromCenterToFront = 0.18;

    smothHist.resize((int) 2*M_PI/alpha,0);

    mStateMsg.data = 0;
    mState = IDLE;

    nH.getParam("window_size", windowSize);
    nH.getParam("a", a);
    nH.getParam("b", b);
    nH.getParam("alpha", alpha);
    nH.getParam("l", l);
    nH.getParam("T", T);
    nH.getParam("s_max", s_max);
    nH.getParam("V_max", V_max);
    nH.getParam("h_m", h_m);
    nH.getParam("P_angle", P_angle);
    nH.getParam("robot_width", robotWidth);
    nH.getParam("distance_between_front_and_center", mLengthFromCenterToFront);
    nH.getParam("max_search_width", mMaxSearchWidth);

    service = nH.advertiseService("/robot/goTo", &PathFollower::servicCallBack, this);
    abort_follow_service = nH.advertiseService("/robot/abort_follow_service", &PathFollower::abortServicCallBack, this);
    rotate_Service = nH.advertiseService("/robot/rotate_to_target", &PathFollower::rotateToTarget, this);
    relative_rotate_Service = nH.advertiseService("/robot/relative_rotate_to_target", &PathFollower::relRotateToTarget, this);
    point_subscriber = nH.subscribe("/localization/pose",1, &PathFollower::PoseCallback, this);
    grid_subscriber = nH.subscribe("/mapping/grid",1, &PathFollower::gridCallback, this);
    twist_publisher = nH.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    marker_publisher = nH.advertise<visualization_msgs::MarkerArray>("markers", 1);
    state_publisher = nH.advertise<std_msgs::Int32>("state",1);

    tf::Vector3 orig(0,0,0);
    this->robotPoseInWorld.setOrigin(orig);
  }

  bool abortServicCallBack(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
    shutDownMotors();
    mState = IDLE;
    return true;
  }

  bool relRotateToTarget(ras_follower::Rotate::Request &req, ras_follower::Rotate::Response &rsp){
    tf::Quaternion angleQ = tf::createQuaternionFromYaw(req.target.angular.z + tf::getYaw(robotPoseInWorld.getRotation()));
    geometry_msgs::Pose p;
    tf::quaternionTFToMsg(angleQ, p.orientation);
    mTargetPose = p;
    mState = ROTATING_TO_POSE;
    mErrorFollowing = false;
    return true;

  }

  bool rotateToTarget(ras_follower::Rotate::Request &req, ras_follower::Rotate::Response &rsp){
      double targetAngleInWorld = std::atan2(req.target.linear.y - robotPoseInWorld.getOrigin().getY(),
                                             req.target.linear.x - robotPoseInWorld.getOrigin().getX());
      tf::Quaternion angleQ = tf::createQuaternionFromYaw(targetAngleInWorld);
      geometry_msgs::Pose p;
      tf::quaternionTFToMsg(angleQ, p.orientation);
      mTargetPose = p;
      mState = ROTATING_TO_POSE;
      mErrorFollowing = false;
      return true;
  }

  void gridCallback(const nav_msgs::OccupancyGridConstPtr &grid){
    //Assume grid is in world.
    gridMap = *grid;
  }

  void PoseCallback(const geometry_msgs::PoseStampedConstPtr &pose) {
      tf::poseStampedMsgToTF(*pose, this->robotPoseInWorld);
  }

  bool servicCallBack(ras_follower::moveTo::Request &req,
                      ras_follower::moveTo::Response &res) {
    mErrorFollowing = false;
    mTarget = tf::Vector3(req.goal.pose.position.x, req.goal.pose.position.y, 0);
    mTargetPose = req.goal.pose;
    mTolerance = req.tolerance;
    mShouldKillMotorsOnTarget = req.turnOffMotors.data;
    mState = RUNNING;
    return true;
  }

  void mainLoop(){
    ros::Rate tRate(10);
    mState = IDLE;
    while(nH.ok()){
      float velocity;
      float angle;
      tf::Quaternion tfQ;
      tRate.sleep();
      ros::spinOnce();
      switch (mState) {
      case IDLE:
        mStateMsg.data = 0;
        state_publisher.publish(mStateMsg);
        break;
      case RUNNING:
        mStateMsg.data = 1;
        state_publisher.publish(mStateMsg);
        ROS_INFO("Calculating Vel and angle...");
        robotPointInGrid = tf::Vector3(gridMap.info.resolution*gridMap.info.width/2,gridMap.info.resolution*gridMap.info.height/2, 0 );
        this->calculateVelocity(mTarget, velocity, angle);
        broadcastSections();
        ROS_INFO("Tol: %f", mTolerance);
        if(isRobotOnTarget(mTarget, mTolerance)) {
          //Sucess!
          if(mShouldKillMotorsOnTarget){
            mState = ROTATING_TO_POSE;
            continue;
          } else {
            mState = IDLE;
            continue;
          }
        } else if (mErrorFollowing) {
          //Fail...
          shutDownMotors();
          mStateMsg.data =  -1;
          state_publisher.publish(mStateMsg);
          mState = IDLE;
          continue;
        }
        ROS_INFO("Sending Vel: %f, and angle: %f", velocity, angle);
        this->sendForceToMotorController(velocity,angle);
        break;
      case ROTATING_TO_POSE:
        mStateMsg.data = 8;
        state_publisher.publish(mStateMsg);
        tf::quaternionMsgToTF(mTargetPose.orientation, tfQ);
        mTargetAngle = tf::getYaw(tfQ);
        angle =mTargetAngle- tf::getYaw(robotPoseInWorld.getRotation());
        if(std::abs(angle) < M_PI/50){
          shutDownMotors();
          mState = IDLE;
          continue;
        }
        velocity = 0;
        ROS_INFO_STREAM("target: " << mTargetAngle << ", angle: " << tf::getYaw(robotPoseInWorld.getRotation()) << ", diff: " << angle);
        ROS_INFO("Sending Vel: %f, and angle: %f", velocity, angle);
        this->sendForceToMotorController(velocity,angle);
        break;
      case ABORTED:
        mStateMsg.data = 2;
        state_publisher.publish(mStateMsg);
        mState = IDLE;
        break;
      default:
        break;
      }
    }
    shutDownMotors();
  }

  bool isRobotOnTarget(tf::Vector3& targetVector, float tolerance){
    bool answer = (this->robotPoseInWorld.getOrigin() - targetVector).length() < tolerance;
    tf::Transform robotCenterFromWorldTransform;
    robotCenterFromWorldTransform.setOrigin(robotPoseInWorld.getOrigin());
    robotCenterFromWorldTransform.setRotation(robotPoseInWorld.getRotation());
    tf::Vector3 tipPose = robotCenterFromWorldTransform(tf::Vector3(mLengthFromCenterToFront,0,0));
    answer |=   (tipPose - targetVector).length() < tolerance;
    return answer;
  }

  void broadcastSections() {
    int maxK = (int) 2*M_PI/alpha;
    visualization_msgs::MarkerArray allSections;
    allSections.markers.resize(maxK);
    for (int k = 0; k < maxK; ++k) {
      visualization_msgs::Marker aMarker;
      tf::Stamped<tf::Pose> markerPose;
      markerPose.setOrigin(robotPointInGrid);
      markerPose.setRotation(tf::createQuaternionFromYaw(k*alpha));
      geometry_msgs::PoseStamped aPose;
      tf::poseStampedTFToMsg(markerPose, aPose);
      aMarker.pose = aPose.pose;
      aMarker.type = aMarker.ARROW;
      aMarker.color.a = 0.6;
      aMarker.scale.x = 0.5;
      aMarker.scale.y = 0.01;
      aMarker.scale.z = 0.01;
      aMarker.header.frame_id = "/live_map";
      aMarker.header.stamp = ros::Time::now();
      aMarker.id = k;
      aMarker.ns = "sectors";
      if(smothHist[k] > T){
        aMarker.color.r = 1;
      } else {
        aMarker.color.g = 1;
      }
      allSections.markers[k] = aMarker;
    }
    marker_publisher.publish(allSections);
  }

  void getSmoothPolarHistogram(std::vector<double>& smothHist, tf::Vector3& targetVector)
  {
    int cellMinX = int((robotPointInGrid.getX() - windowSize/2)/gridMap.info.resolution);
    int cellMinY = int((robotPointInGrid.getY() - windowSize/2)/gridMap.info.resolution);
    int cellMaxX = int((robotPointInGrid.getX() + windowSize/2)/gridMap.info.resolution);
    int cellMaxY = int((robotPointInGrid.getY() + windowSize/2)/gridMap.info.resolution);
    if(cellMinX<0){
      cellMinX = 0;
    }
    if(cellMinY<0){
      cellMinY = 0;
    }
    if(gridMap.info.width <= cellMaxX){
      cellMaxX = gridMap.info.width-1;
    }
    if(gridMap.info.height <= cellMaxY){
      cellMaxY = gridMap.info.height-1;
    }
    for (int Ycell = cellMinY ; Ycell <= cellMaxY; ++Ycell) {
      for (int Xcell = cellMinX; Xcell <= cellMaxX; ++Xcell) {
        int rowNr = Ycell*gridMap.info.width + Xcell;
        float x = Xcell*gridMap.info.resolution;
        float y = Ycell*gridMap.info.resolution;
        tf::Vector3 objectVec(x,y,0);
        objectVec = objectVec - robotPointInGrid; //Moves the vector to be robot -> object.
        //See report @ <url:http://www-personal.umich.edu/~johannb/Papers/paper16.pdf/>
        float beta = std::atan2((y - robotPointInGrid.getY()),(x - robotPointInGrid.getX()));
        float distance = sqrt(std::pow(y - robotPointInGrid.getY(),2) + std::pow(x - robotPointInGrid.getX(), 2));
        float extraAngle = std::atan2(robotWidth/2, distance);
        float m = std::pow(gridMap.data[rowNr], 2)*(a-b*distance);
        for (int sector = (int)( (beta-extraAngle)/alpha); sector <= (int)((beta+extraAngle)/alpha); sector++) {
          if(objectVec.dot(targetVector)/targetVector.length() < targetVector.length())
            smothHist[mod(sector,smothHist.size())] += m;
        }
      }
    }

  }

  void getKnKfWithTargetInside(int targetSector, std::vector<double> &smothHist, int &k_f, int &k_n)
  {
    k_n = targetSector;
    k_f = targetSector;
  }

  void getKnKfWithTargetOutside(std::vector<double> &smothHist, int targetSector,int& k_n, int& k_f)
  {
    int i = 1;
    bool haveNotFoundGoodVallyStart = true;
    bool goingRight = true;

    while(haveNotFoundGoodVallyStart) {
      int upIndex = targetSector + i;
      int downIndex = targetSector - i;
      if(smothHist[mod(upIndex,smothHist.size())] < T){
        haveNotFoundGoodVallyStart = false;
        k_n = upIndex;
        goingRight = false;
      } else if(smothHist[mod(downIndex,smothHist.size())] < T){
        haveNotFoundGoodVallyStart = false;
        k_n = downIndex;
        goingRight = true;
      }
      if(i > mMaxSearchWidth){ //We have searched the hole space and can not move... :(
        mErrorFollowing = true;
        return;
      }
      i++;
    }
    int direction;
    if(goingRight){
      direction = -1;
    } else {
      direction = 1;
    }

    bool haveNotFoundEnd = true;
    i = 0;
    while(haveNotFoundEnd) {
      int index = k_n + i;
      if(smothHist[mod(index,smothHist.size())] > T){
        haveNotFoundEnd = false;
        k_f = index;
      }
      i += direction;
    }

    if(abs(k_f-k_n) > s_max){
      if(goingRight){
        k_f = k_n - s_max;
      } else {
        k_f = k_n + s_max;
      }
    }
    return;
  }

  void calculateVelocity(tf::Vector3 &target, float &velocity, float &angle) {
    //Look for okey vally:
    tf::Transform transform;
    transform.setOrigin(robotPoseInWorld.getOrigin());
    transform.setRotation(robotPoseInWorld.getRotation());
    transform = transform.inverse();
    tf::Vector3 targetInRobotCenter = transform(target);
    targetSector = int(atan2(targetInRobotCenter.getY(), targetInRobotCenter.getX())/alpha);
    if(std::isnan(targetSector)){
      return;
    }
    //Create circle histogram
    std::fill(smothHist.begin(), smothHist.end(), 0);
    getSmoothPolarHistogram(smothHist, targetInRobotCenter);

    bool targetIsInSection = smothHist[mod(targetSector,smothHist.size())] < T;
    //Check if target is in good vally, if so look for edges.
    if(targetIsInSection) {
      getKnKfWithTargetInside(targetSector, smothHist, k_f, k_n);
    } else {
      getKnKfWithTargetOutside(smothHist, targetSector,k_n, k_f);
      if(mErrorFollowing)
        return;
    }
    //We have sektin k_n -> k_f
    //Time to calculate direction.
    int theata_sector = int((k_n + k_f)/2);

     //Calculate speed
    float h_prim = smothHist[mod(theata_sector,smothHist.size())];
    float h_primprim = std::min(h_prim, h_m);
    float V_prim = V_max*(1-h_primprim/h_m);
    //Return force
    velocity = V_prim;
    angle = theata_sector*alpha;
  }

  void sendForceToMotorController(float velocity, float angle) {
    geometry_msgs::Twist controllTwist;
    double angleDiff = mod2(angle, 2*M_PI);
    if(abs(angleDiff)> M_PI){
      if(angleDiff< 0){
        angleDiff += 2*M_PI;
      } else {
        angleDiff -= 2*M_PI;
      }
    }
    controllTwist.linear.x = velocity*std::cos(angleDiff);
    controllTwist.angular.z =P_angle*(angleDiff);
    controllTwist.angular.z = std::min(std::max(controllTwist.angular.z,-2.0),2.0);

    if(smothHist[0] >  T || controllTwist.linear.x < 0) {
		controllTwist.linear.x = 0;
    	if(controllTwist.angular.z > 0)
            controllTwist.angular.z = std::max(controllTwist.angular.z, 0.5);
    	else if(controllTwist.angular.z < 0)
            controllTwist.angular.z = std::min(controllTwist.angular.z, -0.5);
        mStateMsg.data = 8;
        state_publisher.publish(mStateMsg);
    } else {
    	mStateMsg.data = 1;
        state_publisher.publish(mStateMsg);
    }
    twist_publisher.publish(controllTwist);
  }
  void shutDownMotors() {
    geometry_msgs::Twist controllTwist;
    controllTwist.linear.x = 0;
    controllTwist.angular.z = 0;
    twist_publisher.publish(controllTwist);
  }

  int mod(int a, int b)
  { return (a%b+b)%b; }

  float mod2(float a, float b)
  { return fmod((fmod(a,b)+b),b); }

};

int main(int argc, char **argv)
{
  // Set up ROS.
  ros::init(argc, argv, "path_follower");
  PathFollower pF;
  pF.mainLoop();
}


#include "LinearRegression_binding.hpp"

#include <armadillo>
#include <libKriging/LinearRegression.hpp>
#include <random>

#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <armadillo>

#include <carma/carma.h>

PyLinearRegression::PyLinearRegression() : m_internal{new LinearRegression{}} {}

PyLinearRegression::~PyLinearRegression() {}

void PyLinearRegression::fit(const py::array_t<double>& y, const py::array_t<double>& X) {
  arma::mat mat_y = carma::arr_to_col<double>(y, true);
  arma::mat mat_X = carma::arr_to_mat<double>(X, true);
  m_internal->fit(mat_y, mat_X);
}

std::tuple<py::array_t<double>, py::array_t<double>> PyLinearRegression::predict(const py::array_t<double>& X) {
  arma::mat mat_X = carma::arr_to_mat<double>(X, true);
  auto [y_predict, y_stderr] = m_internal->predict(mat_X);
  return std::make_tuple(carma::col_to_arr(y_predict, true), carma::col_to_arr(y_stderr, true));
}


/*
(c) 2012 Fengtao Fan
*/
#include "AnnotationComputation.h"

#include "Annotated_polyhedron_3.h"
#include "Polyhedron_annotator_3.h"

#include <CGAL/Simple_cartesian.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include<CGAL/Polyhedron_incremental_builder_3.h>

#include <boost/filesystem.hpp>

#include <fstream>

#include <boost/progress.hpp>

#include "SimpleMesh.h"

/********************************/
typedef CGAL::Simple_cartesian<float> Kernel;
typedef CGAL::Annotated_polyhedron_3<Kernel> Polyhedron;
typedef Polyhedron::HalfedgeDS HalfedgeDS;
/*Modifier*/
// A modifier creating a triangle with the incremental builder.
template<class HDS>
class polyhedron_builder : public CGAL::Modifier_base<HDS> {
public:
    std::vector<float> &coords;
    std::vector<int> &tris;

    polyhedron_builder(std::vector<float> &_coords, std::vector<int> &_tris) : coords(_coords), tris(_tris) {}

    void operator()(HDS &hds) {
        typedef typename HDS::Vertex Vertex;
        typedef typename Vertex::Point Point;

        // create a cgal incremental builder
        CGAL::Polyhedron_incremental_builder_3<HDS> B(hds, true);
        B.begin_surface(coords.size() / 3, tris.size() / 3); //100

        // add the polyhedron vertices
        for (int i = 0; i < (int) coords.size(); i += 3) {
            B.add_vertex(Point(coords[i + 0], coords[i + 1], coords[i + 2]));
        }

        // add the polyhedron triangles
        for (int i = 0; i < (int) tris.size(); i += 3) {
            B.begin_facet();
            B.add_vertex_to_facet(tris[i + 0]);
            B.add_vertex_to_facet(tris[i + 1]);
            B.add_vertex_to_facet(tris[i + 2]);
            B.end_facet();
        }

        // finish up the surface
        B.end_surface();
    }
};

/********************************/
int ComputeAnnotation(_SimpleMesh &inMesh, std::vector<std::vector<char> > &vecEdgeAnno, std::vector<int> &tris) {
    std::vector<float> coords;
    //std::vector<int>	tris;
    //
    for (unsigned int i = 0; i < inMesh.vecVertex.size(); i++) {
        coords.push_back(inMesh.vecVertex[i].x);
        coords.push_back(inMesh.vecVertex[i].y);
        coords.push_back(inMesh.vecVertex[i].z);
    }
    //
    //std::cout << coords.size() << std::endl;
    //
    //for (unsigned int i = 0; i < orientTri.size(); i++)
    //{
    //	tris.push_back(orientTri[i][0]);
    //	tris.push_back(orientTri[i][1]);
    //	tris.push_back(orientTri[i][2]);
    //}
    //std::cout << tris.size() << std::endl;
    /*******************************************/
    using namespace std;
    using namespace CGAL;
    using namespace boost;
    //using namespace filesystem;

    typedef Polyhedron::Vertex_handle Vertex_handle;
    typedef Polyhedron::Edge_iterator Edge_iterator;

//	string input_filename( "eight.off" );
//	ifstream input( input_filename.c_str() );
//	if ( !input.is_open() )
//	{
//		cerr << "Cannot open " << input_filename << " for reading" << endl;
//		return EXIT_FAILURE;
//	}
    Polyhedron polyhedron;
    //input >> polyhedron;
    //input.close();
    polyhedron_builder<HalfedgeDS> builder(coords, tris);
    polyhedron.delegate(builder);

    //cout << polyhedron.size_of_vertices() << " vertices" << endl;
    //cout << polyhedron.size_of_facets() << " facets" << endl;

    //CGAL_assertion( polyhedron.is_triangle( polyhedron.halfedges_begin()));

    std::cout << "Time for computing edge annotation : " << std::endl;
    {
        boost::progress_timer t;
        annotate_edges(polyhedron);
    }

    size_t index(0);
    Polyhedron::Vertex_iterator it_v(polyhedron.vertices_begin());
    for (; it_v != polyhedron.vertices_end(); ++it_v)
        it_v->index = index++;

    /////////////
    //string output_filename( "model_annotations.txt" );
    //ofstream output( output_filename );
    //if ( !output.is_open() )
    //{
    //	cerr << "Cannot open " << output_filename << " for writing" << endl;
    //	return EXIT_FAILURE;
    //}
    Edge_iterator it_e(polyhedron.edges_begin());
    //output << it_e->annotation.size() << endl;
    //
    vecEdgeAnno.resize(inMesh.vecEdge.size());
    std::vector<char> tempAnnotation(it_e->annotation.size());
    //
    for (; it_e != polyhedron.edges_end(); ++it_e) {
        Vertex_handle h_a(it_e->vertex());
        Vertex_handle h_b(it_e->opposite()->vertex());

        //output << h_a->index << ' ' << h_b->index << ' ' <<
        //	it_e->annotation << endl;
        for (boost::dynamic_bitset<>::size_type i = 0; i < it_e->annotation.size(); i++) {
            tempAnnotation[i] = it_e->annotation[i];
        }
        //
        int edge_index = -1;
        for (int i = 0; i < inMesh.vecVertex[h_a->index].adjEdges.size(); i++) {
            int loc_edge_index = inMesh.vecVertex[h_a->index].adjEdges[i];
            if (inMesh.vecEdge[loc_edge_index].v0 == h_b->index ||
                inMesh.vecEdge[loc_edge_index].v1 == h_b->index) {
                edge_index = loc_edge_index;
                break;
            }
        }
        if (edge_index < 0) {
            std::cout << "EDGE NOT MATCHED in ANNOTATION COMPUTATION" << std::endl;
            exit(9);
        }
        //
        vecEdgeAnno[edge_index] = tempAnnotation;
        //

    }
    //std::cout << vecEdgeAnno[0].size() << std::endl;
    //output.close();
    //cout << "Annotations written to " << output_filename << endl;

    return 0;
}


/// \file
/// Maintainer: Felice Serena
///

#include "disparity_registration.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <boost/log/trivial.hpp>

namespace MouseTrack {
PointCloud DisparityRegistration::operator()(const FrameWindow &window) const {
  const auto &frames = window.frames();

  // how many points will there be at most?
  int expected_points = 0;
  for (const auto &f : frames) {
    expected_points += f.normalizedDisparityMap.size();
  }

  // absolute transformation matrix relative to first camera
  auto Ts = absoluteTransformations(window);

  std::vector<Inverse> inverses(frames.size());
  for (size_t i = 0; i < frames.size(); i += 1) {
    Eigen::Matrix4d mat = frames[i].rotationCorrection * Ts[i];
    inverses[i] = prepareInverseTransformation(mat);
  }

  int labelsCount = frames[0].labels.size();

  // Allocate point cloud enough large to capture all points
  PointCloud cloud;
  cloud.resize(expected_points, labelsCount);
  int next_insert = 0;
  const int border = frameBorder();
  const double xshift = correctingXShift();
  const double yshift = correctingYShift();
  const double minDisp = minDisparity();
  // go through each frame, converting the disparity values to 3d points
  // relative to first camera
  for (size_t i = 0; i < frames.size(); i += 1) {
    const auto &f = frames[i];
    const auto &disp = f.normalizedDisparityMap;
    // convert each pixel
    for (int y = border - 1; y < disp.rows() - border; y += 1) {
      for (int x = border - 1; x < disp.cols() - border; x += 1) {
        // disparity is returned between [0,1],
        // but originally stored as [0,255]
        double disparity = 255 * disp(y, x);
        if (disparity < minDisp) {
          // just skip those points
          continue;
        }
        const double invDisparity = 1.0 / disparity;
        auto p = cloud[next_insert];
        p.x((x + xshift - f.ccx) * f.baseline * invDisparity);
        p.y((y + yshift - f.ccy) * f.baseline * invDisparity);
        p.z(f.focallength * f.baseline * invDisparity);

        Eigen::Vector4d tmp = applyInverseTransformation(
            inverses[i], Eigen::Vector4d(p.x(), p.y(), p.z(), 1.0));
        p.x(tmp[0]);
        p.y(tmp[1]);
        p.z(tmp[2]);
        p.intensity(f.referencePicture(y, x));
        PointCloud::LabelVec labels(f.labels.size());
        for (size_t l = 0; l < f.labels.size(); ++l) {
          labels[l] = f.labels[l](y, x);
        }
        p.labels(std::move(labels));
        next_insert += 1;
      }
    }
  }
  cloud.resize(next_insert, labelsCount); // shrink to actual number of points

  auto min = cloud.posMin();
  auto max = cloud.posMax();

  BOOST_LOG_TRIVIAL(debug) << "Found point cloud with " << cloud.size()
                           << " points, xyz-min: [" << min[0] << ", " << min[1]
                           << ", " << min[2] << "], xyz-max: [" << max[0]
                           << ", " << max[1] << ", " << max[2] << "]"
                           << std::flush;
  return cloud;
}

std::vector<Eigen::Matrix4d> DisparityRegistration::absoluteTransformations(
    const FrameWindow &window) const {
  const auto &frames = window.frames();
  std::vector<Eigen::Matrix4d> Ts(frames.size());
  Ts[0] = Eigen::Matrix4d::Identity();
  for (size_t i = 1; i < frames.size(); i += 1) {
    Ts[i] =
        frames[i].camChainPicture * frames[i - 1].camChainDisparity * Ts[i - 1];
  }
  return Ts;
}

DisparityRegistration::Inverse
DisparityRegistration::prepareInverseTransformation(
    const Eigen::Matrix4d &mat) const {
  // For the moment, we just return the inverse.
  // We could also use a decomposition object from Eigen
  // to increase robustness.
  // Or we could use domain knowledge about the
  // transformation to speed up computations.
  return mat.inverse();
}

Eigen::Vector4d DisparityRegistration::applyInverseTransformation(
    const DisparityRegistration::Inverse &inv, const Eigen::Vector4d &p) const {
  // for the moment just a simple multiplication
  return inv * p;
}

double &DisparityRegistration::minDisparity() { return _min_disparity; }

/// Lowest disparity value we accept (we remove points at infinity)
const double &DisparityRegistration::minDisparity() const {
  return _min_disparity;
}

/// Set X shift to correct disparity map position
int &DisparityRegistration::correctingXShift() { return _xshift; }

/// Read X shift to correct disparity map position
const int &DisparityRegistration::correctingXShift() const { return _xshift; }

/// Set Y shift to correct disparity map position
int &DisparityRegistration::correctingYShift() { return _yshift; }

/// Read Y shift to correct disparity map position
const int &DisparityRegistration::correctingYShift() const { return _yshift; }

/// Ignores boder of n pixels around disparity map
int &DisparityRegistration::frameBoder() { return _frame_border; }

/// Ignores boder of n pixels around disparity map
const int &DisparityRegistration::frameBorder() const { return _frame_border; }

} // namespace MouseTrack


#include "ADLER32Strategy.hpp"
#include "checksum.h"

#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstring>
#include <boost/algorithm/string/predicate.hpp>

#include "base64.h"

namespace irods {

    const std::string ADLER32_NAME( "adler32" );

    struct adler32_parts {
        uint32_t a;
        uint32_t b;
    };

    adler32_parts adler32_init() {
        return adler32_parts{1, 0};
    }

    static adler32_parts adler32_update(const adler32_parts& parts, const unsigned char *data, size_t len) {

        const uint32_t MOD_ADLER = 65521;

        uint32_t a = parts.a, b = parts.b;

        // Process each byte of the data in order
        for (size_t index = 0; index < len; ++index)
        {
            a = (a + data[index]) % MOD_ADLER;
            b = (b + a) % MOD_ADLER;
        }

        return adler32_parts{a, b};
    }

    static uint32_t adler32_final(const adler32_parts& parts) {
        return (parts.b << 16) | parts.a;
    }

    error
    ADLER32Strategy::init( boost::any& _context ) const {
        _context = adler32_init();
        return SUCCESS();
    }

    error
    ADLER32Strategy::update( const std::string& data, boost::any& _context ) const {

        _context = adler32_update(boost::any_cast<adler32_parts>(_context), reinterpret_cast<const unsigned char*>(data.c_str()), data.size());
        return SUCCESS();
    }

    error
    ADLER32Strategy::digest( std::string& _messageDigest, boost::any& _context ) const {

        const unsigned int ADLER32_DIGEST_LENGTH = 4;

        uint32_t result = adler32_final(boost::any_cast<adler32_parts>(_context));

        std::stringstream ss;
        ss << std::setfill('0') << std::hex << std::setw(ADLER32_DIGEST_LENGTH * 2) << result;

        _messageDigest = ADLER32_CHKSUM_PREFIX;
        _messageDigest += ss.str();

        return SUCCESS();
    }

    bool
    ADLER32Strategy::isChecksum( const std::string& _chksum ) const {
        return boost::starts_with( _chksum, ADLER32_CHKSUM_PREFIX );
    }
}; // namespace irods


// Andrew Long
// Updated: Feb 13, 2017
// DFS Matrix Alignment Codes

#ifndef __DFS_ALIGN__
#define __DFS_ALIGN__

#include <vector>
#include <queue>
#include <algorithm>
#include <functional>
#include <utility>
#include <armadillo>

#include "GridDLL.hpp"
#include "Heuristic.h"

#define DO_MAX_CUT 0
#define MAX_EVALS (N1*N1*N2*N2*n_branch)

inline double
L11_norm(const arma::mat& A, const arma::mat& B)
{
	if(A.size() != B.size())
	{
		throw std::logic_error("L1,1-norm works only for equal sized graphs");
	}
	return arma::norm(arma::vectorise(A - B),1);
}

inline void
fill_range(arma::uvec& v, long N)
{
	v.set_size(N);
	for(long i = 0; i < N; ++i)
		v(i) = i;
}

typedef std::pair<double, int> MoveIndex;

class DFSAlign
{
private:
	double min_dist, opt_dist;
	long N1, N2, Nelem;
    long n_branch;
	arma::mat M1;
	arma::mat M1_t;
	arma::mat M2;
	arma::uvec min_perm1;
	arma::uvec min_perm2;
	arma::mat min_perm;
	arma::uvec fevals;
	arma::mat score;
	GridDLL full_list;
    std::vector<GridPoint> all_moves;
    std::vector<MoveIndex> all_move_dists;
    MatchHeuristic heuristic;
    long num_all;

	bool isAligned;

	void recursiveHelper(int depth, double currentD, GridPoint root, GridDLL& list, arma::uvec& perm1, arma::uvec& perm2)
	{
		if(min_dist == opt_dist)
			return;

        perm1[depth] = root.first;
        perm2[depth] = root.second;
        //TODO: if N1 != N2, you're done "matching", so it shouldn't really matter what else happens
        if(depth == N1-1)
        {

            if(N1 != N2)
            {
                std::vector<int> vec2;
                // fill in the rest of the values
                // unassigned N1 will be N1..N2-1
                // unassigned N2 requires identification
                arma::uvec bp = arma::sort(perm2,"ascend");
                int c = 0;
                for(int i =0; i < N1; ++i)
                {

                    while(c < (int)bp[i])
                    {
                        vec2.push_back(c++);
                    }
                    ++c;
                }
                for(int i = (int)(bp[N1-1]+1); i < N2; ++i)
                {
                    vec2.push_back(i);
                }
                c = (int)vec2.size();
                for(int i = 0; i < c; ++i)
                {
                    ++depth;
                    perm1[depth] = N1+i;
                    perm2[depth] = vec2[i];
                    currentD = computeDist(perm1,perm2,currentD,depth);
                }

                for(int i = 0; i < c; ++i)
                {
                    perm1[depth] = -1;
                    perm2[depth] = -1;
                    depth--;
                }
            }

            if(min_dist > currentD)
            {
                min_dist = currentD;
                min_perm1 = perm1;
                min_perm2 = perm2;
            }

#if defined (DO_MAX_CUT)
    #if DO_MAX_CUT == 1
            if(fevals(depth-1)>=(uint32_t)MAX_EVALS){
                opt_dist = min_dist;
            }
    #endif
#endif
            return;
        }

		list.clearOverlaps(root.first, root.second);

        long nK = list.topK(n_branch, all_moves, num_all);
        sortAllMoves(nK, perm1, perm2, currentD, depth+1);
        fevals(depth) += nK;
        int start = (int)num_all;
        num_all += nK;

        for(int i = start; i < num_all; ++i)
        {
            // already worse than the best matching
            if(all_move_dists[i].first > min_dist)
                continue;

            recursiveHelper(depth+1,
                            all_move_dists[i].first,
                            all_moves[all_move_dists[i].second],
                            list, perm1, perm2);

            if(min_dist == opt_dist)
                return;
        }
        num_all -= nK;
        list.relinkStack(depth);
	}

    void sortAllMoves(long nK, arma::uvec& perm1, arma::uvec& perm2, const double curD, int depth)
    {
        int start = (int)num_all;
        int end = (int)(num_all+nK);
        for(int i = start; i < end; ++i)
        {
            perm1[depth] = all_moves.at(i).first;
            perm2[depth] = all_moves.at(i).second;
            all_move_dists[i].first = computeDist(perm1,perm2,curD, depth);
            all_move_dists[i].second = i;
        }
        std::sort(all_move_dists.begin()+start, all_move_dists.begin()+end,
                  [](MoveIndex a, MoveIndex b) -> bool
                  {
                      return a.first < b.first;
                  });
        perm1[depth] = 0;
        perm2[depth] = 0;
    }

	void sortMoves(long nK, const std::vector<GridPoint>& moves, std::vector<MoveIndex>& move_dists, arma::uvec& perm1, arma::uvec& perm2, const double curD, int depth)
	{
		for(unsigned int i = 0; i < nK; ++i)
		{
			perm1[depth] = moves.at(i).first;
			perm2[depth] = moves.at(i).second;
            move_dists[i].first = computeDist(perm1,perm2,curD, depth);
            move_dists[i].second = i;
		}
		std::sort(move_dists.begin(), move_dists.begin()+nK,
			[](MoveIndex a, MoveIndex b) -> bool
				{
					return a.first < b.first;
				});
        perm1[depth] = 0;
        perm2[depth] = 0;
	}

    // NOTE: uses symmetry of the underlying M1/M2 matrices to speed up the process
    // WILL NOT WORK FOR NON-SYMMETRIC MATRICES
	double computeDist(const arma::uvec& p1, const arma::uvec& p2, const double curD, int depth)
	{
        double d = curD;
        int mdepth = depth-1;
        for(int i = 0; i <= mdepth; ++i)
        {
            d += 2*std::abs(M1_t(p1(depth),p1(i))-M2(p2(depth),p2(i)));
        }
        d += std::abs(M1_t(p1(depth),p1(depth)) - M2(p2(depth),p2(depth)));

        return d;
	}

public:
	DFSAlign(const arma::mat& m1, const arma::mat& m2, int branch = 1, MatchHeuristic h = MatchHeuristic::ISORANK)
	{
		init(m1, m2, branch, h);
		reset();
	}

	void init(const arma::mat& m1, const arma::mat& m2, int branch, MatchHeuristic h)
	{
		if(m1.n_rows <= m2.n_rows)
		{
			M1 = m1;
			M2 = m2;
		}
		else
		{
			M1 = m2;
			M2 = m1;
		}
		N1 = M1.n_rows;
		N2 = M2.n_rows;

		// setting up number for normalization
		Nelem = N2*(N2-1);
		if(Nelem == 0)
			Nelem = 1;
		// optimal distance
		M1_t = M1;
		M1_t.resize(N2,N2);

		arma::vec av = arma::vectorise(M1_t);
		av = arma::sort(av,"descend");
		arma::vec bv = arma::vectorise(M2);
		bv = arma::sort(bv,"descend");
		opt_dist = arma::norm(av-bv,1);
		if(N1 == 1 || N2 == 1)
		{
			min_dist = opt_dist;
			isAligned = true;
			return;
		}

        // score measuring via matching heuristic
        heuristic = h;
        heuristic_func hfunc = heuristic_map[heuristic];
        score = (*hfunc)(M1, M2);
        /*
		av = arma::sum(M1,1);
		av = av/arma::norm(av,2);
		bv = arma::sum(M2,1);
		bv = bv/arma::norm(bv,2);

		score = av * (bv.t());
        */
		full_list.buildFromMat(score);

		n_branch = branch;
		fevals = arma::uvec(N2,arma::fill::zeros);

        all_moves.resize(N2*N2*n_branch);
        all_move_dists.resize(N2*N2*n_branch);

        num_all = 0;
		isAligned = false;
	}

	void reset(int branch = -1)
	{
		isAligned = false;

		min_dist = L11_norm(M1_t,M2);

		fill_range(min_perm1,N2);
		fill_range(min_perm2,N2);

		if(branch != -1)
        {
			n_branch = branch;
            all_moves.resize(N2*N2*2);
            all_move_dists.resize(N2*N2*n_branch);
        }
	}

	void align()
	{
		reset();

		int depth = 0;
		// grab the top BRANCH score pairs
		GridDLL current_list(full_list);

        int nK = (int)current_list.topK(n_branch,all_moves,0);
        num_all = nK;

		// loop over the top values
        arma::uvec perm1(N2,arma::fill::ones); perm1 *= -1;
        arma::uvec perm2(N2,arma::fill::ones); perm2 *= -1;

		for(int i = 0; i < nK; ++i)
		{
            if(min_dist == opt_dist)
                break;
            recursiveHelper(depth, 0, all_moves.at(i), current_list, perm1, perm2);
		}

		// generate permutation
		min_perm.zeros(N1,N2);
		for(int i=0; i < N1; ++i)
		{
			min_perm( min_perm1(i), min_perm2(i) ) = 1;
		}
		min_dist = arma::norm(arma::vectorise(min_perm.t() * M1 * min_perm - M2),1);
		isAligned = true;
	}

	double getDistance(arma::mat& perm)
	{
		double d = getDistance();
		perm = min_perm;
		return d;
	}

	double getDistance()
	{
		if(!isAligned)
			align();
		return min_dist;
	}

	double getNormalizedDistance()
	{
		if(!isAligned)
			align();
		return min_dist / Nelem;
	}

	arma::uvec getFEvals()
	{
		return fevals;
	}

    unsigned long long getTotalEvals()
    {
        return arma::sum(fevals);
    }
};

double
dist_dfs(arma::mat& a, arma::mat& b, uint branches, MatchHeuristic h = MatchHeuristic::ISORANK)
{
	DFSAlign aligner(a, b, branches, h);
	return aligner.getNormalizedDistance();
}

#endif


#include "lineturtle.h"
#include "lineshape.h"
#include <algorithm>
#include <boost/lambda/bind.hpp>
#include <lap/lap.h>
#include <cmath>
#include "common.h"

using namespace boost::lambda;

inline float radians(float angleDegrees) { return M_PI * angleDegrees / 180.0f; }

LineTurtle::LineTurtle(float lineLength):
_advance(lineLength),
_heading(),
_x(),
_y()
{
}

bool LineTurtle::init(const std::string& program)
{
  _actions['F'] = &LineTurtle::draw;
  _actions['f'] = &LineTurtle::forward;
  _actions['+'] = &LineTurtle::left;
  _actions['-'] = &LineTurtle::right;
  _dol = DOL::makeDOL(program);
  if (!_dol) return false;
  std::cout << "DOL: " << *_dol << std::endl;
  reset();
  return true;
}

void LineTurtle::step()
{
//  shared_ptr<Mesh<VertexP> mesh(new Mesh<VertexP>());
  _mesh = shared_ptr<lap::Mesh<lap::VertexP> >(new lap::Mesh<lap::VertexP>() );
  for_each(_state.begin(), _state.end(), bind(&LineTurtle::doAction, this, _1));

  _shape = makeLineShape(_mesh);
  _state = _dol->step(_state);
}
void LineTurtle::reset()
{
  _state = _dol->axiom();
  _x = _y = 0.0f;
  _heading = 0.0f;
  _shape.reset();
}
void LineTurtle::render(QGLShaderProgram* program)
{
if (_shape)  _shape->render(program);
}

void LineTurtle::draw()
{
  _mesh->_vertices.push_back(makeVertex(_x, _y, 0.0));
  _x = _x + _advance * cosf(radians(_heading));
  _y = _y + _advance * sinf(radians(_heading));
  _mesh->_vertices.push_back(makeVertex(_x, _y, 0.0));

}
void LineTurtle::forward()
{
  _x = _x + _advance * cosf(radians(_heading));
  _y = _y + _advance * sinf(radians(_heading));
}
void LineTurtle::left()
{
  _heading += _dol->theta();
}
void LineTurtle::right()
{
  _heading -= _dol->theta();
}


#include <QApplication>
#include <QKeyEvent>
#include <QPushButton>
#include "GLModelViewer.h"
#include "GLModel.h"
#include "GLShaderProgram.h"
#include "Grid.h"
#include <iostream>
#include <chrono>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include "Timer.h"
#include "Projection.h"
#include "Volumetric_helper.h"

#if 0
void raycast_volume()
{
	timer.start();

	Eigen::Vector3d origin = T.first.col(3).head<3>();
	Eigen::Vector3d window_coord_norm;

	std::vector<Eigen::Vector3d> output_cloud;

	// Sweep the volume looking for the zero crossing
	for (int y = 0; y < window_height * 0.1; ++y)
	{
		std::cout << "Ray casting to image... " << (double)y / window_height * 100 << "%" << std::endl;

		for (int x = 0; x < window_width * 0.1; ++x)
		{
			window_coord_norm.x() = ((double)x / window_width * 2.0) - 1.0;
			window_coord_norm.y() = ((double)y / window_height * 2.0) - 1.0;
			window_coord_norm.z() = origin.z() + near_plane;
			Eigen::Vector3d direction = (window_coord_norm - origin).normalized();

			std::vector<int> intersections = Grid::find_intersections(grid.data, volume_size, voxel_size, grid.transformation, origin, direction, near_plane, far_plane);
			Grid::sort_intersections(intersections, grid.data, origin);

			for (int i = 1; i < intersections.size(); ++i)
			{
				const Voxeld& prev = grid.data.at(i - 1);
				const Voxeld& curr = grid.data.at(i);

				const bool& same_sign = ((prev.tsdf < 0) == (curr.tsdf < 0));

				if (!same_sign)		// it is a zero-crossing
				{
					output_cloud.push_back(curr.point);
				}
			}
		}
	}

	timer.print_interval("Raycasting volume   : ");

	export_obj("../../data/output_cloud.obj", output_cloud);
}
#endif

// Usage: ./Volumetricd.exe ../../data/monkey.obj 256 4 2 90
int main(int argc, char **argv)
{
	if (argc < 6)
	{
		std::cerr << "Missing parameters. Abort."
			<< std::endl
			<< "Usage:  ./Volumetricd.exe ../../data/monkey.obj 256 8 2 90"
			<< std::endl;
		return EXIT_FAILURE;
	}
	Timer timer;
	const std::string filepath = argv[1];
	const int vol_size = atoi(argv[2]);
	const int vx_size = atoi(argv[3]);
	const int cloud_count = atoi(argv[4]);
	const int rot_interval = atoi(argv[5]);

	std::pair<std::vector<double>, std::vector<double>> depth_buffer;

	//
	// Projection and Modelview Matrices
	//
	Eigen::Matrix4d K = perspective_matrix(fov_y, aspect_ratio, near_plane, far_plane);
	std::pair<Eigen::Matrix4d, Eigen::Matrix4d>	T(Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity());

	//
	// Creating volume
	//
	Eigen::Vector3d voxel_size(vx_size, vx_size, vx_size);
	Eigen::Vector3d volume_size(vol_size, vol_size, vol_size);
	Eigen::Vector3d voxel_count(volume_size.x() / voxel_size.x(), volume_size.y() / voxel_size.y(), volume_size.z() / voxel_size.z());
	//
	Eigen::Affine3d grid_affine = Eigen::Affine3d::Identity();
	grid_affine.translate(Eigen::Vector3d(0, 0, -256));
	grid_affine.scale(Eigen::Vector3d(1, 1, -1));	// z is negative inside of screen

	Grid<double> grid(volume_size, voxel_size, grid_affine.matrix());

	//
	// Importing .obj
	//
	timer.start();
	std::vector<Eigen::Vector3d> points3DOrig, pointsTmp;
	import_obj(filepath, points3DOrig);
	timer.print_interval("Importing monkey    : ");
	std::cout << "Monkey point count  : " << points3DOrig.size() << std::endl;

	//
	// Translating and rotating monkey point cloud
	std::pair<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>> cloud;
	//
	Eigen::Affine3d rotate = Eigen::Affine3d::Identity();
	Eigen::Affine3d translate = Eigen::Affine3d::Identity();
	translate.translate(Eigen::Vector3d(0, 0, -256));

	//
	// Compute first cloud
	//
	for (Eigen::Vector3d p3d : points3DOrig)
	{
		Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
		rot /= rot.w();
		cloud.first.push_back(rot.head<3>());
	}
	//
	// Update grid with first cloud
	//
	timer.start();
	create_depth_buffer<double>(depth_buffer.first, cloud.first, K, Eigen::Matrix4d::Identity(), far_plane);
	timer.print_interval("CPU compute depth   : ");

	timer.start();
	update_volume(grid, depth_buffer.first, K, T.first);
	timer.print_interval("CPU Update volume   : ");

	//
	// Compute next clouds
	Eigen::Matrix4d cloud_mat = Eigen::Matrix4d::Identity();
	Timer iter_timer;
	for (int i = 1; i < cloud_count; ++i)
	{
		std::cout << std::endl << i << " : " << i * rot_interval << std::endl;
		iter_timer.start();

		// Rotation matrix
		rotate = Eigen::Affine3d::Identity();
		rotate.rotate(Eigen::AngleAxisd(DegToRad(i * rot_interval), Eigen::Vector3d::UnitY()));

		cloud.second.clear();
		for (Eigen::Vector3d p3d : points3DOrig)
		{
			Eigen::Vector4d rot = translate.matrix() * rotate.matrix() * p3d.homogeneous();
			rot /= rot.w();
			cloud.second.push_back(rot.head<3>());
		}

		//export_obj("../../data/cloud_cpu_2.obj", cloud.second);

		timer.start();
		create_depth_buffer<double>(depth_buffer.second, cloud.second, K, Eigen::Matrix4d::Identity(), far_plane);
		timer.print_interval("Compute depth buffer: ");

		//export_depth_buffer("../../data/cpu_depth_buffer_2.obj", depth_buffer.second);

		timer.start();
		Eigen::Matrix4d icp_mat;
		ComputeRigidTransform(cloud.first, cloud.second, icp_mat);
		timer.print_interval("Compute rigid transf: ");

		//std::cout << std::fixed << std::endl << "icp_mat " << std::endl << icp_mat << std::endl;

		// accumulate matrix
		cloud_mat = cloud_mat * icp_mat;

		//std::cout << std::fixed << std::endl << "cloud_mat " << std::endl << cloud_mat << std::endl;

		timer.start();
		//update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
		update_volume(grid, depth_buffer.second, K, cloud_mat.inverse());
		timer.print_interval("Update volume       : ");

		// copy second point cloud to first
		cloud.first = cloud.second;
		//depth_buffer.first = depth_buffer.second;

		iter_timer.print_interval("Iteration time      : ");
	}

	//std::cout << "------- // --------" << std::endl;
	//for (int i = 0; i <  grid.data.size(); ++i)
	//{
	//	const Eigen::Vector3d& point = grid.data[i].point;

	//	std::cout << point.transpose() << "\t\t" << grid.data[i].tsdf << " " << grid.data[i].weight << std::endl;
	//}
	//std::cout << "------- // --------" << std::endl;

//	timer.start();
//	export_volume("../../data/grid_volume_cpu.obj", grid.data);
//	timer.print_interval("Exporting volume    : ");
//	return 0;

	QApplication app(argc, argv);

	//
	// setup opengl viewer
	//
	GLModelViewer glwidget;
	glwidget.resize(640, 480);
	glwidget.setPerspective(60.0f, 0.1f, 10240.0f);
	glwidget.move(320, 0);
	glwidget.setWindowTitle("Point Cloud");
	glwidget.setWeelSpeed(0.1f);
	glwidget.setPosition(0, 0, -0.5f);
	glwidget.show();

	Eigen::Matrix4d to_origin = Eigen::Matrix4d::Identity();
	to_origin.col(3) << -(volume_size.x() / 2.0), -(volume_size.y() / 2.0), -(volume_size.z() / 2.0), 1.0;	// set translate

	std::vector<Eigen::Vector4f> vertices, colors;

	int i = 0;
	for (int z = 0; z <= volume_size.z(); z += voxel_size.z())
	{
		for (int y = 0; y <= volume_size.y(); y += voxel_size.y())
		{
			for (int x = 0; x <= volume_size.x(); x += voxel_size.x(), i++)
			{
				const float tsdf = grid.data.at(i).tsdf;

				//Eigen::Vector4d p = grid_affine.matrix() * to_origin * Eigen::Vector4d(x, y, z, 1);
				Eigen::Vector4d p = to_origin * Eigen::Vector4d(x, y, z, 1);
				p /= p.w();

				if (tsdf > 0.1)
				{
					vertices.push_back(p.cast<float>());
					colors.push_back(Eigen::Vector4f(0, 1, 0, 1));
				}
				else if (tsdf < -0.1)
				{
					vertices.push_back(p.cast<float>());
					colors.push_back(Eigen::Vector4f(1, 0, 0, 1));
				}
			}
		}
	}

	//
	// setup model
	//
	std::shared_ptr<GLModel> model(new GLModel);
	model->initGL();
	model->setVertices(&vertices[0][0], vertices.size(), 4);
	model->setColors(&colors[0][0], colors.size(), 4);
	glwidget.addModel(model);

	//
	// setup kinect shader program
	//
	std::shared_ptr<GLShaderProgram> kinectShaderProgram(new GLShaderProgram);
	if (kinectShaderProgram->build("color.vert", "color.frag"))
		model->setShaderProgram(kinectShaderProgram);

	return app.exec();
}


#ifndef POLIMIDL_LAYERS_INTERNAL_DEPTHWISE_CONVOLUTION_HPP
#define POLIMIDL_LAYERS_INTERNAL_DEPTHWISE_CONVOLUTION_HPP

#include <algorithm>
#include <chrono>

#include <Eigen/Dense>

#include "../alignment.hpp"
#include "../layer.hpp"

namespace polimidl {
namespace layers {
namespace internal {
template <typename type_t, typename components, typename kernel,
          typename stride, typename padding>
class depthwise_convolution : public layer<type_t, components> {
 public:
  depthwise_convolution(type_t const *coeff) :
      coeff_(coeff),
      batch_size_(1) {}

  static std::size_t output_rows(std::size_t input_rows) {
    return (input_rows + padding::rows - kernel::rows) / stride::rows + 1;
  }
  static std::size_t output_columns(std::size_t input_columns) {
    return (input_columns + padding::columns - kernel::columns) /
        stride::columns + 1;
  }
  static std::size_t temporary_size(std::size_t input_rows,
                                    std::size_t input_columns,
                                    std::size_t number_of_workers) {
    return cell_size * number_of_workers;
  }

  template <typename input_t, typename temporary_t, typename output_t,
            typename scheduler_t>
  void operator()(input_t input, temporary_t temporary, output_t output,
                  std::size_t input_rows, std::size_t input_columns,
                  const scheduler_t& scheduler) const {
    const std::size_t output_rows = this->output_rows(input_rows);
    const std::size_t output_columns = this->output_columns(input_columns);
    const std::size_t cells = output_rows * output_columns;
    const unsigned int number_of_workers = scheduler.number_of_workers();

    for (std::size_t start_cell = 0; start_cell < cells;
         start_cell += batch_size_) {
      const std::size_t end_cell = std::min(cells, start_cell + batch_size_);
      scheduler.schedule([=] (std::size_t worker) {
        auto temporary_worker = temporary.slice(worker, number_of_workers);
        for (std::size_t cell = start_cell; cell < end_cell; ++cell) {
          const std::size_t output_row = cell / output_columns;
          const std::size_t output_column = cell % output_columns;
          for (std::size_t kernel_row = 0; kernel_row < kernel::rows;
               ++kernel_row) {
            const std::size_t input_row =
                stride::rows * output_row + kernel_row;
            auto i_temp = &temporary_worker[
                components::value * kernel::columns * kernel_row];
            if constexpr (padding::top && padding::bottom) {
              if (input_row < padding::top ||
                  input_row >= input_rows + padding::top) {
                std::fill_n(i_temp, components::value * kernel::columns,
                            type_t(0));
                continue;
              }
            } else if constexpr (padding::top) {
              if (input_row < padding::top) {
                std::fill_n(i_temp, components::value * kernel::columns,
                            type_t(0));
                continue;
              }
            } else if constexpr (padding::bottom) {
              if (input_row >= input_rows) {
                std::fill_n(i_temp, components::value * kernel::columns,
                            type_t(0));
                continue;
              }
            }
            if constexpr (padding::left || padding::right) {
              for (std::size_t kernel_column = 0;
                   kernel_column < kernel::columns; ++kernel_column) {
                const std::size_t input_column =
                    stride::columns * output_column + kernel_column;
                if constexpr (padding::left && padding::right) {
                  if (input_column < padding::left ||
                      input_column >= input_columns + padding::left) {
                    std::fill_n(i_temp + components::value * kernel_column,
                                components::value, type_t(0));
                    continue;
                  }
                } else if constexpr (padding::left) {
                  if (input_column < padding::left) {
                    std::fill_n(i_temp + components::value * kernel_column,
                                components::value, type_t(0));
                    continue;
                  }
                } else if constexpr (padding::right) {
                  if (input_column >= input_columns) {
                    std::fill_n(i_temp + components::value * kernel_column,
                                components::value, type_t(0));
                    continue;
                  }
                }
                auto i_input = &input[components::value * (
                    input_columns * (input_row - padding::top) +
                    (input_column - padding::left))];
                std::copy_n(i_input, components::value,
                            i_temp + components::value * kernel_column);
              }
            } else {
              const std::size_t input_column = stride::columns * output_column;
              auto i_input = &input[components::value * (
                  input_columns * (input_row - padding::top) +
                  input_column)];
              std::copy_n(i_input, components::value * kernel::columns, i_temp);
            }
          }
          matrix_input_t val(&temporary_worker[0]);
          auto i_output = &output[components::value * (
              output_columns * output_row + output_column)];
          matrix_output_t out(i_output);
          out.noalias() = (val * coeff_).diagonal();
        }
      });
    }
    scheduler.wait();
  }

  template <typename input_t, typename temporary_t, typename output_t,
            typename scheduler_t>
  void optimize_for(input_t input, temporary_t temporary, output_t output,
                    std::size_t input_rows, std::size_t input_columns,
                    const scheduler_t& scheduler) {
      const std::size_t output_rows = this->output_rows(input_rows);
      const std::size_t output_columns = this->output_columns(input_columns);
      const std::size_t cells = output_rows * output_columns;

      if (scheduler.number_of_workers() == 1) {
        batch_size_ = cells;
        return;
      }

      auto best_duration = std::chrono::high_resolution_clock::duration::max();

      std::size_t best_batch_size = 1;
      auto test_and_maybe_set = [&]() {
          for (std::size_t iteration = 0; iteration < 3; ++iteration) {
              const auto start = std::chrono::high_resolution_clock::now();
              operator()(input, temporary, output, input_rows, input_columns,
                         scheduler);
              const auto current_duration =
                  std::chrono::high_resolution_clock::now() - start;
              if (current_duration < best_duration) {
                  best_duration = current_duration;
                  best_batch_size = batch_size_;
              }
          }
      };
      batch_size_ = 1;
      while (batch_size_ < cells) {
          test_and_maybe_set();
          batch_size_ = batch_size_ * 2;
      }
      batch_size_ = cells;
      test_and_maybe_set();
      batch_size_ = cells / scheduler.number_of_workers() +
                    cells % scheduler.number_of_workers() ? 1 : 0;
      while (batch_size_ > 1) {
          test_and_maybe_set();
          batch_size_ = batch_size_ / 2;
      }
      batch_size_ = best_batch_size;
  }

 private:
  static constexpr std::size_t cell_size =
     components::value * kernel::rows * kernel::columns;
  using alignment_t = alignment<type_t>;
  using matrix_input_t = Eigen::Map<
     Eigen::Matrix<type_t, components::value, kernel::columns * kernel::rows>,
     alignment_t::eigen_alignment>;
  using matrix_output_t = Eigen::Map<
     Eigen::Matrix<type_t, components::value, 1>>;
  using matrix_coefficients_t = Eigen::Map<
     const Eigen::Matrix<type_t, kernel::columns * kernel::rows,
                         components::value, Eigen::RowMajor>,
     alignment_t::eigen_alignment>;
  matrix_coefficients_t coeff_;
  std::size_t batch_size_;
};
}
}
}

#endif  // POLIMIDL_LAYERS_INTERNAL_DEPTHWISE_CONVOLUTION_HPP


//  (C) Copyright Jeremy Siek 2002.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <boost/pending/disjoint_sets.hpp>
#include <boost/test/minimal.hpp>
#include <boost/cstdlib.hpp>

template <typename DisjointSet>
struct test_disjoint_set {
  static void do_test()
  {
    // The following tests are pretty lame, just a basic sanity check.
    // Industrial strength tests still need to be written.

#if !defined(__MWERKS__) || __MWERKS__ > 0x3003
    std::size_t elts[]
#else
    std::size_t elts[4]
#endif
        = { 0, 1, 2, 3 };

    const int N = sizeof(elts)/sizeof(*elts);

    DisjointSet ds(N);

    ds.make_set(elts[0]);
    ds.make_set(elts[1]);
    ds.make_set(elts[2]);
    ds.make_set(elts[3]);

    BOOST_CHECK(ds.find_set(0) != ds.find_set(1));
    BOOST_CHECK(ds.find_set(0) != ds.find_set(2));
    BOOST_CHECK(ds.find_set(0) != ds.find_set(3));
    BOOST_CHECK(ds.find_set(1) != ds.find_set(2));
    BOOST_CHECK(ds.find_set(1) != ds.find_set(3));
    BOOST_CHECK(ds.find_set(2) != ds.find_set(3));

    ds.union_set(0, 1);
    ds.union_set(2, 3);
    BOOST_CHECK(ds.find_set(0) != ds.find_set(3));
    int a = ds.find_set(0);
    BOOST_CHECK(a == ds.find_set(1));
    int b = ds.find_set(2);
    BOOST_CHECK(b == ds.find_set(3));

    ds.link(a, b);
    BOOST_CHECK(ds.find_set(a) == ds.find_set(b));
    BOOST_CHECK(1 == ds.count_sets(elts, elts + N));

    ds.normalize_sets(elts, elts + N);
    ds.compress_sets(elts, elts + N);
    BOOST_CHECK(1 == ds.count_sets(elts, elts + N));
  }
};

int
test_main(int, char*[])
{
  using namespace boost;
  {
    typedef
      disjoint_sets_with_storage<identity_property_map, identity_property_map,
      find_with_path_halving> ds_type;
    test_disjoint_set<ds_type>::do_test();
  }
  {
    typedef
      disjoint_sets_with_storage<identity_property_map, identity_property_map,
      find_with_full_path_compression> ds_type;
    test_disjoint_set<ds_type>::do_test();
  }
  return boost::exit_success;
}


#include <cxxtest/TestDrive.h>

#include <Eigen_openma/Utils/sign.h>

CXXTEST_SUITE(SignTest)
{
  CXXTEST_TEST(positive)
  {
    TS_ASSERT_EQUALS(sign(1292) > 0, true);
    TS_ASSERT_EQUALS(sign(9999) > 0, true);
    TS_ASSERT_EQUALS(sign(1) > 0, true);
  };

  CXXTEST_TEST(negative)
  {
    TS_ASSERT_EQUALS(sign(-11) < 0, true);
    TS_ASSERT_EQUALS(sign(-5000) < 0, true);
    TS_ASSERT_EQUALS(sign(-1) < 0, true);
  };

  CXXTEST_TEST(null)
  {
    TS_ASSERT_EQUALS(sign(+0) == 0, true);
    TS_ASSERT_EQUALS(sign(-0) == 0, true);
    TS_ASSERT_EQUALS(sign(0) == 0, true);
  };
};

CXXTEST_SUITE_REGISTRATION(SignTest)
CXXTEST_TEST_REGISTRATION(SignTest, positive)
CXXTEST_TEST_REGISTRATION(SignTest, negative)
CXXTEST_TEST_REGISTRATION(SignTest, null)


// Original work Copyright (c) 2017, University of Minnesota
// Modified work Copyright 2020, Yue Peng
//
// ADMM-Elastic Uses the BSD 2-Clause License (http://www.opensource.org/licenses/BSD-2-Clause)
// Redistribution and use in source and binary forms, with or without modification, are
// permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright notice, this list of
//    conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice, this list
//    of conditions and the following disclaimer in the documentation and/or other materials
//    provided with the distribution.
// THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR  A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE UNIVERSITY OF MINNESOTA, DULUTH OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
// IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
// OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef ADMM_FORCE_H
#define ADMM_FORCE_H 1

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

namespace admm {

//
//	Lame constants
//
class Lame {
public:
    static Lame rubber(){ return Lame(10000000,0.499); } // true rubber
    static Lame soft_rubber(){ return Lame(10000000,0.399); } // fun rubber!
    static Lame very_soft_rubber(){ return Lame(1000000,0.299); } // more funner!

    long double mu, lambda;
    long double bulk_modulus() const { return lambda + (2.0/3.0)*mu; }

    // Hard strain limiting (e.g. [0.95,1.05]), default no limit
    // with  min: -inf to 1, max: 1 to inf.
    // In practice if max>99 it's basically no limiting.
    long double limit_min, limit_max;

    // k: Youngs (Pa), measure of stretch
    // v: Poisson, measure of incompressibility
    Lame( long double k, long double v ) :
        mu(k/(2.0*(1.0+v))),
        lambda(k*v/((1.0+v)*(1.0-2.0*v))),
        limit_min(-100.0),
        limit_max(100.0) {
    }

    // Use custom mu, lambda
    Lame(): limit_min(-100.0),
        limit_max(100.0) {}
};

//
//	Base class tets: Linear (non-corotated) elastic
//
class EnergyTerm {
private:
    typedef Eigen::Matrix<double,Eigen::Dynamic,1> VecX;
    typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatX;
    typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMat;
    int g_index; // global idx (starting row of reduction matrix)

public:

    virtual ~EnergyTerm() {}

    // Called by the solver to create the global reduction and weight matrices
    inline void get_reduction( std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights );

    // Called by the solver for a local step update
    //inline void update( const SparseMat &D, const VecX &x, VecX &z, VecX &u );

    // Called by the solver for a local step update
    inline void update_z(const SparseMat &D, const SparseMat &W_inv, const SparseMat &W, const VecX &x, VecX &z, const VecX &u , const VecX &c);

    // Called by the solver for DR update
    inline void update_DR_z(const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                         const VecX &s, const VecX &u, const VecX &c );

    inline void update_DR_xzu_z(const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                         const VecX &s);

    // Called by the solver for a local step update
    inline void update_u(const SparseMat &D, const SparseMat &W, const VecX &x, const VecX &z, VecX &u , const VecX &c);

    // Computes energy of the force (used for debugging)
    inline double energy( const SparseMat &D, const VecX &x );

    inline double get_all_energy(const VecX &x );
    // Compute energy and gradient for a first-order opt. solver
//	inline double gradient( const SparseMat &D, const VecX &x, VecX &grad );
    inline void gradient( const SparseMat &D, const VecX &x, VecX &grad );

    inline void get_all_gradient(const VecX &z, VecX &grad );

    inline void compute_x_gradient(const VecX &grad, VecX &all_grad );

    // Dimension of deformation gradient.
    virtual int get_dim() const = 0;

    // Return the scalar weight of the energy term
    virtual double get_weight() const = 0;

    virtual double get_volume() const = 0;

protected:

    virtual void get_gradient_for_vertices( const VecX &grad, VecX &all_grad) = 0;

    // Get a local reduction matrix
    virtual void get_reduction( std::vector< Eigen::Triplet<double> > &triplets ) = 0;

    // Proximal update
    virtual void prox( const MatX &W, VecX &zi, const VecX &vi) = 0;

    // Returns energy of the force
    virtual double energy( const VecX &F ) = 0;

    virtual double energyLBFGS( const VecX &F ) = 0;

    // Computes a first-order update (energy and gradient)
    virtual void gradient(const VecX &x, VecX &grad) = 0;

    virtual void get_gradient( const VecX &F, VecX &grad ) = 0;

}; // end class EnergyTerm

//
//  Implementation
//

inline void EnergyTerm::get_reduction( std::vector< Eigen::Triplet<double> > &triplets, std::vector<double> &weights ){
    std::vector< Eigen::Triplet<double> > temp_triplets;

    get_reduction( temp_triplets );
    int n_trips = temp_triplets.size();
    g_index = weights.size();

    for( int i=0; i<n_trips; ++i ){
        const Eigen::Triplet<double> &trip = temp_triplets[i];
        triplets.emplace_back( trip.row()+g_index, trip.col(), trip.value() );
    }

    int dim = get_dim();
    double w = get_weight();
    if( w <= 0.0 ){
        throw std::runtime_error("**EnergyTerm::get_reduction Error: Some weight leq 0");
    }
    for( int i=0; i<dim; ++i ){ weights.emplace_back( w ); }
}

inline void EnergyTerm::update_u( const SparseMat &D, const SparseMat &W, const VecX &x, const VecX &z, VecX &u, const VecX &c ){
    int dof = x.rows();
    int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    VecX ui = u.segment(g_index,dim);
//    VecX zi = W.block(g_index,g_index,dim,dim) * z.segment(g_index,dim);
    VecX zi = z.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    ui += (Dix - zi - ci);
    u.segment(g_index,dim) = ui;
}

inline void EnergyTerm::update_z( const SparseMat &D, const SparseMat &W_inv, const SparseMat &W, const VecX &x, VecX &z, const VecX &u, const VecX &c ){
    int dof = x.rows();
    const int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    VecX ui = u.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    VecX vi = Dix + ui - ci;
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * vi;
//    vi = zi;
    VecX zi = vi;
    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, vi);
    z.segment(g_index,dim) = zi;
}

inline void EnergyTerm::update_DR_z( const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                     const VecX &s, const VecX &u, const VecX &c ){
    const int dim = get_dim();
    VecX ui = u.segment(g_index,dim);
    VecX ci = c.segment(g_index,dim);
    VecX si = s.segment(g_index,dim);
    VecX vi = 2*ui - si - ci;
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * vi;
//    vi = zi;
    VecX zi = vi;
    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, vi);
    z.segment(g_index,dim) = zi;
}

inline void EnergyTerm::update_DR_xzu_z( const SparseMat &W_inv, const SparseMat &W, VecX &z,
                                     const VecX &s){
    const int dim = get_dim();
    VecX si = s.segment(g_index,dim);
//    VecX zi = W_inv.block(g_index,g_index,dim,dim) * si;
//    si = zi;
    VecX zi = si;

    Eigen::Matrix<double,9,9> Wi = W.block(g_index,g_index,dim,dim);
    prox(Wi, zi, si);
    z.segment(g_index,dim) = zi;
}

inline double EnergyTerm::energy( const SparseMat &D, const VecX &x ){
    (void)(D);
    int dim = get_dim();
    VecX Dix = x.segment(g_index,dim);
    return energy( Dix );
}

inline double EnergyTerm::get_all_energy( const VecX &x ){
    int dim = get_dim();
    VecX Dix = x.segment(g_index,dim);
    return energyLBFGS( Dix );
}

inline void EnergyTerm::gradient( const SparseMat &D, const VecX &x, VecX &grad ){
    int dof = x.rows();
    int dim = get_dim();
    VecX Dix = D.block(g_index,0,dim,dof)*x;
    gradient( Dix, grad );
}

inline void EnergyTerm::get_all_gradient( const VecX &z, VecX &grad ){
    int dim = get_dim();
    VecX zi = z.segment(g_index,dim);
    VecX grad_z = grad.segment(g_index,dim);
    get_gradient( zi, grad_z );
    grad.segment(g_index,dim) = grad_z;
}

inline void EnergyTerm::compute_x_gradient(const VecX &grad, VecX &all_grad ){
    int dim = get_dim();
    VecX gi = grad.segment(g_index,dim);
    get_gradient_for_vertices(gi, all_grad);
}
} // end namespace admm

#endif


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/35/problem35.hpp"

BOOST_AUTO_TEST_SUITE( Problem35 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem35::solve(100);
        BOOST_CHECK_EQUAL(res, 13);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem35::solve();
        BOOST_CHECK_EQUAL(res, 55);
    }

BOOST_AUTO_TEST_SUITE_END()


#ifndef _TRIFORCE_MESH_GEN_H_
#define _TRIFORCE_MESH_GEN_H_
#include <array>
#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <Eigen/Dense>
#include "compat.h"
template <typename Scalar>
class TriforceMeshFactory{
    public:
        typedef mtao::compat::array<int, 3> Face;
        typedef mtao::compat::array<int, 2> Edge;
        typedef Scalar Scalar;
        typedef typename Eigen::Matrix<Scalar,3,1> Vector;
        using VecVector = mtao::vector<Vector>;
        TriforceMeshFactory(int depth=3);
        void triforce(const Face & f, int depth);
        int add_edge(Edge e);
        void write(const std::string & filename);
        void write(std::ostream & outstream);
        const std::vector<Face> faces() const {return m_faces;}
        const VecVector vertices() const {return m_vertices;}

    private:
        const int m_depth = 0;
        VecVector m_vertices;
        std::vector<Face> m_faces;
        mtao::IndexMap<2> m_edges;
        //std::map<Edge,  int> m_edges;

};

template <typename T>
TriforceMeshFactory<T>::TriforceMeshFactory(int depth): m_depth(depth) {
}
template <typename T>
void TriforceMeshFactory<T>::run_in_place(VecVector& vertices, std::vector<Face>& faces) {
    //Create icosahedron base

    Scalar gr = .5 * (1 + std::sqrt(Scalar(5)));
    m_vertices.resize(5);

    m_vertices[ 0] = Vector(     0,    - 1,      0);
    m_vertices[ 1] = Vector(     0,      1,      0);
    m_vertices[ 2] = Vector(   - 1,      0,      0);
    m_vertices[ 3] = Vector(     1,      0,      0);
    m_vertices[ 4] = Vector(     0,      0,      1);

    triforce({{ 0, 1, 4}},depth);
    triforce({{ 1, 2, 4}},depth);
    triforce({{ 2, 3, 4}},depth);
    triforce({{ 3, 0, 4}},depth);

}
template <typename T>
void TriforceMeshFactory<T>::triforce(const Face & f, int depth) {
    if(depth <= 0) {
        m_faces.push_back(f);
    } else {
        int e01 = add_edge({{f[0],f[1]}});
        int e12 = add_edge({{f[1],f[2]}});
        int e02 = add_edge({{f[0],f[2]}});
        triforce({{f[0],e01,e02}},depth-1);
        triforce({{f[1],e12,e01}},depth-1);
        triforce({{f[2],e02,e12}},depth-1);
        triforce({{e01 ,e12,e02}},depth-1);
    }

}

template <typename T>
int TriforceMeshFactory<T>::add_edge(Edge e) {
    if(e[0] > e[1]) {
        int tmp = e[0];
        e[0] = e[1];
        e[1] = tmp;
    }
    auto it = m_edges.find(e);
    if(it != m_edges.end()) {
        return it->second;
    } else {
        m_edges[e] = m_vertices.size();
        m_vertices.push_back(
                (m_vertices[e[0]] + m_vertices[e[1]]).normalized()
                );
        return m_vertices.size()-1;
    }

}

template <typename T>
void TriforceMeshFactory<T>::write(const std::string & filename) {
    std::ofstream outstream(filename.c_str());
    write(outstream);
}

template <typename T>
void TriforceMeshFactory<T>::write(std::ostream & outstream) {
    outstream << "#Icosahedral subdivision to depth " << m_depth << std::endl;
    for(auto&& v: m_vertices) {
        outstream << "v " << v.transpose() << std::endl;
    }

    for(auto&& f: m_faces) {
        outstream << "f " << f[0]+1 << " " << f[1]+1 << " " << f[2]+1 << std::endl;
    }
}
#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>

#include <boost/property_map/property_map.hpp>

#include <iostream>
#include <fstream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel  Kernel;
typedef Kernel::Point_3                                      Point;

typedef CGAL::Surface_mesh<Point>                            Mesh;
typedef boost::graph_traits<Mesh>::face_descriptor           face_descriptor;
typedef boost::graph_traits<Mesh>::faces_size_type           faces_size_type;

typedef Mesh::Property_map<face_descriptor, faces_size_type> FCCmap;
typedef CGAL::Face_filtered_graph<Mesh>                      Filtered_graph;

namespace PMP = CGAL::Polygon_mesh_processing;

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/blobby_3cc.off");

  Mesh mesh;
  if(!PMP::IO::read_polygon_mesh(filename, mesh))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  FCCmap fccmap = mesh.add_property_map<face_descriptor, faces_size_type>("f:CC").first;
  faces_size_type num = PMP::connected_components(mesh,fccmap);
  std::cerr << "- The graph has " << num << " connected components (face connectivity)" << std::endl;

  std::cout << "The faces in component 0 are:" << std::endl;
  Filtered_graph ffg(mesh, 0, fccmap);
  for(boost::graph_traits<Filtered_graph>::face_descriptor f : faces(ffg))
    std::cout << f << std::endl;

  if(num > 1)
  {
    std::vector<faces_size_type> components;
    components.push_back(0);
    components.push_back(1);

    std::cout << "The faces in components 0 and 1 are:" << std::endl;
    ffg.set_selected_faces(components, fccmap);
    for(Filtered_graph::face_descriptor f : faces(ffg))
      std::cout  << f << std::endl;
  }

  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
struct pad {
  Index size() const { return out_size; }
  Index operator[] (Index i) const { return std::max<Index>(0,i-(out_size-in_size)); }
  Index in_size, out_size;
};

Matrix3i A;
A.reshaped() = VectorXi::LinSpaced(9,1,9);
cout << "Initial matrix A:\n" << A << "\n\n";
MatrixXi B(5,5);
B = A(pad{3,5}, pad{3,5});
cout << "A(pad{3,N}, pad{3,N}):\n" << B << "\n\n";

}
  return 0;
}