#include "distributions.h"

#include <Eigen/Dense>
#include <random>
#include <stan/math/prim.hpp>

#include "src/utils/proto_utils.h"

int bayesmix::categorical_rng(const Eigen::VectorXd &probas,
                              std::mt19937_64 &rng, int start /*= 0*/) {
  return stan::math::categorical_rng(probas, rng) + (start - 1);
}

double bayesmix::multi_normal_prec_lpdf(const Eigen::VectorXd &datum,
                                        const Eigen::VectorXd &mean,
                                        const Eigen::MatrixXd &prec_chol,
                                        double prec_logdet) {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  double base = prec_logdet + NEG_LOG_SQRT_TWO_PI * datum.size();
  double exp = (prec_chol * (datum - mean)).squaredNorm();
  return 0.5 * (base - exp);
}

Eigen::VectorXd bayesmix::multi_normal_prec_lpdf_grid(
    const Eigen::MatrixXd &data, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &prec_chol, double prec_logdet) {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  Eigen::VectorXd exp =
      ((data.rowwise() - mean.transpose()) * prec_chol.transpose())
          .rowwise()
          .squaredNorm();
  Eigen::VectorXd base = Eigen::ArrayXd::Ones(data.rows()) * prec_logdet +
                         NEG_LOG_SQRT_TWO_PI * data.cols();
  return (base - exp) * 0.5;
}

double bayesmix::multi_student_t_invscale_lpdf(
    const Eigen::VectorXd &datum, double df, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &invscale_chol, double scale_logdet) {
  int dim = datum.size();
  double exp =
      0.5 * (df + dim) *
      std::log(1 + (invscale_chol * (datum - mean)).squaredNorm() / df);
  double base = stan::math::lgamma((df + dim) * 0.5) -
                stan::math::lgamma(df * 0.5) - (0.5 * dim) * std::log(df) -
                (0.5 * dim) * stan::math::LOG_PI + 0.5 * scale_logdet;
  return base - exp;
}

Eigen::VectorXd bayesmix::multi_student_t_invscale_lpdf_grid(
    const Eigen::MatrixXd &data, double df, const Eigen::VectorXd &mean,
    const Eigen::MatrixXd &invscale_chol, double scale_logdet) {
  int dim = data.cols();
  int n = data.rows();
  double base_coeff = stan::math::lgamma((df + dim) * 0.5) -
                      stan::math::lgamma(df * 0.5) -
                      (0.5 * dim) * std::log(df) -
                      (0.5 * dim) * stan::math::LOG_PI + 0.5 * scale_logdet;
  Eigen::VectorXd base = Eigen::VectorXd::Ones(n) * base_coeff;
  Eigen::VectorXd quadforms =
      ((data.rowwise() - mean.transpose()) * invscale_chol.transpose())
          .rowwise()
          .squaredNorm();
  Eigen::VectorXd exp =
      (quadforms.array() / df + 1.0).log() * 0.5 * (df + dim);
  return base - exp;
}

double bayesmix::gaussian_mixture_dist(
    Eigen::VectorXd means1, Eigen::VectorXd vars1, Eigen::VectorXd weights1,
    Eigen::VectorXd means2, Eigen::VectorXd vars2, Eigen::VectorXd weights2) {
  double mix1 = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : mix1)
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means1.size(); j++) {
      mix1 += weights1(i) * weights1(j) *
              std::exp(stan::math::normal_lpdf(means1(i), means1(j),
                                               vars1(i) + vars1(j)));
    }
  }

  double mix2 = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : mix2)
  for (int i = 0; i < means2.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      mix2 += weights2(i) * weights2(j) *
              std::exp(stan::math::normal_lpdf(means2(i), means2(j),
                                               vars2(i) + vars2(j)));
    }
  }

  double inter = 0.0;
#pragma omp parallel for collapse(2) reduction(+ : inter)
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      inter += weights1(i) * weights2(j) *
               std::exp(stan::math::normal_lpdf(means1(i), means2(j),
                                                vars1(i) + vars2(j)));
    }
  }

  return mix1 + mix2 - 2 * inter;
}

double bayesmix::gaussian_mixture_dist(std::vector<Eigen::VectorXd> means1,
                                       std::vector<Eigen::MatrixXd> precs1,
                                       Eigen::VectorXd weights1,
                                       std::vector<Eigen::VectorXd> means2,
                                       std::vector<Eigen::MatrixXd> precs2,
                                       Eigen::VectorXd weights2) {
  std::vector<Eigen::MatrixXd> vars1;
  std::vector<Eigen::MatrixXd> vars2;

  for (const auto &p : precs1) vars1.push_back(stan::math::inverse_spd(p));

  for (const auto &p : precs2) vars2.push_back(stan::math::inverse_spd(p));

  double mix1 = 0.0;
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means1.size(); j++) {
      Eigen::MatrixXd var_ij = vars1[i] + vars1[j];
      mix1 += weights1(i) * weights1(j) *
              std::exp(
                  stan::math::multi_normal_lpdf(means1[i], means1[j], var_ij));
    }
  }

  double mix2 = 0.0;
  for (int i = 0; i < means2.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      Eigen::MatrixXd var_ij = vars2[i] + vars2[j];
      mix2 += weights2(i) * weights2(j) *
              std::exp(
                  stan::math::multi_normal_lpdf(means2[i], means2[j], var_ij));
    }
  }

  double inter = 0.0;
  for (int i = 0; i < means1.size(); i++) {
    for (int j = 0; j < means2.size(); j++) {
      Eigen::MatrixXd var_ij = vars1[i] + vars2[j];
      inter += weights1(i) * weights2(j) *
               std::exp(stan::math::multi_normal_lpdf(means1[i], means2[j],
                                                      var_ij));
    }
  }

  return mix1 + mix2 - 2 * inter;
}

double bayesmix::gaussian_mixture_dist(
    std::vector<bayesmix::AlgorithmState::ClusterState> clus1,
    Eigen::VectorXd weights1,
    std::vector<bayesmix::AlgorithmState::ClusterState> clus2,
    Eigen::VectorXd weights2) {
  double out;

  if (clus1[0].has_uni_ls_state()) {
    Eigen::VectorXd means1(clus1.size());
    Eigen::VectorXd vars1(clus1.size());
    Eigen::VectorXd means2(clus2.size());
    Eigen::VectorXd vars2(clus2.size());
    for (int i = 0; i < clus1.size(); i++) {
      means1(i) = clus1[i].uni_ls_state().mean();
      vars1(i) = clus1[i].uni_ls_state().var();
    }

    for (int i = 0; i < clus2.size(); i++) {
      means2(i) = clus2[i].uni_ls_state().mean();
      vars2(i) = clus2[i].uni_ls_state().var();
    }

    out = gaussian_mixture_dist(means1, vars1, weights1, means2, vars2,
                                weights2);
  } else if (clus1[0].has_multi_ls_state()) {
    std::vector<Eigen::VectorXd> means1, means2;
    std::vector<Eigen::MatrixXd> precs1, precs2;

    for (const auto &c : clus1) {
      means1.push_back(bayesmix::to_eigen(c.multi_ls_state().mean()));
      precs1.push_back(bayesmix::to_eigen(c.multi_ls_state().prec()));
    }

    for (const auto &c : clus2) {
      means2.push_back(bayesmix::to_eigen(c.multi_ls_state().mean()));
      precs2.push_back(bayesmix::to_eigen(c.multi_ls_state().prec()));
    }

    out = gaussian_mixture_dist(means1, precs1, weights1, means2, precs2,
                                weights2);
  } else {
    throw std::invalid_argument("Parameter type not recognized");
  }

  return out;
}


//
// Copyright (c) 2019 CNRS
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/center-of-mass-derivatives.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_kinematics_derivatives_vcom)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoid(model);

  Data data(model), data_ref(model);

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

  // Compute dvcom_dq using the algorithm
  Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3,model.nv);
  computeForwardKinematicsDerivatives(model,data,q,vq,aq);
  centerOfMass(model,data,q,vq);
  getCenterOfMassVelocityDerivatives(model,data,dvcom_dq);

  // Approximate dvcom_dq by finite diff.
  Eigen::Vector3d vcom0 = data.vcom[0];
  const double alpha = 1e-8;
  Eigen::VectorXd dq = VectorXd::Zero(model.nv);
  Data::Matrix3x dvcom_dqn(3,model.nv);

  for(int k = 0; k < model.nv; ++k)
  {
    dq[k] = alpha;
    centerOfMass(model,data,integrate(model,q,dq),vq);
    dvcom_dqn.col(k) = (data.vcom[0]-vcom0)/alpha;
    dq[k] = 0;
  }

  // Check that algo result and finite-diff approx are similar.
  BOOST_CHECK(dvcom_dq.isApprox(dvcom_dqn,sqrt(alpha)));
}

BOOST_AUTO_TEST_SUITE_END()


// Tests from ndarray lib
#include <boost/python.hpp>
#include <Eigen/Eigen>
#include "eigen_numpy.h"

#include <iostream>

namespace bp = boost::python;

template <typename M>
bool acceptMatrix(M m) {
    return (m(0,0) == 1) && (m(0,1) == 2) && (m(0,2) == 3)
        && (m(1,0) == 4) && (m(1,1) == 5) && (m(1,2) == 6);
}

template <typename M>
bool acceptVector(M m) {
    return (m[0] == 1) && (m[1] == 2) && (m[2] == 3) && (m[3] == 4);
}

template <typename M>
void fillMatrix(M & m) {
    m(0,0) = 1;
    m(0,1) = 2;
    m(0,2) = 3;
    m(1,0) = 4;
    m(1,1) = 5;
    m(1,2) = 6;
}

template <typename M>
M returnMatrix() {
    static typename boost::remove_const<typename boost::remove_reference<M>::type>::type m(2,3);
    fillMatrix(m);
    return m;
}

template <typename M>
void fillVector(M & m) {
    m[0] = 1;
    m[1] = 2;
    m[2] = 3;
    m[3] = 4;
}

template <typename M>
M returnVector() {
    static typename boost::remove_const<typename boost::remove_reference<M>::type>::type m(4);
    fillVector(m);
    return m;
}

template <typename M>
bp::object returnObject() {
    static typename boost::remove_const<typename boost::remove_reference<M>::type>::type m(2,3);
    fillMatrix(m);
    bp::object o(m);
    return o;
}

static const int X = Eigen::Dynamic;

