/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3Q.cpp
 * @brief   Rotation (internal: quaternion representation*)
 * @author  Richard Roberts
 */

#ifdef GTSAM_DEFAULT_QUATERNIONS

#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>

using namespace std;

namespace gtsam {

	static const Matrix I3 = eye(3);

  /* ************************************************************************* */
	Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}

  /* ************************************************************************* */
	Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
      quaternion_((Eigen::Matrix3d() <<
          r1.x(), r2.x(), r3.x(),
          r1.y(), r2.y(), r3.y(),
          r1.z(), r2.z(), r3.z()).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(double R11, double R12, double R13,
      double R21, double R22, double R23,
      double R31, double R32, double R33) :
        quaternion_((Eigen::Matrix3d() <<
            R11, R12, R13,
            R21, R22, R23,
            R31, R32, R33).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Matrix& R) :
      quaternion_(Eigen::Matrix3d(R)) {}

//  /* ************************************************************************* */
//   Rot3::Rot3(const Matrix3& R) :
//       quaternion_(R) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}

  /* ************************************************************************* */
  Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

  /* ************************************************************************* */
  Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }

  /* ************************************************************************* */
  Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }

  /* ************************************************************************* */
  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
      Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
      Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
      Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
  }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w, double theta) {
    return Quaternion(Eigen::AngleAxisd(theta, w)); }

  /* ************************************************************************* */
  Rot3 Rot3::rodriguez(const Vector& w) {
    double t = w.norm();
    if (t < 1e-10) return Rot3();
    return rodriguez(w/t, t);
  }

  /* ************************************************************************* */
  bool Rot3::equals(const Rot3 & R, double tol) const {
    return equal_with_abs_tol(matrix(), R.matrix(), tol);
  }

  /* ************************************************************************* */
  Rot3 Rot3::compose(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = R2.transpose();
    if (H2) *H2 = I3;
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Point3 Rot3::operator*(const Point3& p) const {
    Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
    return Point3(r(0), r(1), r(2));
  }

  /* ************************************************************************* */
  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
    if (H1) *H1 = -matrix();
    return Rot3(quaternion_.inverse());
  }

  /* ************************************************************************* */
  Rot3 Rot3::between(const Rot3& R2,
  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    if (H1) *H1 = -(R2.transpose()*matrix());
    if (H2) *H2 = I3;
    return between_default(*this, R2);
  }

  /* ************************************************************************* */
  Rot3 Rot3::operator*(const Rot3& R2) const {
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  Point3 Rot3::rotate(const Point3& p,
        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
    Matrix R = matrix();
    if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    if (H2) *H2 = R;
    Eigen::Vector3d r = R * p.vector();
    return Point3(r.x(), r.y(), r.z());
  }

  /* ************************************************************************* */
  // see doc/math.lyx, SO(3) section
  Point3 Rot3::unrotate(const Point3& p,
      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
    const Matrix Rt(transpose());
    Point3 q(Rt*p.vector()); // q = Rt*p
    if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
    if (H2) *H2 = Rt;
    return q;
  }

  /* ************************************************************************* */
  // Log map at identity - return the canonical coordinates of this rotation
  Vector3 Rot3::Logmap(const Rot3& R) {
    Eigen::AngleAxisd angleAxis(R.quaternion_);
    if(angleAxis.angle() > M_PI)      // Important:  use the smallest possible
      angleAxis.angle() -= 2.0*M_PI;  // angle, e.g. no more than PI, to keep
    if(angleAxis.angle() < -M_PI)     // error continuous.
      angleAxis.angle() += 2.0*M_PI;
    return angleAxis.axis() * angleAxis.angle();
  }

  /* ************************************************************************* */
	Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
		return compose(Expmap(omega));
	}

	/* ************************************************************************* */
	Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
		return Logmap(between(t2));
	}

  /* ************************************************************************* */
  Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); }

  /* ************************************************************************* */
  Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }

  /* ************************************************************************* */
  Point3 Rot3::column(int index) const{
    if(index == 3)
      return r3();
    else if(index == 2)
      return r2();
    else if(index == 1)
      return r1(); // default returns r1
    else
      throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
  }

  /* ************************************************************************* */
  Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

  /* ************************************************************************* */
  Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }

  /* ************************************************************************* */
  Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }

  /* ************************************************************************* */
  Vector3 Rot3::xyz() const {
    Matrix I;Vector3 q;
    boost::tie(I,q)=RQ(matrix());
    return q;
  }

  /* ************************************************************************* */
  Vector3 Rot3::ypr() const {
  	Vector3 q = xyz();
    return Vector3(q(2),q(1),q(0));
  }

  /* ************************************************************************* */
  Vector3 Rot3::rpy() const {
  	Vector3 q = xyz();
    return Vector3(q(0),q(1),q(2));
  }

  /* ************************************************************************* */
  Quaternion Rot3::toQuaternion() const { return quaternion_; }

  /* ************************************************************************* */
  pair<Matrix3, Vector3> RQ(const Matrix3& A) {

    double x = -atan2(-A(2, 1), A(2, 2));
    Rot3 Qx = Rot3::Rx(-x);
    Matrix3 B = A * Qx.matrix();

    double y = -atan2(B(2, 0), B(2, 2));
    Rot3 Qy = Rot3::Ry(-y);
    Matrix3 C = B * Qy.matrix();

    double z = -atan2(-C(1, 0), C(1, 1));
    Rot3 Qz = Rot3::Rz(-z);
    Matrix3 R = C * Qz.matrix();

    Vector xyz = Vector3(x, y, z);
    return make_pair(R, xyz);
  }

} // namespace gtsam

#endif


#ifndef STAN_MATH_PRIM_MAT_FUN_MEAN_HPP
#define STAN_MATH_PRIM_MAT_FUN_MEAN_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
  namespace math {

    /**
     * Returns the sample mean (i.e., average) of the coefficients
     * in the specified standard vector.
     * @param v Specified vector.
     * @return Sample mean of vector coefficients.
     * @throws std::domain_error if the size of the vector is less
     * than 1.
     */
    template <typename T>
    inline
    typename boost::math::tools::promote_args<T>::type
    mean(const std::vector<T>& v) {
      check_nonzero_size("mean", "v", v);
      T sum(v[0]);
      for (size_t i = 1; i < v.size(); ++i)
        sum += v[i];
      return sum / v.size();
    }

    /**
     * Returns the sample mean (i.e., average) of the coefficients
     * in the specified vector, row vector, or matrix.
     * @param m Specified vector, row vector, or matrix.
     * @return Sample mean of vector coefficients.
     */
    template <typename T, int R, int C>
    inline
    typename boost::math::tools::promote_args<T>::type
    mean(const Eigen::Matrix<T, R, C>& m) {
      check_nonzero_size("mean", "m", m);
      return m.mean();
    }

  }
}
#endif


#ifdef PLOT_FIGURES

#include <math.h>
#include <armadillo>
#include "contrib/matplotlibcpp/matplotlibcpp.h"

namespace plt = matplotlibcpp;
using namespace arma;
using namespace std;
using stdvec = std::vector<double>;
using stdnestedvec = std::vector<std::vector<double>>;

stdvec arma_vec_to_std_vector(arma::mat x)
{
    return arma::conv_to<stdvec>::from(x);
}

stdnestedvec arma_mat_to_std_vec(arma::mat &A)
{
    stdnestedvec V(A.n_rows);
    for (size_t i = 0; i < A.n_rows; ++i)
    {
        V[i] = arma::conv_to<stdvec>::from(A.row(i));
    };
    return V;
};

void plot_arma_vec(arma::mat x, long figure = 1, string title = "")
{
    plt::figure(figure);
    plt::plot(arma_vec_to_std_vector(x));
    plt::title(title);
}

void plot_arma_mat(arma::mat x, long figure = 1, string title = "")
{
    plt::figure(figure);
    for (auto &&row : arma_mat_to_std_vec(x))
    {
        plt::plot(row);
    }
    plt::title(title);
}

#endif


#include <string>
#include <vector>
#include <random>
#include <fmt/core.h>
#include <cxxopts.hpp>
#include <boost/dynamic_bitset.hpp>
#include <zipfian_int_distribution.h>
#include <ProgressBar.hpp>
#include "io.hpp"

boost::dynamic_bitset<> generateBitset(unsigned int universe,
                                       unsigned int iterations,
                                       std::string& distribution,
                                       double param,
                                       zipfian_int_distribution<unsigned int>::param_type p);
std::vector<unsigned int> bitsetToVector(boost::dynamic_bitset<>& bitset);

// taken from zipfian_int_distribution.h (it's private)
double zeta(unsigned long __n, double __theta);

int main(int argc, char** argv)
{
    try {
        std::string outputPath;
        std::string distribution = "uniform";
        unsigned int iterations = 10000000;
        double bernoulliP = 0.5; // bernoulli probability
        double stddev = 2.0; // standard deviation
        unsigned int k = 100; // dataset cardinality
        double skew = 0.9;

        cxxopts::Options options(argv[0], "Generate dataset");

        options.add_options()
                ("distribution", "Set element distribution {uniform|normal|bernoulli|zipf} (default: uniform)", cxxopts::value<std::string>(distribution))
                ("output", "Output prefix name for the generated dataset", cxxopts::value<std::string>(outputPath))
                ("prob", "Bernoulli probability", cxxopts::value<double>(bernoulliP))
                ("stdev", "Standard Deviation for normal distribution (default: 2)", cxxopts::value<double>(stddev))
                ("skew", "Theta (i.e. skew factor for zipf distribution)", cxxopts::value<double>(skew))
                ("iterations", "Number of iterations (i.e. approximate set size, default: 10000000)", cxxopts::value<unsigned int>(iterations))
                ("universe", "Universe cardinality", cxxopts::value<unsigned long>())
                ("k", "Dataset size (i.e. number of sets)", cxxopts::value<unsigned int>(k))
                ("help", "Print help");

        auto result = options.parse(argc, argv);

        if (result.count("help")) {
            fmt::print("{}\n", options.help());
            return 0;
        }

        if (!result.count("universe")) {
            fmt::print("{}\n", "No universe given! Exiting...");
            return 1;
        }

        unsigned long universe = result["universe"].as<unsigned long>();

        if (!result.count("output")) {
            outputPath = "out";
        }

        // construct output name
        std::string tmp = distribution + "_" + std::to_string(k) + "_" + std::to_string(universe);

        double param = 0;

        if (distribution == "uniform") {
            tmp += "_" + std::to_string(iterations);
        } else if (distribution == "normal") {
            tmp += "_" + std::to_string(iterations) + "_" + std::to_string(stddev);
            param = stddev;
        } else if (distribution == "bernoulli") {
            tmp += "_" + std::to_string(bernoulliP);
            param = bernoulliP;
        } else { // zipf
            tmp += "_" + std::to_string(iterations) + "_" + std::to_string(skew);
            param = skew;
        }

        tmp.append(".bin");

        // construct dataset
        std::vector<std::vector<unsigned int>> dataset;
        dataset.reserve(k);

        progresscpp::ProgressBar progressBar(k, 70, '#', '-');

        unsigned long totalElements = 0;

        zipfian_int_distribution<unsigned int>::param_type p;

        if (distribution == "zipf") {
            // calculate zeta just once
            p = zipfian_int_distribution<unsigned int>::param_type(1, universe, param, zeta(universe, param));
        }

        for (unsigned int i = 0; i < k; ++i) {
            boost::dynamic_bitset<> bitset = generateBitset(universe, iterations, distribution, param, p);
            std::vector<unsigned int> set = bitsetToVector(bitset);
            dataset.push_back(set);
            totalElements += set.size();
            ++progressBar;
            progressBar.display();
        }

        progressBar.done();

        fmt::print("Sorting sets in ascending order\n");

        std::sort(dataset.begin(), dataset.end(), [](const std::vector<unsigned int>& a, const std::vector<unsigned int>& b) {
            return a.size() < b.size();
        });

        std::string outname = outputPath + "_asc_" + tmp;
        fmt::print("Writing dataset to {}\n", outname);
        writeDataset(k, universe, totalElements, dataset, outname);

        fmt::print("Sorting sets in descending order\n");

        std::sort(dataset.begin(), dataset.end(), [](const std::vector<unsigned int>& a, const std::vector<unsigned int>& b) {
            return a.size() > b.size();
        });

        outname = outputPath + "_desc_" + tmp;
        fmt::print("Writing dataset to {}\n", outname);
        writeDataset(k, universe, totalElements, dataset, outname);

        fmt::print("Finished!\n");

    } catch (const cxxopts::OptionException& e) {
        fmt::print("{}\n", e.what());
        return 1;
    }
    return 0;
}

