#include <cylbot_mcl/pose_cloud.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/bind.hpp>
#include <cmath>
#include <vector>
#include <utility>
#include <algorithm>
#include <tf/transform_datatypes.h>
#include <limits>
#include <set>

namespace cylbot_mcl
{
	PoseCloud::PoseCloud(const RobotModel& model, const int num_samples) :
		model(model)
	{
		this->pose_array.header.frame_id = "/map";

		this->pose_array.poses = generateUniformPoses(std::make_pair(-20.0, 20.0),
													  std::make_pair(-20.0, 20.0),
													  num_samples).poses;
	}

	PoseCloud::PoseCloud(const RobotModel& model,
						 const geometry_msgs::PoseWithCovarianceStamped& initial_pose,
						 const int num_particles) :
		model(model)
	{
		this->pose_array.header.frame_id = "/map";
		this->resetCloud(initial_pose, num_particles);
	}

	void PoseCloud::resetCloud(const geometry_msgs::PoseWithCovarianceStamped& initial_pose, const int num_particles)
	{
		pose_array.poses = generatePoses(initial_pose, num_particles).poses;
	}

	geometry_msgs::PoseArray PoseCloud::generatePoses(const geometry_msgs::PoseWithCovarianceStamped& initial_pose,
													  const int num_particles)
	{
		Eigen::VectorXd mean(3);
		Eigen::MatrixXd covar(3,3);

		mean << initial_pose.pose.pose.position.x,
			    initial_pose.pose.pose.position.y,
			    tf::getYaw(initial_pose.pose.pose.orientation);
		covar << initial_pose.pose.covariance[0], 0,                                  0,
			     0,                               initial_pose.pose.covariance[7],    0,
			     0,                               0,                                  initial_pose.pose.covariance[35];

		Eigen::MatrixXd normTransform(3, 3);
		Eigen::LLT<Eigen::MatrixXd> cholSolver(covar);

		if(cholSolver.info() == Eigen::Success)
		{
			normTransform = cholSolver.matrixL();
		}
		else
		{
			Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
			normTransform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
		}

		Eigen::MatrixXd samples = (normTransform * Eigen::MatrixXd::NullaryExpr(3, num_particles, randn)).colwise() + mean;

		geometry_msgs::PoseArray new_pose_array;
		for(int i=0; i<num_particles; i++)
		{
			geometry_msgs::Pose pose;
			pose.position.x = samples(0, i);
			pose.position.y = samples(1, i);
			pose.position.z = 0;

			// generate random yaw then convert it to a quaternion
			pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, samples(2, i));

			new_pose_array.poses.push_back(pose);
		}

		return new_pose_array;
	}

	geometry_msgs::PoseArray PoseCloud::generateUniformPoses(const std::pair<double, double> x_range,
															 const std::pair<double, double> y_range,
															 const int num_poses)
	{
		boost::random::uniform_real_distribution<> x_dist(x_range.first, x_range.second);
		boost::random::uniform_real_distribution<> y_dist(y_range.first, y_range.second);
		boost::random::uniform_real_distribution<> yaw_dist(0.0, 2*3.14159);

		boost::variate_generator<boost::mt19937, boost::random::uniform_real_distribution<> > x_rand(rng, x_dist);
		boost::variate_generator<boost::mt19937, boost::random::uniform_real_distribution<> > yaw_rand(rng, yaw_dist);

		geometry_msgs::PoseArray new_pose_array;
		for(int i=0; i<num_poses; i++)
		{
			geometry_msgs::Pose pose;
			pose.position.x = x_rand();
			pose.position.y = x_rand();
			pose.position.z = 0;

			pose.orientation = tf::createQuaternionMsgFromYaw(yaw_rand());

			new_pose_array.poses.push_back(pose);
		}

		return new_pose_array;
	}

	void PoseCloud::motionUpdate(const geometry_msgs::Twist& u, double dt)
	{
		last_cmd = u;
		// don't do anything if the robot isn't moving
		if(fabs(u.linear.y) < 0.1 && fabs(u.angular.z) < 0.1)
			return;

		double v_hat_variance = model.alpha[0]*u.linear.y*u.linear.y + model.alpha[1]*u.angular.z*u.angular.z;
		double w_hat_variance = model.alpha[2]*u.linear.y*u.linear.y + model.alpha[3]*u.angular.z*u.angular.z;
		double gamma_variance = model.alpha[4]*u.linear.y*u.linear.y + model.alpha[5]*u.angular.z*u.angular.z;

		boost::normal_distribution<> v_hat_dist(0.0, sqrt(v_hat_variance));
		boost::normal_distribution<> w_hat_dist(0.0, sqrt(w_hat_variance));
		boost::normal_distribution<> gamma_dist(0.0, sqrt(gamma_variance));

		double v_hat = u.linear.y + v_hat_dist(rng);
		double w_hat = u.angular.z + w_hat_dist(rng);
		double gamma = gamma_dist(rng);

		double v_w_ratio = v_hat/w_hat;

		for(geometry_msgs::PoseArray::_poses_type::iterator pose = pose_array.poses.begin();
			pose != pose_array.poses.end();
			pose++)
		{
			double yaw = tf::getYaw(pose->orientation);

			double x_prime = pose->position.x - v_w_ratio*sin(yaw) + v_w_ratio*sin(yaw + w_hat*dt);
			double y_prime = pose->position.y + v_w_ratio*cos(yaw) - v_w_ratio*cos(yaw + w_hat*dt);
			double yaw_prime = yaw + w_hat*dt + gamma*dt;

			pose->position.x = x_prime;
			pose->position.y = y_prime;
			pose->orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_prime);
		}

	}

	geometry_msgs::PoseArray PoseCloud::getPoses()
	{
		return this->pose_array;
	}

}


/*
 * Copyright 2021 MusicScience37 (Kenta Kabashima)
 *
 * 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
 * \brief Test of kernel_interpolator class.
 */
#include "num_collect/interp/kernel/kernel_interpolator.h"

#include <Eigen/Core>
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating.hpp>

#include "eigen_approx.h"
#include "is_finite.h"
#include "num_collect/interp/kernel/calc_kernel_mat.h"
#include "num_collect/interp/kernel/euclidean_distance.h"
#include "num_collect/interp/kernel/gaussian_rbf.h"
#include "num_collect/interp/kernel/rbf_kernel.h"

TEST_CASE("num_collect::interp::kernel::kernel_interpolator") {
    using num_collect::interp::kernel::euclidean_distance;
    using num_collect::interp::kernel::gaussian_rbf;
    using num_collect::interp::kernel::kernel_interpolator;
    using num_collect::interp::kernel::rbf_kernel;

    const auto vars = std::vector<double>{0.0, 0.1, 0.2, 0.4, 0.6, 1.0};
    const auto data = Eigen::VectorXd{{0.0, 0.2, 0.4, 0.7, 1.0, 2.0}};

    using kernel_type =
        rbf_kernel<euclidean_distance<double>, gaussian_rbf<double>>;

    SECTION("interpolate using defaults") {
        auto interpolator = kernel_interpolator<kernel_type>();

        interpolator.compute(vars, data);
        REQUIRE(interpolator.reg_param() == 0.0);  // NOLINT
        REQUIRE(interpolator.kernel().len_param() > 0.0);

        constexpr double tol_error = 1e-8;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(interpolator(vars[i]),
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));
        }
    }

    SECTION("interpolate with fixed kernel parameters") {
        auto interpolator = kernel_interpolator<kernel_type>();
        constexpr double len_param = 0.1;
        interpolator.fix_kernel_param(std::log10(len_param));

        interpolator.compute(vars, data);
        REQUIRE(interpolator.reg_param() == 0.0);                 // NOLINT
        REQUIRE(interpolator.kernel().len_param() == len_param);  // NOLINT

        constexpr double tol_error = 1e-4;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(interpolator(vars[i]),
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));
        }
    }

    SECTION("interpolate with a fixed regularization parameter") {
        auto interpolator = kernel_interpolator<kernel_type>();
        constexpr double len_param = 0.1;
        interpolator.fix_kernel_param(std::log10(len_param));
        constexpr double reg_param = 1e-4;
        interpolator.regularize_with(reg_param);

        interpolator.compute(vars, data);
        REQUIRE(interpolator.reg_param() == reg_param);
        REQUIRE(interpolator.kernel().len_param() > 0.0);

        constexpr double tol_error = 1e-2;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(interpolator(vars[i]),
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));
        }
    }

    SECTION("interpolate with automatic regularization") {
        auto interpolator = kernel_interpolator<kernel_type>();
        constexpr double len_param = 0.1;
        interpolator.fix_kernel_param(std::log10(len_param));
        interpolator.regularize_automatically();

        interpolator.compute(vars, data);
        REQUIRE(interpolator.reg_param() > 0.0);
        REQUIRE(interpolator.kernel().len_param() > 0.0);

        constexpr double tol_error = 1e-2;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(interpolator(vars[i]),
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));
        }
    }

    SECTION("interpolate with automatic regularization and kernel parameters") {
        auto interpolator = kernel_interpolator<kernel_type>();
        interpolator.regularize_automatically();

        interpolator.compute(vars, data);
        REQUIRE(interpolator.reg_param() > 0.0);
        REQUIRE(interpolator.kernel().len_param() > 0.0);

        constexpr double tol_error = 1e-2;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            REQUIRE_THAT(interpolator(vars[i]),
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));
        }
    }

    SECTION("evaluate variance without regularization") {
        auto interpolator = kernel_interpolator<kernel_type>();
        constexpr double len_param = 0.1;
        interpolator.fix_kernel_param(std::log10(len_param));
        interpolator.disable_regularization();

        interpolator.compute(vars, data);

        constexpr double tol_error = 1e-4;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            const auto [mean, variance] =
                interpolator.evaluate_mean_and_variance_on(vars[i]);
            REQUIRE_THAT(mean,
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));

            REQUIRE(variance >= 0.0);
            REQUIRE_THAT(variance, Catch::Matchers::WithinAbs(0.0, tol_error));
        }
    }

    SECTION("evaluate variance with regularization") {
        auto interpolator = kernel_interpolator<kernel_type>();
        constexpr double len_param = 0.1;
        interpolator.fix_kernel_param(std::log10(len_param));
        constexpr double reg_param = 1e-4;
        interpolator.regularize_with(reg_param);

        interpolator.compute(vars, data);

        constexpr double tol_error = 1e-2;
        for (std::size_t i = 0; i < vars.size(); ++i) {
            INFO("i = " << i);
            const auto [mean, variance] =
                interpolator.evaluate_mean_and_variance_on(vars[i]);
            REQUIRE_THAT(mean,
                Catch::Matchers::WithinAbs(
                    data(static_cast<num_collect::index_type>(i)), tol_error));

            REQUIRE(variance > 0.0);
            REQUIRE_THAT(variance,
                Catch::Matchers::WithinAbs(
                    interpolator.common_coeff() * reg_param, tol_error));
        }
    }
}


#ifndef EXPSUM_MODIFIED_PRONY_TRUNCATION_HPP
#define EXPSUM_MODIFIED_PRONY_TRUNCATION_HPP

#include <armadillo>

namespace expsum
{

//
// Truncation of sum-of-exponentials with small exponents.
//
// This class find the optimal sum-of-exponentials with small exponents with
// smaller number of terms using the modified Prony method proposed by Beylkin
// and Monzon (2010).
//
template <typename T>
class modified_prony_truncation
{
public:
    using size_type    = arma::uword;
    using value_type   = T;
    using real_type    = typename arma::get_pod_type<T>::result;
    using complex_type = std::complex<real_type>;

    using vector_type      = arma::Col<value_type>;
    using real_vector_type = arma::Col<real_type>;
    // using complex_vector_type = arma::Col<complex_type>;
    using complex_vector_type = arma::Col<value_type>;

    using matrix_type = arma::Mat<value_type>;
    // using complex_matrix_type = arma::Mat<complex_type>;
    using complex_matrix_type = arma::Mat<value_type>;

private:
    size_type size_;
    complex_vector_type exponent_;
    complex_vector_type weight_;