BOOST_PYTHON_MODULE(test_eigen_numpy_mod) {
  SetupEigenConverters();
  bp::def("acceptMatrix_23d_cref", acceptMatrix< Eigen::Matrix<double,2,3> const &>);
  bp::def("acceptMatrix_X3d_cref", acceptMatrix< Eigen::Matrix<double,X,3> const &>);
  bp::def("acceptMatrix_2Xd_cref", acceptMatrix< Eigen::Matrix<double,2,X> const &>);
  bp::def("acceptMatrix_XXd_cref", acceptMatrix< Eigen::Matrix<double,X,X> const &>);
  bp::def("acceptVector_41d_cref", acceptVector< Eigen::Matrix<double,4,1> const &>);
  bp::def("acceptVector_X1d_cref", acceptVector< Eigen::Matrix<double,X,1> const &>);
  bp::def("acceptVector_14d_cref", acceptVector< Eigen::Matrix<double,1,4> const &>);
  bp::def("acceptVector_1Xd_cref", acceptVector< Eigen::Matrix<double,1,X> const &>);
  bp::def("returnVector_41d", returnVector< Eigen::Matrix<double,4,1> >);
  bp::def("returnVector_14d", returnVector< Eigen::Matrix<double,1,4> >);
  bp::def("returnVector_X1d", returnVector< Eigen::Matrix<double,X,1> >);
  bp::def("returnVector_1Xd", returnVector< Eigen::Matrix<double,1,X> >);
  bp::def("returnMatrix_23d", returnMatrix< Eigen::Matrix<double,2,3> >);
  bp::def("returnMatrix_X3d", returnMatrix< Eigen::Matrix<double,X,3> >);
  bp::def("returnMatrix_2Xd", returnMatrix< Eigen::Matrix<double,2,X> >);
  bp::def("returnMatrix_XXd", returnMatrix< Eigen::Matrix<double,X,X> >);
  bp::def("returnMatrix_23d_c", returnMatrix< Eigen::Matrix<double,2,3> const>);
  bp::def("returnMatrix_X3d_c", returnMatrix< Eigen::Matrix<double,X,3> const>);
  bp::def("returnMatrix_2Xd_c", returnMatrix< Eigen::Matrix<double,2,X> const>);
  bp::def("returnMatrix_XXd_c", returnMatrix< Eigen::Matrix<double,X,X> const>);
  bp::def("returnObject_23d", returnObject< Eigen::Matrix<double,2,3> >);
  bp::def("returnObject_X3d", returnObject< Eigen::Matrix<double,X,3> >);
  bp::def("returnObject_2Xd", returnObject< Eigen::Matrix<double,2,X> >);
  bp::def("returnObject_XXd", returnObject< Eigen::Matrix<double,X,X> >);
}


#include "catch.hpp"
#include "ezsolver.hpp"
#include "shiftinvert_solver.hpp"
#include "timer.hpp"

#include <Eigen/Dense>
#include <complex>
#include <fmt/format.h>
#include <iostream>
#include <string>

using namespace Eigen;
using namespace std::complex_literals;

TEST_CASE("run through", "[solver]") {
  int ndim = 4;
  MatrixXd matA = MatrixXd::Random(ndim, ndim);
  MatrixXd matB = MatrixXd::Random(ndim, ndim);
  GeneralizedEigenSolver<MatrixXd> ges(matA, matB);
  VectorXcd ev = ges.eigenvalues();
  for (int i = 0; i < ev.size(); ++i) {
    fmt::print("({:12.5f}, {:12.5f})\n", ev(i).real(), ev(i).imag());
  }
  int maxiter = 20;
  double tol = 1.0e-5;
  ShiftinvertSolver sis(matA, matB, maxiter, tol);
  std::complex<double> sigma = 0.59;
  std::complex<double> ev2 = sis.compute(sigma);
  fmt::print("eigenvalues near to ({:12.5f}, {:12.5f}): ({:12.5f}, {:12.5f})\n",
             sigma.real(), sigma.imag(), ev2.real(), ev2.imag());
  sigma = 2.8;
  ev2 = sis.compute(sigma);
  fmt::print("eigenvalues near to ({:12.5f}, {:12.5f}): ({:12.5f}, {:12.5f})\n",
             sigma.real(), sigma.imag(), ev2.real(), ev2.imag());
  sigma = 0.3 + 0.5i;
  ev2 = sis.compute(sigma);
  fmt::print("eigenvalues near to ({:12.5f}, {:12.5f}): ({:12.5f}, {:12.5f})\n",
             sigma.real(), sigma.imag(), ev2.real(), ev2.imag());

  double sigma3 = 0.03;
  EzSolver ezs(matA, matB);
  int nev = 2;
  VectorXcd ev3 = ezs.compute(sigma3, nev);
  for (int i = 0; i < ev3.size(); ++i) {
    fmt::print("eigenvalues near to ({:12.5f}): ({:12.5f}, {:12.5f})\n", sigma3,
               ev3(i).real(), ev3(i).imag());
  }
}

void start_performance(int ndim, double sigma_in) {
  MatrixXd matA = MatrixXd::Random(ndim, ndim);
  // MatrixXd matB = MatrixXd::Random(ndim, ndim);
  // MatrixXd matB = MatrixXd::Random(ndim, ndim);
  // MatrixXd matA = MatrixXd::Identity(ndim, ndim);
  MatrixXd matB = MatrixXd::Identity(ndim, ndim);
  for (int i = 1; i < ndim; ++i) {
    for (int j = 0; j < i; ++j) {
      matA(j, i) = matA(i, j);
      matB(j, i) = matB(i, j);
    }
  }
  // for (int i = 0; i < ndim; ++i) {
  //   for (int j = 0; j < ndim; ++j) {
  //     fmt::print("{:9.4f}", matA(i, j));
  //   }
  //   fmt::print("\n");
  // }
  // fmt::print("\n");
  // for (int i = 0; i < ndim; ++i) {
  //   for (int j = 0; j < ndim; ++j) {
  //     fmt::print("{:9.4f}", matB(i, j));
  //   }
  //   fmt::print("\n");
  // }

  Timer::begin("GeneralizedEigenSolver");
  GeneralizedEigenSolver<MatrixXd> ges(matA, matB);
  VectorXcd ev = ges.eigenvalues();
  Timer::end("GeneralizedEigenSolver");
  fmt::print("\nGeneralizedEigenSolver\n");
  for (int i = 0; i < ev.size(); ++i) {
    fmt::print("({:12.5f}, {:12.5f})\n", ev(i).real(), ev(i).imag());
  }
  fmt::print("---------------------------------------------\n");

  Timer::begin("shift-invert");
  int maxiter = 100;
  double tol = 1.0e-5;
  ShiftinvertSolver sis(matA, matB, maxiter, tol);
  std::complex<double> sigma = sigma_in;
  std::complex<double> ev2 = sis.compute(sigma);
  Timer::end("shift-invert");
  fmt::print("eigenvalues near to ({:12.5f}, {:12.5f}):\n", sigma.real(),
             sigma.imag());
  fmt::print("({:12.5f}, {:12.5f})\n", ev2.real(), ev2.imag());
  fmt::print("---------------------------------------------\n");

  Timer::begin("ezsolver-asym");
  EzSolver ezs(matA, matB);
  int nev = 4;
  double sigma3 = sigma_in;
  VectorXcd ev3 = ezs.compute(sigma3, nev);
  Timer::end("ezsolver-asym");
  Timer::begin("ezsolver-sym");
  EzSolver ezs2(matA, matB);
  VectorXcd ev4 = ezs2.compute_sym(sigma3, nev);
  Timer::end("ezsolver-sym");
  fmt::print("eigenvalues near to {:12.5f}:\n", sigma3);
  for (int i = 0; i < nev; ++i) {
    fmt::print("({:12.5f}, {:12.5f})\n", ev3(i).real(), ev3(i).imag());
  }
  fmt::print("---------------------------------------------\n");
  fmt::print("eigenvalues near to {:12.5f}:\n", sigma3);
  for (int i = 0; i < nev; ++i) {
    fmt::print("({:12.5f}, {:12.5f})\n", ev4(i).real(), ev4(i).imag());
  }
  fmt::print("---------------------------------------------\n");

  std::cout << Timer::summery() << std::endl;
}

TEST_CASE("performance 10", "[solver]") { start_performance(10, 0.59); }

TEST_CASE("performance 20", "[solver]") { start_performance(20, 0.59); }

TEST_CASE("performance 100", "[solver]") { start_performance(100, 0.59); }

TEST_CASE("performance 400", "[solver]") { start_performance(400, 0.59); }

TEST_CASE("ezsolver performance", "[solver]") {
  int ndim = 100;
  MatrixXd matA = MatrixXd::Random(ndim, ndim);
  MatrixXd matB = MatrixXd::Random(ndim, ndim);
  double sigma = 0.1;
  EzSolver ezs(matA, matB);
  VectorXi nevs(5);
  nevs << 1, 10, 20, 40, 80;
  for (int i = 0; i < nevs.size(); ++i) {
    int nev = nevs(i);
    std::string tag = fmt::format("nev = {:d}", nev);
    Timer::begin(tag);
    VectorXcd ev = ezs.compute(sigma, nev);
    Timer::end(tag);
    fmt::print("{:s}\n", tag);
    for (int i = 0; i < nev; ++i) {
      fmt::print("({:12.5f}, {:12.5f})\n", ev(i).real(), ev(i).imag());
    }
    fmt::print("----------------------------\n");
  }
  std::cout << Timer::summery() << std::endl;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. 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)

#ifndef BOOST_MP_CPP_COMPLEX_HPP
#define BOOST_MP_CPP_COMPLEX_HPP

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/complex_adaptor.hpp>

namespace boost {
   namespace multiprecision {

#ifndef BOOST_NO_CXX11_TEMPLATE_ALIASES

      template <unsigned Digits, backends::digit_base_type DigitBase = backends::digit_base_10, class Allocator = void, class Exponent = int, Exponent MinExponent = 0, Exponent MaxExponent = 0>
      using cpp_complex_backend = complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent> >;

      template <unsigned Digits, backends::digit_base_type DigitBase = digit_base_10, class Allocator = void, class Exponent = int, Exponent MinExponent = 0, Exponent MaxExponent = 0, expression_template_option ExpressionTemplates = et_off>
      using cpp_complex = number<complex_adaptor<cpp_bin_float<Digits, DigitBase, Allocator, Exponent, MinExponent, MaxExponent> >, ExpressionTemplates>;

      typedef cpp_complex<50> cpp_complex_50;
      typedef cpp_complex<100> cpp_complex_100;

      typedef cpp_complex<24, backends::digit_base_2, void, boost::int16_t, -126, 127> cpp_complex_single;
      typedef cpp_complex<53, backends::digit_base_2, void, boost::int16_t, -1022, 1023> cpp_complex_double;
      typedef cpp_complex<64, backends::digit_base_2, void, boost::int16_t, -16382, 16383> cpp_complex_extended;
      typedef cpp_complex<113, backends::digit_base_2, void, boost::int16_t, -16382, 16383> cpp_complex_quad;
      typedef cpp_complex<237, backends::digit_base_2, void, boost::int32_t, -262142, 262143> cpp_complex_oct;

#else

      typedef number<complex_adaptor<cpp_bin_float<50> >, et_off> cpp_complex_50;
      typedef number<complex_adaptor<cpp_bin_float<100> >, et_off> cpp_complex_100;

      typedef number<complex_adaptor<cpp_bin_float<24, backends::digit_base_2, void, boost::int16_t, -126, 127> >, et_off> cpp_complex_single;
      typedef number<complex_adaptor<cpp_bin_float<53, backends::digit_base_2, void, boost::int16_t, -1022, 1023> >, et_off> cpp_complex_double;
      typedef number<complex_adaptor<cpp_bin_float<64, backends::digit_base_2, void, boost::int16_t, -16382, 16383> >, et_off> cpp_complex_extended;
      typedef number<complex_adaptor<cpp_bin_float<113, backends::digit_base_2, void, boost::int16_t, -16382, 16383> >, et_off> cpp_complex_quad;
      typedef number<complex_adaptor<cpp_bin_float<237, backends::digit_base_2, void, boost::int32_t, -262142, 262143> >, et_off> cpp_complex_oct;

#endif

   }
}

#endif


#include <iostream>
#include <vector>
#include <unordered_map>
#include <string>
#include <Eigen/Dense>
#include "include/sample_network.h"

using namespace Eigen;