boost::dynamic_bitset<> generateBitset(unsigned int universe,
                                       unsigned int iterations,
                                       std::string& distribution,
                                       double param,
                                       zipfian_int_distribution<unsigned int>::param_type p)
{
    boost::dynamic_bitset<> bitset(universe);

    std::mt19937_64 gen(19937);

    if (distribution == "uniform") {
        std::uniform_int_distribution<> ud(1, universe);

        for (unsigned int i = 0; i < iterations; ++i) {
            bitset.set(std::round(ud(gen)));
        }
    } else if (distribution == "normal") {
        std::normal_distribution<> nd(universe / 2, param);

        for (unsigned int i = 0; i < iterations; ++i) {
            bitset.set(std::round(nd(gen)));
        }
    } else if (distribution == "bernoulli") {
        std::bernoulli_distribution bd(param);
        for (unsigned int i = 0; i < universe; ++i) {
            if (bd(gen)) {
                bitset.set(i);
            }
        }
    } else { // zipf distribution
        zipfian_int_distribution<unsigned int> zipf(p);

        for (unsigned int i = 0; i < iterations; ++i) {
            bitset.set(zipf(gen));
        }
    }

    return bitset;
}

double zeta(unsigned long __n, double __theta)
{
    double ans = 0.0;
    for(unsigned long i = 1; i <= __n; ++i)
        ans += std::pow(1.0 / i, __theta);
    return ans;
}

std::vector<unsigned int> bitsetToVector(boost::dynamic_bitset<>& bitset)
{
    std::vector<unsigned int> set;
    set.reserve(bitset.count());
    for (unsigned int i = 0; i < bitset.size(); ++i) {
        if (bitset[i]) {
            set.push_back(i + 1);
        }
    }
    return set;
}


#include "CameraCalibration.h"
#include "CameraProjection.h"
#include "vtkEigenTools.h"

#include <Eigen/Dense>

//----------------------------------------------------------------------------
int TestFisheyeModelCalibration(std::string matchedFilename, std::string groundtruthFilename)
{
  // relatively high epsilon since the data are stored
  // in a .csv file with low digit
  double epsilon = 1e-2;

  // Load the 3D - 2D matches
  std::vector<Eigen::Vector3d> X;
  std::vector<Eigen::Vector2d> x;
  LoadMatchesFromCSV(matchedFilename, X, x);
  if (X.size() == 0)
  {
    return 0;
  }

  // Estimate the fisheye camera model parameters
  Eigen::Matrix<double, 3, 4> P;
  LinearPinholeCalibration(X, x, P);
  Eigen::Matrix3d R, K;
  Eigen::Vector3d T;
  CalibrationMatrixDecomposition(P, K, R, T);
  Eigen::Matrix<double, 11, 1> W;
  GetParametersFromMatrix(K, R, T, W);
  NonLinearPinholeCalibration(X, x, W);
  Eigen::Matrix<double, 15, 1> Wf = Eigen::Matrix<double, 15, 1>::Zero();
  for (int i = 0; i < 11; ++i)
  {
    Wf(i) = W(i);
  }
  NonLinearFisheyeCalibration(X, x, Wf);

  // Load the expected parameters and test
  Eigen::VectorXd Wg;
  LoadCameraParamsFromCSV(groundtruthFilename, Wg);
  for (int i = 0; i < 15; ++i)
  {
    if (std::abs(Wg(i) - Wf(i)) / std::abs(Wg(i)) > epsilon)
    {
      std::cout << "Expected: " << Wg(i) << " got: " << Wf(i) << std::endl;
      return 0;
    }
  }
  return 0;
}

//----------------------------------------------------------------------------
int main(int argc, char* argv[])
{
  if (argc != 2)
  {
    return 0;
  }

  int errors = 0;

  std::string fisheyeMatchesFilename = std::string(argv[1]) + "/fisheye_camera.csv";
  std::string fisheyeGroundtruthFilename = std::string(argv[1]) + "/FisheyeParamsExpected.csv";
  errors += TestFisheyeModelCalibration(fisheyeMatchesFilename, fisheyeGroundtruthFilename);

  return errors;
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Cholesky

// #include <iostream>

#include <random>
#include <boost/test/unit_test.hpp>
#include <geomc/linalg/Cholesky.h>

using namespace geom;
using namespace std;

typedef std::mt19937_64 rng_t;

template <typename T>
SimpleMatrix<T,0,0> random_matrix(index_t sz, rng_t* rng) {
    typedef std::normal_distribution<T> d_normal_t;
    d_normal_t N = d_normal_t(0., 1); // (ctr, variance)
    SimpleMatrix<T,0,0> mx(sz,sz);

    for (index_t r = 0; r < sz; ++r) {
        for (index_t c = 0; c < sz; ++c) {
            mx(r,c) = N(*rng);
        }
    }
    return mx;
}

template <typename T>
T matrix_diff(const SimpleMatrix<T,0,0>& a, const SimpleMatrix<T,0,0>& b) {
    BOOST_CHECK_EQUAL(a.rows(), b.rows());
    BOOST_CHECK_EQUAL(a.cols(), b.cols());

    T residual = 0;
    for (index_t r = 0; r < a.rows(); ++r) {
        for (index_t c = 0; c < a.cols(); ++c) {
            T z = a[r][c] - b[r][c];
            residual += z * z;
        }
    }
    return std::sqrt(residual);
}

template <typename T>
void run_cholesky(index_t sz, rng_t* rng) {
    SimpleMatrix<T,0,0> mx = random_matrix<T>(sz, rng);
    SimpleMatrix<T,0,0> mxT(sz, sz);
    SimpleMatrix<T,0,0> A(sz, sz);
    SimpleMatrix<T,0,0> C(sz, sz);

    // make mx a positive definite matrix `A` by taking mx * mx^T:
    transpose(&mxT, mx);
    mul(&A, mx, mxT);

    // cholesky decompose `A` into `mx`.
    mtxcopy(&mx, A);
    BOOST_CHECK(cholesky(&mx));

    // confirm `mx^T * mx = A`
    transpose(&mxT, mx);
    mul(&C, mx, mxT);
    T rms = matrix_diff(C, A);
    BOOST_CHECK_SMALL(rms, (T)1e-5);
}

BOOST_AUTO_TEST_SUITE(cholesky)

BOOST_AUTO_TEST_CASE(verify_cholesky) {
    rng_t rng(11937294775LL);
    std::uniform_int_distribution<> rnd_int(2, 16);
    const index_t n = 50000;
    for (index_t i = 0; i < n; ++i) {
        index_t k = rnd_int(rng);
        run_cholesky<double>(k, &rng);
    }
}

BOOST_AUTO_TEST_SUITE_END()


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the linear model predictive
    control tool from the dlib C++ Library.  To explain what it does, suppose
    you have some process you want to control and the process dynamics are
    described by the linear equation:
        x_{i+1} = A*x_i + B*u_i + C
    That is, the next state the system goes into is a linear function of its
    current state (x_i) and the current control (u_i) plus some constant bias or
    disturbance.

    A model predictive controller can find the control (u) you should apply to
    drive the state (x) to some reference value, which is what we show in this
    example.  In particular, we will simulate a simple vehicle moving around in
    a planet's gravity.  We will use MPC to get the vehicle to fly to and then
    hover at a certain point in the air.

*/

#include <dlib/gui_widgets.h>
#include <dlib/control.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

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

int main()
{
    const int STATES = 4;
    const int CONTROLS = 2;

    // The first thing we do is setup our vehicle dynamics model (A*x + B*u + C).
    // Our state space (the x) will have 4 dimensions, the 2D vehicle position
    // and also the 2D velocity.  The control space (u) will be just 2 variables
    // which encode the amount of force we apply to the vehicle along each axis.
    // Therefore, the A matrix defines a simple constant velocity model.
    matrix<double,STATES,STATES> A;
    A = 1, 0, 1, 0,  // next_pos = pos + velocity
        0, 1, 0, 1,  // next_pos = pos + velocity
        0, 0, 1, 0,  // next_velocity = velocity
        0, 0, 0, 1;  // next_velocity = velocity

    // Here we say that the control variables effect only the velocity. That is,
    // the control applies an acceleration to the vehicle.
    matrix<double,STATES,CONTROLS> B;
    B = 0, 0,
        0, 0,
        1, 0,
        0, 1;

    // Let's also say there is a small constant acceleration in one direction.
    // This is the force of gravity in our model.
    matrix<double,STATES,1> C;
    C = 0,
        0,
        0,
        0.1;

    const int HORIZON = 30;
    // Now we need to setup some MPC specific parameters.  To understand them,
    // let's first talk about how MPC works.  When the MPC tool finds the "best"
    // control to apply it does it by simulating the process for HORIZON time
    // steps and selecting the control that leads to the best performance over
    // the next HORIZON steps.
    //
    // To be precise, each time you ask it for a control, it solves the
    // following quadratic program:
    //
    //     min     sum_i trans(x_i-target_i)*Q*(x_i-target_i) + trans(u_i)*R*u_i
    //    x_i,u_i
    //
    //     such that: x_0     == current_state
    //                x_{i+1} == A*x_i + B*u_i + C
    //                lower <= u_i <= upper
    //                0 <= i < HORIZON
    //
    // and reports u_0 as the control you should take given that you are currently
    // in current_state.  Q and R are user supplied matrices that define how we
    // penalize variations away from the target state as well as how much we want
    // to avoid generating large control signals.  We also allow you to specify
    // upper and lower bound constraints on the controls.  The next few lines
    // define these parameters for our simple example.

    matrix<double,STATES,1> Q;
    // Setup Q so that the MPC only cares about matching the target position and
    // ignores the velocity.
    Q = 1, 1, 0, 0;

    matrix<double,CONTROLS,1> R, lower, upper;
    R = 1, 1;
    lower = -0.5, -0.5;
    upper =  0.5,  0.5;

    // Finally, create the MPC controller.
    mpc<STATES,CONTROLS,HORIZON> controller(A,B,C,Q,R,lower,upper);

    // Let's tell the controller to send our vehicle to a random location.  It
    // will try to find the controls that makes the vehicle just hover at this
    // target position.
    dlib::rand rnd;
    matrix<double,STATES,1> target;
    target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
    controller.set_target(target);

    // Now let's start simulating our vehicle.  Our vehicle moves around inside
    // a 400x400 unit sized world.
    matrix<rgb_pixel> world(400,400);
    image_window win;
    matrix<double,STATES,1> current_state;
    // And we start it at the center of the world with zero velocity.
    current_state = 200,200,0,0;

    int iter = 0;
    while(!win.is_closed())
    {
        // Find the best control action given our current state.
        matrix<double,CONTROLS,1> action = controller(current_state);
        cout << "best control: " << trans(action);

        // Now draw our vehicle on the world.  We will draw the vehicle as a
        // black circle and its target position as a green circle.
        assign_all_pixels(world, rgb_pixel(255,255,255));
        const dpoint pos = point(current_state(0),current_state(1));
        const dpoint goal = point(target(0),target(1));
        draw_solid_circle(world, goal, 9, rgb_pixel(100,255,100));
        draw_solid_circle(world, pos, 7, 0);
        // We will also draw the control as a line showing which direction the
        // vehicle's thruster is firing.
        draw_line(world, pos, pos-50*action, rgb_pixel(255,0,0));
        win.set_image(world);

        // Take a step in the simulation
        current_state = A*current_state + B*action + C;
        dlib::sleep(100);

        // Every 100 iterations change the target to some other random location.
        ++iter;
        if (iter > 100)
        {
            iter = 0;
            target = rnd.get_random_double()*400,rnd.get_random_double()*400,0,0;
            controller.set_target(target);
        }
    }
}

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


// Copyright 2020 Xanadu Quantum Technologies Inc.

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/**
 * @file
 * \rst
 * Contains tensor representations of supported gates in ``lightning.qubit``.
 * \endrst
 */
#pragma once

#define _USE_MATH_DEFINES

#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

using Eigen::MatrixXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;
using Eigen::Tensor;

using State_1q = Eigen::Tensor<std::complex<double>, 1>;
using State_2q = Eigen::Tensor<std::complex<double>, 2>;
using State_3q = Eigen::Tensor<std::complex<double>, 3>;

using Gate_1q = Eigen::Tensor<std::complex<double>, 2>;
using Gate_2q = Eigen::Tensor<std::complex<double>, 4>;
using Gate_3q = Eigen::Tensor<std::complex<double>, 6>;

using Pairs = Eigen::IndexPair<int>;
using Pairs_1q = Eigen::array<Pairs, 1>;
using Pairs_2q = Eigen::array<Pairs, 2>;

const double SQRT_2 = sqrt(2);
const std::complex<double> IMAG(0, 1);
const std::complex<double> NEGATIVE_IMAG(0, -1);

/**
* Generates the identity gate.
*
* @return the identity tensor
*/
Gate_1q Identity() {
    Gate_1q X(2, 2);
    X.setValues({{1, 0}, {0, 1}});
    return X;
}

/**
* Generates the X gate.
*
* @return the X tensor
*/
Gate_1q X() {
    Gate_1q X(2, 2);
    X.setValues({{0, 1}, {1, 0}});
    return X;
}

/**
* Generates the Y gate.
*
* @return the Y tensor
*/
Gate_1q Y() {
    Gate_1q Y(2, 2);
    Y.setValues({{0, NEGATIVE_IMAG}, {IMAG, 0}});
    return Y;
}

/**
* Generates the Z gate.
*
* @return the Z tensor
*/
Gate_1q Z() {
    Gate_1q Z(2, 2);
    Z.setValues({{1, 0}, {0, -1}});
    return Z;
}

/**
* Generates the H gate.
*
* @return the H tensor
*/
Gate_1q H() {
    Gate_1q H(2, 2);
    H.setValues({{1/SQRT_2, 1/SQRT_2}, {1/SQRT_2, -1/SQRT_2}});
    return H;
}

/**
* Generates the S gate.
*
* @return the S tensor
*/
Gate_1q S() {
    Gate_1q S(2, 2);
    S.setValues({{1, 0}, {0, IMAG}});
    return S;
}

/**
* Generates the T gate.
*
* @return the T tensor
*/
Gate_1q T() {
    Gate_1q T(2, 2);

    const std::complex<double> exponent(0, M_PI/4);
    T.setValues({{1, 0}, {0, std::pow(M_E, exponent)}});
    return T;
}

/**
* Generates the X rotation gate.
*
* @param parameter the rotation angle
* @return the RX tensor
*/
Gate_1q RX(const double& parameter) {
    Gate_1q RX(2, 2);

    const std::complex<double> c (std::cos(parameter / 2), 0);
    const std::complex<double> js (0, std::sin(-parameter / 2));

    RX.setValues({{c, js}, {js, c}});
    return RX;
}

/**
* Generates the Y rotation gate.
*
* @param parameter the rotation angle
* @return the RY tensor
*/
Gate_1q RY(const double& parameter) {
    Gate_1q RY(2, 2);

    const double c = std::cos(parameter / 2);
    const double s = std::sin(parameter / 2);

    RY.setValues({{c, -s}, {s, c}});
    return RY;
}

/**
* Generates the Z rotation gate.
*
* @param parameter the rotation angle
* @return the RZ tensor
*/
Gate_1q RZ(const double& parameter) {
    Gate_1q RZ(2, 2);

    const std::complex<double> exponent(0, -parameter/2);
    const std::complex<double> exponent_second(0, parameter/2);
    const std::complex<double> first = std::pow(M_E, exponent);
    const std::complex<double> second = std::pow(M_E, exponent_second);

    RZ.setValues({{first, 0}, {0, second}});
    return RZ;
}

/**
* Generates the phase-shift gate.
*
* @param parameter the phase shift
* @return the phase-shift tensor
*/
Gate_1q PhaseShift(const double& parameter) {
    Gate_1q PhaseShift(2, 2);

    const std::complex<double> exponent(0, parameter);
    const std::complex<double> shift = std::pow(M_E, exponent);

    PhaseShift.setValues({{1, 0}, {0, shift}});
    return PhaseShift;
}

/**
* Generates the arbitrary single qubit rotation gate.
*
* The rotation is achieved through three separate rotations:
* \f$R(\phi, \theta, \omega)= RZ(\omega)RY(\theta)RZ(\phi)\f$.
*
* @param phi the first rotation angle
* @param theta the second rotation angle
* @param omega the third rotation angle
* @return the rotation tensor
*/
Gate_1q Rot(const double& phi, const double& theta, const double& omega) {
    Gate_1q Rot(2, 2);

    const std::complex<double> e00(0, (-phi - omega)/2);
    const std::complex<double> e10(0, (-phi + omega)/2);
    const std::complex<double> e01(0, (phi - omega)/2);
    const std::complex<double> e11(0, (phi + omega)/2);

    const std::complex<double> exp00 = std::pow(M_E, e00);
    const std::complex<double> exp10 = std::pow(M_E, e10);
    const std::complex<double> exp01 = std::pow(M_E, e01);
    const std::complex<double> exp11 = std::pow(M_E, e11);

    const double c = std::cos(theta / 2);
    const double s = std::sin(theta / 2);

    Rot.setValues({{exp00 * c, -exp01 * s}, {exp10 * s, exp11 * c}});

    return Rot;
}

/**
* Generates the CNOT gate.
*
* @return the CNOT tensor
*/
Gate_2q CNOT() {
    Gate_2q CNOT(2,2,2,2);
    CNOT.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{1, 0}}}});
    return CNOT;
}