    complex_vector_type vec_work_;
    complex_matrix_type mat_work_;

public:
    template <typename VecP, typename VecW>
    typename std::enable_if<(arma::is_basevec<VecP>::value &&
                             arma::is_basevec<VecW>::value),
                            void>::type
    run(const VecP& p, const VecW& w, real_type eps);

    void resize(size_type n)
    {
        if (exponent_.size() < n)
        {
            exponent_.set_size(n);
            weight_.set_size(n);
            vec_work_.set_size(2 * n);
            mat_work_.set_size(2 * n, n);
        }
    }

    //
    // @return number of terms after truncation
    //
    size_type size() const
    {
        return size_;
    }

    //
    // @return Vector view to the exponents.
    //
    auto exponents() const -> decltype(exponent_.head(size_))
    {
        return exponent_.head(size_);
    }
    //
    // @return Vector view to the weights.
    //
    auto weights() const -> decltype(weight_.head(size_))
    {
        return weight_.head(size_);
    }
};

template <typename T>
template <typename VecP, typename VecW>
typename std::enable_if<(arma::is_basevec<VecP>::value &&
                         arma::is_basevec<VecW>::value),
                        void>::type
modified_prony_truncation<T>::run(const VecP& p, const VecW& w, real_type eps)
{
    const auto n1 = p.n_elem;
    resize(n1);

    if (n1 == size_type())
    {
        size_ = n1;
        return;
    }

    value_type* h_ptr = reinterpret_cast<value_type*>(mat_work_.memptr());
    value_type* v_ptr = h_ptr + n1 * n1;

    vector_type p_pow(h_ptr, n1, false, true);
    vector_type h_vec(v_ptr, 2 * n1, false, true);

    p_pow = p;

    h_vec(0) = arma::sum(w);
    h_vec(1) = -arma::sum(w % p_pow);

    size_type n2   = 1;
    auto factorial = real_type(1);
    while (n2 < n1)
    {
        p_pow %= p;
        h_vec(2 * n2) = arma::sum(w % p_pow);
        p_pow %= p;
        h_vec(2 * n2 + 1) = -arma::sum(w % p_pow);

        factorial *= real_type(2 * n2 * (2 * n2 + 1));

        if (std::abs(h_vec(2 * n2 + 1)) < eps * factorial)
        {
            // Taylor expansion converges with the tolerance eps.
            ++n2;
            break;
        }
        ++n2;
    }

    size_ = n2;
    //
    // Construct a Hankel matrix from the sequence h_vec, and solve the linear
    // equation, H q = b, with b = -h_vec(m:2m-1).
    //
    std::cout << "***** Hankel matrix" << std::endl;
    matrix_type H(h_ptr, n2, n2, false, true);

    for (size_type k = 0; k < n2; ++k)
    {
        H.col(k) = h_vec.subvec(k, k + n2 - 1);
    }

    value_type* q_ptr = reinterpret_cast<value_type*>(exponent_.memptr());
    vector_type q(q_ptr, n2, false, true);
    std::cout << "***** linear solve" << std::endl;
    q = arma::solve(H, h_vec.subvec(n2, 2 * n2 - 1));

    //
    // Find the roots of the Prony polynomial,
    //
    // q(z) = \sum_{k=0}^{m-1} q_k z^{k}.
    //
    // The roots of q(z) can be obtained as the eigenvalues of the companion
    // matrix,
    //
    //     (0  0  ...  0 -q[0]  )
    //     (1  0  ...  0 -q[1]  )
    // C = (0  1  ...  0 -q[2]  )
    //     (.. .. ...  .. ..    )
    //     (0  0  ...  1 -q[m-1])
    //
    std::cout << "***** find roots" << std::endl;
    matrix_type& C = H; // overwrite H
    C.zeros();
    for (size_type i = 0; i < n2 - 1; ++i)
    {
        C(i + 1, i) = real_type(1);
    }
    C.col(n2 - 1) = -q;
    // exponent_.head(n2) = arma::eig_gen(C);
    // exponent_.head(n2) = -exponent_.head(n2);
    auto eigvals       = arma::eig_gen(C);
    exponent_.head(n2) = -arma::real(eigvals);

    //
    // Solve overdetermined Vandermonde system,
    //
    // V(0:2m-1,0:m-1) w(0:m-1) = h(0:2m-1)
    //
    // by the least square method.
    //
    std::cout << "***** least squares" << std::endl;
    complex_vector_type b2(vec_work_.memptr(), 2 * n2, false, true);
    for (size_type i = 0; i < 2 * n2; ++i)
    {
        b2(i) = h_vec(i);
    }
    // Construct Vandermonde matrix from exponents
    complex_matrix_type V(mat_work_.memptr(), 2 * n2, n2, false, true);
    for (size_type i = 0; i < n2; ++i)
    {
        // We assume all the eigenvalues are real here
        const auto z = exponent_(i);
        V(0, i) = T(1);
        for (size_type j = 1; j < V.n_rows; ++j)
        {
            V(j, i) = V(j - 1, i) * z; // z[i]**j
        }
    }

    weight_.subvec(0, n2 - 1) = arma::solve(V, b2);
}

} // namespace expsum

#endif /* EXPSUM_MODIFIED_PRONY_TRUNCATION_HPP */


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

#ifndef __pinocchio_math_rpy_hxx__
#define __pinocchio_math_rpy_hxx__

#include <Eigen/Geometry>

#include "pinocchio/math/sincos.hpp"

namespace pinocchio
{
  namespace rpy
  {
    template<typename Scalar>
    Eigen::Matrix<Scalar,3,3>
    rpyToMatrix(const Scalar& r,
                const Scalar& p,
                const Scalar& y)
    {
      typedef Eigen::AngleAxis<Scalar> AngleAxis;
      typedef Eigen::Matrix<Scalar,3,1> Vector3s;
      return (AngleAxis(y, Vector3s::UnitZ())
              * AngleAxis(p, Vector3s::UnitY())
              * AngleAxis(r, Vector3s::UnitX())
             ).toRotationMatrix();
    }

    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
      return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
    }

    template<typename Matrix3Like>
    Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
    matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
      assert(R.isUnitary() && "R is not a unitary matrix");

      typedef typename Matrix3Like::Scalar Scalar;
      typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> ReturnType;
      static const Scalar pi = PI<Scalar>();

      ReturnType res = R.eulerAngles(2,1,0).reverse();

      if(res[1] < -pi/2)
        res[1] += 2*pi;

      if(res[1] > pi/2)
      {
        res[1] = pi - res[1];
        if(res[0] < Scalar(0))
          res[0] += pi;
        else
          res[0] -= pi;
        // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
        res[2] -= pi;
      }

      return res;
    }

    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
    {
      typedef typename Vector3Like::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(1.0), Scalar(0.0),   -sp,
               Scalar(0.0),          cr, sr*cp,
               Scalar(0.0),         -sr, cr*cp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << cp*cy,         -sy, Scalar(0.0),
               cp*sy,          cy, Scalar(0.0),
                 -sp, Scalar(0.0), Scalar(1.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }

    template<typename Vector3Like>
    Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
    {
      typedef typename Vector3Like::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      Scalar tp = sp/cp;
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(1.0), sr*tp, cr*tp,
               Scalar(0.0),    cr,   -sr,
               Scalar(0.0), sr/cp, cr/cp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << cy/cp,  sy/cp, Scalar(0.0),
                 -sy,     cy, Scalar(0.0),
               cy*tp,  sy*tp, Scalar(1.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }

    template<typename Vector3Like0, typename Vector3Like1>
    Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
    computeRpyJacobianTimeDerivative(const Eigen::MatrixBase<Vector3Like0> & rpy, const Eigen::MatrixBase<Vector3Like1> & rpydot, const ReferenceFrame rf)
    {
      typedef typename Vector3Like0::Scalar Scalar;
      typedef Eigen::Matrix<typename Vector3Like0::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options> ReturnType;
      ReturnType J;
      const Scalar p = rpy[1];
      const Scalar dp = rpydot[1];
      Scalar sp, cp;
      SINCOS(p, &sp, &cp);
      switch (rf)
      {
        case LOCAL:
        {
          const Scalar r = rpy[0];
          const Scalar dr = rpydot[0];
          Scalar sr, cr; SINCOS(r, &sr, &cr);
          J << Scalar(0.0), Scalar(0.0),               -cp*dp,
               Scalar(0.0),      -sr*dr,  cr*cp*dr - sr*sp*dp,
               Scalar(0.0),      -cr*dr, -sr*cp*dr - cr*sp*dp;
          return J;
        }
        case WORLD:
        case LOCAL_WORLD_ALIGNED:
        {
          const Scalar y = rpy[2];
          const Scalar dy = rpydot[2];
          Scalar sy, cy; SINCOS(y, &sy, &cy);
          J << -sp*cy*dp - cp*sy*dy,      -cy*dy, Scalar(0.0),
                cp*cy*dy - sp*sy*dp,      -sy*dy, Scalar(0.0),
                             -cp*dp, Scalar(0.0), Scalar(0.0);
          return J;
        }
        default:
        {
          throw std::invalid_argument("Bad reference frame.");
        }
      }
    }

  } // namespace rpy
}
#endif //#ifndef __pinocchio_math_rpy_hxx__


// Unit test for the isEqualArray1D function in testing_functions.cpp
// This function determines if each element in two 1D arrays of 1D arrays are equal within some tolerance
// This is important because there is inherent error in double precision numbers
// COMPLETE. This function works as intended. 2021/11/21

// Include headers
#include <iostream>
#include <Eigen/Dense>
#include "../main/fvm_1D_functions.h"
#include "../main/testing_functions.h"

using namespace std;
using namespace Eigen;