int main()
{
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::vector;
    using std::unordered_map;

    // I/O containers
    vector<MatrixXd> inputs;
    vector<MatrixXd> outputs;
    MatrixXd X = -2 * MatrixXd::Identity(2, 2) + MatrixXd::Ones(2, 2);
    MatrixXd t = MatrixXd::Identity(2, 2);
    inputs.push_back(X);

    // Define MLP(2-Layer)
    TwoLayerNet net(2, 3, 2, 0.01);

    cout << "--- predict ---" << endl;
    outputs = net.predict(inputs);
    cout << outputs[0] << endl;

    cout << "--- loss ---" << endl;
    inputs.push_back(t);
    outputs = net.loss(inputs);
    cout << outputs[0] << endl;

    cout << "--- accuracy ---" << endl;
    double accuracy = net.accuracy(inputs);
    cout << accuracy << endl;

    cout << "--- gradient ---" << endl;
    unordered_map<string, MatrixXd> grads;
    grads = net.gradient(inputs);

    for (auto grad: grads)
    {
        cout << grad.first << endl;
        cout << grad.second << endl;
    }

    // cout << "--- numerical gradient ---" << endl;
    // grads = net.numerical_gradient(inputs);

    // for (auto grad : grads)
    // {
    //     cout << grad.first << endl;
    //     cout << grad.second << endl;
    // }

    // 勾配更新確認
    for (int i = 0; i<5; i++){
        cout << *(net.params["b2"]) << endl;
        *(net.params["b2"]) -= grads["b2"];
        cout << "--- parameter b2 ---" << endl;
        cout << *(net.params["b2"]) << endl;
    }

    return 0;
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_10 : \f$\log(10)\f$

    @par Semantic:

    @code
    T r = Log_10<T>();
    @endcode

    is similar to:

    @code
      r =  T(2.302585092994045684017991454684364207601101488628773);
    @endcode

**/
  template<typename T> T Log_10();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_10. (\f$\log(10)\f$)

      Generate the  constant log_10.

      @return The Log_10 constant for the proper type
    **/
    const boost::dispatch::functor<tag::log_10_> log_10 = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log_10.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the 4th root of constant @ref Eps : \f$\sqrt[4]\epsilon\f$.

    @par Semantic:

    @code
    T r = Fourthrooteps<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = T(1)
    else if T is double
      r =  pow(2.0, -13);
    else if T is float
      r =  pow(2.0f, -5.75f);
    @endcode

    @return The Fourthrooteps constant for the proper type
  **/
  template<typename T> T Fourthrooteps();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant fourthrooteps.

      @return The Fourthrooteps constant for the proper type
    **/
    const boost::dispatch::functor<tag::fourthrooteps_> fourthrooteps = {};
  }
} }
#endif

#include <boost/simd/constant/definition/fourthrooteps.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


///////////////////////////////////////////////////////////////////////////////
// percentage_effective_sample_size.hpp                                      //
//                                                                           //
//  Copyright 2008 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_PERCENTAGE_EFFECTIVE_SAMPLE_SIZE_HPP_ER_2008_04
#include <cmath>

#include <boost/mpl/size_t.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>

#include <boost/call_traits.hpp>
#include <boost/array.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/type_traits/add_const.hpp>
#include <boost/numeric/conversion/converter.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/integrated_acvf.hpp>
#include <boost/accumulators/statistics/acv0.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ////////////////////////////////////////////////////////////////////////////
    // percentage_effective_sample_size
    template<typename T,typename I>
    class percentage_effective_sample_size_impl
      : public accumulator_base
    {
    public:
        typedef std::size_t result_type;

        percentage_effective_sample_size_impl(dont_care):val(0){}

        template<typename Args>
        void operator()(const Args& args)
        {
            T iacvf = integrated_acvf<I>(args[accumulator]);
            if(iacvf>static_cast<T>(0)){

                typedef boost::numeric::converter<result_type,T> T2res;
                T acv0_val = acv0<I>(args[accumulator]);
                T tmp = static_cast<T>(100)*acv0_val/iacvf;
                val = T2res::convert(tmp);
            }else{
                val = 0;
            }
        }

        result_type result(dont_care) const
        {
            return val;
        }
    private:
        result_type val;
    };

} // namespace impl
///////////////////////////////////////////////////////////////////////////////
// tag::integrated_acvf
//

namespace tag
{
    template <typename I = default_delay_discriminator>
    struct percentage_effective_sample_size
      : depends_on<acv0<I>, integrated_acvf<I> >
    {
        /// INTERNAL ONLY
      typedef
        accumulators::impl::percentage_effective_sample_size_impl<
            mpl::_1,I> impl;

    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::percentage_effective_sample_size
//

namespace extract
{

//  extractor<tag::percentage_effective_sample_size<> >
//    const percentage_effective_sample_size = {};

  // see acvf about default_delay_discriminator
  template<typename I,typename AccumulatorSet>
  typename mpl::apply<
    AccumulatorSet,tag::percentage_effective_sample_size<I>
    >::type::result_type
  percentage_effective_sample_size(AccumulatorSet const& acc){
    typedef tag::percentage_effective_sample_size<I> the_tag;
    return extract_result<the_tag>(acc);
  }

//  TODO
//  overload (default) see acvf

}

using extract::percentage_effective_sample_size;
}}
#endif


// Copyright (C) 2017-2019 Chris N. Richardson and Garth N. Wells
//
// This file is part of FEniCS-miniapp (https://www.fenicsproject.org)
//
// SPDX-License-Identifier:    MIT

#include "poisson_problem.h"
#include "Poisson.h"
#include <Eigen/Dense>
#include <cfloat>
#include <dolfinx/common/Timer.h>
#include <dolfinx/fem/DirichletBC.h>
#include <dolfinx/fem/Function.h>
#include <dolfinx/fem/FunctionSpace.h>
#include <dolfinx/fem/assembler.h>
#include <dolfinx/fem/petsc.h>
#include <dolfinx/fem/utils.h>
#include <dolfinx/la/PETScMatrix.h>
#include <dolfinx/la/PETScVector.h>
#include <dolfinx/mesh/Mesh.h>
#include <memory>
#include <utility>

std::tuple<dolfinx::la::PETScMatrix, dolfinx::la::PETScVector,
           std::shared_ptr<dolfinx::fem::Function<PetscScalar>>>
poisson::problem(std::shared_ptr<dolfinx::mesh::Mesh> mesh)
{
  dolfinx::common::Timer t0("ZZZ FunctionSpace");

  auto V = dolfinx::fem::create_functionspace(
      create_functionspace_form_Poisson_a, "u", mesh);

  t0.stop();

  dolfinx::common::Timer t1("ZZZ Assemble");

  // Define boundary condition
  auto u0 = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);
  std::fill(u0->x()->mutable_array().begin(), u0->x()->mutable_array().end(),
            0.0);

  const std::vector<std::int32_t> bdofs
      = dolfinx::fem::locate_dofs_geometrical({*V}, [](auto& x) {
          return (x.row(0) < DBL_EPSILON or x.row(0) > 1.0 - DBL_EPSILON);
        });

  auto bc = std::make_shared<dolfinx::fem::DirichletBC<PetscScalar>>(u0, bdofs);

  // Define coefficients
  auto f = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);
  auto g = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);
  f->interpolate([](auto& x) {
    auto dx = x.row(0) - 0.5;
    auto dy = x.row(1) - 0.5;
    return 10 * (-(dx * dx + dy * dy).exp() / 0.02);
  });
  g->interpolate([](auto& x) {
    {
      return (5.0 * x.row(0)).sin();
    }
  });

  // Define variational forms
  auto L = dolfinx::fem::create_form<PetscScalar>(create_form_Poisson_L, {V},
                                                  {{"f", f}, {"g", g}}, {}, {});
  auto a = dolfinx::fem::create_form<PetscScalar>(create_form_Poisson_a, {V, V},
                                                  {}, {}, {});

  // Create matrices and vector, and assemble system
  dolfinx::la::PETScMatrix A(dolfinx::fem::create_matrix(*a), false);
  dolfinx::la::PETScVector b(*L->function_spaces()[0]->dofmap()->index_map,
                             L->function_spaces()[0]->dofmap()->index_map_bs());

  MatZeroEntries(A.mat());
  dolfinx::common::Timer t2("ZZZ Assemble matrix");
  dolfinx::fem::assemble_matrix(dolfinx::la::PETScMatrix::add_fn(A.mat()), *a,
                                {bc});
  dolfinx::fem::add_diagonal(dolfinx::la::PETScMatrix::add_fn(A.mat()), *V,
                             {bc});
  MatAssemblyBegin(A.mat(), MAT_FINAL_ASSEMBLY);
  MatAssemblyEnd(A.mat(), MAT_FINAL_ASSEMBLY);
  t2.stop();

  VecSet(b.vec(), 0.0);
  VecGhostUpdateBegin(b.vec(), INSERT_VALUES, SCATTER_FORWARD);
  VecGhostUpdateEnd(b.vec(), INSERT_VALUES, SCATTER_FORWARD);

  dolfinx::common::Timer t3("ZZZ Assemble vector");
  dolfinx::fem::assemble_vector_petsc(b.vec(), *L);
  dolfinx::fem::apply_lifting_petsc(b.vec(), {a}, {{bc}}, {}, 1.0);
  VecGhostUpdateBegin(b.vec(), ADD_VALUES, SCATTER_REVERSE);
  VecGhostUpdateEnd(b.vec(), ADD_VALUES, SCATTER_REVERSE);
  dolfinx::fem::set_bc_petsc(b.vec(), {bc}, nullptr);
  t3.stop();

  t1.stop();

  // Create Function to hold solution
  auto u = std::make_shared<dolfinx::fem::Function<PetscScalar>>(V);

  return {std::move(A), std::move(b), u};
}


/*
 * TypeDefs.hpp
 *
 *  Created on: March 18, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

// Eigen
#include <Eigen/Core>

#pragma once

namespace grid_map {

  typedef Eigen::MatrixXf Matrix;
  typedef Matrix::Scalar DataType;
  typedef Eigen::Vector2d Position;
  typedef Eigen::Vector2d Vector;
  typedef Eigen::Vector3d Position3;
  typedef Eigen::Vector3d Vector3;
  typedef Eigen::Array2i Index;
  typedef Eigen::Array2i Size;
  typedef Eigen::Array2d Length;
  typedef uint64_t Time;

  /*
   * Interpolations are ordered in the order
   * of increasing accuracy and computational complexity.
   * INTER_NEAREST - fastest, but least accurate,
   * INTER_CUBIC - slowest, but the most accurate.
   * see:
   * https://en.wikipedia.org/wiki/Bicubic_interpolation
   * https://web.archive.org/web/20051024202307/http://www.geovista.psu.edu/sites/geocomp99/Gc99/082/gc_082.htm
   * for more info. Cubic convolution algorithm is also known as piecewise cubic
   * interpolation and in general does not guarantee continuous
   * first derivatives.
   */
  enum class InterpolationMethods{
      INTER_NEAREST, // nearest neighbor interpolation
      INTER_LINEAR,   // bilinear interpolation
      INTER_CUBIC_CONVOLUTION, //piecewise bicubic interpolation using convolution algorithm
      INTER_CUBIC // standard bicubic interpolation
  };

} /* namespace */


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
#include <type_traits>
using namespace std;

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

#include <boost/integer_traits.hpp>
using namespace boost;

void case1()
{
    cout << integer_traits<int >::const_max << endl;
    cout << integer_traits<bool>::const_min << endl;
    cout << integer_traits<long>::is_signed << endl;
}

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

#include <boost/cstdint.hpp>
#include <limits>