/**
* Generates the SWAP gate.
*
* @return the SWAP tensor
*/
Gate_2q SWAP() {
    Gate_2q SWAP(2,2,2,2);
    SWAP.setValues({{{{1, 0},{0, 0}},{{0, 0},{1, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 1}}}});
    return SWAP;
}

/**
* Generates the CZ gate.
*
* @return the CZ tensor
*/
Gate_2q CZ() {
    Gate_2q CZ(2,2,2,2);
    CZ.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{1, 0}},{{0, 0},{0, -1}}}});
    return CZ;
}

/**
* Generates the Toffoli gate.
*
* @return the Toffoli tensor
*/
Gate_3q Toffoli() {
    Gate_3q Toffoli(2,2,2,2,2,2);
    Toffoli.setValues({{{{{{1, 0},{0, 0}},{{0, 0},{0, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 0}}}},
            {{{{0, 0},{1, 0}},{{0, 0},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{0, 0}}}}
        },
        {   {{{{0, 0},{0, 0}},{{1, 0},{0, 0}}},{{{0, 0},{0, 0}},{{0, 1},{0, 0}}}},
            {{{{0, 0},{0, 0}},{{0, 0},{0, 1}}},{{{0, 0},{0, 0}},{{0, 0},{1, 0}}}}
        }});
    return Toffoli;
}

/**
* Generates the CSWAP gate.
*
* @return the CSWAP tensor
*/
Gate_3q CSWAP() {
    Gate_3q CSWAP(2,2,2,2,2,2);
    CSWAP.setValues({{{{{{1, 0},{0, 0}},{{0, 0},{0, 0}}},{{{0, 1},{0, 0}},{{0, 0},{0, 0}}}},
            {{{{0, 0},{1, 0}},{{0, 0},{0, 0}}},{{{0, 0},{0, 1}},{{0, 0},{0, 0}}}}
        },
        {   {{{{0, 0},{0, 0}},{{1, 0},{0, 0}}},{{{0, 0},{0, 0}},{{0, 0},{1, 0}}}},
            {{{{0, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{0, 0}},{{0, 0},{0, 1}}}}
        }});
    return CSWAP;
}

/**
* Generates the controlled-X rotation gate.
*
* @param parameter the rotation angle
* @return the CRX tensor
*/
Gate_2q CRX(const double& parameter) {
    Gate_2q CRX(2, 2, 2, 2);

    const std::complex<double> c (std::cos(parameter / 2), 0);
    const std::complex<double> js (0, std::sin(-parameter / 2));

    CRX.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{c, js}},{{0, 0},{js, c}}}});
    return CRX;
}

/**
* Generates the controlled-Y rotation gate.
*
* @param parameter the rotation angle
* @return the CRY tensor
*/
Gate_2q CRY(const double& parameter) {
    Gate_2q CRY(2, 2, 2, 2);

    const double c = std::cos(parameter / 2);
    const double s = std::sin(parameter / 2);

    CRY.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{c, -s}},{{0, 0},{s, c}}}});
    return CRY;
}

/**
* Generates the controlled-Z rotation gate.
*
* @param parameter the rotation angle
* @return the CRZ tensor
*/
Gate_2q CRZ(const double& parameter) {
    Gate_2q CRZ(2, 2, 2, 2);

    const std::complex<double> exponent(0, -parameter/2);
    const std::complex<double> exponent_second(0, parameter/2);
    const std::complex<double> first = std::pow(M_E, exponent);
    const std::complex<double> second = std::pow(M_E, exponent_second);

    CRZ.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{first, 0}},{{0, 0},{0, second}}}});
    return CRZ;
}

/**
* Generates the controlled rotation gate.
*
* This gate implements a rotation on a target qubit depending on a control qubit. The rotation
* on the target qubit is achieved through three separate rotations:
* \f$R(\phi, \theta, \omega)= RZ(\omega)RY(\theta)RZ(\phi)\f$.
*
* @param phi the first rotation angle
* @param theta the second rotation angle
* @param omega the third rotation angle
* @return the controlled rotation tensor
*/
Gate_2q CRot(const double& phi, const double& theta, const double& omega) {
    Gate_2q CRot(2,2,2,2);

    const std::complex<double> e00(0, (-phi - omega)/2);
    const std::complex<double> e10(0, (-phi + omega)/2);
    const std::complex<double> e01(0, (phi - omega)/2);
    const std::complex<double> e11(0, (phi + omega)/2);

    const std::complex<double> exp00 = std::pow(M_E, e00);
    const std::complex<double> exp10 = std::pow(M_E, e10);
    const std::complex<double> exp01 = std::pow(M_E, e01);
    const std::complex<double> exp11 = std::pow(M_E, e11);

    const double c = std::cos(theta / 2);
    const double s = std::sin(theta / 2);

    CRot.setValues({{{{1, 0},{0, 0}},{{0, 1},{0, 0}}},{{{0, 0},{exp00 * c, -exp01 * s}},
            {{0, 0},{exp10 * s, exp11 * c}}
        }});
    return CRot;
}