int main() {

    // Print what we are doing
    cout << "------------------------------------- " << \
    "This is a test of the isEqualArray1D_of_Array1D function." << \
    " -------------------------------------" << endl;

    // Set a tolerance
    double TOL = 1e-10;

    // Set an array of values
    Array<ArrayXd, 3, 1> vals;
    for (int i = 0; i < 3; i++) {
        vals(i) = ArrayXd::Zero(4) + 1.1;
    }

    // We can test 4 different modes of this function to determine how well it works:
    // Greater and within tolerance
    // Greater and outside tolerance
    // Lower and within tolerance
    // Lower and outside tolerance

    // Build the test array
    Array<ArrayXd, 3, 1> testArray;
    for (int i = 0; i < 3; i++) {
        // Initialize internal arrays
        testArray(i) = ArrayXd::Zero(4);

        // Populate internal arrays
        testArray(i)(0) = vals(i)(0) + 0.1*TOL;
        testArray(i)(1) = vals(i)(1) + 1.1*TOL;
        testArray(i)(2) = vals(i)(2) - 0.1*TOL;
        testArray(i)(3) = vals(i)(3) - 1.1*TOL;
    }

    // Use the isEqualArray1D function to see if these are equal.
    Array<Array<bool,Dynamic,1>, 3, 1> boolArray = isEqualArray1D_of_Array1D(vals, testArray, TOL);

    // First, we will test to make sure that val+0.1*TOL is considered equal since it will by definition be within the tolerance
    // This should result in 1
    if (boolArray(0)(0) == 1 && boolArray(1)(0) == 1 && boolArray(2)(0) == 1 ) {  //
        cout << "Test of higher but within tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of higher but within tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val+1.1*TOL is not considered equal since it will by definition be outside the tolerance
    // This should result in 0
    if (boolArray(0)(1) == 0 && boolArray(1)(1) == 0 && boolArray(2)(1) == 0) {  //
        cout << "Test of higher over tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of higher over tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val-0.1*TOL is considered equal since it will by definition be within the tolerance
    // This should result in 1
    if (boolArray(0)(2) == 1 && boolArray(1)(2) == 1 && boolArray(2)(2) == 1) {  //
        cout << "Test of lower but within tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of lower but within tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    // Test to make sure that val-1.1*TOL is not considered equal since it will by definition be outside the tolerance
    // This should result in 0
    if (boolArray(0)(3) == 0 && boolArray(1)(3) == 0 && boolArray(2)(3) == 0) {  //
        cout << "Test of lower but outside tolerance is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of lower but outside tolerance is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    cout << endl << endl;
}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
  // Constructor
  arma::mat x,y;

  x << 0.1778 << 0.1203 << -0.2264 << endr
    << 0.0957 << 0.2403 << -0.3400 << endr
    << 0.1397 << 0.1925 << -0.3336 << endr
    << 0.2256 << 0.3144 << -0.8695 << endr;

  y <<  1  <<  1  << -1  << endr
    <<  1  << -1  <<  1  << endr
    << -1  <<  1  <<  1  << endr
    <<  1  <<  1  <<  1  << endr;

  // Forward
  arma::mat loss_none = arma::log(1 + arma::exp(-y % x));
  double loss_sum = arma::sum(arma::sum(loss_none));
  double loss_mean = loss_sum / x.n_elem;

  // Backward
  arma::mat output ;
  output.set_size(size(x));
  arma::mat numerator = -y % arma::exp(-y % x);
  arma::mat denominator = 1 + arma::exp(-y % x);
  output = numerator / denominator;

  // Display
  cout << "------------------------------------------------------------------" << endl;
  cout << "USER-PROVIDED MATRICES : " << endl;
  cout << "------------------------------------------------------------------" << endl;
  cout << "Input shape : "<< x.n_rows << " " << x.n_cols << endl;
  cout << "Input : " << endl << x << endl;
  cout << "Target shape : "<< y.n_rows << " " << y.n_cols << endl;
  cout << "Target : " << endl << y << endl;
  cout << "------------------------------------------------------------------" << endl;
  cout << "SUM " << endl;
  cout << "------------------------------------------------------------------" << endl;
  cout << "FORWARD : " << endl;
  cout << "Loss : \n" << loss_none << '\n';
  cout << "Loss (sum):\n" << loss_sum << '\n';
  cout << "BACKWARD : " << endl;
  cout << "Output shape : "<< output.n_rows << " " << output.n_cols << endl;
  cout << "Output (sum) : " << endl << output << endl;
  cout << "Sum of all values in this matrix : " << arma::as_scalar(arma::accu(output)) << endl;
  cout << "------------------------------------------------------------------" << endl;
  cout << "MEAN " << endl;
  cout << "------------------------------------------------------------------" << endl;
  cout << "FORWARD : " << endl;
  cout << "Loss (mean):\n" << loss_mean << '\n';
  cout << "BACKWARD : " << endl;
  cout << "Output shape : "<< output.n_rows << " " << output.n_cols << endl;
  cout << "Output (mean) : " << endl << output / x.n_elem << endl;
  cout << "Sum of all values in this matrix : " << arma::as_scalar(arma::accu(output / x.n_elem)) << endl;
  cout << "------------------------------------------------------------------" << endl;
  return 0;
}


#include <iostream>
#include <cmath>
#include <chrono>
#include <fstream>

#include <Eigen/Dense>

struct Domain {
    constexpr Domain(double start, double end) : x_start(start), x_end(end) {}
    double x_start;
    double x_end;
};

template <typename Derived>
void init_solution(Eigen::ArrayBase<Derived>& V, const Domain& domain) {
    const std::size_t nx = V.cols();
    const double hx = (domain.x_end - domain.x_start) / nx;
    const double xc = 0.5 * (domain.x_start + domain.x_end) + (nx / 8.0) * hx;

    for(std::size_t i=0; i < nx; ++i) {
        double x = domain.x_start + (i - 0.5) * hx;
        V(0, i) = std::abs(x - xc) < 0.2 ? 1. : 0.;
        V(1, i) = 0.;
    }
}

void compute_flux(Eigen::Array<double, 2, 1>& flux, double V1, double V2, double tol)
{
    flux(0) = V2;
    if(std::abs(V1) < tol)
        flux(1) = 0.;
    else
        flux(1) = V2 * V2 / V1 + 0.5 * 9.81 * V1 * V1;
}

template<typename ArrayType1d, typename ArrayType2d>
void scheme_LaxFriedrich(ArrayType2d& V, const ArrayType2d& Vold,
                         const ArrayType1d& lambdas,
                         double dt, double dx, double tol)
{
    std::size_t Nx = lambdas.cols();
    double Cx = dt / dx;

    Eigen::Array<double, 2, 1> flux, flux1, flux2;
    compute_flux(flux1, Vold(0, 0), Vold(1, 0), tol);

    V(1, 0) += Cx * (flux1(1) - lambdas(0) * Vold(1, 0));
    for(std::size_t i=0; i<Nx-1; ++i) {
        compute_flux(flux2, Vold(0, i+1), Vold(1, i+1), tol);
        flux = 0.5 * Cx * ((flux2 + flux1) - std::max(lambdas(i), lambdas(i+1)) * (Vold.col(i+1) - Vold.col(i)));
        V.col(i) -= flux;
        V.col(i+1) += flux;
        flux1.swap(flux2);
    }
    V(1, Nx-1) -= Cx * (flux1(1) + lambdas(Nx-1) * Vold(1, Nx-1));
}

template<typename ArrayType1d, typename ArrayType2d>
double updateCFL(ArrayType1d& lambdas, const ArrayType2d& V,
                 double dx, double tol)
{
    std::size_t Nx = lambdas.cols();
    double M = 0.;

    for(std::size_t i=0; i<Nx; ++i) {
        if(std::abs(V(0, i)) < tol) {
            lambdas(i) = 0.;
        }
        else {
            lambdas(i) = std::abs(V(1, i) / V(0, i)) + std::sqrt(9.81 * V(0, i));
            M = std::max(M, lambdas(i));
        }
    }

    return 0.5 * dx / M;
}

template<typename ArrayType1d, typename ArrayType2d>
double update_to_time(ArrayType2d& V, ArrayType2d& Vold, ArrayType1d& lambdas,
                      double t, double tframe, double dx, double tol)

{
    while(t < tframe) {
        double dt = std::min(updateCFL(lambdas, V, dx, tol),
                             tframe - t);
        Vold = V;
        scheme_LaxFriedrich(V, Vold, lambdas, dt, dx, tol);

        t += dt;
    }

    return t;
}

int main() {
    constexpr Domain domain(0., 1.);
    constexpr std::size_t Nx = 16384;
    constexpr double T = 2.0;
    constexpr double dx = (domain.x_end - domain.x_start) / Nx;
    constexpr double tol = 1e-15;

    Eigen::Array<double, 2, Eigen::Dynamic> V(2, Nx);
    init_solution(V, domain);

    Eigen::Array<double, 2, Eigen::Dynamic> Vold(2, Nx);
    Eigen::Array<double, 1, Eigen::Dynamic> lambdas(Nx);

    double t = 0;
    std::cout << "Initial time t = " << t << std::endl;

    auto time_start = std::chrono::high_resolution_clock::now();
    t = update_to_time(V, Vold, lambdas, t, T, dx, tol);
    auto time_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_seconds = time_end - time_start;

    std::cout << "Elapsed time: " << elapsed_seconds.count() << "s" << std::endl;
    std::cout << "End of simulation, t = " << t << std::endl;

    std::cout << "mean(h) = " << V.row(0).mean() << std::endl;

    const Eigen::IOFormat to_file(Eigen::FullPrecision, Eigen::DontAlignCols, "\n", "\n");
    std::ofstream file("sol-cpu");
    file << V.row(0).format(to_file);

    return 0;
}


//#include <vcp/imats.hpp>
//#include <vcp/matrix.hpp>
//#include <vcp/matrix_assist.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
#include "kv/interval.hpp"
// define rounding operations for double
// if "rdouble.hpp" is not included, result of computation is not "verified"
#include "kv/rdouble.hpp"
typedef kv::interval<double> itv;
namespace ub = boost::numeric::ublas;
itv factorial(int k){
    itv sum;
    sum=1.;
    for (int i = 1; i <= k; ++i)
    {
        sum *= i;
    }
    return sum;
}
int main(void){
std::cout.precision(17);
  int n=2,M=42;
  /*  vcp::matrix< itv, vcp::imats< double > > A,B,res,sum;
  sum.zeros(n);
  A(0,0)=1.;
  A(0,1)=-3.;
  A(1,0)=-2.;
  A(1,1)=5.;
  for(int N=0;N<=M;N++){
    sum=sum*A+1/factorial(N);
  }
  norm=max(abs(A));
  error=exp(norm)*pow(norm,M+1)/factorial(M+1);
  b=max(abs(error));
  B.ones(n);
  B=B*itv(-b,b);
  */
  // use ublas
  ub::matrix< itv > A(n, n),B(n, n),res(n, n),sum(n, n),pro(n,n);
  itv error,norm;
  double b;
  for(int i=0;i<n;i++){
    for(int j=0;j<n;j++){
      sum(i,j)=0.;
      if(i==j)pro(i,j)=1.;
      else pro(i,j)=0.;
    }
  }
	A(0, 0)=1.; A(0, 1)=-3.;
	A(1, 0)=-2; A(1, 1)=5.;
  for(int N=0;N<=M;N++){
   sum+=(1/factorial(N))*pro;
   pro=prod(pro,A);
  }
  //fprintf( stderr, "Check\n" );
  //norm=max(abs(A));
  norm=abs(A(0,0));
  for(int i1=0;i1<n;i1++){
    for(int j1=0;j1<n;j1++){
      if (A(i1,j1)>abs(norm)) norm=abs(A(i1,j1));
    }
  }

  error=exp(norm)*pow(norm,M+1)/factorial(M+1);
  b=(abs(error)).upper();

  for(int k1=0;k1<n;k1++){
    for(int l1=0;l1<n;l1++){
      B(k1,l1).assign(-1.,1.);
      B(k1,l1)=b*B(k1,l1);
    }
  }
  res=sum+B;

  std::cout<<res<<std::endl;
  return 0;
}


#include "LinearRegression.h"
#include "LogisticRegression.h"
#include <armadillo>
#include <assert.h>
#include <iostream>
using namespace std;
using namespace arma;

void testFailed() {
  mat x = mat("1 0 0; 0 0 1");
  vec y = vec("0 1");
  LinearRegression *A = new LinearRegression(x, y);
  A->Train(normalEquation);
  vec p = vec("1 0 0");
}

void testSucc() {
  mat x = mat("3 0; 0 4;2 5; 5 2");
  vec y = vec("3 8 12 9 ");
  LinearRegression *A = new LinearRegression(x, y);
  A->Train(normalEquation);
  vec p = vec("3 0");
  std::cout << A->Predict(p) << std::endl;
  vec q = vec("0 8");
  std::cout << A->Predict(q) << std::endl;
  mat ex = mat("0 0;");
  vec ey = vec("0");

  A->AddData(ex, ey);
  A->Train(normalEquation);
  std::cout << A->Predict(q) << std::endl;
}

void testSucc2() {
  mat x = mat("3 0; 0 4;2 5;0 0; 5 2; 7 0; 0 8");
  vec y = vec("3 8 12 0 9 7 16");
  LinearRegression *A = new LinearRegression(x, y);
  A->Train(gradientDescent, 0.1, 10000);
  vec p = vec("3 0");
  std::cout << A->Predict(p) << std::endl;
  vec q = vec("0 8");
  std::cout << A->Predict(q) << std::endl;
  mat ex = mat("0 0;");
  vec ey = vec("0");

  A->AddData(ex, ey);
  A->Train(gradientDescent, 0.01, 1000);
  std::cout << A->Predict(q) << std::endl;
  std::cout << A->SelfCost() << std::endl;
}

void test3() {
  mat x = mat("2 2; 3 3; 4 4; -1 2; -10 8; 8 9; -4 -4;1 -3; 8 1; 3 -4; 1 -1; "
              "-1 1; 4 -6");
  vec y = vec("1 1 1 0 0 1 0 0 1 0 0 0 0 ");
  LogisticRegression *A = new LogisticRegression(x, y);
  A->Train(gradientDescent, 0.1, 100000);

  vec p = vec("3 0");
  vec q = vec("5 -5");
  vec f = vec("3 7");
  std::cout << A->Predict(p) << std::endl;
  std::cout << A->Predict(q) << std::endl;
  std::cout << A->Predict(f) << std::endl;
  std::cout << A->SelfCost() << std::endl;
}

int main() {
  testFailed();
  testSucc();
  testSucc2();
  test3();
}


#include <stdio.h>
#include <iostream>
#include <vector>
#include <deque>
#include <map>
#include <algorithm>
#include <limits>
#include <assert.h>
#include <sstream>
#include <boost/foreach.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
using namespace boost::multiprecision;

#include "input.h"
#include <input.hpp>
#include <assert.hpp>

// https://rosettacode.org/wiki/Modular_inverse#C.2B.2B
int256_t modinv(int256_t a, int256_t b) {
  int256_t b0 = b, t, q;
  int256_t x0 = 0, x1 = 1;
  if (b == 1) return 1;
  while (a > 1) {
    q = a / b;
    t = b, b = a % b, a = t;
    t = x0, x0 = x1 - q * x0, x1 = t;
  }
  if (x1 < 0) x1 += b0;
  return x1;
}

enum ShuffleKind { Deal = 0, Cut = 1, DealInc = 2 };

struct Params {
  int256_t a;
  int256_t b;
};

class Shuffle {
public:
  ShuffleKind kind;
  int256_t num;
  Shuffle(string s) {
    if (s.rfind("deal into", 0) == 0) {
      kind = Deal;
      num = 0;
      return;
    }
    num = stoi(s.substr(s.rfind(" ")+1));
    if (s.rfind("cut", 0) == 0) {
      kind = Cut;
    } else {
      kind = DealInc;
    }
  }
  Params params(int256_t a, int256_t b, int256_t num_cards) {
    int256_t na = 0;
    int256_t nb = 0;
    switch (this->kind) {
    case Deal:
      na = -a;
      nb = -b-1;
      break;
    case Cut:
      na = a;
      nb = b - this->num;
      break;
    default:
      na = a * this->num;
      nb = b * this->num;
    }
    return Params{(na+num_cards) % num_cards, (nb+num_cards) % num_cards};
  }
  Params reverse_params(int256_t a, int256_t b, int256_t num_cards) {
    int256_t na = 0;
    int256_t nb = 0;
    switch (this->kind) {
    case Deal:
      na = -a;
      nb = -b-1;
      break;
    case Cut:
      na = a;
      nb = b + this->num;
      break;
    default:
      int256_t m = modinv(this->num, num_cards);
      na = a * m;
      nb = b * m;
    }
    return Params{(na+num_cards) % num_cards, (nb+num_cards) % num_cards};
  }
};

int256_t modpow(int256_t b, int256_t e, int256_t m) {
  int256_t res = 1;
  while (e > 0) {
    if ((e%2) == 1) {
      res = (res * b) % m;
    }
    e = e / 2;
    b = (b * b) % m;
  }
  return res;
}

class Game {
  vector<Shuffle> shuffles;
  int256_t cards;
public:
  Game(vector<string> &inp, int256_t num_cards) {
    for (auto l : inp) {
      shuffles.push_back(Shuffle(l));
    }
    cards = num_cards;
  }
  Params params() {
    int256_t a = 1;
    int256_t b = 0;
    for (auto s : this->shuffles) {
      Params p = s.params(a, b, this->cards);
      a = p.a;
      b = p.b;
    }
    return Params{a, b};
  }
  int256_t forward(int256_t card) {
    Params p = this->params();
    return (((p.a * card) + p.b) % this->cards);
  }
  Params reverse_params() {
    int256_t a = 1;
    int256_t b = 0;
    BOOST_REVERSE_FOREACH(auto s, this->shuffles) {
      Params p = s.reverse_params(a, b, this->cards);
      a = p.a;
      b = p.b;
    }
    return Params{a, b};
  }
  int256_t backward(int256_t card, int256_t rounds) {
    Params p = this->reverse_params();
    int256_t exp = modpow(p.a, rounds, this->cards);
    int256_t im = modinv(p.a-1, this->cards);
    return ((((exp-1) * im) * p.b) + (exp * card)) % this->cards;
  }
};

void tests() {
}

void run(unsigned int inp_len, unsigned char* inp, bool is_bench) {
  vector<string> ln = lines(inp_len, inp);
  Game* game = new Game(ln, 10007);
  auto p1 = game->forward(2019);
  game = new Game(ln, 119315717514047);
  int256_t rounds = 101741582076661;
  auto p2 = game->backward(2020, rounds);
  if (!is_bench) {
    cout << "Part 1: " << p1 << "\n";
    cout << "Part 2: " << p2 << "\n";
  }
}

int main(int argc, char **argv) {
  if (is_test()) { tests(); }
  benchme(argc, argv, input_txt_len, input_txt, run);
}


#include <iostream>
#include <cmath>

#include <Eigen/Core>

#include "vice/integrate.h"
#include "vice/functors.h"

int main () {
  double texp = 0.1;

  //double depth    = 0.1;
  //double duration = 0.1;
  //double ingress  = 0.2;
  //vice::functors::TrapFunctor<double> func(depth, duration, ingress);

  double tau = 0.5;
  double r = 0.1;
  double b = 0.5;
  Eigen::VectorXd u(2);
  u << 0.3, 0.2;
  vice::functors::StarryFunctor<double> func(u, r, b, tau);

  //double tmin = -duration - 2*ingress;
  //double tmax =  duration + 2*ingress;
  double tmin = -tau - texp;
  double tmax =  tau + texp;
  int npts = 500;
  double dt = (tmax - tmin) / (npts - 1);

  int neval = 15;
  double tol = 1e-9;

  std::cout << "time,flux,truth,reimann,n_reimann,trap_fixed,n_trap_fixed,simp_fixed,n_simp_fixed,trap_adapt,n_trap_adapt,simp_adapt,n_simp_adapt,\n";
  std::cout.precision(25);
  std::cout << std::scientific;

  for (int i = 0; i < npts; ++i) {
    double t = tmin + i * dt;
    double lower = t - 0.5*texp;
    double upper = t + 0.5*texp;

    func.reset();
    double flux = func(t);
    double truth = func.integrate(lower, upper) / texp;

    std::cout << t << "," << flux << "," << truth << ",";

    func.reset();
    double f_reimann = vice::reimann::integrate_fixed(func, lower, upper, neval) / texp;
    std::cout << f_reimann << "," << func.get_n_eval() << ",";

    func.reset();
    double f_trap_fixed = vice::trapezoid::integrate_fixed(func, lower, upper, neval) / texp;
    std::cout << f_trap_fixed << "," << func.get_n_eval() << ",";

    func.reset();
    double f_simp_fixed = vice::simpson::integrate_fixed(func, lower, upper, neval) / texp;
    std::cout << f_simp_fixed << "," << func.get_n_eval() << ",";

    func.reset();
    double f_trap_adapt = vice::trapezoid::integrate_adapt(func, lower, upper, tol, 50) / texp;
    std::cout << f_trap_adapt << "," << func.get_n_eval() << ",";

    func.reset();
    double f_simp_adapt = vice::simpson::integrate_adapt(func, lower, upper, tol, 50) / texp;
    std::cout << f_simp_adapt << "," << func.get_n_eval() << ",";

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


#include <Eigen/Dense>
#include <iostream>
#include <cstdio>
#include <string>
#include "nn.h"
#include <fstream>
#include <vector>
#include <sstream>

using namespace std;

int main(int argc, const char *argv[])
{
	string filename(argv[1]);
	ifstream fin;
	string directory("../Datasets/");
	string filepath = directory + filename;
	fin.open(filepath);

	string str;
	int n_input = 0;  // input dimensionality
	int n_output = 0; // output dimensionality
	int n_sample = 0; // number of training samples

	while (fin >> str)
	{
		n_sample++;
	}

	stringstream sst(str);
	double each_input;
	while (sst >> each_input)
	{
		n_input++;
		if (sst.peek() == ',')
			sst.ignore();
	}
	n_output = atoi(argv[2]);
	n_input = n_input - n_output;
	//cout << n_input << "  " << n_output;

	int n_layer = 3;		  // number of layers
	int max_steps = 5;	  // number of optimization steps
	int n_epoch = max_steps;
	double lambda = 0.000001; // regularization parameter

	matrix_t X(n_sample, n_input);
	matrix_t Y(n_sample, n_output);

	fin.clear();
	fin.seekg(0, ios::beg);
	int row = 0;
	while (fin >> str)
	{
		int col = 0;
		double data;
		stringstream ss(str);
		while (ss >> data)
		{
			if (col < n_input)
			{
				X(row, col) = data;
			}
			else
			{
				int pos = col - n_input;
				Y(row, pos) = data;
			}
			if (ss.peek() == ',')
			{
				ss.ignore();
			}
			col++;
		}
		row++;
	}
	fin.close();

	// cout << "training input: " << endl
	// 	 << X << endl;
	// cout << "training output: " << endl
	// 	 << Y << endl;

	// specify network topology
	Eigen::VectorXi topo(n_layer);
	topo << n_input, n_input, n_output;
	// cout << "topology: " << endl
	// 	 << topo << endl;

	// initialize a neural network with given topology
	NeuralNet nn(topo);

	nn.autoscale(X, Y);
	// train the network
	std::cout << "starting training" << endl;
	double err;
	double prevAccuracy = 0;
	double accuracy;
	for (int i = 0; i < max_steps; ++i)
	{
		err = nn.loss(X, Y, lambda);
		nn.rprop();
		//nn.forward_pass(X);
		matrix_t Y_prime = nn.get_activation();
		accuracy = nn.measure_accuracy(Y, Y_prime);
		//cout << "Accuracys: " << accuracy << "  Step No: " << i<<endl;
		cout << err << endl;
		if (accuracy - prevAccuracy < .001 && accuracy>90)
		{
			n_epoch = i;
			break;
		}
		else
		{
			prevAccuracy = accuracy;
		}
		//printf("%3i   %4.4f\n", i, err);
	}

	// write model to disk
	nn.write("example.nn");

	// read model from disk
	NeuralNet nn2("example.nn");

	// testing
	nn2.forward_pass(X);
	matrix_t Y_test = nn2.get_activation();

	//std::cout << "test input:" << endl
	//  << X << endl;
	//std::cout << "test output:" << endl
	//  << Y_test << endl;
	int correct = 0;
	for (int i = 0; i < n_sample; i++)
	{
		int j = 0;
		for (; j < n_output; j++)
		{
			int out = Y_test(i, j) > 0.7 ? 1 : 0;
			//printf("%3i   %4.4f", (int)Y(i, j), Y_test(i, j));
			if (out != Y(i, j))
				break;
		}
		//cout << endl;
		if (j == n_output)
			correct++;
	}
	cout<< "Dataset: " << filename << endl;
	cout << "Correct answer " << correct << " among " << n_sample << " test" << endl;
	cout << "Accuracy: " << (double)correct / (double)n_sample * 100 << "%" << endl;
	cout << "Step Count: " << n_epoch << endl;
	return 0;
}


#include <iostream>
#include <cmath>
#include <random>
#include <Eigen/Dense>
#include <boost/range/irange.hpp>
#include <unsupported/Eigen/FFT>
#include "utils.h"

// Define aliases with using declarations
using Matrix = utils::Matrix;
using Vector = utils::Vector;
using VectorXcd = utils::VectorXcd;

// Construct Toeplitz matrix from column (symmetric)
Matrix utils::buildToep(const Vector & col, const Vector & row)
{
  auto n = static_cast<int>(col.rows());
  Matrix toep(n,n);
  for (auto i : boost::irange(0,n))
    {
      toep.diagonal(i) = Eigen::VectorXd::Ones(n-i)*row[i];
      toep.diagonal(-i) = Eigen::VectorXd::Ones(n-i)*col[i];
    }
  return toep;
};

// Construct Toeplitz matrix from column (symmetric)
Matrix utils::buildToep(const Vector & col) { return buildToep(col, col); };

// Embed Toeplitz matrix into column of circulant matrix (general)
void utils::embedToep(Vector & toepCol, Vector & toepRow, VectorXcd & eigVals)
{
  // Specify first column of circulant matrix embedding
  auto n = static_cast<int>(toepCol.rows());
  Vector embedCol(2*n);
  embedCol.head(n) = toepCol;
  embedCol.tail(n-1) = toepRow.tail(n-1).reverse();

  // Compute eigenvalues of cirulant matrix
  Eigen::FFT<double> fft;
  fft.fwd(eigVals, embedCol);
};

// Embed Toeplitz matrix into column of circulant matrix (symmetric)
void utils::embedToep(Vector & toepCol, VectorXcd & eigVals) { embedToep(toepCol, toepCol, eigVals); };

// Define matvec product for circular matrix using FFT (non-symmetric / complex eigVals)
void utils::circMatVec(const VectorXcd & eigVals, Vector & x, Vector & result)
{
  VectorXcd freqvec;
  Eigen::FFT<double> fft;
  fft.fwd(freqvec,x);
  freqvec = eigVals.cwiseProduct(freqvec);
  fft.inv(result,freqvec);
};

// Define matvec product for Toeplitz matrix using FFT (non-symmetric)
Vector utils::toepMatVec(Vector & col, Vector & row, Vector & x, bool sym)
{
  auto n = static_cast<int>(col.rows());
  Vector xembed = Eigen::VectorXd::Zero(2*n);
  xembed.head(n) = x;
  VectorXcd eigVals(2*n);
  Vector result(2*n);
  embedToep(col, row, eigVals);

  // Note: Should be possible to optimize this better using
  // the fact that eigVals is real in the symmetric case
  if (sym)
      circMatVec(eigVals.real(), xembed, result);
  else
      circMatVec(eigVals, xembed, result);

  return result.head(n);

};

// Define matvec product for Toeplitz matrix using FFT (symmetric)
Vector utils::toepMatVec(Vector & col, Vector & x) { return toepMatVec(col, col, x, true); };

// NOTE: the "fast" version avoids extra function calls and
// is marginally faster than the more pedagogical "toepMatVec"

// Define matvec product for Toeplitz matrix using FFT (non-symmetric)
Vector utils::fastToepMatVec(Vector & toepCol, Vector & toepRow, Vector & x)
{
  auto n = static_cast<int>(toepCol.rows());
  Vector result(2*n);

  // Embed RHS into 2n-vector with trailing zeros
  Vector xembed = Eigen::VectorXd::Zero(2*n);
  xembed.head(n) = x;

  // Specify first column of circulant matrix embedding
  Vector embedCol(2*n);
  embedCol.head(n) = toepCol;
  embedCol.tail(n-1) = toepRow.tail(n-1).reverse();

  // Compute eigenvalues of cirulant matrix
  Eigen::FFT<double> fft;
  VectorXcd eigVals(2*n), freqvec(2*n);
  fft.fwd(eigVals, embedCol);

  // Apply component-wise multiplication in frequency space
  fft.fwd(freqvec, xembed);
  freqvec = eigVals.cwiseProduct(freqvec);
  fft.inv(result,freqvec);

  // Return first n values corresponding to the original system
  return result.head(n);

};

// Define matvec product for Toeplitz matrix using FFT (symmetric)
Vector utils::fastToepMatVec(Vector & col, Vector & x) { return fastToepMatVec(col, col, x); };

// Define matvec product for Toeplitz matrix using FFT (non-symmetric)
Matrix utils::fastToepMatMat(Vector & toepCol, Vector & toepRow, Matrix & X)
{
  auto n = static_cast<int>(toepCol.rows());
  auto m = static_cast<int>(X.cols());
  Matrix result(n,m);
  for (auto i : boost::irange(0,m))
    {
      Vector X_i = X.col(i);
      result.col(i) = fastToepMatVec(toepCol, toepRow, X_i);
    }
  return result;
}

// Define matvec product for Toeplitz matrix using FFT (non-symmetric)
Matrix utils::fastToepMatMat(Vector & toepCol, Matrix & X) { return fastToepMatMat(toepCol, toepCol, X); };

// Compute Lanczos vectors for matrix-vector pair (A,v)
void utils::Lanczos(Matrix & A, Vector & v, Matrix & Q, Matrix & T, int N)
{
  // Use full decomposition if step count N is not specified
  auto n = static_cast<int>(A.rows());
  if (N == 0)
    N = n;

  // Initialize the algorithm's vectors and parameters
  Vector q = v/v.norm();
  auto alpha = static_cast<double>( (A*q).dot(q) );
  Vector r = A*q - alpha*q;
  Q.col(0) = q;
  T(0,0) = alpha;
  Vector u;

  // Define orthogonality and vector norm tolerances
  //double epsilon = 1.0e-15;
  //double eta = std::pow(epsilon, 0.75);
  double tolerance = 0.00001;

  for (auto j : boost::irange(1,N))
    {
      auto beta = static_cast<double>(r.norm());
      if (beta < tolerance)
        {
          std::cout << "\nStopped Early\n";
          Q.resize(n,j);
          break;
        }

      // Basic implementation
      //v_k = vt_k/beta;
      //alpha = v_k.transpose().dot(A*v_k);
      //vt_k = A*v_k - alpha*v_k - beta*Q.col(k-1);

      // Improved stability implementation
      q = r/beta;
      u = A*q - beta*Q.col(j-1);
      alpha = q.transpose().dot(u);
      r = u - alpha*q;

      // Complete re-orthogonalization [ roughly ~ O(n^3) for combined iterations ]
      r = r - Q*(Q.transpose()*r);

      /*
      // Check if additional re-orthogonalization is necessary
      for (auto k : boost::irange(0,j))
        {
          if ( std::abs((r/r.norm()).dot(Q.col(k))) > eta )
            {
              //std::cout << "Reorthogonalizing..." << std::endl;
              r = r/r.norm();
              r = r - Q*(Q.transpose()*r);
              break;
            }
        }
      */

      // Assign Lanczos vector and tridiagonal entries
      Q.col(j) = q;
      T(j,j) = alpha;
      T(j,j-1) = T(j-1,j) = beta;

    }

};


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <libv/lma/time/tictoc.hpp>
template<class T> using AlignedVector = std::vector<T,Eigen::aligned_allocator<T>>;

/*
  dense
  crs
  bcrs
  sbcrs
*/

Eigen::SparseMatrix<double> dense_to_sparse(const Eigen::MatrixXd& m)
{
  typedef Eigen::Triplet<double> Triplet;

  std::vector<Triplet> tripletList;

  for(int i = 0 ; i < m.rows() ; ++i)
    for(int j = 0 ; j < m.cols() ; ++j)
      if (m(i,j)!=0)
        tripletList.emplace_back(i,j,m(i,j));

  Eigen::SparseMatrix<double> s(m.rows(),m.cols());
  s.setFromTriplets(tripletList.begin(), tripletList.end());
  return s;
}

struct Dense
{
  Eigen::MatrixXd m;
  Eigen::VectorXd r,v;

  Dense(int rows, int cols, int block_size):m(rows,cols),r(rows),v(rows)
  {
    for(int i = 0 ; i < rows/block_size ; ++i)
      m.block(i*block_size,i*block_size,block_size,block_size) = Eigen::MatrixXd::Random(block_size,block_size);
    v = v.Random(rows);
  }
};

struct Sparse
{
  Eigen::SparseMatrix<double> m;
  Eigen::SparseVector<double> r,v;

  Sparse(const Dense& dense)
  {
     m = dense_to_sparse(dense.m);
     v = dense_to_sparse(dense.v);
  }
};

template<class Storage> void compute(Storage& st)
{
  st.r = st.m * st.v;
}

template<int BlockSize> struct Blocks
{
  AlignedVector<Eigen::Matrix<double,BlockSize,BlockSize>> m;
  AlignedVector<Eigen::Matrix<double,BlockSize,1>> r,v;

  Blocks(const Dense& dense)
  {
    for(int i = 0 ; i < dense.m.rows()/BlockSize ; ++i)
    {
      m.push_back(dense.m.block<BlockSize,BlockSize>(i,i));
      v.push_back(dense.v.block<BlockSize,1>(i,0));
    }
    r.resize(v.size());
  }

  void eval(int i)
  {
    r[i] = m[i] * v[i];
  }
};

template<int BlockSize> void compute(Blocks<BlockSize>& blocks)
{
  for(size_t i = 0 ; i < blocks.r.size(); ++i)
  {
    blocks.eval(i);
  }
}

int main()
{
  size_t nb_block = 1000;
  static const size_t BlockSize = 6;
  size_t rows = nb_block*BlockSize, cols = nb_block*BlockSize;

  Dense dense(rows,cols,BlockSize);
  Sparse sparse(dense);
  Blocks<BlockSize> blocks(dense);

  utils::Tic<true> ticd("dense");
  compute(dense);
  ticd.disp();

  utils::Tic<true> tics("sparse");
  compute(sparse);
  tics.disp();

  utils::Tic<true> ticb("blocks");
  compute(blocks);
  ticb.disp();

  // std::cout << m << std::endl;
  // std::cout << std::endl;
  // std::cout << v.transpose() << std::endl;

  //r = s * v;

  return 0;
}


#include <string>
#include <sstream>
#include <iostream>
#include <map>
#include <regex>
#include <fstream>
#include <boost/algorithm/string.hpp>

using namespace std;

int fuel(int mass) {
    return int(floor(mass / 3.0)) - 2;
}

int improved_fuel(int mass) {
    int f = fuel(mass);
    if (f > 0) {
        // Calculate the additional fuel required for this additional fuel
        return f + improved_fuel(f);
    }
    return 0;
}

int main() {
    ifstream file ("2019/1.txt");
    if (!file.is_open()) {
        cout << "Failed to open file: " << strerror(errno) << endl;
        return -1;
    }

    int answer1 = 0;
    int answer2 = 0;

    string line;
    while (getline(file, line, '\n')) {
        boost::trim(line);

        if (line == "") {
            continue;
        }

        int mass = stoi(line);
        answer1 += fuel(mass);
        answer2 += improved_fuel(mass);
    }

    /*
    cout << improved_fuel(12) << endl;
    cout << improved_fuel(14) << endl;
    cout << improved_fuel(1969) << endl;
    cout << improved_fuel(100756) << endl;
    */

    cout << "Answer 1.1: " << answer1 << endl;
    cout << "Answer 1.2: " << answer2 << endl;

    file.close();
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/matrix/determinant.hpp>
#include <fcppt/math/matrix/identity.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_determinant
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		int,
		3,
		3
	>
	large_matrix_type;

	typedef
	fcppt::math::matrix::static_<
		int,
		2,
		2
	>
	small_matrix_type;

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::determinant(
			large_matrix_type(
				fcppt::math::matrix::row(
					1,2,3
				),
				fcppt::math::matrix::row(
					4,5,6
				),
				fcppt::math::matrix::row(
					7,8,9
				)
			)
		),
		0
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::determinant(
			small_matrix_type(
				fcppt::math::matrix::row(
					-3,-5
				),
				fcppt::math::matrix::row(
					-1,-2
				)
			)
		),
		1
	);

	BOOST_CHECK_EQUAL(
		fcppt::math::matrix::determinant(
			fcppt::math::matrix::identity<
				large_matrix_type
			>()
		),
		1
	);
}


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaQ, Fvar) {
  using boost::math::gamma_q;
  using stan::math::fvar;
  using stan::math::gamma_q;

  fvar<double> x(0.5);
  x.d_ = 1.0;
  fvar<double> y(1.0);
  y.d_ = 1.0;

  fvar<double> a = gamma_q(x, y);
  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(0.18228334, a.d_);

  double z = 1.0;
  double w = 0.5;

  a = gamma_q(x, z);
  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(0.38983709, a.d_);

  a = gamma_q(w, y);
  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_);

  EXPECT_THROW(gamma_q(-x, y), std::domain_error);
  EXPECT_THROW(gamma_q(x, -y), std::domain_error);
}

TEST(AgradFwdGammaQ, FvarFvarDouble) {
  using boost::math::gamma_q;
  using stan::math::fvar;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = gamma_q(x, y);

  EXPECT_FLOAT_EQ(gamma_q(0.5, 1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(0.38983709, a.val_.d_);
  EXPECT_FLOAT_EQ(-boost::math::gamma_p_derivative(0.5, 1.0), a.d_.val_);
  EXPECT_FLOAT_EQ(-0.40753385, a.d_.d_);
}

struct gamma_q_fun {
  template <typename T0, typename T1>
  inline typename boost::math::tools::promote_args<T0, T1>::type operator()(
      const T0 arg1, const T1 arg2) const {
    return gamma_q(arg1, arg2);
  }
};

TEST(AgradFwdGammaQ, nan_0) {
  gamma_q_fun gamma_q_;
  test_nan_fwd(gamma_q_, 3.0, 5.0, false);
}


#include <gtest/gtest.h>

#include "../IntegralImage/integralimage.hh"
#include <Eigen/Core>

using namespace bold;
using namespace std;
using namespace Eigen;

TEST (IntegralImageTests, create)
{
  int rows = 10;
  int cols = 12;
  cv::Mat image(cv::Size(cols, rows), CV_8UC1);

  image = cv::Scalar(1);

  for (int x = 0; x < cols; x++)
  {
    for (int y = 0; y < rows; y++)
      ASSERT_EQ(1, image.at<uchar>(y, x));
  }

  IntegralImage integral = IntegralImage::create(image);

  // 1 1 1 1
  // 1 1 1 1
  // 1 1 1 1

  // 1 2 3 4
  // 2 4 6 8
  // 3 6 9 12

  for (int y = 0; y < rows; y++)
  {
    for (int x = 0; x < cols; x++)
      ASSERT_EQ((x+1) * (y+1), integral.at(x, y)) << "Failed when x=" << x << ", y=" << y;
  }

  EXPECT_EQ ( 5*3, integral.getSummedArea(Vector2i(2, 3), Vector2i(6, 5)) );
  EXPECT_EQ ( 1, integral.getSummedArea(Vector2i(2, 3), Vector2i(2, 3)) );
  EXPECT_EQ ( rows*cols, integral.getSummedArea(Vector2i(0, 0), Vector2i(cols-1, rows-1)) );
  EXPECT_EQ ( 2*2, integral.getSummedArea(Vector2i(0, 2), Vector2i(1, 3)) );
  EXPECT_EQ ( 2*2, integral.getSummedArea(Vector2i(2, 0), Vector2i(3, 1)) );
  EXPECT_EQ ( 4*2, integral.getSummedArea(Vector2i(0, 0), Vector2i(3, 1)) );
  EXPECT_EQ ( 2*4, integral.getSummedArea(Vector2i(0, 0), Vector2i(1, 3)) );
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  typedef Matrix<double, 5, 3> Matrix5x3;
typedef Matrix<double, 5, 5> Matrix5x5;
Matrix5x3 m = Matrix5x3::Random();
cout << "Here is the matrix m:" << endl << m << endl;
Eigen::FullPivLU<Matrix5x3> lu(m);
cout << "Here is, up to permutations, its LU decomposition matrix:"
     << endl << lu.matrixLU() << endl;
cout << "Here is the L part:" << endl;
Matrix5x5 l = Matrix5x5::Identity();
l.block<5,3>(0,0).triangularView<StrictlyLower>() = lu.matrixLU();
cout << l << endl;
cout << "Here is the U part:" << endl;
Matrix5x3 u = lu.matrixLU().triangularView<Upper>();
cout << u << endl;
cout << "Let us now reconstruct the original matrix m:" << endl;
cout << lu.permutationP().inverse() * l * u * lu.permutationQ().inverse() << endl;

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include "../simple_lib/include/simple_layer.h"

using namespace Eigen;
using namespace MyDL;

int main()
{
    using std::cout;
    using std::endl;

    MatrixXd x = MatrixXd::Zero(2, 2);
    MatrixXd y, dx;
    MatrixXd dout = MatrixXd::Ones(2, 2);
    x <<    1, -0.5,
         -2.0,  3.0;

    ReLU relu;
    y = relu.forward(x);
    cout << "forward: " << y << endl;
    dx = relu.backward(dout);
    cout << "backward: " << dx << endl;

    return 0;
}


#ifndef helpers_hpp
#define helpers_hpp

#include <vector>
#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/tools/minima.hpp>
#include <fstream>
#include <sys/stat.h>

const double PI = 3.14159265358979;

using namespace std;
using namespace boost::math;
using namespace boost::random;
using namespace boost::math::tools;

typedef vector<vector<double> > Table;
typedef vector<vector<vector<double> > > Table3D;

typedef boost::variate_generator<boost::mt19937&, boost::uniform_real<> > RandUnif;
typedef boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > RandNorm;
typedef boost::variate_generator<boost::mt19937&, boost::uniform_int<> > RandIntUnif;

class RandomSampleHelper
{
public:
  RandomSampleHelper(long seedval) : rng(seedval) {}
  double sampleBeta(double alpha, double beta)
  {
    boost::gamma_distribution<> xgamma(alpha);
    boost::gamma_distribution<> ygamma(beta);
    boost::variate_generator<boost::mt19937&, boost::gamma_distribution<> > xgen(rng, xgamma);
    boost::variate_generator<boost::mt19937&, boost::gamma_distribution<> > ygen(rng, ygamma);
    double xsample = xgen();
    double ysample = ygen();
    return xsample/(xsample+ysample);
  }

  double sampleGaussian(double mu, double sigma)
  {
    boost::normal_distribution<> normdist(mu, sigma);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > sgen(rng, normdist);
    return sgen();
  }
private:
  boost::mt19937 rng;
};

/* courtesy of http://stackoverflow.com/questions/12774207/fastest-way-to-check-if-a-file-exist-using-standard-c-c11-c */
inline bool exists_file(const std::string& name) {
  struct stat buffer;
  return (stat (name.c_str(), &buffer) == 0);
}

void loadFromCSV( const std::string& filename, std::vector< std::vector<double> >& matrix, const int maxrows, const int maxcols)
{
  if( !exists_file(filename) )
  {
    cerr << "cannot find required file " << filename << " to load." << endl;
    exit(1);
  }
  std::ifstream  file( filename.c_str() );
  std::vector<double> row;
  std::string line;
  std::string  cell;
  int rows = 0;
  while( file && rows < maxrows )
  {
    std::getline(file,line);
    std::stringstream lineStream(line);
    row.clear();
    int cols = 0;
    while( std::getline( lineStream, cell, ',' ) && cols < maxcols)
    {
      double number = strtod(cell.c_str(), NULL);
      row.push_back(number);
      ++cols;
    }

    if( !row.empty() )
    {
      matrix.push_back(row);
    }
    ++rows;
  }
}

void loadLineFromCSV( const std::string& filename, std::vector<double>& arrs, const int maxentries)
{
  std::ifstream       file( filename.c_str() );
  std::string line;
  std::string  cell;
  std::getline(file,line);
  std::stringstream lineStream(line);
  int idx = 0;
  while( std::getline( lineStream, cell, ',' ) && idx < maxentries)
  {
    double number = strtod(cell.c_str(), NULL);
    arrs[idx] = number;
    ++idx;
  }
}

double kl(double p1, double p2)
{
  return p1*log(p1/p2)+(1-p1)*log((1-p1)/(1-p2));
}

double maxkl(double n, double muh, double delta)
{
  double begin = muh + 1e-10;
  double end = 1.0-1e-10;
  double mid = (begin+end)/2.0;
  while (begin < end - 1e-10)
  {
    if (n*kl(muh, mid) > delta)
    {
      end = mid;
    }
    else
    {
      begin = mid;
    }
    mid = (begin + end)/2.0;
  }
  return mid;
}

const int NUM_AGI_ITERS = 9;

double computeAGI(const double gamma, const double a, const double nn)
{
  double p = double(a)/double(nn);
  const double b = nn - a;
  const double r = ((double)a/(double)nn);

  for(int k = 0; k < NUM_AGI_ITERS; ++k)
  {
    if((p > 1 || p < r || p != p))
    {
      cerr << "Found that p = " << p << ". invalid value" << endl;
      cerr << "a = " << a << endl;
      cerr << "b = " << b << endl;
      cerr << "gamma = " << gamma << endl;
      exit(1);
    }
    const double safe = p/(1-gamma);

    const double lhs = r*(1-ibeta(a+1,b,p));
    const double expmax = lhs + p*ibeta((double)a,b,p);
    const double risky = r+ gamma/(1-gamma)*expmax;

    const double orig = risky - safe;
    const double deriv = gamma/(1-gamma)*ibeta(a,b,p) - 1/(1-gamma);
    p -= orig/deriv;
  }
  return p;
}

double computeAGIGaussian(const double gamma, const double mu, const double sigma)
{
  double lam = mu;
  boost::math::normal_distribution<> normdist(mu,sigma);
  for(int k = 0; k < NUM_AGI_ITERS; ++k)
  {
    double plam = cdf(normdist, lam);
    double flam =  pdf(normdist, lam);
    double f = mu + gamma*sigma/(sqrt(2*PI))*exp(-pow(lam-mu,2)/(2*sigma*sigma)) + gamma*(lam-mu)*plam - lam;
    double fp = -gamma*(lam - mu)/(sqrt(2*PI)*sigma)*exp(-pow((lam-mu),2)/(2*sigma*sigma)) + gamma*plam + gamma*(lam-mu)*flam - 1;
    lam -= f/fp;
  }
  return lam;
}

double computeDummyProblemVal(double lambda, double gamma, double a, double b, int k, bool take_max = false)
{
  double xbar = a/(a+b);
  double result = 0.0;
  if (k == 1)
  {
    result += xbar;
    result += gamma/(1.0-gamma) * (xbar * (1.0 - ibeta(a+1, b, lambda)) + lambda*ibeta(a,b,lambda));
    return take_max ? max(result,lambda/(1.0 - gamma)) : result;
  }
  result += xbar * computeDummyProblemVal(lambda, gamma, a+1, b, k-1, true);
  result += (1.0 - xbar) * computeDummyProblemVal(lambda, gamma, a, b+1, k-1, true);
  result = xbar + gamma * result;
  return take_max ? max(result,lambda/(1.0 - gamma)) : result;
}

double computeApproxGI(double gamma, double a, double b, int k, double prec = 0.0001)
{
  if (k == 1)
  {
    return computeAGI(gamma, a, a + b);
  }
  double start = a/(a+b);
  for (double lambda = start; lambda < 1.0; lambda += prec)
  {
    double rightVal = computeDummyProblemVal(lambda, gamma, a, b, k, false);
    double leftVal = lambda / (1.0 - gamma);
    if (leftVal > rightVal)
    {
      return lambda - prec/2.0;
    }
  }
  return 1.0;
}

void computeIndices(const int a0, const int b0, const int n, const double step, const double beta, vector<vector<double> >& indices)
{
  vector<vector<double> > tmpindices(n-1, vector<double>(n-1, 0));
  for(int a = 1; a < n; ++a)
  {
    tmpindices[a-1][n-a-1] = computeAGI(beta, a0 + a - 1, n + a0 + b0 - 2);
  }
  for(double p=(step/2.0); p <= 1.0; p+=step)
  {
    const double safe = p/(1-beta);
    for(int nn = n-1; nn >= 2; --nn)
    {
      for(int a = 1; a <= nn-1; ++a)
      {
        const double r = (double(a - 1 + a0)/double(nn + a0 + b0 - 2));
        const double risky =  r*(1 + beta*tmpindices[a][nn-a-1]) + (1-r)*beta*tmpindices[a-1][nn-a];
        if(indices[a-1][nn-a-1] == 0 && safe > risky)
        {
          indices[a-1][nn-a-1] = p - step/2.0;
        }
        tmpindices[a-1][nn-a-1] = max(safe,risky);
      }
    }
  }
}

#endif


#include <cmath>
#include <jni.h>
#include <boost/math/special_functions/erf.hpp>

#ifndef _Included_simianquant_strata_jnibridge_AtomicFunction
#define _Included_simianquant_strata_jnibridge_AtomicFunction
#ifdef __cplusplus
extern "C" {
#endif

JNIEXPORT jdouble JNICALL Java_simianquant_strata_jnibridge_AtomicFunction_normalCdf
  (JNIEnv * env, jobject obj, jdouble arg){
  	return (0.5 * erfc(-arg * M_SQRT1_2));
  }

JNIEXPORT jdouble JNICALL Java_simianquant_strata_jnibridge_AtomicFunction_normalQuantile
  (JNIEnv * env, jobject obj, jdouble arg){
  	return -M_SQRT2 * boost::math::erfc_inv(2 * arg);
  }

#ifdef __cplusplus
}
#endif
#endif


/**
 * @date Fri Feb 10 20:02:07 2012 +0200
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <stdexcept>
#include <boost/format.hpp>

#include <bob.math/log.h>

/**
 * Computes log(a+b)=log(exp(log(a))+exp(log(b))) from log(a) and log(b),
 * while dealing with numerical issues
 */
double bob::math::Log::logAdd(double log_a, double log_b)
{
  if(log_a < log_b)
  {
    double tmp = log_a;
    log_a = log_b;
    log_b = tmp;
  }

  double minusdif = log_b - log_a;
  //#ifdef DEBUG
  if(std::isnan(minusdif))
  {
    boost::format m("logadd: minusdif (%f) log_b (%f) or log_a (%f) is nan");
    m % minusdif % log_b % log_a;
    throw std::runtime_error(m.str());
  }
  //#endif
  if(minusdif < MINUS_LOG_THRESHOLD) return log_a;
  else return log_a + log1p(exp(minusdif));
}

/**
 * Computes log(a-b)=log(exp(log(a))-exp(log(b))) from log(a) and log(b),
 * while dealing with numerical issues
 */
double bob::math::Log::logSub(double log_a, double log_b)
{
  double minusdif;

  if(log_a < log_b)
  {
    boost::format m("logsub: log_a (%f) should be greater than log_b(%f)");
    m % log_a % log_b;
    throw std::runtime_error(m.str());
  }

  minusdif = log_b - log_a;
  //#ifdef DEBUG
  if(std::isnan(minusdif))
  {
    boost::format m("logsub: minusdif (%f) log_b (%f) or log_a (%f) is nan");
    m % minusdif % log_b % log_a;
    throw std::runtime_error(m.str());
  }
  //#endif
  if(log_a == log_b) return bob::math::Log::LogZero;
  else if(minusdif < MINUS_LOG_THRESHOLD) return log_a;
  else return log_a + log1p(-exp(minusdif));
}


#include <fstream>
#include <iostream>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Octree.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/Point_set_3/IO.h>

#include <boost/iterator/function_output_iterator.hpp>

// Type Declarations
typedef CGAL::Simple_cartesian<double> Kernel;
typedef Kernel::Point_3 Point;
typedef CGAL::Point_set_3<Point> Point_set;
typedef Point_set::Point_map Point_map;

typedef CGAL::Octree<Kernel, Point_set, Point_map> Octree;

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

  // Point set will be used to hold our points
  Point_set points;

  // Load points from a file.
  std::ifstream stream((argc > 1) ? argv[1] : CGAL::data_file_path("points_3/cube.pwn"));
  stream >> points;
  if (0 == points.number_of_points()) {

    std::cerr << "Error: cannot read file" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "loaded " << points.number_of_points() << " points" << std::endl;

  // Create an octree from the points
  Octree octree(points, points.point_map());

  // Build the octree
  octree.refine(10, 20);

  // Find the nearest points to a few locations
  std::vector<Point> points_to_find = {
          {0, 0, 0},
          {1, 1, 1},
          {-1, -1, -1},
          {-0.46026, -0.25353, 0.32051},
          {-0.460261, -0.253533, 0.320513}
  };
  for (const Point& p : points_to_find)
    octree.nearest_neighbors
      (p, 1, // k=1 to find the single closest point
       boost::make_function_output_iterator
       ([&](const Point& nearest)
        {
          std::cout << "the nearest point to (" << p <<
            ") is (" << nearest << ")" << std::endl;
        }));

  return EXIT_SUCCESS;
}


/*
 * File: dht.hpp
 * Created Date: 2019-12-29
 * Author: Lei Pan
 * Contact: <panlei7@gmail.com>
 *
 * Last Modified: Sunday December 29th 2019 12:00:12 pm
 *
 * MIT License
 *
 * Copyright (c) 2019 Lei Pan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the 'Software'), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED 'AS IS', WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 * -----
 * HISTORY:
 * Date      	 By	Comments
 * ----------	---
 * ----------------------------------------------------------
 */

#ifndef DHT_DHT_H_
#define DHT_DHT_H_

#include <Eigen/Dense>

class DiscreteHankelTransform {

public:
  DiscreteHankelTransform(int order, int nr);
  ~DiscreteHankelTransform();

  Eigen::VectorXd r_sampling(double rmax);
  Eigen::VectorXd k_sampling(double rmax); // k is rho in the paper

  Eigen::VectorXd forward(const Eigen::Ref<const Eigen::VectorXd> &fr);
  Eigen::VectorXd backward(const Eigen::Ref<const Eigen::VectorXd> &fk);

  Eigen::VectorXd shift(const Eigen::Ref<const Eigen::VectorXd> &raw, int m);

  Eigen::MatrixXd tmatrix() const;

protected:
  int order_;
  double rmax_;
  int nr_;

  Eigen::VectorXd roots_;
  Eigen::MatrixXd tmatrix_;
};

#endif


#include "RayTracer/Transform.h"

#include <Eigen/Geometry>

namespace {
using RayTracer::Axes;

Eigen::Vector3f getAxisVector(Axes axis)
{
    switch (axis)
    {
    case Axes::X:
        return Eigen::Vector3f::UnitX();
    case Axes::Y:
        return Eigen::Vector3f::UnitY();
    case Axes::Z:
        return Eigen::Vector3f::UnitZ();
    }

    return Eigen::Vector3f::Zero();
}
}

namespace RayTracer {

Transformation Transformation::IdentityTransformation()
{
    return Transformation{};
}

const Matrix4f& Transformation::matrix() const
{
    return matrix_;
}

Transformation& Transformation::translate(float x, float y, float z)
{
    matrix_ = Transform::Translation(x, y, z) * matrix_;
    return *this;
}

Transformation& Transformation::translate(const Vector4f& v)
{
    matrix_ = Transform::Translation(v) * matrix_;
    return *this;
}

Transformation& Transformation::scale(float x, float y, float z)
{
    matrix_ = Transform::Scaling(x, y, z) * matrix_;
    return *this;
}

Transformation& Transformation::rotate(Axes axis, float angle)
{
    matrix_ = Transform::Rotation(axis, angle) * matrix_;
    return *this;
}

Transformation& Transformation::shear(float xy, float xz, float yx, float yz, float zx, float zy)
{
    matrix_ = Transform::Shearing(xy, xz, yx, yz, zx, zy) * matrix_;
    return *this;
}

namespace Transform {

Matrix4f Translation(float x, float y, float z)
{
    Eigen::Affine3f transform{ Eigen::Translation3f(x, y, z) };
    return transform.matrix();
}

Matrix4f Translation(const Vector4f& v)
{
    return Translation(v.x(), v.y(), v.z());
}

Matrix4f Scaling(float x, float y, float z)
{
    Eigen::Affine3f transform{ Eigen::Scaling(x, y, z) };
    return transform.matrix();
}

Matrix4f Rotation(Axes axis, float angle)
{
    Eigen::Affine3f transform{ Eigen::AngleAxisf(angle, getAxisVector(axis)) };
    return transform.matrix();
}

Matrix4f Shearing(float xy, float xz, float yx, float yz, float zx, float zy)
{
    Matrix4f transformationMatrix = Matrix4f::Identity();
    transformationMatrix(0, 1) = xy;
    transformationMatrix(0, 2) = xz;
    transformationMatrix(1, 0) = yx;
    transformationMatrix(1, 2) = yz;
    transformationMatrix(2, 0) = zx;
    transformationMatrix(2, 1) = zy;

    return transformationMatrix;
}

} // Transform

} // RayTracer


#include "RBF.h"

#include <Eigen/Eigen>

namespace Chaf
{
	RBFInterpolation::RBFInterpolation(const std::vector<glm::f64vec2>& points, double d) :
		m_d(d)
	{
		Eigen::VectorXd y(points.size());
		Eigen::MatrixXd A(points.size(), points.size());
		m_ps.resize(points.size());
		m_params.resize(points.size());

		m_constant = 0.f;

		for (uint32_t i = 0; i < points.size(); i++)
		{
			m_ps[i] = points[i].x;
			y(i) = points[i].y;
			m_constant += points[i].y;
		}

		m_constant /= static_cast<double>(points.size());
		y = y.array() - m_constant;

		for (uint32_t i = 0; i < points.size(); i++)
		{
			for (uint32_t j = 0; j < points.size(); j++)
			{
				A(i, j) = 1.f / ((points[i].x - m_ps[j]) * (points[i].x - m_ps[j]) + m_d);
			}
		}

		Eigen::VectorXd x = A.colPivHouseholderQr().solve(y);

		for (uint32_t i = 0; i < points.size(); i++)
		{
			m_params[i] = x(i);
		}
	}

	double RBFInterpolation::evaluate(double x)
	{
		double result = m_constant;

		for (uint32_t i = 0; i < m_params.size(); i++)
		{
			result += m_params[i] / ((x - m_ps[i]) * (x - m_ps[i]) + m_d);
		}

		return result;
	}

	std::vector<double> RBFInterpolation::evaluate(const std::vector<double>& x)
	{
		std::vector<double> result(x.size());

		for (uint32_t i = 0; i < result.size(); i++)
		{
			result[i] = evaluate(x[i]);
		}
		return result;
	}
}


#include "DenseLattice.h"
#include "SparseLattice.h"
#include "MIAConfig.h"
#include "LibMIAUtil.h"

#include <Eigen/Dense>

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

template<typename data_type>
void random_matrix(LibMIA::DenseLattice<data_type> & lat,double _prob,bool need_ranked=true, bool LSQR=false)
{
    boost::uniform_real<> uni_dist(0,1);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni(LibMIA::LibMIA_gen(), uni_dist);
    boost::uniform_real<> uni_dist2(-10,10);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni2(LibMIA::LibMIA_gen(), uni_dist2);
    lat.zeros();
    for(size_t k=0;k<(size_t)lat.depth();++k){
        auto _start=lat.data_begin()+k*lat.width()*lat.height();
        auto _end=lat.data_begin()+(k+1)*lat.width()*lat.height();
        bool flag=true;
        while(flag){
            for(auto it=_start;it<_end;++it){
                if(uni()<_prob){
                    *it=uni2();
                }
                else
                    *it=0;

            }
            if(need_ranked){
                if(LSQR){
                    auto _SVD=lat.tab_matrix(k).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
                    if(!_SVD.nonzeroSingularValues()!=lat.width())
                        flag=false;
                }
                else{
                    auto _QR=lat.tab_matrix(k).colPivHouseholderQr();
                    if(_QR.rank()==lat.width())
                        flag=false;
                }
            }
            else
                flag=false;

        }
    }
}

template<typename data_type>
void solvework(size_t m1, size_t n1, size_t n2, size_t p){

    typedef LibMIA::DenseLattice<data_type> denseType;
    typedef LibMIA::SparseLattice<data_type> sparseType;

    denseType DenseLat1(m1,n1,p);
    denseType DenseLat2(m1,n2,p);

    denseType DenseLat3;
    denseType DenseLat4;
    sparseType SparseLat1(m1,n1,p);
    sparseType SparseLat2(m1,n2,p);
    sparseType SparseLat3;

    random_matrix(DenseLat1,0.4);
    random_matrix(DenseLat2,0.4,false);

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1.solve(DenseLat2);
    //SparseLat1.print();

    //SparseLat3=SparseLat1.solve(SparseLat2);

    DenseLat4=SparseLat1.solve(SparseLat2);

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat4,test_precision<data_type>()),std::string("Full Dimension Solve Test for ")+typeid(data_type).name());

    //do least squares test, comparing against the result with dense computations
    DenseLat1=denseType(m1*2,n1,p);
    DenseLat2=denseType(m1*2,n2,p);
    random_matrix(DenseLat2,0.4);
    random_matrix(DenseLat1,0.4,false,true);
    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;
    DenseLat3=DenseLat1.solve(DenseLat2);
    DenseLat4=SparseLat1.solve(SparseLat2);

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat4,test_precision<data_type>()),std::string("Least-Squares Solve Test for ")+typeid(data_type).name());

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is subject to 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)

//[touches_two_geometries
//` Checks if two geometries have at least one touching point (tangent - non overlapping)

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Checks if the two geometries touch
    bg::model::polygon<bg::model::d2::point_xy<double> > poly1;
    bg::read_wkt("POLYGON((0 0,0 4,4 4,4 0,0 0))", poly1);
    bg::model::polygon<bg::model::d2::point_xy<double> > poly2;
    bg::read_wkt("POLYGON((0 0,0 -4,-4 -4,-4 0,0 0))", poly2);
    bool check_touches = bg::touches(poly1, poly2);
    if (check_touches) {
         std::cout << "Touches: Yes" << std::endl;
    } else {
        std::cout << "Touches: No" << std::endl;
    }

    bg::model::polygon<bg::model::d2::point_xy<double> > poly3;
    bg::read_wkt("POLYGON((1 1,0 -4,-4 -4,-4 0,1 1))", poly3);
    check_touches = bg::touches(poly1, poly3);
    if (check_touches) {
         std::cout << "Touches: Yes" << std::endl;
    } else {
        std::cout << "Touches: No" << std::endl;
    }

    return 0;
}