void case2()
{
    uint8_t         u8;
    int_fast16_t    i16;
    int_least32_t   i32;
    uintmax_t       um;

    u8  = 255;
    i16 = 32000;
    i32 = i16;
    um  = u8 + i16 + i32;

    assert(sizeof(u8) == 1);

    cout << "u8 :" << sizeof(u8)
        << " v = "<< (short)u8 << endl;
    cout << "i16 :" << sizeof(i16)
        << " v = "<< i16 << endl;
    cout << "i32 :" << sizeof(i32)
        << " v = "<< i32 << endl;
    cout << "um :" << sizeof(um)
        << " v = "<< um << endl;

    cout << (short)numeric_limits<int8_t>::max() << endl;
    cout << numeric_limits<uint_least16_t>::max() << endl;
    cout << numeric_limits<int_fast32_t>::max() << endl;
    cout << numeric_limits<intmax_t>::min() << endl;
}

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

#include <boost/integer.hpp>
#include <boost/type_index.hpp>

void case3()
{
    typedef int_fast_t<char>::fast cfast;
    cout << typeindex::type_id<cfast>().pretty_name() << endl;

    typedef int_fast_t<int>::fast ifast;
    cout << typeindex::type_id<ifast>().pretty_name() << endl;

    typedef int_fast_t<uint16_t>::fast u16fast;
    cout << typeindex::type_id<u16fast>().pretty_name() << endl;
}

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

#include <boost/format.hpp>

template<typename T>
string type_name()
{
    return typeindex::type_id<T>().pretty_name();
}

void case4()
{
    format fmt("type:%s,size=%dbit\n"); //一个format对象

    typedef uint_t<15>::fast u15;                 //可容纳15位的无符号最快整数
    cout << fmt % type_name<u15>() % (sizeof(u15) * 8) ;

    typedef int_max_value_t<32700>::fast i32700;     //可处理32700的最快整数
    cout << fmt % type_name<i32700>() % (sizeof(i32700) * 8);

    typedef int_min_value_t<-33000>::fast i33000;        //可处理-33000的最快整数
    cout << fmt % type_name<i33000>() % (sizeof(33000) * 8);

    typedef uint_value_t<33000>::fast u33000;            //可处理33000的最快无符号整数
    cout << fmt % type_name<u33000>() % (sizeof(u33000) * 8);
}

int main()
{
    case1();
    case2();
    case3();
    case4();
}


#include <cstdio>
#include <boost/math/distributions/beta.hpp>

#include <iostream>
#include <string>
#include <map>
#include <random>

using namespace boost::math;
using namespace std;

int main() {
	srand(time(0));
	double alpha = 0.5, beta = 0.5;
	beta_distribution<> dist(alpha, beta);

	for (int i = 0 ; i < 1000 ; i++) {
		double rngfloat = (double)(rand() % 101) / 100;
		cout<<" "<<quantile(dist, rngfloat)<<endl;
	}
}


#pragma once

#include <Eigen/Sparse>
#include <vector>

#include "mtao/eigen/shape_checks.hpp"
#include "mtao/geometry/volume.hpp"

namespace mtao::simulation::simplicial {
template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian(
    const Eigen::MatrixBase<VDerived>& V, const Eigen::MatrixBase<SDerived>& S);

namespace internal {
    // NOTE: yes we do have a second impl in mtao/geometry/mesh/laplacian.hpp
template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian2(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    using Scalar = typename VDerived::Scalar;
    if (S.size() == 0) {
        return {};
    }
    int size = S.maxCoeff() + 1;
    Eigen::SparseMatrix<Scalar> L(size, size);
    std::vector<Eigen::Triplet<Scalar>> trips;
    trips.reserve(4 * 3 * S.cols());
    eigen::row_check<3>(S);
    assert(V.cols() >= size);

    for (int sidx = 0; sidx < S.cols(); ++sidx) {
        auto s = S.col(sidx);
        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            const auto bi = s((j + 1) % 3);
            const auto ci = s((j + 2) % 3);
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            auto u = b - a;
            auto v = c - a;

            Scalar dot = u.dot(v);
            Scalar cross;
            if constexpr (VDerived::RowsAtCompileTime == 2) {
                cross = u.x() * v.y() - u.y() * v.x();
            } else if constexpr (VDerived::RowsAtCompileTime == 3) {
                cross = u.cross(v);
            }
            Scalar val = std::abs(.5 * dot / cross);
            trips.emplace_back(bi, ci, -val);
            trips.emplace_back(ci, bi, -val);
            trips.emplace_back(bi, bi, val);
            trips.emplace_back(ci, ci, val);
        }
    }
    L.setFromTriplets(trips.begin(), trips.end());
    return L;
}

template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian3(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    using Scalar = typename VDerived::Scalar;
    if (S.size() == 0) {
        return {};
    }
    int size = S.maxCoeff() + 1;
    Eigen::SparseMatrix<Scalar> L(size, size);
    std::vector<Eigen::Triplet<Scalar>> trips;
    trips.reserve(4 * 4 * S.cols());

    eigen::row_check<4>(S);
    eigen::row_check<3>(V);

    auto vols = geometry::volumes(V, S);

    assert(V.cols() >= size);
    for (int sidx = 0; sidx < S.cols(); ++sidx) {
        auto s = S.col(sidx);
        auto vol = vols(sidx);
        mtao::Matrix<Scalar, 3, 4> N;

        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            const auto bi = s((j + 1) % 4);
            const auto ci = s((j + 2) % 4);
            const auto di = s((j + 3) % 4);
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            auto d = V.col(di);
            auto u = c - b;
            auto v = d - b;
            auto n = N.col(j);
            n = u.cross(v);
            n.normalize();
            Scalar len = (a - b).dot(n);
            if (len < 0) {
                n *= -len * len;
            } else {
                n *= len * len;
            }
        }
        for (int j = 0; j < S.rows(); ++j) {
            const auto ai = s(j);
            auto na = N.col(j);
            for (int k = j+1; k < S.rows(); ++k) {
                const auto bi = s(k);
                auto nb = N.col(k);
                Scalar val = vol * na.dot(nb);
                trips.emplace_back(ai, bi, -val);
                trips.emplace_back(bi, ai, -val);
                trips.emplace_back(ai, ai, val);
                trips.emplace_back(bi, bi, val);
            }
        }
    }
    L.setFromTriplets(trips.begin(), trips.end());
    return L;
}
}  // namespace internal

template <typename VDerived, typename SDerived>
Eigen::SparseMatrix<typename VDerived::Scalar> laplacian(
    const Eigen::MatrixBase<VDerived>& V,
    const Eigen::MatrixBase<SDerived>& S) {
    // TODO: make this use V with constant col sizes
    if constexpr(SDerived::RowsAtCompileTime == 3) {
        return internal::laplacian2(V, S);
    } else if constexpr(SDerived::RowsAtCompileTime == 4) {
        return internal::laplacian3(V, S);
    }
    return {};
}

}  // namespace mtao::simulation::simplicial


/**
 * August 8th, 2018
 * source: https://zhuanlan.zhihu.com/p/38745950
 * Constant velocity prediction Kalman filter for shield local position
 */

#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/TwistStamped.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

string visual_topic, publisher_topic, debug_topic, real_visual_topic, transform_topic;
ros::Publisher filter_pub, debug_pub, transform_pub;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

VectorXd x(6);      // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd Q = MatrixXd::Identity(6, 6); // prediction noise covariance
MatrixXd R = MatrixXd::Identity(3, 3); // observation noise covariance
MatrixXd A = MatrixXd::Identity(6, 6); // state transfer function

bool visual_initialized = false, visual_valid = false;

ros::Time t_prev;
double R_pos, Q_pos, Q_vel, P_weight;
const double CV_UPDATE_TIME_MAX = 0.1; // maxium allowed update time
const double DELAY_MAX = 0.05;
const int ROS_FREQ = 100;
double OUTLIER_THRESHOLD = 10000.0;

Vector3d camera_T_shield_prev = MatrixXd::Zero(3, 1);
Vector3d imu_T_shield_prev = MatrixXd::Zero(3, 1);
bool vel_is_outlier = false;
double chi_square = 0;
double outlier_l2_norm_ratio = 1.5;
double yaw_delay = 0.0;
double pitch_delay = 0.0;
Vector3d OUTPUT_BOUND = MatrixXd::Zero(3, 1);;

static void pub_result(const ros::Time &stamp, double delay_dt)
{
//    geometry_msgs::TwistStamped odom;
    rm_cv::ArmorRecord odom;
    odom.header.stamp = stamp;
    double predict_x = x(0) + (delay_dt + yaw_delay)   * x(3);
    double predict_y = x(1) + (delay_dt + pitch_delay) * x(4);
    double predict_z = x(2) + delay_dt * x(5);
    odom.armorPose.linear.x  = (predict_x < OUTPUT_BOUND[0]) ? predict_x * 1000 : OUTPUT_BOUND[0] * 1000;
    odom.armorPose.linear.y  = (predict_y < OUTPUT_BOUND[1]) ? predict_y * 1000 : OUTPUT_BOUND[1] * 1000;
    odom.armorPose.linear.z  = (predict_z < OUTPUT_BOUND[2]) ? predict_z * 1000 : OUTPUT_BOUND[2] * 1000;
    odom.armorPose.angular.x = x(3) * 1000;
    odom.armorPose.angular.y = x(4) * 1000;
    odom.armorPose.angular.z = x(5) * 1000;
    filter_pub.publish(odom);
}

static void pub_preprocessed(const std_msgs::Header &header,
                      const Ref<const Vector3d> p,
                      const Ref<const Vector3d> v,
                      ros::Publisher &publisher)
{
    geometry_msgs::TwistStamped debug;
    debug.header = header;
    debug.twist.linear.x  = p[0] * 1000;
    debug.twist.linear.y  = p[1] * 1000;
    debug.twist.linear.z  = p[2] * 1000;
    debug.twist.angular.x = v[0] * 1000;
    debug.twist.angular.y = v[1] * 1000;
    debug.twist.angular.z = v[2] * 1000;
    publisher.publish(debug);
}

// Chi-square test for outlier rejection
static bool velocity_is_outlier(const Vector3d &pos)
{
    VectorXd r = MatrixXd::Zero(3, 1);
    VectorXd z = MatrixXd::Zero(3, 1);
    MatrixXd S = MatrixXd::Zero(3, 3);
    z << pos[0], pos[1], pos[2];

    double pos_norm   = z.segment<3>(0).norm();
    double state_norm = x.segment<3>(0).norm();
    ROS_INFO("pos_norm %f, state_norm %f", pos_norm, state_norm);
    if (pos_norm > state_norm * outlier_l2_norm_ratio) {
        return true;
    } else if (pos_norm < state_norm / outlier_l2_norm_ratio) {
        return true;
    }

    // r = z - H * x, residual
    // S = H P H' + R, residual covariance
    MatrixXd H = MatrixXd::Identity(3, 6); // observation matrix

//    MatrixXd K_next = (H * P * H.transpose() + R).ldlt().solve(P * H.transpose());
//    MatrixXd x_next = x + K_next * (z - H * x);
//    MatrixXd P_next = P - K_next * H * P;

    r = z - H * x;
    S = H * P * H.transpose() + R;
    chi_square = r.transpose() * S * r;
    // cout << "chi_square " << endl << chi_square << endl;

    return (chi_square > OUTLIER_THRESHOLD);
}