// Creating aliases based on the function signatures of each operation
typedef Gate_1q (*pfunc_1q)();
typedef Gate_1q (*pfunc_1q_one_param)(const double&);
typedef Gate_1q (*pfunc_1q_three_params)(const double&, const double&, const double&);

typedef Gate_2q (*pfunc_2q)();
typedef Gate_2q (*pfunc_2q_one_param)(const double&);
typedef Gate_2q (*pfunc_2q_three_params)(const double&, const double&, const double&);

typedef Gate_3q (*pfunc_3q)();

// Defining the operation maps
const std::map<std::string, pfunc_1q> OneQubitOps = {
    {"Identity", Identity},
    {"PauliX", X},
    {"PauliY", Y},
    {"PauliZ", Z},
    {"Hadamard", H},
    {"S", S},
    {"T", T}
};

const std::map<std::string, pfunc_1q_one_param> OneQubitOpsOneParam = {
    {"RX", RX},
    {"RY", RY},
    {"RZ", RZ},
    {"PhaseShift", PhaseShift}
};

const std::map<std::string, pfunc_1q_three_params> OneQubitOpsThreeParams = {
    {"Rot", Rot}
};

const std::map<std::string, pfunc_2q> TwoQubitOps = {
    {"CNOT", CNOT},
    {"SWAP", SWAP},
    {"CZ", CZ}
};

const std::map<std::string, pfunc_2q_one_param> TwoQubitOpsOneParam = {
    {"CRX", CRX},
    {"CRY", CRY},
    {"CRZ", CRZ}
};

const std::map<std::string, pfunc_2q_three_params> TwoQubitOpsThreeParams = {
    {"CRot", CRot}
};

const std::map<std::string, pfunc_3q> ThreeQubitOps = {
    {"Toffoli", Toffoli},
    {"CSWAP", CSWAP}
};


// Boost.Geometry

// Copyright (c) 2020-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP

#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/precise_math.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Predicate to check at which side of a segment a point lies:
    left of segment (>0), right of segment (< 0), on segment (0).
\ingroup strategies
\tparam CalculationType \tparam_calculation
\details This predicate determines at which side of a segment a point lies
*/
template
<
    typename CalculationType = void
>
struct side_non_robust
{
public:
    //! \brief Computes double the signed area of the CCW triangle p1, p2, p
    template
    <
        typename P1,
        typename P2,
        typename P
    >
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename select_calculation_type_alt
            <
                CalculationType,
                P1,
                P2,
                P
            >::type CoordinateType;
        typedef typename select_most_precise
            <
                CoordinateType,
                double
            >::type PromotedType;

        CoordinateType const x = get<0>(p);
        CoordinateType const y = get<1>(p);

        CoordinateType const sx1 = get<0>(p1);
        CoordinateType const sy1 = get<1>(p1);
        CoordinateType const sx2 = get<0>(p2);
        CoordinateType const sy2 = get<1>(p2);

        //non-robust 1
        //the following is 2x slower in some generic cases when compiled with g++
        //(tested versions 9 and 10)
        //
        //auto detleft = (sx1 - x) * (sy2 - y);
        //auto detright = (sy1 - y) * (sx2 - x);
        //return detleft > detright ? 1 : (detleft < detright ? -1 : 0 );

        //non-robust 2
        PromotedType const dx = sx2 - sx1;
        PromotedType const dy = sy2 - sy1;
        PromotedType const dpx = x - sx1;
        PromotedType const dpy = y - sy1;

        PromotedType sv = geometry::detail::determinant<PromotedType>
                (
                    dx, dy,
                    dpx, dpy
                );
        PromotedType const zero = PromotedType();

        return sv == 0 ? 0 : sv > zero ? 1 : -1;
    }

};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_SIDE_NON_ROBUST_HPP


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Marc Glisse
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

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

#include <boost/range/iterator_range.hpp>

#include <wasserstein.h> // Hera

#include <array>

namespace py = pybind11;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> Dgm;

double wasserstein_distance(
    Dgm d1, Dgm d2,
    double wasserstein_power, double internal_p,
    double delta)
{
  py::buffer_info buf1 = d1.request();
  py::buffer_info buf2 = d2.request();
  // shape (n,2) or (0) for empty
  if((buf1.ndim!=2 || buf1.shape[1]!=2) && (buf1.ndim!=1 || buf1.shape[0]!=0))
    throw std::runtime_error("Diagram 1 must be an array of size n x 2");
  if((buf2.ndim!=2 || buf2.shape[1]!=2) && (buf2.ndim!=1 || buf2.shape[0]!=0))
    throw std::runtime_error("Diagram 2 must be an array of size n x 2");
  typedef std::array<double, 2> Point;
  auto p1 = (Point*)buf1.ptr;
  auto p2 = (Point*)buf2.ptr;
  auto diag1 = boost::make_iterator_range(p1, p1+buf1.shape[0]);
  auto diag2 = boost::make_iterator_range(p2, p2+buf2.shape[0]);

  hera::AuctionParams<double> params;
  params.wasserstein_power = wasserstein_power;
  // hera encodes infinity as -1...
  if(std::isinf(internal_p)) internal_p = hera::get_infinity<double>();
  params.internal_p = internal_p;
  params.delta = delta;
  // The extra parameters are purposedly not exposed for now.
  return hera::wasserstein_dist(diag1, diag2, params);
}

PYBIND11_MODULE(hera, m) {
      m.def("wasserstein_distance", &wasserstein_distance,
          py::arg("X"), py::arg("Y"),
          py::arg("order") = 1,
          py::arg("internal_p") = std::numeric_limits<double>::infinity(),
          py::arg("delta") = .01,
          R"pbdoc(
        Compute the Wasserstein distance between two diagrams.
        Points at infinity are supported.

        Parameters:
            X (n x 2 numpy array): First diagram
            Y (n x 2 numpy array): Second diagram
            order (float): Wasserstein exponent W_q
            internal_p (float): Internal Minkowski norm L^p in R^2
            delta (float): Relative error 1+delta

        Returns:
            float: Approximate Wasserstein distance W_q(X,Y)
    )pbdoc");
}


/** @file GammaDist.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2019
 * @brief GammaDist class definition
 *
 */
#include "PriorHessian/GammaDist.h"
#include "PriorHessian/util.h"
#include "PriorHessian/PriorHessianError.h"

#include <cmath>
#include <sstream>
#include <limits>

#include <boost/math/special_functions/gamma.hpp>

namespace prior_hessian {

const StringVecT GammaDist::_param_names = { "scale", "shape" };
const GammaDist::NparamsVecT GammaDist::_param_lbound = {0, 0}; //Lower bound on valid parameter values
const GammaDist::NparamsVecT GammaDist::_param_ubound = {INFINITY, INFINITY}; //Upper bound on valid parameter values

/* Constructors */
GammaDist::GammaDist(double scale, double shape)
    : UnivariateDist(),
      _scale(checked_scale(scale)),
      _shape(checked_shape(shape)),
      llh_const_initialized(false)
{ }

/* Non-static member functions */

void GammaDist::set_scale(double val)
{
    _scale = checked_scale(val);
    llh_const_initialized = false;
}

void GammaDist::set_shape(double val)
{
    _shape = checked_shape(val);
    llh_const_initialized = false;
}

void GammaDist::set_params(double scale, double shape)
{
    _scale = checked_scale(scale);
    _shape = checked_shape(shape);
    llh_const_initialized = false;
}

double GammaDist::cdf(double x) const
{
   return boost::math::gamma_p(_shape, x / _scale);
}

double GammaDist::icdf(double u) const
{
    if(u == 0) return 0;
    if(u == 1) return INFINITY;
    return boost::math::gamma_p_inv(_shape, u) * _scale;
}

double GammaDist::pdf(double x) const
{
    if(x==0) return 0;
    double inv_scale = 1/_scale;
    return boost::math::gamma_p_derivative(_shape, x*inv_scale) * inv_scale;
}

double GammaDist::llh(double x) const
{
    if(!llh_const_initialized) initialize_llh_const();
    return rllh(x) + llh_const;
}

void GammaDist::initialize_llh_const() const
{
    llh_const = compute_llh_const(shape(),scale());
    llh_const_initialized = true;
}

double GammaDist::compute_llh_const(double shape, double scale)
{
    return -shape*log(scale) - std::lgamma(shape);
}

double GammaDist::checked_scale(double val)
{
    if(!std::isfinite(val) || val <= 0) {
        std::ostringstream msg;
        msg<<"GammaDist: got bad scale value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

double GammaDist::checked_shape(double val)
{
    if(!std::isfinite(val) || val <= 0) {
        std::ostringstream msg;
        msg<<"GammaDist: got bad shape value:"<<val;
        throw ParameterValueError(msg.str());
    }
    return val;
}

} /* namespace prior_hessian */


///////////////////////////////////////////////////////////////////////////////
// vector_space::functional::l2_distance_squared.hpp                         //
//                                                                           //
//  Copyright 2009 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_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l2_DISTANCE_SQUARED_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l2_DISTANCE_SQUARED_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <functional>
#include <boost/type_traits.hpp>
#include <boost/function.hpp>
#include <boost/call_traits.hpp>
#include <boost/range.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/bind.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace vector_space{

    // Stores a range r, and when passed another range r1, computes the squared
    // Euclidian distance of r-r1.
    //
    // Examples:
    // R == const std::vector<double>&
    // R == iterator_range<std::vector<double> >
    template<typename R>
    class l2_distance_squared{
        typedef typename is_reference<R>::type is_ref_;
    public:
        typedef typename remove_reference<R>::type const_range_type;
        typedef typename remove_const<const_range_type>::type range_type;
        typedef typename range_value<range_type>::type result_type;

        // Constructor
        l2_distance_squared(){} // Warning, x_ not initialized
        l2_distance_squared(typename call_traits<R>::param_type x);
        l2_distance_squared(const l2_distance_squared& that);
        l2_distance_squared& operator=(const l2_distance_squared& that);

        // Call
        template<typename R1>
        result_type operator()(const R1& y)const;

        private:
        typename call_traits<R>::value_type x_;
    };

    template<typename R>
    l2_distance_squared<R>
    make_l2_distance_squared(const R& x){
        return l2_distance_squared<R>(x);
    }

    // Definitions
    template<typename R>
    l2_distance_squared<R>::l2_distance_squared(
        typename call_traits<R>::param_type x
    ):x_(x){}

    template<typename R>
    l2_distance_squared<R>::l2_distance_squared(
        const l2_distance_squared& that
    ):x_(that.x_){}

    template<typename R>
    l2_distance_squared<R>&
    l2_distance_squared<R>::operator=(const l2_distance_squared& that){
        if(&that != this){
            x_ = that.x_;
        }
        return *this;
    }

    template<typename R>
    template<typename R1>
    typename l2_distance_squared<R>::result_type
    l2_distance_squared<R>::operator()(const R1& y)const{
        BOOST_ASSERT(
            size(x_) == size(y)
        );
        return std::inner_product(
            begin(y),
            end(y),
            begin(x_),
            static_cast<result_type>(0),
            std::plus<result_type>(),
            (lambda::_1 - lambda::_2) * (lambda::_1 - lambda::_2)
        );
    };

}// vector_space
}// detail
}// statistics
}// boost

#endif


#ifndef NODEUTILS_HPP
#define NODEUTILS_HPP

#include <type_traits>

#include <geometry_msgs/PointStamped.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <cmath>
#include <tuple>

#include <ros/ros.h>