//]

//[touches_two_geometries_output
/*`
Output:
[pre
Touches: Yes

[$img/algorithms/touches_two_geometries.png]

Touches: No
]
*/
//]


#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/base_binary_edge.h>

#include <sophus/se3.h>
#include <sophus/so3.h>

#include <chrono>

/* vertex of lie algebra */
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, Sophus::SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    bool read(std::istream& is) {}
    bool write(std::ostream& os) const {}

    virtual void setToOriginImpl()
    {
        _estimate = Sophus::SE3();
    }

    /* update */
    virtual void oplusImpl(const double* update)
    {
        Sophus::SE3 up(
            Sophus::SO3(update[3], update[4], update[5]),
            Eigen::Vector3d(update[0], update[1], update[2])
        );

        setEstimate(up * _estimate);  // or _estimate = up * _estimate;
    }

};

class VertexPoint3D : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual bool read(std::istream& is) {}
    virtual bool write(std::ostream& os) const {}

    virtual void setToOriginImpl()
    {
        _estimate.fill(0);
    }

    virtual void oplusImpl(const double* update)
    {
        Eigen::Map<const Eigen::Vector3d> v(update);
        _estimate += v;
    }
};

class EdgeProject3D22D : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexPoint3D, VertexSE3LieAlgebra>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProject3D22D()
    {
        _cam = nullptr;
        resizeParameters(1);
        installParameter(_cam, 0);
    }

    virtual bool read(std::istream& is) {}
    virtual bool write(std::ostream& os) const {}

    void computeError()
    {
        Eigen::Vector3d point3d = static_cast<VertexPoint3D*>(_vertices[0])->estimate();
        Sophus::SE3 pose = static_cast<VertexSE3LieAlgebra*>(_vertices[1])->estimate();

        const g2o::CameraParameters * cam = static_cast<const g2o::CameraParameters *>(parameter(0));

        _error = _measurement - cam->cam_map(pose * point3d);
    }

    void linearizeOplus()
    {
        Eigen::Vector3d point3d = static_cast<VertexPoint3D*>(_vertices[0])->estimate();
        Sophus::SE3 pose = static_cast<VertexSE3LieAlgebra*>(_vertices[1])->estimate();

        Eigen::Vector3d xyz_trans(pose * point3d);

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        double z_2 = z * z;

        const g2o::CameraParameters * cam = static_cast<const g2o::CameraParameters *>(parameter(0));

        Eigen::Matrix<double, 2, 3, Eigen::ColMajor> tmp;
        tmp(0, 0) = -cam->focal_length / z;
        tmp(0, 1) = 0;
        tmp(0, 2) = cam->focal_length * x / z_2;
        tmp(1, 0) = 0;
        tmp(1, 1) = -cam->focal_length / z;
        tmp(1, 2) = cam->focal_length * y / z_2;

        _jacobianOplusXi = tmp * pose.rotation_matrix();

        _jacobianOplusXj(0, 0) = cam->focal_length * x * y / z_2;
        _jacobianOplusXj(0, 1) = -cam->focal_length * (1 + x*x/z_2);
        _jacobianOplusXj(0, 2) = cam->focal_length * y / z;
        _jacobianOplusXj(0, 3) = -cam->focal_length / z;
        _jacobianOplusXj(0, 4) = 0;
        _jacobianOplusXj(0, 5) = cam->focal_length * x / z_2;

        _jacobianOplusXj(1, 0) = cam->focal_length * (1 + y*y/z_2);
        _jacobianOplusXj(1, 1) = -cam->focal_length * x * y / z_2;
        _jacobianOplusXj(1, 2) = -cam->focal_length * x / z;
        _jacobianOplusXj(1, 3) = 0;
        _jacobianOplusXj(1, 4) = -cam->focal_length / z;
        _jacobianOplusXj(1, 5) = cam->focal_length * y / z_2;
    }

    g2o::CameraParameters* _cam;
};

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