static void preprocess_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist,
                              Vector3d &pos, double dt_update)
{

    Vector3d camera_T_shield, camera_vel_shield, imu_T_shield, imu_vel_shield;

    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;

    camera_T_shield *= 0.001; // Convert millimeter to meter

    // calculate and check the velocity
    camera_vel_shield = (camera_T_shield - camera_T_shield_prev) / dt_update;
    camera_T_shield_prev = camera_T_shield;

    vel_is_outlier = velocity_is_outlier(camera_T_shield);

    if (vel_is_outlier) {
        pos = x.segment<3>(0);
        ROS_INFO("outlier rejected");
    }
    else {
        pos = camera_T_shield;
    }

    // store the state
    pub_preprocessed(header, camera_T_shield, camera_vel_shield, debug_pub);

    imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera * 0.001;
    imu_vel_shield = (imu_T_shield - imu_T_shield_prev) / dt_update;
    imu_T_shield_prev = imu_T_shield;
    pub_preprocessed(header, imu_T_shield, imu_vel_shield, transform_pub);
}

static void propagate(const double &dt) {
    A.topRightCorner(3, 3) = dt * MatrixXd::Identity(3, 3);
    // cout << "A " << endl << A << endl;
    x = A * x;
    P = A * P * A.transpose() + Q;
    // cout << "P " << endl << P << endl;
}

static void update(const Vector3d &pos)
{
    MatrixXd H, K, z;

    H = MatrixXd::Identity(3, 6); // observation matrix
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
    z = MatrixXd::Zero(3, 1);
    z << pos[0], pos[1], pos[2];

    x = x + K * (z - H * x);
    P = P - K * H * P;
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist)
{
    ros::Time t_update = header.stamp;
    ROS_INFO("visual init at %f", t_update.toSec());

    Vector3d camera_T_shield, imu_T_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;
    // imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;
    camera_T_shield *= 0.001; // Convert millimeter to meter

    x.segment<3>(0) = camera_T_shield; // init velocity with zero
    x.segment<3>(3).setZero(); // init velocity with zero

    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;
    camera_T_shield_prev = camera_T_shield;
    t_prev = t_update;
    visual_initialized = true;
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
                     pnp->twist.linear.y == 0 &&
                     pnp->twist.linear.z == 0 );

    if (visual_valid) {
        if (!visual_initialized) {
            initialize_visual(pnp->header, pnp->twist);
        }
        else {
            double running_time = ros::Time::now().toSec();
            double delay_dt = running_time - pnp->header.stamp.toSec();
            delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

            ros::Time t_update = pnp->header.stamp;
            double dt_update = (t_update - t_prev).toSec();
            dt_update = (dt_update < CV_UPDATE_TIME_MAX) ? dt_update : CV_UPDATE_TIME_MAX;

            Vector3d pos;

            preprocess_visual(pnp->header, pnp->twist, pos, dt_update);

            update(pos);

            pub_result(pnp->header.stamp, delay_dt);
            t_prev = t_update;
        }
    }
}

/**
 * handle, save, and process visual messages
 * @param armor
 */
void real_visual_cb(const rm_cv::ArmorRecord::ConstPtr &armor)
{
    visual_valid = !(armor->armorPose.linear.x == 0 &&
                     armor->armorPose.linear.y == 0 &&
                     armor->armorPose.linear.z == 0 );

    if (visual_valid) {
        if (!visual_initialized) {
            initialize_visual(armor->header, armor->armorPose);
        }
        else {

            double running_time = ros::Time::now().toSec();
            double delay_dt = running_time - armor->header.stamp.toSec();
            delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

            ros::Time t_update = armor->header.stamp;
            double dt_update = (t_update - t_prev).toSec();
            dt_update = (dt_update < CV_UPDATE_TIME_MAX) ? dt_update : CV_UPDATE_TIME_MAX;

            Vector3d pos;
            preprocess_visual(armor->header, armor->armorPose, pos, dt_update);

            update(pos);

            propagate(dt_update);

            pub_result(armor->header.stamp, delay_dt);
            t_prev = t_update;
        }
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "prediction_kalman_filter");
    ros::NodeHandle n("~");

    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("real_visual_topic", real_visual_topic, string("/detected_armor"));
    n.param("publisher_topic", publisher_topic, string("/prediction_kf/predict"));
    n.param("debug_topic", debug_topic, string("/prediction_kf/preprocessed"));
    n.param("transform_topic", transform_topic, string("/prediction_kf/transformed"));
    n.param("chi_square_threshold", OUTLIER_THRESHOLD, 10000.0);
    n.param("outlier_l2_norm_ratio", outlier_l2_norm_ratio, 1.5);
    n.param("R_pos", R_pos, 16.0);
    n.param("Q_pos", Q_pos, 0.2);
    n.param("Q_vel", Q_vel, 1.0);
    n.param("P_matrix_weight", P_weight, 1.0);
    n.param("yaw_delay", yaw_delay, 0.0);
    n.param("pitch_delay", pitch_delay, 0.0);

    // For chassis reading only
    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;
    imu_T_camera <<  110, -70, -40; // in millimeter

    OUTPUT_BOUND << 10.0, 10.0, 10.0;

    x.setZero();
    R = R_pos * MatrixXd::Identity(3, 3);
    // R(0, 1) = R_pos * 0.1;
    // R(0, 2) = R_pos * 0.1;
    // R(1, 2) = R_pos * 0.1;
    // R(1, 0) = R_pos * 0.1;
    // R(2, 0) = R_pos * 0.1;
    // R(2, 1) = R_pos * 0.1;
    Q.topLeftCorner(3, 3)     = Q_pos * MatrixXd::Identity(3, 3);
    Q.bottomRightCorner(3, 3) = Q_vel * MatrixXd::Identity(3, 3);
    P.topLeftCorner(3, 3)     = P_weight * MatrixXd::Identity(3, 3);
    // P.topRightCorner(3, 3)    = 0.5 * P_weight * MatrixXd::Identity(3, 3);
    // P.bottomLeftCorner(3, 3)  = 0.5 * P_weight * MatrixXd::Identity(3, 3);
    P.bottomRightCorner(3, 3) = 2 * P_weight * MatrixXd::Identity(3, 3);
    cout << "R " << endl << R << endl;
    cout << "Q " << endl << Q << endl;
    cout << "P " << endl << P << endl;

    ros::Subscriber s1 = n.subscribe(visual_topic, 40, visual_callback);
    ros::Subscriber s2 = n.subscribe(real_visual_topic, 40, real_visual_cb);
    filter_pub = n.advertise<rm_cv::ArmorRecord>(publisher_topic, 40);
    debug_pub  = n.advertise<geometry_msgs::TwistStamped>(debug_topic, 40);
    transform_pub = n.advertise<geometry_msgs::TwistStamped>(transform_topic, 40);
    ros::Rate r(ROS_FREQ);
    ros::spin();

}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    cpp_int x; cin >> x;
    for (int i = -118; i <= 119; i++) {
        for (int j = -119; j <= 118; j++) {
            if (i * i * i * i * i - j * j * j * j * j == x) {
                cout << i << " " << j << endl;
                return 0;
            }
        }
    }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <cmath>

#include <boost/random.hpp>
#include <boost/make_shared.hpp>

#if USE_GSL
#include <gsl/gsl_qrng.h>
#endif

#include "tudat/math/statistics/randomSampling.h"
namespace tudat
{

namespace statistics
{

//! Generate sample of random vectors, with entries of each vector independently, but not identically, distributed.
std::vector< Eigen::VectorXd > generateRandomSampleFromGenerator(
        const int numberOfSamples,
        const std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators )
{
    std::vector< Eigen::VectorXd > randomSamples;
    Eigen::VectorXd randomSample( randomVariableGenerators.size( ) );

    // Generate samples
    for( int i = 0; i < numberOfSamples; i++ )
    {
        for( unsigned int j = 0; j < randomVariableGenerators.size( ); j++ )
        {
            randomSample( j ) = randomVariableGenerators.at( j )->getRandomVariableValue( );
        }
        randomSamples.push_back( randomSample );
    }

    return randomSamples;
}

//! Generate sample of random vectors, with entries of each vector independently and identically distributed.
std::vector< Eigen::VectorXd > generateRandomSampleFromGenerator(
        const int numberOfSamples, const int numberOfDimensions,
        const std::shared_ptr< RandomVariableGenerator< double > > randomVariableGenerator )
{
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    for( int i = 0; i < numberOfDimensions; i++ )
    {
        randomVariableGenerators.push_back( randomVariableGenerator );
    }

    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator
std::vector< Eigen::VectorXd > generateUniformRandomSample(
        const int seed, const int numberOfSamples,
        const Eigen::VectorXd& lowerBound, const Eigen::VectorXd& upperBound )
{
    if( lowerBound.rows( ) != upperBound.rows( ) )
    {
        throw std::runtime_error( "Error when making uniformly distributed samples, input is inconsistent" );
    }

    // Create distributions
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    std::vector< double > currentParameters;
    for( int i = 0; i < lowerBound.rows( ); i++ )
    {
        currentParameters = { lowerBound( i ), upperBound( i ) };
        randomVariableGenerators.push_back(
                    createBoostContinuousRandomVariableGenerator(
                        uniform_boost_distribution, currentParameters, seed + i ) );
    }

    // Generate samples
    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator
std::vector< Eigen::VectorXd > generateUniformRandomSample(
        const int seed, const int numberOfSamples, const int numberOfDimensions,
         const double lowerBound, const double upperBound )
{
    return generateUniformRandomSample(
                seed, numberOfSamples,
                Eigen::VectorXd::Constant( numberOfDimensions, lowerBound ),
                Eigen::VectorXd::Constant( numberOfDimensions, upperBound ) );
}

//! Generator random vector using pseudo random generator with gaussian distribution (without correlation)
std::vector< Eigen::VectorXd > generateGaussianRandomSample(
        const int seed, const int numberOfSamples,
        const Eigen::VectorXd& mean, const Eigen::VectorXd& standardDeviation )
{
    if( mean.rows( ) != standardDeviation.rows( ) )
    {
        throw std::runtime_error( "Error when making Gaussian distributed samples, input is inconsistent" );
    }

    // Create distributions
    std::vector< std::shared_ptr< RandomVariableGenerator< double > > > randomVariableGenerators;
    std::vector< double > currentParameters;
    for( int i = 0; i < mean.rows( ); i++ )
    {
        currentParameters = { mean( i ), standardDeviation( i ) };
        randomVariableGenerators.push_back(
                    createBoostContinuousRandomVariableGenerator(
                        normal_boost_distribution, currentParameters, seed + i ) );
    }

    // Generate samples
    return generateRandomSampleFromGenerator( numberOfSamples, randomVariableGenerators );
}

//! Generator random vector using pseudo random generator with gaussian distribution (without correlation)
std::vector< Eigen::VectorXd > generateGaussianRandomSample(
        const int seed, const int numberOfSamples, const int numberOfDimensions,
        const double mean, const double standardDeviation )
{
    return generateGaussianRandomSample(
                seed, numberOfSamples,
                Eigen::VectorXd::Constant( numberOfDimensions, mean ),
                Eigen::VectorXd::Constant( numberOfDimensions, standardDeviation ) );
}

#if USE_GSL

//! Generator random vector using Sobol sampler
std::vector< Eigen::VectorXd > generateVectorSobolSample(
        int numberOfSamples,
        const Eigen::VectorXd& lowerBound, const Eigen::VectorXd& upperBound )
{
    int numberOfDimensions = upperBound.rows( );

    // Compute propertie
    Eigen::VectorXd width = upperBound - lowerBound;
    Eigen::VectorXd average = (upperBound + lowerBound)/2.0 ;

    std::vector< Eigen::VectorXd > sobolSamples( numberOfSamples );

    Eigen::VectorXd randomSample( numberOfDimensions ) ;
    double randomSampleArray[ numberOfDimensions ];

    gsl_qrng * q = gsl_qrng_alloc (gsl_qrng_sobol, numberOfDimensions );

    // Loop over samples
    for( int j = 0 ; j < numberOfSamples ; j++ )
    {
        gsl_qrng_get( q, randomSampleArray ); // Generate sobol [0,1]

        // Fill vector
        for( int i = 0 ; i < numberOfDimensions ; i++ )
        {
            randomSample( i ) = randomSampleArray[ i ] - 0.5 ;
        }
        sobolSamples[ j ] = randomSample.cwiseProduct( width ) + average ; // Save vector
    }

    gsl_qrng_free (q); // Deallocate GSL variables

    return sobolSamples;
}

//! Generator random vector using Sobol sampler
std::vector< Eigen::VectorXd > generateVectorSobolSample(
        const int numberOfDimensions, int numberOfSamples,
        const double lowerBound, const double upperBound )
{
    return generateVectorSobolSample( numberOfSamples,
                                      Eigen::VectorXd::Constant( numberOfDimensions, lowerBound ),
                                      Eigen::VectorXd::Constant( numberOfDimensions, upperBound ) );
}
#endif

} // Close Namespace statistics

} // Close Namespace tudat


#ifndef DART_REALTIME_VECTOR_LOG
#define DART_REALTIME_VECTOR_LOG

#include <vector>

#include <Eigen/Dense>

#include "dart/math/MathTypes.hpp"

namespace dart {
namespace realtime {

struct VectorObservation
{
  long time;
  Eigen::VectorXs value;