namespace igvc
{
/**
Calculates euclidian distance between two points

@tparam T the data type of the input points to calculate the euclidian distance for
@param[in] x1 x value of first point
@param[in] y1 y value of first point
@param[in] x2 x value of second point
@param[in] y2 y value of second point
@return the euclidian distance between both points
*/
template <typename T>
inline T get_distance(T x1, T y1, T x2, T y2)
{
  return std::hypot(x2 - x1, y2 - y1);
}

/**
Calculates euclidian distance between two points

@tparam T the data type of the input points to calculate the euclidian distance for
@param[in] p1 the <x,y> coords of the first point
@param[in] p2 the <x,y> coords of the second point
@return the euclidian distance between both points
*/
inline double get_distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2)
{
  return igvc::get_distance(p1.x, p1.y, p2.x, p2.y);
}

/**
Calculates euclidian distance between two points, taking tuples for each
(x,y) point as arguments

@tparam T the data type contained within each input tuple
@param[in] p1 the first point
@param[in] p2 the second point
@return the euclidian distance between both points
*/
template <typename T>
inline T get_distance(const std::tuple<T, T>& p1, const std::tuple<T, T>& p2)
{
  return igvc::get_distance(std::get<0>(p1), std::get<1>(p1), std::get<0>(p2), std::get<1>(p2));
}

/**
symmetric round up
Bias: away from zero

@tparam T the data type to round up
@param[in] the value to round up
@return the value rounded away from zero
*/
template <typename T>
T ceil0(const T& value)
{
  return (value < 0.0) ? std::floor(value) : std::ceil(value);
}

/**
Adjust angle to lie within the polar range [-PI, PI]
*/
inline void fit_to_polar(double& angle)
{
  angle = std::fmod(angle, 2 * M_PI);
  if (angle > M_PI)
  {
    angle -= 2 * M_PI;
  }
  else if (angle < -M_PI)
  {
    angle += 2 * M_PI;
  }
}

/**
Computes the egocentric polar angle of vec2 wrt vec1 in 2D, that is:
  - clockwise: negative
  - counter-clockwise: positive

source: https://stackoverflow.com/questions/14066933/direct-way-of-computing-clockwise-angle-between-2-vectors

@param[in] angle the double variable to assign the computed angle to
@param[in] vec2 the vector the angle is computed with respect to
@param[in] vec1 the reference vector
*/
inline void compute_angle(double& angle, Eigen::Vector3d vec2, Eigen::Vector3d vec1)
{
  double dot = vec2[0] * vec1[0] + vec2[1] * vec1[1];  // dot product - proportional to cos
  double det = vec2[0] * vec1[1] - vec2[1] * vec1[0];  // determinant - proportional to sin

  angle = atan2(det, dot);
}

}  // namespace igvc
#endif


#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include "tinymp.cpp"
#include "rational.cpp"

namespace utf = boost::unit_test;

BOOST_AUTO_TEST_CASE( rational_construct )
{
	rational<short> rs1, rs2(1), rs3(0, 2);
	rational<int> ri1, ri2(1), ri3(0, 2), ri4(rs3);
	rational<tinymp> rt1, rt2(5), rt3(1, 5), rt4(-rt3), rt5(rs3), rt6(ri3);
	BOOST_TEST( rs3 == ri4 );
	BOOST_TEST( ri4 == rs3 );
	BOOST_TEST( ri3 == rt6 );
	BOOST_TEST( rt6 == ri3 );
	BOOST_TEST( rs3 == rt5 );
	BOOST_TEST( rt5 == rs3 );
}

const int LT = 0, EQ = 1, GT = 2;
struct comparison
{
	int ln, ld, rn, rd;
	int type;
};
std::ostream& operator<<(std::ostream &os, const comparison& v)
{
	return os << v.ln << '/' << v.ld << ',' << v.rn << '/' << v.rd << ':' << v.type;
}

comparison cases[] = {
	{ -1, 0,  2, 0, LT },
	{  2, 0, -1, 0, GT },
	{  1, 0,  2, 0, EQ },
	{  2, 0,  1, 0, EQ },
	{ -1, 0, -2, 0, EQ },
	{ -2, 0, -1, 0, EQ },

	{  1, 1,  2, 2, EQ },
	{  2, 2,  1, 1, EQ },
	{ -1, 1, -2, 2, EQ },
	{ -2, 2, -1, 1, EQ },
	{  0, 1,  0, 2, EQ },
	{  0, 2,  0, 1, EQ },

	{ -2, 1, -1, 1, LT },
	{ -1, 1,  2, 1, LT },
	{ -1, 1,  0, 1, LT },
	{  0, 1,  1, 1, LT },
	{  1, 1,  2, 1, LT },
	{ -4, 2, -1, 1, LT },
	{ -1, 1,  4, 2, LT },
	{ -2, 2,  0, 1, LT },
	{  0, 1,  2, 2, LT },
	{  2, 2,  2, 1, LT },

	{ -2, 1, -3, 1, GT },
	{  1, 1, -2, 1, GT },
	{  0, 1, -2, 1, GT },
	{  3, 1,  0, 1, GT },
	{  3, 1,  2, 1, GT },
	{ -4, 2, -3, 1, GT },
	{  1, 1, -4, 2, GT },
	{  0, 2, -4, 1, GT },
	{  4, 2,  0, 2, GT },
	{  6, 2,  2, 1, GT }
};

BOOST_DATA_TEST_CASE( rational_comparison, utf::data::make(cases), input )
{
	rational<short> rsl(input.ln, input.ld), rsr(input.rn, input.rd);
	rational<int> ril(input.ln, input.ld), rir(input.rn, input.rd);
	switch(input.type) {
	case LT:
	        BOOST_TEST( !( rsl == rsr ) );
	        BOOST_TEST(    rsl != rsr   );
	        BOOST_TEST(    rsl <  rsr   );
	        BOOST_TEST(    rsl <= rsr   );
	        BOOST_TEST( !( rsl >  rsr ) );
	        BOOST_TEST( !( rsl >= rsr ) );
	        BOOST_TEST( !( ril == rir ) );
	        BOOST_TEST(    ril != rir   );
	        BOOST_TEST(    ril <  rir   );
	        BOOST_TEST(    ril <= rir   );
	        BOOST_TEST( !( ril >  rir ) );
	        BOOST_TEST( !( ril >= rir ) );
	        BOOST_TEST( !( rsl == rir ) );
	        BOOST_TEST(    rsl != rir   );
	        BOOST_TEST(    rsl <  rir   );
	        BOOST_TEST(    rsl <= rir   );
	        BOOST_TEST( !( rsl >  rir ) );
	        BOOST_TEST( !( rsl >= rir ) );
	        BOOST_TEST( !( ril == rsr ) );
	        BOOST_TEST(    ril != rsr   );
	        BOOST_TEST(    ril <  rsr   );
	        BOOST_TEST(    ril <= rsr   );
	        BOOST_TEST( !( ril >  rsr ) );
	        BOOST_TEST( !( ril >= rsr ) );
		break;
	case EQ:
	        BOOST_TEST(    rsl == rsr   );
	        BOOST_TEST( !( rsl != rsr ) );
	        BOOST_TEST( !( rsl <  rsr ) );
	        BOOST_TEST(    rsl <= rsr   );
	        BOOST_TEST( !( rsl >  rsr ) );
	        BOOST_TEST(    rsl >= rsr   );
	        BOOST_TEST(    ril == rir   );
	        BOOST_TEST( !( ril != rir ) );
	        BOOST_TEST( !( ril <  rir ) );
	        BOOST_TEST(    ril <= rir   );
	        BOOST_TEST( !( ril >  rir ) );
	        BOOST_TEST(    ril >= rir   );
	        BOOST_TEST(    rsl == rir   );
	        BOOST_TEST( !( rsl != rir ) );
	        BOOST_TEST( !( rsl <  rir ) );
	        BOOST_TEST(    rsl <= rir   );
	        BOOST_TEST( !( rsl >  rir ) );
	        BOOST_TEST(    rsl >= rir   );
	        BOOST_TEST(    ril == rsr   );
	        BOOST_TEST( !( ril != rsr ) );
	        BOOST_TEST( !( ril <  rsr ) );
	        BOOST_TEST(    ril <= rsr   );
	        BOOST_TEST( !( ril >  rsr ) );
	        BOOST_TEST(    ril >= rsr   );
		break;
	case GT:
	        BOOST_TEST( !( rsl == rsr ) );
	        BOOST_TEST(    rsl != rsr   );
	        BOOST_TEST( !( rsl <  rsr ) );
	        BOOST_TEST( !( rsl <= rsr ) );
	        BOOST_TEST(    rsl >  rsr   );
	        BOOST_TEST(    rsl >= rsr   );
	        BOOST_TEST( !( ril == rir ) );
	        BOOST_TEST(    ril != rir   );
	        BOOST_TEST( !( ril <  rir ) );
	        BOOST_TEST( !( ril <= rir ) );
	        BOOST_TEST(    ril >  rir   );
	        BOOST_TEST(    ril >= rir   );
	        BOOST_TEST( !( rsl == rir ) );
	        BOOST_TEST(    rsl != rir   );
	        BOOST_TEST( !( rsl <  rir ) );
	        BOOST_TEST( !( rsl <= rir ) );
	        BOOST_TEST(    rsl >  rir   );
	        BOOST_TEST(    rsl >= rir   );
	        BOOST_TEST( !( ril == rsr ) );
	        BOOST_TEST(    ril != rsr   );
	        BOOST_TEST( !( ril <  rsr ) );
	        BOOST_TEST( !( ril <= rsr ) );
	        BOOST_TEST(    ril >  rsr   );
	        BOOST_TEST(    ril >= rsr   );
		break;
	}
}

BOOST_AUTO_TEST_CASE( rational_arithmetic_additive )
{
}

BOOST_AUTO_TEST_CASE( rational_arithmetic_multiplicative )
{
}


#include "truncated_svd.hpp"

#include <armadillo>
#include <cmath>
#include <stdexcept>

template <class Real>
void TruncatedSvd(long m, long n, std::vector<Real> &A, Real tolerance,
                  bool is_relative, long &r, std::vector<Real> &U,
                  std::vector<Real> &s, std::vector<Real> &Vt) {
    if (m < 1 || n < 1) {
        throw std::invalid_argument("Matrix dimensions must be positive");
    }

    if (A.size() != m * n) {
        throw std::invalid_argument(
            "Matrix dimensions are incompatible with number of entries");
    }

    const long k = std::min(m, n);

    arma::Mat<Real> A_temp(m, n);
    for (long j = 0; j < n; ++j) {
        for (long i = 0; i < m; ++i) {
            A_temp(i, j) = A[i + j * m];
        }
    }

    arma::Mat<Real> U_thin;
    arma::Col<Real> s_thin;
    arma::Mat<Real> V_thin;

    bool status = arma::svd_econ(U_thin, s_thin, V_thin, A_temp);

    if (!status) {
        throw std::runtime_error("SVD decomposition failed");
    }

    Real frobenius_max_error = static_cast<Real>(0);
    if (is_relative) {
        for (long i = 0; i < k; ++i) {
            frobenius_max_error += std::pow(s_thin(i), 2);
        }

        frobenius_max_error *= std::pow(tolerance, 2);
    } else {
        frobenius_max_error = std::pow(tolerance, 2);
    }

    Real frobenius_error = static_cast<Real>(0);
    r = k;
    while (r > 0) {
        frobenius_error += std::pow(s_thin(r - 1), 2);
        if (frobenius_error > frobenius_max_error) {
            break;
        }
        --r;
    }

    U.resize(m * r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < m; ++i) {
            U[i + j * m] = U_thin(i, j);
        }
    }

    s.resize(r);
    for (long i = 0; i < r; ++i) {
        s[i] = s_thin(i);
    }

    Vt.resize(r * n);
    for (long j = 0; j < n; ++j) {
        for (long i = 0; i < r; ++i) {
            Vt[i + j * r] = V_thin(j, i);
        }
    }
}