void bundleAdjustment( const std::vector<cv::Point3f>, const std::vector<cv::Point2f>, const cv::Mat&, cv::Mat&, cv::Mat&);

int main(int argc, char** argv)
{
    if (argc != 5) {
        std::cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2." << std::endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);

    if (img_1.empty() || img_2.empty()) {
        std::cout << "img1 or img2 is no find." << std::endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    std::cout << "feature matches points total is " << matches.size() << std::endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat d1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts_3d;
    std::vector<cv::Point2f> pts_2d;
    for (cv::DMatch m:matches) {
        ushort d = d1.ptr<unsigned short>(int (keypoints_1[m.queryIdx].pt.y))[int (keypoints_1[m.queryIdx].pt.x)];

        if (d == 0) {   /* bad depth */
            continue;
        }
        float dd = d/5000.0;
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(cv::Point3d (p1.x*dd, p1.y*dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }
    std::cout << "3d-2d pairs: " << pts_3d.size() << std::endl;

    //-- 4th, use PnP or bundle adjustment compute camera pose.
    cv::Mat r, t;
    cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false);    // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Mat R;
    cv::Rodrigues(r, R);    // r为旋转向量形式，用Rodrigues公式转换为矩阵

    std::cout << "R = " << std::endl << R << std::endl;
    std::cout << "t = " << std::endl << t << std::endl;

    std::cout << "calling bundle adjustment" << std::endl;

    bundleAdjustment(pts_3d, pts_2d, K, R, t);

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= std::max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void bundleAdjustment(
    const std::vector<cv::Point3f> points_3d,
    const std::vector<cv::Point2f> points_2d,
    const cv::Mat& K,
    cv::Mat& R,
    cv::Mat& t )
{
    // Initialize g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;   // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linear_solver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block((std::unique_ptr<Block::LinearSolverType>)(linear_solver));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg((std::unique_ptr<Block>)solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    VertexSE3LieAlgebra * pose = new VertexSE3LieAlgebra();     // camera pose
    Eigen::Matrix3d R_mat;
    R_mat << \
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), \
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), \
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(
        Sophus::SE3(
            R_mat,
            Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
        )
    );
    optimizer.addVertex(pose);

    int index = 1;
    for (const cv::Point3f p:points_3d) {
        VertexPoint3D* point = new VertexPoint3D();
        point->setId(index ++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);   // g2o 中必须设置 marg
        optimizer.addVertex(point);
    }

    // parameter : camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters(
        K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0
    );
    camera->setId(0);
    optimizer.addParameter(camera);

    // edges
    index = 1;
    for (const cv::Point2f p:points_2d) {
        EdgeProject3D22D* edge = new EdgeProject3D22D();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<VertexPoint3D*>(optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }

    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "optimization costs time: " << time_used.count() << " seconds." << std::endl;

    std::cout << std::endl << "after optimization: " << std::endl;
    std::cout << "T = " << std::endl << pose->estimate().matrix() << std::endl;
}


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

#include <iron/math/PrimeCalculator.hpp>

BOOST_AUTO_TEST_SUITE(prime_calculator)

struct Fixture : public ::iron::math::PrimeCalculator {

};

BOOST_FIXTURE_TEST_CASE(PrimesUnderTenTest, Fixture)
{
  BOOST_CHECK_EQUAL(false, isPrime(0));
  BOOST_CHECK_EQUAL(false, isPrime(1));
  BOOST_CHECK_EQUAL(true,  isPrime(2));
  BOOST_CHECK_EQUAL(true,  isPrime(3));
  BOOST_CHECK_EQUAL(false, isPrime(4));
  BOOST_CHECK_EQUAL(true,  isPrime(5));
  BOOST_CHECK_EQUAL(false, isPrime(6));
  BOOST_CHECK_EQUAL(true,  isPrime(7));
  BOOST_CHECK_EQUAL(false, isPrime(8));
  BOOST_CHECK_EQUAL(false, isPrime(9));
}

BOOST_FIXTURE_TEST_CASE(LargePrimesTest, Fixture)
{
  BOOST_CHECK_EQUAL(true,  isPrime(999983));
  BOOST_CHECK_EQUAL(false, isPrime(999999));
}

BOOST_FIXTURE_TEST_CASE(CachedResults100Test, Fixture)
{
  BOOST_CHECK_EQUAL(false, hasCached(100));
  BOOST_CHECK_EQUAL(false, hasCached(121));
  BOOST_CHECK_EQUAL(false, hasCached(122));

  BOOST_CHECK_EQUAL(false, isPrime  (100));

  BOOST_CHECK_EQUAL(true,  hasCached(100));
  BOOST_CHECK_EQUAL(true,  hasCached(121));
  BOOST_CHECK_EQUAL(false, hasCached(122));
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <math.h>
#include <string.h>
#include <Eigen/Eigen>
#include <boost/serialization/version.hpp>
#include "types/Point.hpp"

struct RRCoord {
   /**
    * RRCoord
    * If any param is set to NAN this implies that the value is either
    * not known or irrelevant to the particular context.
    *
    * @param heading       heading from robot to object
    * @param distance      distance from robot to object
    * @param orientation   angle between robot front and object front
    */
   RRCoord(float distance, float heading = NAN, float orientation = NAN)
      : vec(distance, heading, orientation)
   {
      var.setZero();
   }

   Point toCartesian() const {
      return Point(distance()*cosf(heading()), distance()*sinf(heading()));
   }

   RRCoord() {
      vec.setZero();
      var.setZero();
   }

   RRCoord(const RRCoord &other) {
      this->vec = other.vec;
      this->var = other.var;
   }

   RRCoord& operator=(const RRCoord &other) {
      for (unsigned i = 0; i < 3; i++) {
         this->vec(i, 0) = other.vec(i, 0);
      }
      for (unsigned i = 0; i < 9; i++) {
         this->var[i] = other.var[i];
      }

      return *this;
   }

   Eigen::Vector3f vec;
   Eigen::Matrix<float, 3, 3> var;

   /* Distance to object */
   const float distance() const {
      return vec[0];
   }

   float &distance() {
      return vec[0];
   }

   /* Heading to object */
   const float heading() const {
      return vec[1];
   }

   float &heading() {
      return vec[1];
   }

   /* Angle between robot's front and object's front */
   const float orientation() const {
      return vec[2];
   }

   float &orientation() {
      return vec[2];
   }

   bool operator== (const RRCoord &other) const {
      return vec == other.vec;
   }

   template<class Archive>
   void serialize(Archive &ar, const unsigned int file_version);
};

inline std::ostream& operator<<(std::ostream& os, const RRCoord& coord) {
   for (unsigned i = 0; i < 3; i++) {
      float a = coord.vec[i];
      os.write((char*) &a, sizeof(float));
   }

   for (unsigned i = 0; i < 9; i++) {
      float a = coord.var[i];
      os.write((char*) &a, sizeof(float));
   }
   return os;
}

inline std::istream& operator>>(std::istream& is, RRCoord& coord) {
   for (unsigned i = 0; i < 3; i++) {
      float a;
      is.read((char*) &a, sizeof(float));
      coord.vec[i] = a;
   }

   for (unsigned i = 0; i < 9; i++) {
      float a;
      is.read((char*) &a, sizeof(float));
      coord.var[i] = a;
   }

   return is;
}

typedef enum {
   DIST,
   HEADING,
   ORIENTATION,
} RRCoordEnum;

#include "types/RRCoord.tcc"