  VectorObservation(long time, Eigen::VectorXs value);
};

class VectorLog
{
public:
  VectorLog(int dim);

  void record(long time, Eigen::VectorXs val);

  Eigen::MatrixXs getValues(long start, int steps, long millisPerStep);

  void discardBefore(long time);

  long availableHistoryBefore(long time);

protected:
  int mDim;
  long mStartTime;
  std::vector<VectorObservation> mObservations;
};

} // namespace realtime
} // namespace dart

#endif


/*!
 * @file
 * Contains unit tests for `boost::mpl11::fix`.
 */

#include <boost/mpl11/functional.hpp>

#include <boost/mpl11/integer.hpp>
#include <boost/mpl11/logical.hpp>

using namespace boost::mpl11;

// Factorial using fix<>
template <typename f, typename n>
using fact_impl = if_c<n::type::value == 0,
    ullong<1>,
    mult<n, apply<f, pred<n>>>
>;

template <unsigned long long n>
struct fact_fix {
    static constexpr auto value = fix<lift<fact_impl>>::type::
                                  template apply<ullong<n>>::type::value;
};

// Standard recursive factorial
template <unsigned long long n>
struct fact_rec { static constexpr auto value = n * fact_rec<n - 1>::value; };

template <>
struct fact_rec<0> { static constexpr unsigned long long value = 1; };

// Compare both implementations for a couple of values.
template <unsigned long long from, unsigned long long to>
struct test_fact : test_fact<from + 1, to> {
    static_assert(fact_fix<from>::value == fact_rec<from>::value, "");
};

template <unsigned long long to>
struct test_fact<to, to> { };

struct tests
    : test_fact<0, 15>
{ };

int main() { }


#ifdef _DEBUG

#include "../../../library/src/debug_template.hpp"

#define DMP(...) dump(#__VA_ARGS__, __VA_ARGS__)
#else
#define DMP(...) ((void)0)
#endif

#include <cassert>
#include <cstdio>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <set>
#include <map>
#include <unordered_map>
#include <queue>
#include <numeric>
#include <algorithm>
#include <bitset>
#include <variant>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
using lint = long long;
constexpr int INF = 1010101010;
constexpr lint LINF = 1LL << 60;

struct init {
    init() {
        cin.tie(nullptr);
        ios::sync_with_stdio(false);
        cout << fixed << setprecision(10);
    }
} init_;

int main() {

    lint A, B, K;
    cin >> A >> B >> K;
    string s = string(A, 'a') + string(B, 'b');

    int L = A + B;
    boost::multiprecision::cpp_int mpP = 1;
    for (int l = 1; l <= L; ++l) mpP *= l;
    for (int l = 1; l <= A; ++l) mpP /= l;
    for (int l = 1; l <= B; ++l) mpP /= l;

    string ans;
    lint sum = 0;
    while (A + B) {
        if (A == 0) {
            ans += string(B, 'b');
            break;
        }
        A--;
        int a = 2, b = 2;
        lint now = 1;
        for (int i = 2; i <= A + B; ++i) {
            now *= i;
            while (a <= A && now % a == 0) {
                now /= a;
                a++;
            }
            while (b <= B && now % b == 0) {
                now /= b;
                b++;
            }
        }
        if (now + sum >= K) ans += 'a';
        else {
            ans += 'b';
            A++, B--;
            K -= now;
        }
    }

    cout << ans << '\n';

    return 0;
}


/*
 * die_roll.cpp: Die rolling utility. Generates a random number of the following
 * format: NdS(+/-)M.
 *     - N: number of dice being rolled.
 *     - S: number of sides per die; any number is allowed, and % can be used
 *          as a stand-in for 100.
 *     - M: modifier to apply to the roll (positive or negative).
 * This program also allows for multiple rolls (same format for all), if desired.
 *
 * Version:     1.0.0
 * License:     MIT License (see LICENSE.txt for more details)
 * Author:      Joshua Morrison (GitHub: MrM21632)
 * Last Edited: 7/4/2018, 1:45pm
 */

#include <cstdio>
#include <cstdint>
#include <cstdlib>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

int main(int argc, char **argv) {
    if (argc != 5) {
        std::printf("Usage: die_roll total dice sides mod\n");
        std::printf("Generates random numbers in the range [dice + mod, (dice)(sides) + mod].\n\n");
        std::printf("total\t\tTotal rolls to make (> 0)\n");
        std::printf("dice\t\tNumber of dice to roll (> 0)\n");
        std::printf("sides\t\tSides per die (any positive number, or %%)\n");
        std::printf("mod\t\tModifier to die roll (positive or negative)\n");

        return EXIT_FAILURE;
    }

    // Setting up the variables
    int total = std::atoi(argv[1]);
    int dice = std::atoi(argv[2]);
    int mod = std::atoi(argv[4]);

    int min = dice + mod;
    if (min <= 0)
        min = 1;  // Guaranteed minimum result

    int sides;
    if (argv[3][0] == '%')  // Percentile
        sides = 100;
    else
        sides = std::atoi(argv[3]);

    int max = (dice * sides) + mod;
    if (max <= 0)
        max = 1;  // Guaranteed maximum(?) result

    // Roll the bones
    boost::random::random_device rd;
    boost::random::mt19937 mt(rd());
    boost::random::uniform_int_distribution<uint32_t> dist(min, max);

    for (int i = 1; i <= total; ++i)
        std::printf("Die Roll #%d: %u\n", i, dist(mt));
}


#include <iostream>
#include <functional>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <Eigen\dense>

#include "markov.h"
#include "TransitionMatrix.h"

// Hint: Set N - number of simulations low until you have it working
//       Then set it much much higher, and run in release mode so its faster

int main() {

	SetTransitionMatrix();

	// Print Results to File
	std::ofstream myfile;
	myfile.open("markov_results.txt");

	int start = 0;

	//simulate discrete time Markov Chain
	unsigned int N = 50;
	std::map<int, int> hist;
	std::vector<int> discreteMC;
	for (unsigned int i = 0; i < N; ++i) {

		//TODO (add DTMC, and histogram lines.)

		// Code if you wanted to print out results at each step
		//for (auto elem : discreteMC)
		//	std::cout << elem << std::endl;

	}
	//Returns an array discreteMC with the states at each step of the discrete-time Markov Chain
	//The number of transitions is given by steps. The initial state is given by start
	//(the states are indexed from 0 to n-1 where n is the number of arrays in transMatrix).
	//hist is the histogram

	// (double)p.second / N    - (decimal) percentage.
	for (auto p : hist) {
		std::cout << p.first << "\t" << (double)p.second / N << std::endl;
	}

	myfile.close();

	return 1;
}


/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#define SOFA_COMPONENT_ENGINE_INERTIAALIGN_CPP

#include "InertiaAlign.h"
#include <sofa/core/ObjectFactory.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
namespace sofa
{

namespace component
{

using namespace sofa::defaulttype;

int InertiaAlignClass = core::RegisterObject("An engine computing inertia matrix and the principal direction of a mesh.")
        .add< InertiaAlign >()
        ;

SOFA_DECL_CLASS(InertiaAlign)

InertiaAlign::InertiaAlign()
    : targetC( initData(&targetC,"targetCenter","input: the gravity center of the target mesh") )
    , sourceC( initData(&sourceC,"sourceCenter","input: the gravity center of the source mesh") )
    , targetInertiaMatrix( initData(&targetInertiaMatrix,"targetInertiaMatrix","input: the inertia matrix of the target mesh") )
    , sourceInertiaMatrix( initData(&sourceInertiaMatrix,"sourceInertiaMatrix","input: the inertia matrix of the source mesh") )
    , m_positiont( initData(&m_positiont,"targetPosition","input: positions of the target vertices") )
    , m_positions( initData(&m_positions,"sourcePosition","input: positions of the source vertices") )
{

}

InertiaAlign::~InertiaAlign()
{
}

void InertiaAlign::init()
{
    //Activate an output data
    m_positions.setPersistent(false);
    m_positiont.setPersistent(false);

    sourceInertiaMatrix.setPersistent(false);
    // Allow to edit an output data

    helper::WriteAccessor<Data<helper::vector<sofa::defaulttype::Vec<3,SReal> > > > waPositions = m_positions;

    Eigen::MatrixXd eigenSourceInertiaMatrix(3,3);
    Eigen::MatrixXd eigenTargetInertiaMatrix(3,3);

    SReal Sxx,Syy,Szz,Txx,Tyy,Tzz;
    for(unsigned int i=0; i<3; i++ )
    {
        for( unsigned int j=0; j<3; j++ )
        {
            eigenSourceInertiaMatrix(i,j) = sourceInertiaMatrix.getValue().col(j)[i];
            eigenTargetInertiaMatrix(i,j) = targetInertiaMatrix.getValue().col(j)[i];

        }
    }
    sout << "Source intertia matrix"<<sendl<<eigenSourceInertiaMatrix << sendl;
    sout << "Target intertia matrix"<<sendl<<eigenTargetInertiaMatrix << sendl;

    Eigen::EigenSolver<Eigen::Matrix3d> solverTarget(eigenTargetInertiaMatrix);
    Eigen::EigenSolver<Eigen::Matrix3d> solverSource(eigenSourceInertiaMatrix);

    /* Creation of the following transformation matrix:
     *
     *U.x V.x W.x tx
     *U.y V.y W.Y ty
     *U.Z V.Z W.z tz
     * 0   0   0   1
     *
     *With U,V,W The proper vectors of the Source inertia matrix (represent the principal axis of inertia)
     *And tx,ty,tz the translation beetween the source object and the world
     */

    Eigen::Matrix3d eigenvectorsTarget,tmp ;
    Eigen::Vector3d eigenvaluesTarget;
    Eigen::Matrix3cd complexEigenvectorsTarget = solverTarget.eigenvectors();
    Eigen::Vector3cd complexEigenvaluesTarget = solverTarget.eigenvalues();

    Eigen::Matrix3d eigenvectorsSource ;
    Eigen::Vector3d eigenvaluesSource ;
    Eigen::Matrix3cd complexEigenvectorsSource = solverSource.eigenvectors();
    Eigen::Vector3cd complexEigenvaluesSource = solverSource.eigenvalues();

    for(unsigned int i = 0; i<3; i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            eigenvectorsSource(i,j)= real(complexEigenvectorsSource(i,j));
            eigenvectorsTarget(i,j)= real(complexEigenvectorsTarget(i,j));
        }
        eigenvaluesTarget(i) = real(complexEigenvaluesTarget(i));
        eigenvaluesSource(i) = real(complexEigenvaluesSource(i));
    }