template <class Real>
void TruncatedSvd(const arma::Mat<Real> &A, Real tolerance, bool is_relative,
                  arma::Mat<Real> &U, arma::Col<Real> &s, arma::Mat<Real> &V,
                  long &r) {
    arma::Mat<Real> U_thin;
    arma::Col<Real> s_thin;
    arma::Mat<Real> V_thin;
    bool status = arma::svd_econ(U_thin, s_thin, V_thin, A);
    if (!status) {
        throw std::runtime_error("SVD decomposition failed");
    }

    Real frobenius_max_error = static_cast<Real>(0);
    if (is_relative) {
        for (long i = 0; i < s_thin.n_rows; ++i) {
            frobenius_max_error += std::pow(s_thin(i), 2);
        }

        frobenius_max_error *= std::pow(tolerance, 2);
    } else {
        frobenius_max_error = std::pow(tolerance, 2);
    }

    Real frobenius_error = static_cast<Real>(0);
    r = s_thin.n_rows;
    while (r > 0) {
        frobenius_error += std::pow(s_thin(r - 1), 2);
        if (frobenius_error > frobenius_max_error) {
            break;
        }
        --r;
    }

    U.set_size(A.n_rows, r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < A.n_rows; ++i) {
            U(i, j) = U_thin(i, j);
        }
    }

    s.set_size(r);
    for (long i = 0; i < r; ++i) {
        s(i) = s_thin(i);
    }

    V.set_size(A.n_cols, r);
    for (long j = 0; j < r; ++j) {
        for (long i = 0; i < A.n_cols; ++i) {
            V(i, j) = V_thin(i, j);
        }
    }
}

template void TruncatedSvd<float>(long m, long n, std::vector<float> &A,
                                  float tolerance, bool is_relative, long &r,
                                  std::vector<float> &U, std::vector<float> &s,
                                  std::vector<float> &Vt);
template void TruncatedSvd<double>(long m, long n, std::vector<double> &A,
                                   double tolerance, bool is_relative, long &r,
                                   std::vector<double> &U,
                                   std::vector<double> &s,
                                   std::vector<double> &Vt);

template void TruncatedSvd<float>(const arma::Mat<float> &A, float tolerance,
                                  bool is_relative, arma::Mat<float> &U,
                                  arma::Col<float> &s, arma::Mat<float> &V,
                                  long &r);

template void TruncatedSvd<double>(const arma::Mat<double> &A, double tolerance,
                                   bool is_relative, arma::Mat<double> &U,
                                   arma::Col<double> &s, arma::Mat<double> &V,
                                   long &r);


#pragma once

#include <cmath>  // ceil
#include <initializer_list>

#include <Eigen/SparseLU>

#include "InterpolationTemplate.hpp"

namespace intp {

template <typename T, size_t D>
class InterpolationFunction {  // TODO: Add integration
   public:
    using val_type = T;
    using spline_type = BSpline<T, D>;
    using size_type = typename spline_type::size_type;
    using coord_type = typename spline_type::knot_type;

    const size_type order;
    const static size_type dim = D;

   private:
    spline_type __spline;

    template <typename _T>
    using DimArray = std::array<_T, dim>;

    DimArray<coord_type> __dx;
    DimArray<bool> __periodicity;
    DimArray<bool> __uniform;

    friend class InterpolationFunctionTemplate<T, D>;

    // auxiliary methods

    template <size_type... di>
    inline val_type call_op_helper(util::index_sequence<di...>,
                                   DimArray<coord_type> c) const {
        return __spline(std::make_pair(
            c[di], __uniform[di]
                       ? std::min(__spline.knots_num(di) - order - 2,
                                  (size_type)std::ceil(std::max(
                                      0., (c[di] - range(di).first) / __dx[di] -
                                              (__periodicity[di]
                                                   ? 1.
                                                   : .5 * (order + 1)))) +
                                      order)
                       : order)...);
    }

    template <size_type... di>
    inline val_type derivative_helper(util::index_sequence<di...>,
                                      DimArray<coord_type> c,
                                      DimArray<size_type> d) const {
        return __spline.derivative_at(std::make_tuple(
            (coord_type)c[di], (size_type)d[di],
            __uniform[di]
                ? std::min(
                      __spline.knots_num(di) - order - 2,
                      (size_type)std::ceil(std::max(
                          0.,
                          (c[di] - range(di).first) / __dx[di] -
                              (__periodicity[di] ? 1. : .5 * (order + 1)))) +
                          order)
                : order)...);
    }

    // overload for uniform knots
    template <typename _T>
    typename std::enable_if<std::is_arithmetic<_T>::value>::type
    __create_knot_vector(size_type dim_ind,
                         const MeshDimension<dim>& mesh_dimension,
                         DimArray<typename spline_type::KnotContainer>&,
                         std::pair<_T, _T> x_range) {
        __uniform[dim_ind] = true;
        const size_type n = mesh_dimension.dim_size(dim_ind);
        __dx[dim_ind] = (x_range.second - x_range.first) / (n - 1);

        const size_t extra =
            __periodicity[dim_ind] ? 2 * order + (1 - order % 2) : order + 1;

        std::vector<typename spline_type::knot_type> xs(n + extra,
                                                        x_range.first);

        if (__periodicity[dim_ind]) {
            for (size_type i = 0; i < xs.size(); ++i) {
                xs[i] = x_range.first + (i - .5 * extra) * __dx[dim_ind];
            }
        } else {
            for (size_type i = order + 1; i < xs.size() - order - 1; ++i) {
                xs[i] = x_range.first + (i - .5 * extra) * __dx[dim_ind];
            }
            for (size_type i = xs.size() - order - 1; i < xs.size(); ++i) {
                xs[i] = x_range.second;
            }
        }

        __spline.load_knots(dim_ind, std::move(xs), __periodicity[dim_ind]);
    }

    // overload for nonuniform knots, given by iterator pair
    template <typename _T>
    typename std::enable_if<std::is_convertible<
        typename std::iterator_traits<_T>::iterator_category,
        std::input_iterator_tag>::value>::type
    __create_knot_vector(
        size_type dim_ind,
        const MeshDimension<dim>& mesh_dimension,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        std::pair<_T, _T> x_range) {
        __uniform[dim_ind] = false;
        const size_type n = std::distance(x_range.first, x_range.second);
        if (n != mesh_dimension.dim_size(dim_ind)) {
            throw std::range_error(
                std::string("Inconsistency between knot number and "
                            "interpolated value number at dimension ") +
                std::to_string(dim_ind));
        }
        typename spline_type::KnotContainer xs(
            __periodicity[dim_ind] ? n + 2 * order + (1 - order % 2)
                                   : n + order + 1);

        input_coords[dim_ind].reserve(n);
        if (__periodicity[dim_ind]) {
            auto iter = x_range.first;

            input_coords[dim_ind].push_back(*iter);
            for (size_type i = order + 1; i < order + n; ++i) {
                val_type present = *(++iter);
                xs[i] = order % 2 == 0
                            ? .5 * (input_coords[dim_ind].back() + present)
                            : present;
                input_coords[dim_ind].push_back(present);
            }
            val_type period =
                input_coords[dim_ind].back() - input_coords[dim_ind].front();
            for (size_type i = 0; i < order + 1; ++i) {
                xs[i] = xs[n + i - 1] - period;
                xs[xs.size() - i - 1] = xs[xs.size() - i - n] + period;
            }
        } else {
            auto it = x_range.first;
            // Notice that *it++ is not guarantee to work as what you
            // expected for input iterators.
            auto l_knot = *it;
            // fill lestmost *order+1* identical knots
            for (size_type i = 0; i < order + 1; ++i) { xs[i] = l_knot; }
            // first knot is same as first input coordinate
            input_coords[dim_ind].emplace_back(l_knot);
            // Every knot in middle is average of *order* input
            // coordinates. This var is to track the sum of a moving window with
            // width *order*.
            coord_type window_sum{};
            for (size_type i = 1; i < order; ++i) {
                input_coords[dim_ind].emplace_back(*(++it));
                window_sum += input_coords[dim_ind][i];
            }
            for (size_type i = order + 1; i < n; ++i) {
                input_coords[dim_ind].emplace_back(*(++it));
                window_sum += input_coords[dim_ind][i - 1];
                xs[i] = window_sum / order;
                window_sum -= input_coords[dim_ind][i - order];
            }
            auto r_knot = *(++it);
            // fill rightmost *order+1* identical knots
            for (size_type i = n; i < n + order + 1; ++i) { xs[i] = r_knot; }
            // last knot is same as last input coordinate
            input_coords[dim_ind].emplace_back(r_knot);
        }
#ifdef _DEBUG
        std::cout << "[DEBUG] Nonuniform knots along dimension" << dim_ind
                  << ":\n";
        for (auto& c : xs) { std::cout << "[DEBUG] " << c << '\n'; }
        std::cout << std::endl;
#endif

        __spline.load_knots(dim_ind, std::move(xs));
    }

    template <typename... Ts, size_type... di>
    void __create_knots(
        util::index_sequence<di...>,
        MeshDimension<dim> mesh_dimension,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        std::pair<Ts, Ts>... x_ranges) {
#if __cplusplus >= 201703L
        (__create_knot_vector(di, f_mesh, input_coords, x_ranges), ...);
#else
        // polyfill of C++17 fold expression over comma
        std::array<std::nullptr_t, sizeof...(Ts)>{
            (__create_knot_vector(di, mesh_dimension, input_coords, x_ranges),
             nullptr)...};
#endif
    }

    inline void __boundary_check(const DimArray<coord_type>& coord) const {
        for (size_type d = 0; d < dim; ++d) {
            if (!__periodicity[d] &&
                (coord[d] < range(d).first || coord[d] > range(d).second)) {
                throw std::domain_error(
                    "Given coordinate out of interpolation function range!");
            }
        }
    }

   public:
    /**
     * @brief Construct a new 1D Interpolation Function object, mimicking
     * Mathematica's `Interpolation` function, with option `Method->"Spline"`.
     *
     * @tparam InputIter
     * @param order order of interpolation, the interpolated function is of
     * $C^{order-1}$
     * @param periodic whether to construct a periodic spline
     * @param f_range a pair of iterators defining to-be-interpolated data
     * @param x_range a pair of x_min and x_max
     */
    template <
        typename InputIter,
        typename C1,
        typename C2,
        typename std::enable_if<
            dim == 1u &&
            std::is_convertible<
                typename std::iterator_traits<InputIter>::iterator_category,
                std::input_iterator_tag>::value>::type* = nullptr>
    InterpolationFunction(size_type order,
                          bool periodic,
                          std::pair<InputIter, InputIter> f_range,
                          std::pair<C1, C2> x_range)
        : InterpolationFunction(
              order,
              {periodic},
              Mesh<val_type, 1u>{std::make_pair(f_range.first, f_range.second)},
              static_cast<std::pair<typename std::common_type<C1, C2>::type,
                                    typename std::common_type<C1, C2>::type>>(
                  x_range)) {}

    template <
        typename InputIter,
        typename C1,
        typename C2,
        typename std::enable_if<
            dim == 1u &&
            std::is_convertible<
                typename std::iterator_traits<InputIter>::iterator_category,
                std::input_iterator_tag>::value>::type* = nullptr>
    InterpolationFunction(size_type order,
                          std::pair<InputIter, InputIter> f_range,
                          std::pair<C1, C2> x_range)
        : InterpolationFunction(order, false, f_range, x_range) {}

    /**
     * @brief Construct a new nD Interpolation Function object, mimicking
     * Mathematica's `Interpolation` function, with option `Method->"Spline"`.
     * Notice: last value of periodic dimension will be discarded since it is
     * considered same of the first value. Thus inconsistency input data for
     * periodic interpolation will be accepted.
     *
     * @param order order of interpolation, the interpolated function is of
     * $C^{order-1}$
     * @param periodicity an array describing periodicity of each dimension
     * @param f_mesh a mesh containing data to be interpolated
     * @param x_ranges pairs of x_min and x_max or begin and end iterator
     */
    template <typename... Ts>
    InterpolationFunction(size_type order,
                          DimArray<bool> periodicity,
                          const Mesh<val_type, dim>& f_mesh,
                          std::pair<Ts, Ts>... x_ranges)
        : InterpolationFunction(InterpolationFunctionTemplate<val_type, dim>{
              order, periodicity, f_mesh.dimension(), x_ranges...}
                                    .interpolate(f_mesh)) {}

    // Non-periodic for all dimension
    template <typename... Ts>
    InterpolationFunction(size_type order,
                          const Mesh<val_type, dim>& f_mesh,
                          std::pair<Ts, Ts>... x_ranges)
        : InterpolationFunction(order, {}, f_mesh, x_ranges...) {}

    // constructor for partial construction, that is, without interpolated
    // values
    template <typename... Ts>
    InterpolationFunction(
        size_type order,
        DimArray<bool> periodicity,
        DimArray<typename spline_type::KnotContainer>& input_coords,
        MeshDimension<dim> mesh_dimension,
        std::pair<Ts, Ts>... x_ranges)
        : order(order),
          __spline(periodicity, order),
          __periodicity(periodicity) {
        // load knots into spline
        __create_knots(util::make_index_sequence_for<Ts...>{},
                       std::move(mesh_dimension), input_coords, x_ranges...);
    }

    /**
     * @brief Get spline value.
     *
     * @param x coordinates
     */
    template <typename... Coords,
              typename Indices = util::make_index_sequence_for<Coords...>,
              typename = typename std::enable_if<std::is_arithmetic<
                  typename std::common_type<Coords...>::type>::value>::type>
    val_type operator()(Coords... x) const {
        return call_op_helper(Indices{}, DimArray<coord_type>{x...});
    }

    /**
     * @brief Get spline value.
     *
     * @param coord coordinate array
     */
    val_type operator()(DimArray<coord_type> coord) const {
        return call_op_helper(util::make_index_sequence<dim>{}, coord);
    }

    /**
     * @brief Get spline value, but with out of boundary check.
     *
     * @param coord coordinate array
     */
    val_type at(DimArray<coord_type> coord) const {
        __boundary_check(coord);
        return call_op_helper(util::make_index_sequence<dim>{}, coord);
    }