    SReal minSource = eigenvaluesSource(0);

    //Compute the length of the principal axes
    Sxx = eigenvaluesSource[0];
    Syy = eigenvaluesSource[1];
    Szz = eigenvaluesSource[2];
    Txx = eigenvaluesTarget[0];
    Tyy = eigenvaluesTarget[1];
    Tzz = eigenvaluesTarget[2];

    SReal Lxs = sqrt(abs(-Sxx+Syy+Szz)/2);
    SReal Lys = sqrt(abs( Sxx-Syy+Szz)/2);
    SReal Lzs = sqrt(abs( Sxx+Syy-Szz)/2);

    SReal Lxt = sqrt(abs(-Txx+Tyy+Tzz)/2);
    SReal Lyt = sqrt(abs( Txx-Tyy+Tzz)/2);
    SReal Lzt = sqrt(abs( Txx+Tyy-Tzz)/2);

    //Source inversion of axes (to be sure that u is on u', v on v' and w on w')
    //Check thats the proper values are in the same order for the two objects
    //If not switch the proper values.
    //Inversion of u and v
    if (eigenvaluesSource[0] < eigenvaluesSource[1])
    {
        eigenvaluesSource(0)= eigenvaluesSource(1);
        eigenvaluesSource(1)=minSource;
        minSource = eigenvaluesSource(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,1);
            eigenvectorsSource(i,1) =   eigenvectorsSource(i,0);
            eigenvectorsSource(i,0) = tmp(i);
        }

    }
    //Inversion of u and w
    if (eigenvaluesSource[0]<eigenvaluesSource[2])
    {
        eigenvaluesSource(0)= eigenvaluesSource(2);
        eigenvaluesSource(2)=minSource;
        minSource = eigenvaluesSource(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,2);
            eigenvectorsSource(i,2) =   eigenvectorsSource(i,0);
            eigenvectorsSource(i,0) = tmp(i);
        }
    }
    //Inversion of v and w
    minSource = eigenvaluesSource(1);

    if (eigenvaluesSource[1]<eigenvaluesSource[2])
    {
        eigenvaluesSource(1)= eigenvaluesSource(2);
        eigenvaluesSource(2)=minSource;
        minSource = eigenvaluesSource(1);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsSource(i,2);
            eigenvectorsSource(i,2) =   eigenvectorsSource(i,1);
            eigenvectorsSource(i,1) = tmp(i);
        }
    }

    SReal minTarget = eigenvaluesTarget(0);

    //Target Inversion of axes
    //Inversion of u' and v'
    if (eigenvaluesTarget[0]<eigenvaluesTarget[1])
    {
        eigenvaluesTarget(0)= eigenvaluesTarget(1);
        eigenvaluesTarget(1)=minTarget;
        minTarget = eigenvaluesTarget(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,1);
            eigenvectorsTarget(i,1) =   eigenvectorsTarget(i,0);
            eigenvectorsTarget(i,0) = tmp(i);
        }
    }
    //Inversion of u' and w'
    if (eigenvaluesTarget[0]<eigenvaluesTarget[2])
    {
        eigenvaluesTarget(0)= eigenvaluesTarget(2);
        eigenvaluesTarget(2)=minTarget;
        minTarget = eigenvaluesTarget(0);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,2);
            eigenvectorsTarget(i,2) =   eigenvectorsTarget(i,0);
            eigenvectorsTarget(i,0) = tmp(i);
        }
    }
    //Inversion of v' and w'
    minTarget = eigenvaluesTarget(1);

    if (eigenvaluesTarget[1]<eigenvaluesTarget[2])
    {
        eigenvaluesTarget(1)= eigenvaluesTarget(2);
        eigenvaluesTarget(2)=minTarget;
        minTarget = eigenvaluesTarget(1);
        for(unsigned int i = 0; i<3; i++){
            tmp(i) = eigenvectorsTarget(i,2);
            eigenvectorsTarget(i,2) =   eigenvectorsTarget(i,1);
            eigenvectorsTarget(i,1) = tmp(i);
        }
    }
    //Compute the length of the axes.
    //The scale along x is Lxs/Lxt
    //The ratio 1/5m have been supressed due to the ratio beetween the source/target

    Sxx = eigenvaluesSource[0];
    Syy = eigenvaluesSource[1];
    Szz = eigenvaluesSource[2];
    Txx = eigenvaluesTarget[0];
    Tyy = eigenvaluesTarget[1];
    Tzz = eigenvaluesTarget[2];

    Lxs = sqrt(abs(-Sxx+Syy+Szz)/2);
    Lys = sqrt(abs( Sxx-Syy+Szz)/2);
    Lzs = sqrt(abs( Sxx+Syy-Szz)/2);

    Lxt = sqrt(abs(-Txx+Tyy+Tzz)/2);
    Lyt = sqrt(abs( Txx-Tyy+Tzz)/2);
    Lzt = sqrt(abs( Txx+Tyy-Tzz)/2);

    sout << "EigenVectorsSource =" << sendl;
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
            sout << eigenvectorsSource(i,j) << " ";
        sout << sendl;

    }

    SReal scale_u = Lxs / Lxt;
    SReal scale_v = Lys / Lyt;
    SReal scale_w = Lzs / Lzt;

    sout << "Scale X = " << scale_u << "Scale Y = "<< scale_v << "Scale Z = " << scale_w << sendl;

    sout << "Lxs = " <<Lxs<< "Lys = " <<Lys<< "Lzs = " <<Lzs << sendl;
    sout << "Lxt = " <<Lxt<< "Lyt = " <<Lyt<< "Lzt = " <<Lzt << sendl;
    /*Creation of the two 4x4 transformation matrix:
     *
     *MTransformSource :
     *
     *U.x V.x W.x tx
     *U.y V.y W.Y ty
     *U.z V.z W.z tz
     * 0   0   0   1
     *
     *
     *Then inverse R = MtransformSource [0..2][0..2]
     *t = -R*t
     *
     * with U,V and W the eigenvectors of the source inertia matrix in the world coordinates.
     *And tx,ty,tz represents the translation beetween the center of the world and the inertia center of the object S
     *
     *
     *MTransformTarget :
     *
     *U'.x V'.x W'.x tx
     *U'.y V'.y W'.Y ty
     *U'.z V'.z W'.z tz
     *  0    0    0   1
     *

     *
     * with U',V' and W' the eigenvectors of the target inertia matrix in the world coordinates.
     *And tx,ty,tz represents the translation beetween the center of the world and the inertia center of the object T
     *
     *
     */

    Eigen::Matrix3d MRotationSource, MRotationTarget;
    defaulttype::Matrix4 MTransformSource,MTransformTarget,MTransformTargetTest,MTransform,Mscale;
    MRotationSource =  eigenvectorsSource;
    MRotationTarget =  eigenvectorsTarget;
    Eigen::Vector3d u,v,w;
    SReal sdirect;
    Mscale(0,0)=scale_u;
    Mscale(1,1)=scale_v;
    Mscale(2,2)=scale_w;
    Mscale(3,3)=1;
    for(unsigned int i=0;i<3;i++)
    {
        u(i) = MRotationSource(i,0);
        v(i) = MRotationSource(i,1);
        w(i) = MRotationSource(i,2);

    }
    sdirect = u.cross(v).dot(w);

    for(unsigned int i=0;i<3;i++)
    {
        if(sdirect < 0)
        {
            MRotationSource(i,2) = -MRotationSource(i,2);
        }
    }

    sout <<"Source Directe? "<< sdirect << sendl;

    for(unsigned int i=0;i<3;i++)
    {
        u(i) = MRotationTarget(i,0);
        v(i) = MRotationTarget(i,1);
        w(i) = MRotationTarget(i,2);

    }

    //Checks if the axes are direct or not
    sdirect = u.cross(v).dot(w);

    for(unsigned int i=0;i<3;i++)
    {
        if(sdirect == -1)
        {
            MRotationTarget(i,2) = -MRotationTarget(i,2);
        }
    }

    sout <<"Target Directe? "<< sdirect << sendl;

    //Normalised last line
    MTransformTarget(3,3) = 1;

    for(unsigned int i=0;i<3;i++)
    {
        //TODO : vérifier ces histoires de Lxs/Lxt

        MTransformTarget(i,0) = MRotationTarget(i,0)*Lxt;
        MTransformTarget(i,1) = MRotationTarget(i,1)*Lyt;
        MTransformTarget(i,2) = MRotationTarget(i,2)*Lzt;
        MTransformTarget(3,i) = 0;

    }
    Eigen::Vector3d TargetTransform;
    for(unsigned int i=0;i<3;i++)
    {
        TargetTransform(i)= -(*targetC.beginEdit())[0];
    }
    MTransformTarget(0,3) = -(*targetC.beginEdit())[0];
    MTransformTarget(1,3) = -(*targetC.beginEdit())[1];
    MTransformTarget(2,3) = -(*targetC.beginEdit())[2];

    //Construction of S
    MTransformSource(3,3) = 1;
    Vector4 TranslationSource;

    MTransformSource(0,3) = -(*sourceC.beginEdit())[0];
    MTransformSource(1,3) = -(*sourceC.beginEdit())[1];
    MTransformSource(2,3) = -(*sourceC.beginEdit())[2];

    for(unsigned int i=0;i<3;i++)
    {
        //TODO : vérifier ces histoires de Lxs/Lxt

        MTransformSource(i,0) = MRotationSource(i,0) *Lxs;
        MTransformSource(i,1) = MRotationSource(i,1) *Lys;
        MTransformSource(i,2) = MRotationSource(i,2) *Lzs;
        MTransformSource(3,i) = 0;
        TranslationSource(i)  = MTransformSource(i,3);

    }

    sout << "MTransformSource before inversion ="<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout <<MTransformSource(i,j)<< " ";
        }
        sout << sendl;
    }

    MTransformSource = inverseTransform(MTransformSource);
    sout << "MTransformSource after inversion ="<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout <<MTransformSource(i,j)<< " ";
        }
        sout << sendl;
    }
    defaulttype::Matrix4 MTranslation;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            MTranslation(i,j)=0;
            if(i==j)
                MTranslation(i,i) = 1;
        }
    }

    int indice_min =0;
    SReal distance, distance_min;

    distance = std::numeric_limits<SReal>::max();
    distance_min = std::numeric_limits<SReal>::max();

    for (unsigned int i=0;i<4;i++)
        for (unsigned int j=0;j<4;j++)
            MTransformTargetTest(i,j) = MTransformTarget(i,j);

    for(unsigned int i=0;i<4;i++)//Test of the 4 possible permutations
    {
        switch (i)
        {
        case 0 :
            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) = MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) = MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) = MTransformTarget(j,2) ;
            }
            break;
        case 1 :

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) = MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) =-MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) =-MTransformTarget(j,2) ;
            }
            break;
        case 2 :

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) =-MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) = MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) =-MTransformTarget(j,2) ;
            }
            break;
        case 3:

            for(unsigned int j=0;j<3;j++)
            {
                MTransformTargetTest(j,0) =-MTransformTarget(j,0) ;
                MTransformTargetTest(j,1) =-MTransformTarget(j,1) ;
                MTransformTargetTest(j,2) = MTransformTarget(j,2) ;

            }
            break;

        }
        MTransform = MTransformTargetTest * MTransformSource ;

        positionDistSource = (*m_positions.beginEdit());
        for (size_t j = 0; j < waPositions.size(); j++)
        {
            defaulttype::Vector4 pointS,pointT;
            pointS(0) = (*m_positions.beginEdit())[j][0];
            pointS(1) = (*m_positions.beginEdit())[j][1];
            pointS(2) = (*m_positions.beginEdit())[j][2];
            pointS(3) = 1;
            pointT = MTransform * pointS  ;
            (*m_positions.beginEdit())[j][0] =pointT(0);
            (*m_positions.beginEdit())[j][1] =pointT(1);
            (*m_positions.beginEdit())[j][2] =pointT(2);
        }

        distance =computeDistances(*m_positions.beginEdit(),*m_positiont.beginEdit());
        if (distance < distance_min)
        {
            indice_min = i;
            distance_min = distance;
        }
        (*m_positions.beginEdit()) = positionDistSource;
    }
    sout << "Indice choisi : " << indice_min << " avec une distance de " << distance_min << sendl;
    //Compute the best transformation
    switch(indice_min)
    {
    case 0 ://Nothing is inverted
        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) = MTransformTarget(j,0) ;
            MTransformTarget(j,1) = MTransformTarget(j,1) ;
            MTransformTarget(j,2) = MTransformTarget(j,2) ;
        }
        break;
    case 1 ://v and w are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) = MTransformTarget(j,0) ;
            MTransformTarget(j,1) =-MTransformTarget(j,1) ;
            MTransformTarget(j,2) =-MTransformTarget(j,2) ;
        }
        break;
    case 2 ://u and w are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) =-MTransformTarget(j,0) ;
            MTransformTarget(j,1) = MTransformTarget(j,1) ;
            MTransformTarget(j,2) =-MTransformTarget(j,2) ;
        }
        break;
    case 3:// u and v are inverted

        for(unsigned int j=0;j<3;j++)
        {
            MTransformTarget(j,0) =-MTransformTarget(j,0) ;
            MTransformTarget(j,1) =-MTransformTarget(j,1) ;
            MTransformTarget(j,2) = MTransformTarget(j,2) ;
        }
        break;
    }

    for(unsigned int k=0;k<4;k++)
    {
       for(unsigned int j=0;j<4;j++)
       {
         sout << MTransformTarget(k,j) << " ";
       }
       sout << sendl;
    }
    MTransformSource = MTransformTarget * MTransformSource;

    for(unsigned int k=0;k<4;k++)
    {
       for(unsigned int j=0;j<4;j++)
       {
         sout << MTransformSource(k,j) << " ";
       }
       sout << sendl;
    }

    Eigen::Matrix4d MDeterminantTest;
    for(unsigned int k=0;k<3;k++)
    {
       for(unsigned int j=0;j<3;j++)
       {
           MDeterminantTest(k,j)=MTransformSource(k,j);
       }
    }
    if(MDeterminantTest.determinant()<0)
        sout << "The MTransformSourceMatrix is not a Transformation matrix" << sendl;
    for (size_t i = 0; i < waPositions.size(); i++)
    {
        defaulttype::Vector4 pointS,pointT;
        pointS(0) = (*m_positions.beginEdit())[i][0];
        pointS(1) = (*m_positions.beginEdit())[i][1];
        pointS(2) = (*m_positions.beginEdit())[i][2];
        pointS(3) = 1;
        pointT = MTransformSource * pointS  ;
        (*m_positions.beginEdit())[i][0] =pointT(0);
        (*m_positions.beginEdit())[i][1] =pointT(1);
        (*m_positions.beginEdit())[i][2] =pointT(2);
    }
    //After this step, this data cannot be modified
    m_positions.endEdit();

    m_positiont.endEdit();

}