    /**
     * @brief Get spline value, but with out of boundary check.
     *
     * @param x coordinates
     */
    template <typename... Coords,
              typename Indices = util::make_index_sequence_for<Coords...>,
              typename = typename std::enable_if<std::is_arithmetic<
                  typename std::common_type<Coords...>::type>::value>::type>
    val_type at(Coords... x) const {
        return at(DimArray<coord_type>{static_cast<coord_type>(x)...});
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord coordinate array
     * @param derivatives derivative order array
     */
    val_type derivative(DimArray<coord_type> coord,
                        DimArray<size_type> derivatives) const {
        return derivative_helper(util::make_index_sequence<dim>{}, coord,
                                 derivatives);
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord coordinate array
     * @param deriOrder derivative orders
     */
    template <typename... Args>
    val_type derivative(DimArray<coord_type> coord, Args... deriOrder) const {
        return derivative(coord, DimArray<size_type>{(size_type)deriOrder...});
    }

    /**
     * @brief Get spline derivative value.
     *
     * @param coord_deriOrder_pair pairs of coordinate and derivative order
     */
    template <typename... CoordDeriOrderPair>
    val_type derivative(CoordDeriOrderPair... coord_deriOrder_pair) const {
        return derivative(
            DimArray<coord_type>{coord_deriOrder_pair.first...},
            DimArray<size_type>{(size_type)coord_deriOrder_pair.second...});
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord coordinate array
     * @param derivatives derivative order array
     */
    val_type derivative_at(DimArray<coord_type> coord,
                           DimArray<size_type> derivatives) const {
        __boundary_check(coord);
        return derivative_helper(util::make_index_sequence<dim>{}, coord,
                                 derivatives);
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord coordinate array
     * @param deriOrder derivative orders
     */
    template <typename... Args>
    val_type derivative_at(DimArray<coord_type> coord,
                           Args... deriOrder) const {
        return derivative_at(coord,
                             DimArray<size_type>{(size_type)deriOrder...});
    }

    /**
     * @brief Get spline derivative value, but with out of boundary check.
     *
     * @param coord_deriOrder_pair pairs of coordinate and derivative order
     */
    template <typename... CoordDeriOrderPair>
    val_type derivative_at(CoordDeriOrderPair... coord_deriOrder_pair) const {
        return derivative_at(
            DimArray<coord_type>{coord_deriOrder_pair.first...},
            DimArray<size_type>{(size_type)coord_deriOrder_pair.second...});
    }

    // properties

    bool periodicity(size_type dim_ind) const { return __periodicity[dim_ind]; }

    bool uniform(size_type dim_ind) const { return __uniform[dim_ind]; }

    const std::pair<typename spline_type::knot_type,
                    typename spline_type::knot_type>&
    range(size_type dim_ind) const {
        return __spline.range(dim_ind);
    }

    /**
     * @brief Get a ref of underlying spline object
     *
     * @return spline_type&
     */
    const spline_type& spline() const { return __spline; }
};

template <typename T>
class InterpolationFunction1D : public InterpolationFunction<T, size_t{1}> {
   private:
    using base = InterpolationFunction<T, size_t{1}>;

   public:
    template <typename InputIter>
    InterpolationFunction1D(std::pair<InputIter, InputIter> f_range,
                            typename base::size_type order = 3,
                            bool periodicity = false)
        : InterpolationFunction1D(
              std::make_pair(typename base::coord_type{},
                             static_cast<typename base::coord_type>(
                                 (f_range.second - f_range.first) - 1)),
              f_range,
              order,
              periodicity){};

    template <typename C1, typename C2, typename InputIter>
    InterpolationFunction1D(std::pair<C1, C2> x_range,
                            std::pair<InputIter, InputIter> f_range,
                            typename base::size_type order = 3,
                            bool periodicity = false)
        : base(order, periodicity, f_range, x_range){};
};

}  // namespace intp


#include <string>
#include <math.h>
#include <iostream>
#include <boost/regex.hpp>
#include <boost/format.hpp>
#include <fstream>
#include <unordered_map>

#define SIMULATED_ROUNDS 1000
#define COLLISION_ROUNDS 40

using namespace std;

class Particle {
private:
    long x, y, z;
    long vx, vy, vz;
    long ax, ay, az;
public:
    bool annihilated;

    Particle(string line) {
        boost::regex re("[p,v,a,=,<,>,\\s]+");
        boost::sregex_token_iterator
            p(line.begin(), line.end(), re, -1);
        boost::sregex_token_iterator end;

        vector<int> values;
        while(p != end) {
            if(p->str().length() == 0) {
                p++;
                continue;
            }

            values.push_back(atoi((p++)->str().c_str()));
        }

        x = values[0];
        y = values[1];
        z = values[2];
        vx = values[3];
        vy = values[4];
        vz = values[5];
        ax = values[6];
        ay = values[7];
        az = values[8];
        annihilated = false;
    }

    void move() {
        vx += ax;
        vy += ay;
        vz += az;
        x += vx;
        y += vy;
        z += vz;
    }

    float distance() {
        float dist = pow(x, 2);
        dist += pow(y, 2);
        dist += pow(z, 2);
        return dist;
    }

    void simulateMovement(float time) {
        x += int(vx*time) + int(0.5*ax*time*time);
        y += int(vy*time) + int(0.5*ay*time*time);
        z += int(vz*time) + int(0.5*az*time*time);
    }

    string location() {
        return boost::str(boost::format("%d,%d,%d") % x % y % z);
    }

    friend ostream& operator<<(ostream &os, const Particle& p);
};

std::ostream& operator<<(std::ostream &strm, const Particle &p) {
    return strm << boost::format("<%d,%d,%d; %d,%d,%d; %d,%d,%d>") % p.x % p.y % p.z % p.vx % p.vy % p.vz % p.ax % p.ay % p.az;
}

int closestParticle(vector<Particle> particles) {
    vector<float> distances;
    for(int i = 0; i < particles.size(); i++) {
        particles[i].simulateMovement(SIMULATED_ROUNDS);
        distances.push_back(particles[i].distance());
    }

    int minParticle = 0;
    for(int i = 0; i < distances.size(); i++) {
        if(distances[i] < distances[minParticle])
            minParticle = i;
    }
    return minParticle;
}

int numUncollidedParticles(vector<Particle> particles) {
    for(int i = 0; i < COLLISION_ROUNDS; i++) {
        unordered_map<string, int> locations;

        for(int j = 0; j < particles.size(); j++) {
            if(particles[j].annihilated)
                continue;

            particles[j].move();

            string loc = particles[j].location();
            if(locations.find(loc) != locations.end()) {
                particles[j].annihilated = true;
                particles[locations[loc]].annihilated = true;
            } else {
                locations[loc] = j;
            }
        }
    }
    int nonAnnihilated = 0;
    for(int i = 0; i < particles.size(); i++) {
        if(!particles[i].annihilated) nonAnnihilated++;
    }
    return nonAnnihilated;
}

int main() {
    vector<Particle> particles;

    ifstream infile("20.txt");
    string line;
    while (getline(infile, line)) {
        particles.push_back(Particle(line));
    }
    cout << "Part 1: " << closestParticle(particles) << "\n";
    cout << "Part 2: " << numUncollidedParticles(particles) << "\n";
}


/*    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.
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/NumericalQuadrature/trapezoidQuadrature.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{
namespace unit_tests
{

// Function to create linearly spaced data.
std::vector< double > linspace( double start, double end, int numberOfSamples )
{
    double spacing = ( end - start ) / ( static_cast< double >( numberOfSamples - 1 ) );

    std::vector< double > vector(0);
    for( int i = 0 ; i < numberOfSamples ; i++ )
    {
        vector.push_back( start + static_cast< double >( i ) * spacing );
    }

    return vector;
}

BOOST_AUTO_TEST_SUITE( test_trapezoid_integrator )

//! Test if qudrature is computed correctly (sine function with 1E4 data points).
BOOST_AUTO_TEST_CASE( testIntegralSineFunction )
{
    std::vector< int > numberOfSamplesList;
    numberOfSamplesList.push_back( 1E2 );
    numberOfSamplesList.push_back( 1E4 );
    numberOfSamplesList.push_back( 1E6 );

    std::vector< double > tolerances = { 2E-4, 2E-8, 2E-12 };

    double previousError = TUDAT_NAN;
    double currentError = TUDAT_NAN;

    for( unsigned int test = 0; test < 3; test++ )
    {
        // Generate independent variables
        int numberOfSamples = numberOfSamplesList.at( test );

        std::vector< double > bounds( 2 );
        bounds[ 0 ] = 0.0;
        bounds[ 1 ] = mathematical_constants::PI;
        std::vector< double > independentVariables = linspace( bounds[ 0 ], bounds[ 1 ], numberOfSamples );

        std::vector< double > dependentVariables;
        for( unsigned int i = 0 ; i < independentVariables.size( ) ; i++ )
        {
            dependentVariables.push_back( std::sin( independentVariables[ i ] ) );
        }

        tudat::numerical_quadrature::TrapezoidNumericalQuadrature< double, double > integrator(
                    independentVariables, dependentVariables );

        double expectedIntegral = 2.0;
        double computedIntegralTrapezoid = integrator.getQuadrature( );
        double computedIntegralSimpson = tudat::numerical_quadrature::performExtendedSimpsonsQuadrature(
                    independentVariables.at( 1 ) - independentVariables.at( 0 ), dependentVariables );

        // Check if computed integral matches expected value.
        BOOST_CHECK_CLOSE_FRACTION( computedIntegralTrapezoid, expectedIntegral, tolerances.at( test ) );
        BOOST_CHECK_CLOSE_FRACTION( computedIntegralSimpson, expectedIntegral, tolerances.at( test ) );

        currentError = std::fabs( computedIntegralTrapezoid - expectedIntegral );

        // Test order of quadrature
        if( test > 0 )
        {
            BOOST_CHECK_CLOSE_FRACTION( previousError / currentError, 1.0E4, 0.1 );
        }

        previousError = currentError;

        // Reset data and recompute quadrature
        bounds[ 1 ] = 2.0 * mathematical_constants::PI;
        independentVariables = linspace( bounds[ 0 ], bounds[ 1 ], numberOfSamples );

        dependentVariables.clear( );
        for( unsigned int i = 0 ; i < independentVariables.size( ) ; i++ )
        {
            dependentVariables.push_back( std::sin( independentVariables[ i ] ) );
        }

        // Error should be close to zero, as integration (and its errors) are symmetrical
        integrator.resetData( independentVariables, dependentVariables );
        expectedIntegral = 0.0;
        computedIntegralTrapezoid = integrator.getQuadrature( );
        BOOST_CHECK_SMALL( computedIntegralTrapezoid - expectedIntegral, std::max( 5.0E-6, numberOfSamples * 5.0E-20 ) );
    }
}

//! Test if qudrature is computed correctly (exponential function).
BOOST_AUTO_TEST_CASE( testIntegralExpFunction )
{
    // Generate independent variables
    int numberOfSamples = 1E4;

    std::vector< double > bounds( 2 );
    bounds[ 0 ] = 0.0;
    bounds[ 1 ] = 2.0;
    std::vector< double > independentVariables = linspace( bounds[ 0 ], bounds[ 1 ], numberOfSamples );

    std::vector< double > dependentVariables( 0 );
    for( unsigned int i = 0 ; i < independentVariables.size() ; i++ )
    {
        dependentVariables.push_back( std::exp( independentVariables[ i ] ) );
    }

    // Create integrator
    tudat::numerical_quadrature::TrapezoidNumericalQuadrature< double, double > integrator(
                independentVariables, dependentVariables );

    double expectedIntegral = std::exp( 2.0 ) - std::exp( 0.0 );
    double computedIntegralTrapezoid = integrator.getQuadrature( );
    double computedIntegralSimpson = tudat::numerical_quadrature::performExtendedSimpsonsQuadrature(
                independentVariables.at( 1 ) - independentVariables.at( 0 ), dependentVariables );

    // Check if computed sample mean matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( computedIntegralTrapezoid, expectedIntegral, 1E-8 );
    BOOST_CHECK_CLOSE_FRACTION( computedIntegralSimpson, expectedIntegral, 1E-8 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <boost/variant.hpp>
#include <unordered_map>
#include <range/v3/all.hpp>
#include "combinations_algo.h"
#include "matrix.h"
struct Combinations
{
  FixedMatrix<int, 9, 9> networks_;
  std::array<int, 9> a = { 0, 1, 2, 3, 4, 5, 6, 7, 8 };
  int max = std::numeric_limits<int>::min();
  int maxhappiness() noexcept
  {
    auto f = [this](auto b, auto e) {
      int s = networks_(*b, *(e - 1));
      s += networks_(*(e - 1), *b);
      for (; b != e - 1; ++b) {
        s += networks_(*b, *(b + 1));
        s += networks_(*(b + 1), *b);
      }
      max = std::max(s, max);
      return false;
    };
    for_each_reversible_circular_permutation(a.begin(), a.end(), a.end(), f);
    return max;
  }
};
int main(int argc, char **argv)
{
  if (argc > 1) {
    Combinations com;
    std::ifstream ifs(argv[1]);
    int i = 0;
    std::unordered_map<std::string, int> m;
    m["gain"] = 1;
    m["lose"] = -1;
    std::string s;
    fmt::print("a\n");
    for (; std::getline(ifs, s); ++i) {
      int row = i / 7;
      int col = i % 7;
      std::istringstream iss(s);
      std::string ignore, sign;
      int value;
      iss >> ignore >> ignore >> sign >> value;
      fmt::print("sign:{},v:{}\n", sign, value);
      value *= m[sign];
      if (col >= row) {
        com.networks_(row, col + 1) = value;
      } else {
        com.networks_(row, col) = value;
      }
    }

    fmt::print("{}\n", com.maxhappiness());
  }
}


// Test Pose
#include <catch2/catch.hpp>

#include <Eigen/Geometry>
#include <igl/PI.h>

#include <autodiff/autodiff_types.hpp>
#include <interval/interval.hpp>
#include <physics/pose.hpp>

TEST_CASE("Poses to dofs", "[physics][pose]")
{
    using namespace ipc::rigid;
    int dim = GENERATE(2, 3);
    int num_bodies = GENERATE(0, 1, 2, 3, 10, 1000);
    Eigen::VectorXd dofs =
        Eigen::VectorXd::Random(num_bodies * Pose<double>::dim_to_ndof(dim));
    Poses<double> poses = Pose<double>::dofs_to_poses(dofs, dim);
    Eigen::VectorXd returned_dofs = Pose<double>::poses_to_dofs(poses);
    CHECK((dofs - returned_dofs).squaredNorm() == Approx(0));
}

TEST_CASE("Cast poses", "[physics][pose]")
{
    using namespace ipc::rigid;
    int dim = GENERATE(2, 3);
    int num_bodies = GENERATE(0, 1, 2, 3, 10, 1000);

    Eigen::VectorXf dof =
        Eigen::VectorXf::Random(num_bodies * Pose<float>::dim_to_ndof(dim));
    Poses<float> expected_posesf = Pose<float>::dofs_to_poses(dof, dim);

    Poses<float> actual_posesf = cast<float>(cast<double>(expected_posesf));

    CHECK(
        (Pose<float>::poses_to_dofs(expected_posesf)
         - Pose<float>::poses_to_dofs(actual_posesf))
            .squaredNorm()
        == Approx(0.0));
}

TEST_CASE("SE(3) ↦ SO(3)", "[physics][pose]")
{
    using namespace ipc::rigid;
    double angle;
    Eigen::Vector3d axis;

    SECTION("zero")
    {
        angle = 0;
        axis = Eigen::Vector3d::Random();
    }
    SECTION("random")
    {
        angle = GENERATE(take(100, random(0.0, 2 * igl::PI)));
        axis = Eigen::Vector3d::Random();
    }
    axis.normalize();

    Pose<double> p = Pose<double>::Zero(3);
    p.rotation = angle * axis;
    Eigen::Matrix3d R_actual = p.construct_rotation_matrix();
    Eigen::Matrix3d R_expected =
        Eigen::AngleAxisd(angle, axis).toRotationMatrix();
    CHECK((R_actual - R_expected).norm() == Approx(0).margin(1e-12));
}

TEST_CASE("∇²(SE(3) ↦ SO(3))", "[!benchmark][physics][pose]")
{
    using namespace ipc::rigid;
    typedef ipc::rigid::AutodiffType<Eigen::Dynamic, 12> Diff;
    Diff::activate(12);

    Pose<double> p;
    p.position = Eigen::Vector3d::Zero();
    p.rotation = Eigen::Vector3d(0, igl::PI, 0);

    BENCHMARK("Compute R") { return p.construct_rotation_matrix(); };

    Pose<Diff::DDouble1> d1p;
    d1p.position = Diff::d1vars(0, Eigen::Vector3d::Zero());
    d1p.rotation = Diff::d1vars(3, Eigen::Vector3d(0, igl::PI, 0));

    BENCHMARK("Compute R DDouble1") { return d1p.construct_rotation_matrix(); };

    Pose<Diff::DDouble2> d2p;
    d2p.position = Diff::d2vars(0, Eigen::Vector3d::Zero());
    d2p.rotation = Diff::d2vars(3, Eigen::Vector3d(0, igl::PI, 0));

    BENCHMARK("Compute R DDouble2") { return d2p.construct_rotation_matrix(); };
}

TEST_CASE("Interval SE(3) ↦ SO(3)", "[!benchmark][physics][pose]")
{
    using namespace ipc::rigid;
    double angle;
    Eigen::Vector3d axis;

    SECTION("zero")
    {
        angle = 0;
        axis = Eigen::Vector3d::Random();
    }
    SECTION("random")
    {
        angle = GENERATE(take(1, random(0.0, 2 * igl::PI)));
        axis = Eigen::Vector3d::Random();
    }
    axis.normalize();

    Pose<double> p = Pose<double>::Zero(3);
    p.rotation = angle * axis;
    BENCHMARK("Double SE(3) ↦ SO(3)")
    {
        Eigen::Matrix3d R = p.construct_rotation_matrix();
    };
    Pose<ipc::rigid::Interval> pI = p.cast<ipc::rigid::Interval>();
    BENCHMARK("Interval SE(3) ↦ SO(3)")
    {
        Matrix3I R = pI.construct_rotation_matrix();
    };
}


#include <boost/numeric/odeint.hpp>
#include <vector>

using namespace boost::numeric::odeint;

/* The type of container used to hold the state vector */
typedef std::vector<double> state_type;

const double gam = 0.15;

/* The rhs of x' = f(x) */
void harmonic_oscillator(const state_type &x, state_type &dxdt, const double /* t */)
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam * x[1];
}

// An example of observer to record steps in the integration
struct push_back_state_and_time
{
    std::vector<state_type> &m_states;
    std::vector<double> &m_times;

    push_back_state_and_time(std::vector<state_type> &states, std::vector<double> &times)
        : m_states(states), m_times(times) {}

    void operator()(const state_type &x, double t)
    {
        m_states.push_back(x);
        m_times.push_back(t);
    }
};

int main()
{

    // Containers to store solution
    std::vector<state_type> x_vec;
    std::vector<double> times;

    // Unknown variables with initial conditions
    state_type x(2);
    x[0] = 1.0; // start at x=1.0, p=0.0
    x[1] = 0.0;

    // Integration process. Returns the number of performed steps. This one determines
    // the stepper automatically.
    //    auto steps = integrate(harmonic_oscillator,
    //                           x, 0.0, 10.0, 0.1,
    //                           push_back_state_and_time(x_vec, times));

    // Or you can choose it explicitly
    runge_kutta4<state_type> stepper;
    auto steps = integrate_const(
        stepper,
        harmonic_oscillator,
        x,
        0.0,
        10.0,
        0.1,
        push_back_state_and_time(x_vec, times));

    /* output */
    for (size_t i = 0; i <= steps; i++)
    {
        std::cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
    }

    std::cout << "\n*******************\n\n";

    // Or, for a finer control, each step can be performed explicitly as below
    const auto dt = 0.1;
    const auto t0 = 0.0;
    const auto tfinal = 10.0;
    state_type x_explicit(2);
    x_explicit[0] = 1.0; // start at x=1.0, p=0.0
    x_explicit[1] = 0.0;
    std::cout << 0 << '\t' << x_explicit[0] << '\t' << x_explicit[1] << '\n';
    for (double t = t0 + dt; t < tfinal; t += dt)
    {
        stepper.do_step(harmonic_oscillator, x_explicit, t, dt);
        std::cout << t << '\t' << x_explicit[0] << '\t' << x_explicit[1] << '\n';
    }

    return 0;
}