//Hausdorff distance
//good approx but slow

/**
 * Compute the distance from a point to a point cloud
 */

SReal InertiaAlign::distance(sofa::defaulttype::Vec<3,SReal> p, helper::vector<sofa::defaulttype::Vec<3,SReal> > S)
{
    SReal min = std::numeric_limits<SReal>::max();

    for (unsigned int i = 0 ; i < S.size(); i++)
    {
        SReal d = (p-S[i]).norm();
        if (d<min) min = d;
    }

    return min;
}

/**
 * Compute distances between both point clouds (symmetrical and non-symmetrical distances)
 */
SReal InertiaAlign::computeDistances( helper::vector<sofa::defaulttype::Vec<3,SReal> > S, helper::vector<sofa::defaulttype::Vec<3,SReal> > T)
{
    SReal maxST = 0.0;
    for (unsigned int i = 0 ; i < S.size(); i++)
    {
        SReal d = InertiaAlign::distance(S[i], T);
        if (d>maxST) maxST = d;
    }

    SReal maxTS = 0.0;
    for (unsigned int i = 0 ; i < T.size(); i++)
    {
        SReal d = InertiaAlign::distance(T[i], S);
        if (d>maxTS) maxTS = d;
    }

    if (maxTS > maxST)
        return maxST/S.size();
    else
        return maxTS/S.size();

}

Matrix4 InertiaAlign::inverseTransform(Matrix4 transformToInvert)
{
    Matrix3 rotationToInvert;
    Matrix3 rotationInverted;
    Vector3 Translation;
    Matrix4 transformInverted;

    sout << "Before inversion"<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout << transformToInvert(i,j)<< " ";
        }
        sout << sendl;
    }
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            rotationToInvert(i,j) = transformToInvert(i,j);
        }
        Translation(i)=transformToInvert(i,3);
    }

    bool bS = invertMatrix(rotationInverted,rotationToInvert);

    sout << "Translation = " << Translation;
    if(!bS)
    {
        sout <<"Error : Source transformation matrix is not invertible"<<sendl;
    }
    else //Compute R-1 * t
    {
        Translation = (-1*rotationInverted * Translation);
    }
    sout << "Translation = " << Translation;
    for(unsigned int i=0;i<3;i++)
    {
        for(unsigned int j=0;j<3;j++)
        {
            transformInverted(i,j) = rotationInverted(i,j);
        }
        transformInverted(i,3)= Translation(i);
    }
    transformInverted(3,3)=1;

    sout << "After inversion"<< sendl;
    for(unsigned int i=0;i<4;i++)
    {
        for(unsigned int j=0;j<4;j++)
        {
            sout << transformInverted(i,j)<< " ";
        }
        sout << sendl;
    }
    return transformInverted;

}

SReal InertiaAlign::abs(SReal a)
{
    return sqrt(a*a);
}

} // namespace component

} // namespace sofa


#include "sbs/physics/xpbd/green_constraint.h"

#include <Eigen/LU>
#include <Eigen/SVD>
#include <sbs/physics/simulation.h>

namespace sbs {
namespace physics {
namespace xpbd {

green_constraint_t::green_constraint_t(
    scalar_type const alpha,
    scalar_type const beta,
    simulation_t const& simulation,
    index_type bi,
    index_type v1,
    index_type v2,
    index_type v3,
    index_type v4,
    scalar_type young_modulus,
    scalar_type poisson_ratio)
    : constraint_t(alpha, beta),
      bi_(bi),
      v1_(v1),
      v2_(v2),
      v3_(v3),
      v4_(v4),
      DmInv_(),
      V0_(),
      mu_(),
      lambda_()
{
    auto const& p1 = simulation.particles()[bi_][v1_];
    auto const& p2 = simulation.particles()[bi_][v2_];
    auto const& p3 = simulation.particles()[bi_][v3_];
    auto const& p4 = simulation.particles()[bi_][v4_];

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1.x0() - p4.x0()).transpose();
    Dm.col(1) = (p2.x0() - p4.x0()).transpose();
    Dm.col(2) = (p3.x0() - p4.x0()).transpose();

    DmInv_  = Dm.inverse();
    V0_     = (1. / 6.) * Dm.determinant();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

void green_constraint_t::project_positions(simulation_t& simulation, scalar_type dt)
{
    auto& p1 = simulation.particles()[bi_][v1_];
    auto& p2 = simulation.particles()[bi_][v2_];
    auto& p3 = simulation.particles()[bi_][v3_];
    auto& p4 = simulation.particles()[bi_][v4_];

    scalar_type const w1 = p1.invmass();
    scalar_type const w2 = p2.invmass();
    scalar_type const w3 = p3.invmass();
    scalar_type const w4 = p4.invmass();

    auto const Vsigned        = signed_volume(p1.xi(), p2.xi(), p3.xi(), p4.xi());
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);

    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1.xi() - p4.xi());
    Ds.col(1) = (p2.xi() - p4.xi());
    Ds.col(2) = (p3.xi() - p4.xi());

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    Eigen::Matrix3d const Piola = U * Piolahat * V.transpose();

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const dt2         = dt * dt;
    scalar_type const alpha_tilde = alpha() / dt2;
    scalar_type const beta_tilde  = beta() * dt2;
    scalar_type const gamma       = alpha_tilde * beta_tilde / dt;

    // clang-format off
    scalar_type const gradC_dot_displacement =
        f1.dot(p1.xi() - p1.xn()) +
        f2.dot(p2.xi() - p2.xn()) +
        f3.dot(p3.xi() - p3.xn()) +
        f4.dot(p4.xi() - p4.xn());
    // clang-format on

    scalar_type const delta_lagrange_num =
        -(C + alpha_tilde * lagrange_) + gamma * gradC_dot_displacement;
    scalar_type const delta_lagrange_den = (1. + gamma) * (weighted_sum_of_gradients) + alpha_tilde;
    scalar_type const delta_lagrange     = delta_lagrange_num / delta_lagrange_den;

    lagrange_ += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p1.xi() += w1 * -f1 * delta_lagrange;
    p2.xi() += w2 * -f2 * delta_lagrange;
    p3.xi() += w3 * -f3 * delta_lagrange;
    p4.xi() += w4 * -f4 * delta_lagrange;
}

scalar_type green_constraint_t::signed_volume(
    Eigen::Vector3d const& p1,
    Eigen::Vector3d const& p2,
    Eigen::Vector3d const& p3,
    Eigen::Vector3d const& p4) const
{
    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4);
    Dm.col(1) = (p2 - p4);
    Dm.col(2) = (p3 - p4);

    scalar_type const V = (1. / 6.) * Dm.determinant();
    return V;
}

} // namespace xpbd
} // namespace physics
} // namespace sbs


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef META_MATH_SQRT_INCLUDE
#define META_MATH_SQRT_INCLUDE

#include <boost/numeric/meta_math/abs.hpp>

namespace meta_math {

template <long int root, long int x>
struct sqrt_check
{
    static bool const value = root * root <= x && (root+1) * (root+1) > x;
};

namespace impl {

    template <long int guess, long int x, bool Converged>
    struct sqrt_impl
    {
	typedef long int type;
	typedef sqrt_impl   self;
	static long int const quotient = x / guess,
	                      new_value = (quotient + guess) / 2;
	static bool const converging = abs<guess - quotient>::value < 2;
	static long int const value = sqrt_impl<new_value, x, converging>::value;
    };

    // If the condition becomes true the guessed root will be the returned value
    template <long int guess, long int x>
    struct sqrt_impl<guess, x, true>
    {
	static long int const value = guess;
    };

}

template <long int x>
struct sqrt
{
  typedef long int type;
  static long int const value = impl::sqrt_impl<1, x, false>::value;
};

} // namespace meta_math

#endif // META_MATH_SQRT_INCLUDE