#ifndef HYPSYS1D_MODEL_HPP
#define HYPSYS1D_MODEL_HPP

#include <cmath>
#include <memory>

#include <Eigen/Dense>
#include <ancse/config.hpp>

/// Interface for implementing different models,
/// eg. Euler equations, Shallow-water equations
///
/// Add more functions to this interface if needed.
class Model {
  public:
    virtual ~Model() = default;

    virtual Eigen::VectorXd flux(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &u) const = 0;
    virtual double max_eigenvalue(const Eigen::VectorXd &u) const = 0;

    virtual Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u) const = 0;

    virtual int get_nvars() const = 0;
    virtual std::string get_name() const = 0;
};

class Burgers : public Model {
  public:

    Eigen::VectorXd flux(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd f(n_vars);
        f(0) = 0.5*u(0)*u(0);

        return f;
    }

    Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd eigvals(n_vars);
        eigvals(0) = u(0);

        return eigvals;
    }

    Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &) const override
    {
        Eigen::MatrixXd eigvecs(n_vars, n_vars);
        eigvecs (0,0) = 1;

        return eigvecs;
    }

    double max_eigenvalue(const Eigen::VectorXd &u) const override {
        return (eigenvalues(u).cwiseAbs()).maxCoeff();
    }

    Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u) const override {
        return u;
    }

    Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u) const override {
        return u;
    }

    int get_nvars() const override
    {
        return n_vars;
    }

    std::string get_name() const override
    {
        return "burgers";
    }

  private:
    int n_vars = 1;
};

/// Euler equations
class Euler : public Model {
  public:

    Eigen::VectorXd flux(const Eigen::VectorXd &u) const override;

    Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override;

    Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &u) const override;

    double max_eigenvalue(const Eigen::VectorXd &u) const override;

    Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u_cons) const override;

    Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u_prim) const override;

    void set_gamma(double gamma_)
    {
        gamma = gamma_;
    }

    double get_gamma() const
    {
        return gamma;
    }

    int get_nvars() const override
    {
        return n_vars;
    }

    std::string get_name() const override
    {
        return "euler";
    }

  private:
    int n_vars = 3;
    double gamma = 5./3.;
};

std::shared_ptr<Model> make_model (const nlohmann::json &config);

#endif // HYPSYS1D_MODEL_HPP


/*
 * Copyright Nick Thompson, 2021
 * Use, modification and distribution are 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)
 */

#include "math_unit_test.hpp"
#include <numeric>
#include <random>
#include <array>
#include <boost/core/demangle.hpp>
#include <boost/math/interpolators/bilinear_uniform.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::interpolators::bilinear_uniform;

template<class Real>
void test_four_values()
{
    Real x0 = 0;
    Real y0 = 0;
    Real dx = 1;
    Real dy = 1;
    Real value = 1.5;
    std::vector<Real> v(2*2, value);
    auto v_copy = v;
    auto ub = bilinear_uniform<decltype(v)>(std::move(v_copy), 2, 2, dx, dy, x0, y0);
    for (Real x = x0; x <= x0 + dx; x += dx/8) {
        for (Real y = y0; y <= y0 + dx; y += dy/8) {
            CHECK_ULP_CLOSE(value, ub(x, y), 1);
        }
    }

    // Now we test the unit square:
    std::random_device rd;
    std::uniform_real_distribution<Real> dis(1,2);

    int i = 0;
    while (i++ < 300) {
        v[0] = dis(rd);
        v[1] = dis(rd);
        v[2] = dis(rd);
        v[3] = dis(rd);

        // See https://en.wikipedia.org/wiki/Bilinear_interpolation, section: Unit square
        auto f = [&v](Real x, Real y) {
            return v[0]*(1-x)*(1-y) + v[1]*x*(1-y) + v[2]*(1-x)*y + v[3]*x*y;
        };

        v_copy = v;
        ub = bilinear_uniform<decltype(v_copy)>(std::move(v_copy), 2, 2, dx, dy, x0, y0);
        for (Real x = x0; x <= x0 + dx; x += dx/16) {
            for (Real y = y0; y <= y0 + dx; y += dy/16) {
                CHECK_ULP_CLOSE(f(x,y), ub(x, y), 3);
            }
        }
    }
}

template<typename Real>
void test_linear()
{
    std::random_device rd;
    std::uniform_real_distribution<Real> dis(1,2);
    std::array<Real, 4> a{dis(rd), dis(rd), dis(rd), dis(rd)};
    auto f = [&a](Real x, Real y) {
        return a[0] + a[1]*x + a[2]*y + a[3]*x*y;
    };

    for (int rows = 2; rows < 20; ++rows) {
        for (int cols = 2; cols < 20; ++cols) {
            Real dx = dis(rd);
            Real dy = dis(rd);
            Real x0 = dis(rd);
            Real y0 = dis(rd);
            std::vector<Real> v(rows*cols, std::numeric_limits<Real>::quiet_NaN());
            for (int i = 0; i < cols; ++i) {
                for (int j = 0; j < rows; ++j) {
                    v[j*cols + i] = f(x0 + i*dx, y0 + j*dy);
                }
            }
            auto ub = bilinear_uniform<decltype(v)>(std::move(v), rows, cols, dx, dy, x0, y0);

           for (Real x = x0; x < x0 + (cols-1)*dx; x += dx/8) {
                for (Real y = y0; y < y0 + (rows-1)*dy; y += dy/8) {
                    if (!CHECK_ULP_CLOSE(f(x,y), ub(x, y), 13)) {
                        std::cerr << " f(" << x << ", " << y << ") = " << f(x,y) << "\n";
                        std::cerr << "ub(" << x << ", " << y << ") = " << ub(x,y) << "\n";
                    }
                }
            }
        }
    }
}

int main()
{
    test_four_values<float>();
    test_four_values<double>();
    test_four_values<long double>();
    test_linear<double>();
    return boost::math::test::report_errors();
}


#ifndef HELPERS_H
#define HELPERS_H

#include <Eigen/Core>

using Eigen::VectorXd;

//
// Helper functions to fit and evaluate polynomials.
//

// Evaluate a polynomial.
double polyeval(const VectorXd &coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); ++i) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from:
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);

  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); ++i) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); ++j) {
    for (int i = 0; i < order; ++i) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);

  return result;
}

#endif  // HELPERS_H


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

#include <utility>
#include <vector>

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

#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include <graph_laplacian.hpp>

// An matrix equality check copied from: http://lists.boost.org/Archives/boost/2005/11/96235.php
template<class E>
bool equal(boost::numeric::ublas::matrix<E> lhs, boost::numeric::ublas::matrix<E> rhs)
{
  if (&lhs == &rhs) { return true; }
  if (lhs.size1() != rhs.size1()) { return false; }
  if (lhs.size2() != rhs.size2()) { return false; }
  typename boost::numeric::ublas::matrix<E>::iterator1 l(lhs.begin1());
  typename boost::numeric::ublas::matrix<E>::iterator1 r(rhs.begin1());
  while (l != lhs.end1()) {
    if (*l != *r) { return false; }
    ++l;
    ++r;
  }
  return true;
}

using Graph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS>;
using Matrix = boost::numeric::ublas::matrix<int>;

BOOST_AUTO_TEST_SUITE(TestGraphLaplacian)

// An empty graph produces an empty matrix
BOOST_AUTO_TEST_CASE(EmptyGraphTest)
{
  Graph g;

  BOOST_CHECK(equal(graph_laplacian(g), Matrix(0, 0)));
}

// A graph with n vertices and no edges produces an (n x n)-zero matrix
BOOST_AUTO_TEST_CASE(NoEdgesTest)
{
  Graph g(5);

  BOOST_CHECK(equal(graph_laplacian(g), Matrix(5, 5, 0)));
}

// A graph with two vertices and an edge connecting them produces the following Laplacian matrix:
//  1 -1
// -1  1
BOOST_AUTO_TEST_CASE(TwoVerticesOneEdgeGraphTest)
{
  using Edge = std::pair<int, int>;
  std::vector<Edge> edges { Edge(0, 1) };
  Graph g(edges.begin(), edges.end(), 2);

  Matrix expected_mat(2, 2);
  expected_mat <<=  1, -1,
                   -1,  1;

  BOOST_CHECK(equal(graph_laplacian(g), expected_mat));
}

// A free tree on 3 vertices (vertex 1 is the center vertex) produces following Laplacian matrix:
//  1 -1  0
// -1  2 -1
//  0 -1  1
BOOST_AUTO_TEST_CASE(ThreeVerticesFreeTreeTest)
{
  using Edge = std::pair<int, int>;
  std::vector<Edge> edges { Edge(0, 1), Edge(1, 2) };
  Graph g(edges.begin(), edges.end(), 3);

  Matrix expected_mat(3, 3);
  expected_mat <<=  1, -1,  0,
                   -1,  2, -1,
                    0, -1,  1;

  BOOST_CHECK(equal(graph_laplacian(g), expected_mat));
}

// A free tree on 4 vertices (vertices 1, 2 are the middle vertices) produces following
// Laplacian matrix:
//  1 -1  0  0
// -1  2 -1  0
//  0 -1  2 -1
//  0  0 -1  1
BOOST_AUTO_TEST_CASE(FourVerticesFreeTreeTest)
{
  using Edge = std::pair<int, int>;
  std::vector<Edge> edges { Edge(0, 1), Edge(1, 2), Edge(2, 3) };
  Graph g(edges.begin(), edges.end(), 4);

  Matrix expected_mat(4, 4);
  expected_mat <<=  1, -1,  0,  0,
                   -1,  2, -1,  0,
                    0, -1,  2, -1,
                    0,  0, -1,  1;

  BOOST_CHECK(equal(graph_laplacian(g), expected_mat));
}

// A graph forming a square with 4 vertices produces follwoing Laplacian matrix:
//  2 -1  0 -1
// -1  2 -1  0
//  0 -1  2 -1
// -1  0 -1  2
BOOST_AUTO_TEST_CASE(SquareTest)
{
  using Edge = std::pair<int, int>;
  std::vector<Edge> edges { Edge(0, 1), Edge(1, 2), Edge(2, 3), Edge(3, 0) };
  Graph g(edges.begin(), edges.end(), 4);

  Matrix expected_mat(4, 4);
  expected_mat <<=  2, -1,  0, -1,
                   -1,  2, -1,  0,
                    0, -1,  2, -1,
                   -1,  0, -1,  2;

  BOOST_CHECK(equal(graph_laplacian(g), expected_mat));
}

BOOST_AUTO_TEST_SUITE_END()  // TestGraphLaplacian


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

using namespace Eigen;

int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << "Here is the matrix m:\n" << m << std::endl;
  VectorXd v(2);
  v(0) = 4;
  v(1) = v(0) - 1;
  std::cout << "Here is the vector v:\n" << v << std::endl;

}


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

using namespace Eigen;

int main(){
  const int nrows = 20;
  const int ncols = 10;

  MatrixXd grid(nrows, ncols);
  grid = MatrixXd::Zero(nrows, ncols);

  std::cout << grid << std::endl;

  return 0;
}


#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/assert_macros.hpp>
#include <Eigen/Geometry>
#include <limits>
#include <cmath>

namespace sm { namespace kinematics {
        template <typename Scalar_ = double>
        inline bool isLessThenEpsilons4thRoot(Scalar_ x){
          static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0/4.0);
          return x < epsilon4thRoot;
        }

        // quaternion rotation.
        Eigen::Vector4d r2quat(Eigen::Matrix3d const & R){

            const double & c1 = R(0,0);
            const double & c2 = R(1,0);
            const double & c3 = R(2,0);
            const double & c4 = R(0,1);
            const double & c5 = R(1,1);
            const double & c6 = R(2,1);
            const double & c7 = R(0,2);
            const double & c8 = R(1,2);
            const double & c9 = R(2,2);

            Eigen::Vector4d dc(fabs(1.0+c1-c5-c9),
                               fabs(1.0-c1+c5-c9),
                               fabs(1.0-c1-c5+c9),
                               fabs(1.0+c1+c5+c9));

            unsigned maxq = 0;
            double maxqval = dc(0);

            for(unsigned i = 1; i < 4; i++) {
                if(dc(i) > maxqval) {
                    maxq = i;
                    maxqval = dc(i);
                }
            }

            double c;
            Eigen::Vector4d q;
            if(maxq == 0){
                q(0)=0.5*sqrt(dc(0));
                c = 0.25/q(0);
                q(1)=c*(c4+c2);
                q(2)=c*(c7+c3);
                q(3)=c*(c8-c6);
            } else if(maxq == 1){
                q(1)=0.5*sqrt(dc(1));
                c = 0.25/q(1);
                q(0)=c*(c4+c2);
                q(2)=c*(c6+c8);
                q(3)=c*(c3-c7);
            } else if(maxq == 2){
                q(2)=0.5*sqrt(dc(2));
                c = 0.25/q(2);
                q(0)=c*(c3+c7);
                q(1)=c*(c6+c8);
                q(3)=c*(c4-c2);
            } else {
                q(3)=0.5*sqrt(dc(3));
                c = 0.25/q(3);
                q(0)=c*(c8-c6);
                q(1)=c*(c3-c7);
                q(2)=c*(c4-c2);
            }

            if(q(3) < 0)
                q = -q;

            return q;
        }

        Eigen::Matrix3d quat2r(Eigen::Vector4d const & q){

            SM_ASSERT_NEAR_DBG(std::runtime_error,q.norm(),1.f,1e-4, "The quaternion must be a unit vector to represent a rotation");
            //double n = q(3);
            //Eigen::Vector3d e = makeV3(q(0),q(1),q(2));
            //R = (n^2 - e'*e) * eye(3) + 2 * e * e' + 2 * n * crossMx(e);
            //Eigen::Matrix3d R = (n*n - e.dot(e))*Eigen::Matrix3d::Identity() + 2 * e * e.transpose() + 2 * n * crossMx(e);

            Eigen::Matrix3d R;

            // [ q0^2 - q1^2 - q2^2 + q3^2,           2*q0*q1 + 2*q2*q3,           2*q0*q2 - 2*q1*q3]
            // [         2*q0*q1 - 2*q2*q3, - q0^2 + q1^2 - q2^2 + q3^2,           2*q0*q3 + 2*q1*q2]
            // [         2*q0*q2 + 2*q1*q3,           2*q1*q2 - 2*q0*q3, - q0^2 - q1^2 + q2^2 + q3^2]
            R(0,0) = q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
            R(0,1) = q[0]*q[1]*2.0+q[2]*q[3]*2.0;
            R(0,2) = q[0]*q[2]*2.0-q[1]*q[3]*2.0;
            R(1,0) = q[0]*q[1]*2.0-q[2]*q[3]*2.0;
            R(1,1) = -q[0]*q[0]+q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
            R(1,2) = q[0]*q[3]*2.0+q[1]*q[2]*2.0;
            R(2,0) = q[0]*q[2]*2.0+q[1]*q[3]*2.0;
            R(2,1) = q[0]*q[3]*(-2.0)+q[1]*q[2]*2.0;
            R(2,2) = -q[0]*q[0]-q[1]*q[1]+q[2]*q[2]+q[3]*q[3];

            return R;
        }

        Eigen::Matrix4d quatPlus(Eigen::Vector4d const & q)
        {
            // [  q3,  q2, -q1, q0]
            // [ -q2,  q3,  q0, q1]
            // [  q1, -q0,  q3, q2]
            // [ -q0, -q1, -q2, q3]
            Eigen::Matrix4d Q;
            Q(0,0) =  q[3]; Q(0,1) =  q[2]; Q(0,2) = -q[1]; Q(0,3) =  q[0];
            Q(1,0) = -q[2]; Q(1,1) =  q[3]; Q(1,2) =  q[0]; Q(1,3) =  q[1];
            Q(2,0) =  q[1]; Q(2,1) = -q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
            Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];

            return Q;
        }

        Eigen::Matrix4d quatOPlus(Eigen::Vector4d const & q)
        {
            // [  q3, -q2,  q1, q0]
            // [  q2,  q3, -q0, q1]
            // [ -q1,  q0,  q3, q2]
            // [ -q0, -q1, -q2, q3]

            Eigen::Matrix4d Q;
            Q(0,0) =  q[3]; Q(0,1) = -q[2]; Q(0,2) =  q[1]; Q(0,3) =  q[0];
            Q(1,0) =  q[2]; Q(1,1) =  q[3]; Q(1,2) = -q[0]; Q(1,3) =  q[1];
            Q(2,0) = -q[1]; Q(2,1) =  q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
            Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];

            return Q;

        }

        Eigen::Vector4d qplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p)
        {
            Eigen::Vector4d qplus_p;
            // p0*q3 + p1*q2 - p2*q1 + p3*q0
            qplus_p[0] = p[0]*q[3] + p[1]*q[2] - p[2]*q[1] + p[3]*q[0];
            // p2*q0 - p0*q2 + p1*q3 + p3*q1
            qplus_p[1] = p[2]*q[0] - p[0]*q[2] + p[1]*q[3] + p[3]*q[1];
            // p0*q1 - p1*q0 + p2*q3 + p3*q2
            qplus_p[2] = p[0]*q[1] - p[1]*q[0] + p[2]*q[3] + p[3]*q[2];
            // p3*q3 - p1*q1 - p2*q2 - p0*q0
            qplus_p[3] = p[3]*q[3] - p[1]*q[1] - p[2]*q[2] - p[0]*q[0];

            return qplus_p;
        }

        Eigen::Vector4d qoplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p)
        {
            Eigen::Vector4d qoplus_p;
            // p0*q3 - p1*q2 + p2*q1 + p3*q0
            qoplus_p[0] = p[0]*q[3] - p[1]*q[2] + p[2]*q[1] + p[3]*q[0];
            // p0*q2 - p2*q0 + p1*q3 + p3*q1
            qoplus_p[1] = p[0]*q[2] - p[2]*q[0] + p[1]*q[3] + p[3]*q[1];
            // p1*q0 - p0*q1 + p2*q3 + p3*q2
            qoplus_p[2] = p[1]*q[0] - p[0]*q[1] + p[2]*q[3] + p[3]*q[2];
            // p3*q3 - p1*q1 - p2*q2 - p0*q0
            qoplus_p[3] = p[3]*q[3] - p[1]*q[1] - p[2]*q[2] - p[0]*q[0];

            return qoplus_p;
        }

        Eigen::Vector4d quatInv(Eigen::Vector4d const & q)
        {
            Eigen::Vector4d qret = q;
            invertQuat(qret);
            return qret;
        }

        void invertQuat(Eigen::Vector4d & q)
        {
            q.head<3>() = -q.head<3>();
        }

        Eigen::Vector3d qeps(Eigen::Vector4d const & q)
        {
            return q.head<3>();
        }

        Eigen::Vector3f qeps(Eigen::Vector4f const & q)
        {
            return q.head<3>();
        }

        double qeta(Eigen::Vector4d const & q)
        {
            return q[3];
        }

        float qeta(Eigen::Vector4f const & q)
        {
            return q[3];
        }

        Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const & a)
        {
            // Method of implementing this function that is accurate to numerical precision from
            // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics, gpu, and game tools, 3(3):29–48.

            double theta = a.norm();

            // na is 1/theta sin(theta/2)
            double na;
            if(isLessThenEpsilons4thRoot(theta))
            {
                static const double one_over_48 = 1.0/48.0;
                na = 0.5 + (theta * theta) * one_over_48;
            }
            else
            {
                na = sin(theta*0.5) / theta;
            }
            Eigen::Vector3d axis = a*na;
            double ct = cos(theta*0.5);
            return Eigen::Vector4d(axis[0],axis[1],axis[2],ct);
        }

        /**
         * calculate arcsin(x)/x
         * @param x
         * @return
         */
        template <typename Scalar_>
        inline Scalar_ arcSinXOverX(Scalar_ x) {
          if(isLessThenEpsilons4thRoot(fabs(x))){
            return Scalar_(1.0) + x * x * Scalar_(1/6);
          }
          return asin(x) / x;
        }

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 1> quat2AxisAngle(Eigen::Matrix<Scalar_, 4, 1> const & q)
        {
          SM_ASSERT_LT_DBG(std::runtime_error, fabs(q.norm() - 1), 8 * std::numeric_limits<Scalar_>::epsilon(), "This function is inteded for unit quternions only.");
          const Eigen::Matrix<Scalar_, 3, 1> a = qeps(q);
          const Scalar_ na = a.norm(), eta = qeta(q);
          Scalar_ scale;
          if(fabs(eta) < na){ // use eta because it is more precise than na to calculate the scale. No singularities here.
            scale = acos(eta) / na;
          } else {
            /*
             * In this case more precision is in na than in eta so lets use na only to calculate the scale:
             *
             * assume first eta > 0 and 1 > na > 0.
             *               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
             *    sin (u * na) = na
             *  sin^2 (u * na) = na^2
             *  cos^2 (u * na) = 1 - na^2
             *                              (1 = ||q|| = eta^2 + na^2)
             *    cos^2 (u * na) = eta^2
             *                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
             *      cos (u * na) = eta
             *                              (u * na in [ 0, pi/2] )
             *                 u = acos (eta) / na
             *
             * So the for eta > 0 it is acos(eta) / na == asin(na) / na.
             * From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na.
             */
            if(eta > 0){
              // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha) for its series expansion at na = 0. And that is done in the following function.
              scale = arcSinXOverX(na);
            }else{
              // (pi - asin(na))/ na has a pole at na == 0. So we cannot remove this singularity.
              // It is just the cut locus of the unit quaternion manifold at identity and thus the axis angle description becomes necessarily unstable there.
              scale = (M_PI - asin(na)) / na;
            }
          }
          return a * (Scalar_(2) * scale);
        }
        template Eigen::Matrix<double, 3, 1> quat2AxisAngle(Eigen::Matrix<double, 4, 1> const & q);
        template Eigen::Matrix<float, 3, 1> quat2AxisAngle(Eigen::Matrix<float, 4, 1> const & q);

        Eigen::Matrix<double,4,3> quatJacobian(Eigen::Vector4d const & p)
        {
            Eigen::Matrix<double,4,3> J;
            // [  p3, -p2,  p1]
            // [  p2,  p3, -p0]
            // [ -p1,  p0,  p3]
            // [ -p0, -p1, -p2]

            J(0,0) =  p[3];
            J(0,1) = -p[2];
            J(0,2) =  p[1];
            J(1,0) =  p[2];
            J(1,1) =  p[3];
            J(1,2) = -p[0];
            J(2,0) = -p[1];
            J(2,1) =  p[0];
            J(2,2) =  p[3];
            J(3,0) = -p[0];
            J(3,1) = -p[1];
            J(3,2) = -p[2];

            return J*0.5;
        }

        Eigen::Vector4d updateQuat(Eigen::Vector4d const & q, Eigen::Vector3d const & dq)
        {
            // the following code is an optimized version of:
            // Eigen::Vector4d dq4 = axisAngle2quat(dq);
            // Eigen::Vector4d retq = quatPlus(dq4)*q;
            // return retq;

            Eigen::Vector4d dq3 = axisAngle2quat(dq);
            double ca = dq3[3];
            Eigen::Vector4d retq;
            retq[0] = q[0]*ca+dq3[0]*q[3]-dq3[1]*q[2]+dq3[2]*q[1];
            retq[1] = q[1]*ca+dq3[0]*q[2]+dq3[1]*q[3]-dq3[2]*q[0];
            retq[2] = q[2]*ca-dq3[0]*q[1]+dq3[1]*q[0]+dq3[2]*q[3];
            retq[3] = q[3]*ca-dq3[0]*q[0]-dq3[1]*q[1]-dq3[2]*q[2];

            return retq;
        }

        Eigen::Vector3d quatRotate(Eigen::Vector4d const & q_a_b, Eigen::Vector3d const & v_b)
        {
            return v_b + 2.0 * q_a_b.head<3>().cross(q_a_b.head<3>().cross(v_b) - q_a_b[3] * v_b);
        }

        Eigen::Vector4d quatRandom()
        {
            Eigen::Vector4d q_a_b;
            q_a_b.setRandom();
            q_a_b.array() -= 0.5;
            q_a_b /= q_a_b.norm();
            return q_a_b;
        }

        Eigen::Vector4d quatIdentity()
        {
            return Eigen::Vector4d(0,0,0,1);
        }

        Eigen::Matrix<double,3,4> quatS(Eigen::Vector4d q)
        {
            //   [  q3,  q2, -q1, -q0]
            // 2 [ -q2,  q3,  q0, -q1]
            //   [  q1, -q0,  q3, -q2]
            q *= 2.0;

            Eigen::Matrix<double,3,4> S;
            S <<
                q[3],  q[2], -q[1], -q[0],
                -q[2],  q[3],  q[0], -q[1],
                q[1], -q[0],  q[3], -q[2];

            return S;
        }

        Eigen::Matrix<double,4,3> quatInvS(Eigen::Vector4d q)
        {
            q *= 0.5;

            // 1 [  q3, -q2,  q1]
            // - [  q2,  q3, -q0]
            // 2 [ -q1,  q0,  q3]
            //   [ -q0, -q1, -q2]

            Eigen::Matrix<double,4,3> invS;
            invS <<
                q[3], -q[2],  q[1],
                q[2],  q[3], -q[0],
                -q[1],  q[0],  q[3],
                -q[0], -q[1], -q[2];
            return invS;
        }

        Eigen::Vector4d qslerp(const Eigen::Vector4d & q0, const Eigen::Vector4d & q1, double t)
        {
          if(t <= 0.0)
            {
                return q0;
            }
            else if(t >= 1.0)
            {
                return q1;
            }
            else
            {
              if( (q0-q1).squaredNorm() > (q0 + q1).squaredNorm() ) {
                // The quaternions are far away from eachother on the sphere.
                // Flip one around so that this works out.
                return qplus(q0, qexp(t * qlog( qplus(quatInv(q0),-q1))));
              } else {
                return qplus(q0, qexp(t * qlog( qplus(quatInv(q0),q1))));
              }

            }
        }

        /// \brief do linear interpolation between p0 and p1 for times t = [0.0,1.0]
        Eigen::VectorXd lerp(const Eigen::VectorXd & p0, const Eigen::VectorXd & p1, double t)
        {
            SM_ASSERT_EQ(std::runtime_error, p0.size(), p1.size(), "The vectors must be the same size");
            if(t <= 0.0)
            {
                return p0;
            }
            else if(t >= 1.0)
            {
                return p1;
            }
            else
            {
                return (1-t) * p0  +  t * p1;
            }
        }

        Eigen::Matrix<double,3,4> quatLogJacobian(const Eigen::Vector4d& p)
        {
          //      [qx]
          //      [qy]                -2*x
          // p =  [qz],    g(x) = ----------------
          //      [qw]               sqrt(1-qw²)
          //
          //
          //      [2*acos(qw)      0            0            g(qx)]
          // J =  [0            2*acos(qw)      0            g(qy)]
          //      [0            0            2*acos(qw)      g(qz)]

          Eigen::Matrix<double, 3,4> J;
          J.setZero();

          double n = qeps(p).norm();

          double de = n*n*n;//pow(n, 3);
          double u12 = p(1)*p(1) + p(2)*p(2);//pow(p(1), 2) + pow(p(2), 2);
          double u02 = p(0)*p(0) + p(2)*p(2);//pow(p(0), 2) + pow(p(2), 2);
          double u01 = p(0)*p(0) + p(1)*p(1);//pow(p(0), 2) + pow(p(1), 2);
          double a = acos(p(3));
          double uw = sqrt(-(p(3)*p(3) - 1)*n*n);

          J(0,0) = 2*a*u12 / de;
          J(0,1) = -2*a*p(1)*p(0) / de;
          J(0,2) = -2*a*p(2)*p(0) / de;
          J(0,3) = -2*p(0) / uw;
          J(1,0) = -2*a*p(0)*p(1) / de;
          J(1,1) = 2*a*u02 / de;
          J(1,2) = -2*a*p(1)*p(2) / de;
          J(1,3) = -2*p(1) / uw;
          J(2,0) = -2*a*p(0)*p(2) / de;
          J(2,1) = -2*a*p(1)*p(2) / de;
          J(2,2) = 2*a*u01 / de;
          J(2,3) = -2*p(2) / uw;

          return J;
        }

        template <typename Scalar_>
        inline const typename Eigen::Matrix<Scalar_, 4, 3> & quatV(){
          static const Eigen::Matrix<Scalar_, 4, 3> V = 0.5 * Eigen::Matrix<Scalar_, 4, 3>::Identity();
          return V;
        }
        template const Eigen::Matrix<double, 4, 3> & quatV();
        template const Eigen::Matrix<float, 4, 3> & quatV();

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 3> expDiffMat(const Eigen::Matrix<Scalar_, 3, 1> & vec){
          Scalar_ phi = vec.norm();

          if(phi == 0){
            return Eigen::Matrix<Scalar_, 3, 3>::Identity();
          }

          Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

          Scalar_ phiAbs = fabs(phi);
          Scalar_ phiSquare = phi * phi;

          Scalar_ a;
          Scalar_ b;
          if(!isLessThenEpsilons4thRoot(phiAbs)){
            Scalar_ siPhiHalf = sin(phi / 2);
            a = (2 * siPhiHalf * siPhiHalf / phiSquare);
            b = ((1 - sin(phi) / phi)/phiSquare);
          }
          else{
            a = (1.0/2) * (1 - (1.0 / (24 / 2)) * phiSquare);
            b = (1.0/6) * (1 - (1.0 / (120 / 6)) * phiSquare);
          }

          return Eigen::Matrix<Scalar_, 3, 3>::Identity() - a * vecCross + b * vecCross * vecCross;
        }
        template Eigen::Matrix<double, 3, 3> expDiffMat<double>(const Eigen::Matrix<double, 3, 1>  &);
        template Eigen::Matrix<float, 3, 3> expDiffMat<float>(const Eigen::Matrix<float, 3, 1>  &);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_ , 4, 3> quatExpJacobian(const Eigen::Matrix<Scalar_ , 3, 1>& vec){
          return quatOPlus(axisAngle2quat(vec.template cast<double>())).template cast<Scalar_>() * quatV<Scalar_>() * expDiffMat(vec);
        }
        template Eigen::Matrix<double, 4,3> quatExpJacobian(const Eigen::Matrix<double, 3, 1>& vec);
        template Eigen::Matrix<float, 4,3> quatExpJacobian(const Eigen::Matrix<float, 3, 1>& vec);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 3> logDiffMat(const Eigen::Matrix<Scalar_, 3, 1>  & vec){
          Scalar_ phi = vec.norm();
          if(phi == 0){
            return Eigen::Matrix<Scalar_, 3, 3>::Identity();
          }

          Scalar_ phiAbs = fabs(phi);
          Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

          Scalar_ a;
          if(!isLessThenEpsilons4thRoot(phiAbs)){
            Scalar_ phiHalf = 0.5 * phi;
            a = ((1 - phiHalf / tan(phiHalf))/phi/phi);
          }
          else{
            a = 1.0 / 12 * (1 + 1.0 / 60 * phi * phi);
          }
          return Eigen::Matrix<Scalar_, 3, 3>::Identity() + 0.5 * vecCross + a * vecCross * vecCross;
        }
        template Eigen::Matrix<double, 3, 3> logDiffMat<double>(const Eigen::Matrix<double, 3, 1>  &);
        template Eigen::Matrix<float, 3, 3> logDiffMat<float>(const Eigen::Matrix<float, 3, 1>  &);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_ , 3, 4> quatLogJacobian2(const Eigen::Matrix<Scalar_ , 4, 1>& p){
          return logDiffMat(quat2AxisAngle<>(p)) * (quatV<Scalar_>().transpose() * Scalar_(4.0)) * quatOPlus(quatInv(p.template cast<double>())).template cast<Scalar_>();
        }

        template Eigen::Matrix<double, 3, 4> quatLogJacobian2(const Eigen::Matrix<double, 4, 1>&);
        template Eigen::Matrix<float, 3, 4> quatLogJacobian2(const Eigen::Matrix<float, 4, 1>& );
    }} // namespace sm::kinematics


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   SphericalHarmonics.cc
 *  @author Jeremy Roberts
 *  @date   Jun 30, 2011
 *  @brief  SphericalHarmonics member definitions
 */
//---------------------------------------------------------------------------//

#include "SphericalHarmonics.hh"
#include "utilities/Constants.hh"
#include "utilities/DBC.hh"
#include "utilities/GenException.hh"
#include "utilities/SoftEquivalence.hh"
#include <cmath>
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/factorials.hpp>
#endif

namespace detran_angle
{

// Calculate the spherical harmonic of degree l, order m
// given  cos(polar) and azimuthal angle
double SphericalHarmonics::Y_lm(const int    l,
                                const int    m,
                                const double xi,
                                const double varphi )
{
    Require (xi >= -1.0);
    Require (xi <= 1.0);
    Require (varphi >= 0.0);
    Require (varphi <= detran_utilities::two_pi);

    double sintheta = std::sqrt(1.0 - xi*xi);
    double mu  = sintheta*std::cos(varphi);
    double eta = sintheta*std::sin(varphi);
    return get_Y_lm(l, m, mu, eta, xi);

}

// Calculate the spherical harmonic of degree l, order m
// given the triplet of directional cosines
double SphericalHarmonics::Y_lm(const int    l,
                                const int    m,
                                const double mu,
                                const double eta,
                                const double xi )
{
    return get_Y_lm(l, m, mu, eta, xi);
}

// Calculate the Legendre polynomial of degree l
// given the polar angle cosine
double SphericalHarmonics::Y_lm(const int    l,
                                const double xi )
{
    return get_Y_lm(l, 0, 0, 0, xi);
}

double SphericalHarmonics::get_Y_lm(const int    l,
                                    const int    m,
                                    const double mu,
                                    const double eta,
                                    const double xi )
{
  using std::fabs;
  Require(l >= 0);
  Require(m <= l);
  Require(m >= -l);
  Require(mu >= -1.0);
  Require(mu <= 1.0);
  Require(eta >= -1.0);
  Require(eta <= 1.0);
  Require(xi >= -1.0);
  Require(xi <= 1.0);
  double unity = mu * mu + eta * eta + xi * xi;
  if (!detran_utilities::soft_equiv(1.0, unity, 1.0e-5))
  {
    Require( detran_utilities::soft_equiv( fabs(mu), 0.0 ) &&
             detran_utilities::soft_equiv( fabs(eta), 0.0 ) );
  }

  if ( l == 0 )
    return 1.0;
  else if ( l == 1 )
  {
    if ( m == -1 )
      return eta;
    else if ( m == 0 )
      return xi;
    else if ( m == 1 )
      return mu;
  }
  else if ( l == 2 )
  {
    if ( m == -2 )
      return 1.732050807568877*mu*eta;
    else if ( m == -1 )
      return 1.732050807568877*xi*eta;
    else if ( m == 0 )
      return 1.5*xi*xi-0.5;
    else if ( m == 1 )
      return 1.732050807568877*xi*mu;
    else if ( m == 2 )
      return 0.8660254037844385*(mu*mu-eta*eta);
  }
  else if ( l == 3 )
  {
    if ( m == -3 )
      return 0.7905694150420948*eta*(3*mu*mu-eta*eta);
    else if ( m == -2 )
      return 3.872983346207417*xi*mu*eta;
    else if ( m == -1 )
      return 0.6123724356957945*eta*(5.0*xi*xi-1.0);
    else if ( m == 0 )
      return 2.5*xi*xi*xi - 1.5*xi;
    else if ( m == 1 )
      return 0.6123724356957945*mu*(5.0*xi*xi-1.0);
    else if ( m == 2 )
      return 1.936491673103708*xi*(mu*mu-eta*eta);
    else if ( m == 3 )
      return 0.7905694150420948*mu*(mu*mu-3.0*eta*eta);
  }
  else
  {
#ifdef DETRAN_ENABLE_BOOST
    double phi   = std::acos(mu / std::sqrt(1.0 - xi*xi));
    double theta = std::acos(xi);
    // Normalization so that phi_00 = phi, phi_1m = current components, etc.
    // This leads to a match with definitions given by Hebert.
    double del = (m == 0) ? 1.0 : 2.0;
    double norm = std::pow(-1.0, m) *
                  std::sqrt(del * detran_utilities::four_pi / (2.*l + 1.));
    if (m >= 0)
      return norm * boost::math::spherical_harmonic_r(l, m, theta, phi);
    else
      return norm * boost::math::spherical_harmonic_i(l,-m, theta, phi);
#else
    // Degree not implemented.
    THROW("Maximum Legendre order is 3. For higher orders, enable Boost.");
	return 0;
#endif
  }
  return 0;
}

} // end namespace detran_angle

//---------------------------------------------------------------------------//
//              end of SphericalHarmonics.cc
//---------------------------------------------------------------------------//


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <mars/general_functions/utils.h>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cmath>
#include <iostream>

namespace mars
{
Utils::Utils() = default;

Eigen::MatrixXd Utils::EnforceMatrixSymmetry(const Eigen::Ref<const Eigen::MatrixXd>& mat_in)
{
  Eigen::MatrixXd mat_out = (mat_in + mat_in.transpose()) / 2;
  return mat_out;
}

Eigen::Matrix3d Utils::Skew(const Eigen::Vector3d& v)
{
  Eigen::Matrix3d res;
  res << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
  return res;
}

Eigen::Matrix4d Utils::MatExp(const Eigen::Matrix4d& A, const int& order)
{
  Eigen::Matrix4d matexp(Eigen::Matrix4d::Identity());  // initial condition with k=0

  int div = 1;
  Eigen::Matrix4d a_loop = A;

  for (int k = 1; k <= order; k++)
  {
    div = div * k;  // factorial(k)
    matexp = matexp + a_loop / div;
    a_loop = a_loop * A;  // adding one exponent each iteration
  }

  return matexp;
}

Eigen::Matrix4d Utils::OmegaMat(const Eigen::Vector3d& v)
{
  Eigen::Matrix4d res;
  res.setZero();

  res.block(0, 1, 1, 3) = -v.transpose();
  res.block(1, 0, 3, 1) = v;
  res.block(1, 1, 3, 3) = -Skew(v);

  return res;
}

Eigen::Quaterniond Utils::QuatFromSmallAngle(const Eigen::Vector3d& d_theta_vec)
{
  const double d_theta_vec_norm = d_theta_vec.norm();
  const double d_theta_squared = d_theta_vec_norm * d_theta_vec_norm;

  if (d_theta_squared / 4.0 < 1.0)
  {
    return Eigen::Quaterniond(sqrt(1 - d_theta_squared / 4.0), d_theta_vec[0] * 0.5, d_theta_vec[1] * 0.5,
                              d_theta_vec[2] * 0.5);
  }
  else
  {
    // In this case the error angle vector is not small at all.
    std::cout << "Warning: The error angle vector is not small" << std::endl;

    double w = 1.0 / sqrt(1 + d_theta_squared / 4.0);
    double f = w * 0.5;
    return Eigen::Quaterniond(w, d_theta_vec[0] * f, d_theta_vec[1] * f, d_theta_vec[2] * f);
  }
}

Eigen::Quaterniond Utils::ApplySmallAngleQuatCorr(const Eigen::Quaterniond& q_prior, const Eigen::Vector3d& correction)
{
  return (q_prior * QuatFromSmallAngle(correction)).normalized();
}

bool Utils::CheckCov(const Eigen::MatrixXd& cov_mat, const std::string& description, const bool& /*check_cond*/)
{
  bool result = true;
  std::string info("[" + description + "]" + ": ");

  bool is_symmetric = Eigen::MatrixXd::Zero(cov_mat.rows(), cov_mat.cols()).isApprox(cov_mat - cov_mat.transpose());
  if (!is_symmetric)
  {
    std::cout << "Warning: " << info << "The covariance matrix is not symmetric" << std::endl;
    result = false;
  }

  double det = cov_mat.determinant();
  if (det <= 0)
  {
    std::cout << "Warning: " << info << "The determinant of the covariance matrix is not positive (" << det << ")"
              << std::endl;
    result = false;
  }

  double min_eigenvalue = Eigen::EigenSolver<Eigen::MatrixXd>(cov_mat).eigenvalues().real().minCoeff();
  if (min_eigenvalue < 0)
  {
    std::cout << "Warning: " << info << "The covariance matrix is not positive semidefinite min: " << min_eigenvalue
              << std::endl;
    result = false;
  }

  return result;
}
}


/**
 * ODE solver - JMU REU 2021
 *
 * @author Mike Lam
 */

// standard headers
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <cmath>
#include <functional>
using namespace std;

// Boost ODEint headers (individual files compile faster than the catch-all)
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/integrate/integrate_const.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
using namespace boost::numeric::odeint;

// floating-point precision control (control via compiler parameter -Dreal_t)
//typedef double real_t;      // 64 bits
//typedef float real_t;       // 32 bits

// solver state
typedef std::vector<real_t> state_t;

// parameters (most read from command line)
real_t a, b, c, x0;
double t0 = 0.0;
double tn, dt;
const int N = 10;
real_t ac[N+1];

// output stream
ofstream out;

// ODE system
void ode(const state_t &x, state_t &dxdt, const real_t /*t*/)
{
    dxdt[0] = a*pow(x[0],2.0) + b*x[0] + c;
}

// power series solution at x
void calc_coefficients(real_t x)
{
    ac[0] = x;
    ac[1] = a * ac[0]*ac[0] + b * ac[0] + c;

    for (int i = 2; i <= N; i++) {
        ac[i] = 0.0;
        for (int k = 0; k < i; k++) {
            ac[i] += ac[k] * ac[i-1-k];
        }
        ac[i] = (a * ac[i] + b * ac[i-1]) / i;
    }
}

// power series solution using Horner's method
real_t psm(const real_t t)
{
    real_t x = t * ac[N];
    for (int j = N-1; j > 0; j--) {
        x = t * (x + ac[j]);
    }
    x += ac[0];
    return x;
}

// x \left( t \right) =1/2\,{\frac {1}{a} \left( \tan \left( 1/2\,t\sqrt
// {4\,ac-{b}^{2}}+\arctan \left( {\frac {2\,{\it x0}\,a+b}{\sqrt {4\,ac-
// {b}^{2}}}} \right)  \right) \sqrt {4\,ac-{b}^{2}}-b \right) }
//
// @author Kevin Rojas
real_t solution(real_t a,real_t b, real_t c, real_t x0, real_t t)
{
    /* Inputs x:
     * a,b,c: Parameters of the ODE
     * x0   : Initial condition
     * x    : Point to evaluate the exact solution on
     * Output:
     * x(t) where x solves x' = ax+bx+c, x(0) = x0
     */

    real_t disc = b*b-4.0*a*c;
    //cout << "discriminant: " << disc << endl;
    real_t val = 0.0;

    if (disc > 0.0) {
       real_t x1 = (-b+sqrt(disc))/(2.0*a);
       real_t x2 = (-b-sqrt(disc))/(2.0*a);
       real_t A = abs((x0-x2)/(x0-x1))*exp((x2-x1)*a*t);
       real_t val1 = (A*x1-x2)/(A-1);

       real_t B = -A;
       real_t val2 = (B*x1-x2)/(B-1);

       if((val1 >x2 && val1> x1) || (val1<x2 && val1<x1)){
           val = val1;
       }
       else{
           val = val2;
       }
       val = val2;
       // TODO: deal with the absolute value?

    } else if (disc < 0.0) {
       real_t A = b/(2.0*sqrt(a));
       real_t D = -disc/(4.0*a*a);
       real_t c0 = atan((x0+A)/sqrt(D));
       val = sqrt(D)*tan(a*sqrt(D)*t+c0)-A;

    } else {
       real_t x1 = -b/(2.0*a);
       real_t c0 = -1.0/(x0-x1);
       val = (x1-1.0/(a*t+c0));
    }
    return val;
}

// debug output helper
void observe(const state_t &x, const real_t t)
{
    real_t sol = solution(a, b, c, x0, t);
    real_t psm_sol = psm(0);

    // re-center power series
    calc_coefficients(psm(dt));

    // write output
    out << setw(8) << t;
    out << setw(16) << sol;
    out << setw(16) << x[0] << setw(16) << fabs(sol-x[0]);
    out << setw(16) << psm_sol << setw(16) << fabs(sol-psm_sol);
    out << endl;
}

int main(int argc, const char* argv[])
{
    // check parameters
    if (argc != 7) {
        cout << "Usage: " << argv[0] << " <a> <b> <c> <x0> <tn> <dt>" << endl;
        return EXIT_FAILURE;
    }

    // parse parameters
    a  = stod(argv[1], NULL);
    b  = stod(argv[2], NULL);
    c  = stod(argv[3], NULL);
    x0 = stod(argv[4], NULL);
    tn = stod(argv[5], NULL);
    dt = stod(argv[6], NULL);
    // TODO: add stepper and t0?

    // initialize state
    state_t x(1);
    x[0] = x0;

    // precalculate coefficients
    calc_coefficients(x0);

    // show floating-point width
    //cout << "sizeof(real_t)=" << sizeof(real_t) << endl;

    // stepper
    runge_kutta4<state_t> stp;

    // integrate w/ debug output
    out.open("out.dat");
    /*size_t steps =*/ integrate_const(stp, ode, x, t0, tn, dt, observe);
    out.close();

    // show final output
    //cout << "steps=" << steps << " x=" << x[0] << endl;

    return EXIT_SUCCESS;
}


/**
 * @file model_predictive_controller.cpp
 * @brief Finite-horizon Model Predictive Controller.
 *
 */

#include <mpc.hpp>
#include <Eigen/Dense>
#include <iostream>

using namespace controller;

MPC::MPC(const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const double S) :
    Q(Q), R(R), saturation(S), converged(false)
{
    P_new.setZero();
    Ad.setZero(Q.rows(), Q.cols());
    I.setIdentity(Q.rows(), Q.cols());
    cmd_vel.setZero(1, R.rows()); // [translation rate, rotation rate].
}

Eigen::MatrixXd MPC::computeDiscrete(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& E, unsigned int numIteration, double tolarance, double dt)
{
    // A Cost-to-go matrix P evolving backwards in time from Q is defined as,
    P = Q;

    // Discretization.
    Ad = (I + A * dt * 0.5) * (I - A * dt * 0.5).inverse();
    Bd = B * dt;

    // Iterating over a finite-horizon Algebraic Riccati Equation to a steady-state solution; that is, P_new - P ~= 0 + tolarance.
    for(unsigned int i = 0; i < numIteration; i++)
    {
        P_new = Ad.transpose() * P * Ad - (Ad.transpose() * P * Bd) * (R + Bd.transpose() * P * Bd).inverse() * (Bd.transpose() * P * Ad) + Q;

        // Check system convergence.
        difference = fabs((P_new - P).maxCoeff());
        if(difference < tolarance)
        {
            // std::cout << "Solution found at i = " << i << std::endl;

            converged = true;
            break;
        }

        // Update Riccati solution.
        P = P_new;
    }

    // Get cmd_vel.
    if(converged)
    {
        K = (R + Bd.transpose() * P * Bd).inverse() * Bd.transpose() * P * Ad;
        controlUpdate = K * E;

        // Compute magnitude of velocity vector.
        cmd_vel <<
            sqrt(pow(controlUpdate(0, 0), 2.0) + pow(controlUpdate(0, 1), 2.0) + pow(controlUpdate(0, 2), 2.0)),
            sqrt(pow(controlUpdate(1, 0), 2.0) + pow(controlUpdate(1, 1), 2.0) + pow(controlUpdate(1, 2), 2.0));

        // Saturate output signal.
        if(cmd_vel(0, 0) > saturation) cmd_vel(0, 0) = saturation;
        if(cmd_vel(0, 0) < -saturation) cmd_vel(0, 0) = -saturation;
        if(cmd_vel(0, 1) > saturation) cmd_vel(0, 1) = saturation;
        if(cmd_vel(0, 1) < -saturation) cmd_vel(0, 1) = -saturation;

        return cmd_vel;
    }
    else
    {
        std::cout << "MPC controller did not converge. Resetting cmd_vel... 沒有解決方案" << std::endl;
        cmd_vel.setZero();

        return cmd_vel;
    }

    converged = false;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2019.
//  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)
//

// chapter10_08-001_pi_millions_with_boost.cpp

// This program can be used to compute millions of digits of pi.
// In fact, it has been used to compute more than one billion
// decimal digits of pi. Boost.Multiprecision is combined with
// GMP (or MPIR) in order to carry out the calculation of pi.

// This program requires inclusion of Boost.Multiprecision
// and linking with GMP (or MPIR on certain targets).

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <regex>
#include <sstream>
#include <string>

#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/gmp.hpp>

namespace pi { namespace millions { namespace detail {

// *****************************************************************************
// Function    : template<typename float_type>
//               const float_type& calculate_pi_template(const bool print_progress)
//
// Description : Compute pi using a quadratically convergent Gauss AGM,
//               in the Schoenhage variant. For a description of the algorithm,
//               see Algorithm 16.148, Chapter 16, page 236 in the book
//               J. Arndt and C. Haenel, "Pi Unleashed",
//               (Springer Verlag, Heidelberg, 2001).
//
//               The parameter print_progress_to_cout (= true),
//               will print calculation progress to std::cout.
//
//               Book reference for "Pi Unleashed":
//               https://www.springer.com/de/book/9783642567353
//
// *****************************************************************************
template<typename float_type>
const float_type& pi(const bool print_progress = false)
{
  static bool is_init;

  static float_type val_pi;

  if(!is_init)
  {
    is_init = true;

    const std::regex rx("^[^e]+[e0+-]+([0-9]+)$");

    std::match_results<std::string::const_iterator> mr;

    std::stringstream ss;
    ss.setf(std::ios::scientific);
    ss.precision(static_cast<std::streamsize>(4));

    float_type a (1.0F);
    float_type bB(0.5F);
    float_type s (0.5F);
    float_type t (0.375F);

    // This loop is designed for computing a maximum of a few billion
    // decimal digits of pi. The number of digits roughly doubles
    // with each iteration of the loop. After 20 iterations,
    // the precision is about 2.8 million decimal digits.
    // After 29 iterations, the precision is more than one
    // billion decimal digits.

    for(std::uint_least16_t k = UINT8_C(1); k < UINT8_C(64); ++k)
    {
      using std::sqrt;

      a      += sqrt(bB);
      a      /= 2U;
      val_pi  = a;
      val_pi *= val_pi;
      bB      = (val_pi - t);
      bB     *= 2U;

      const float_type iterate_term((bB - val_pi) * (UINT64_C(1) << k));

      s += iterate_term;

      // Extract the base-10 order of magnitude
      // to estimate the base-10 digits in this
      // iteration.

      // Here, we produce a short printout
      // of the iteration term that is subsequently
      // parsed with a regular expression
      // for extracting the base-10 order.

      // Note: We are only extracting a few digits from iterate_term.
      // So piping it to a stringstream is not exorbitantly costly here.
      ss << iterate_term;

      const std::string str_iterate_term(ss.str());

      const bool is_match =
        std::regex_match(str_iterate_term, mr, rx);

      const std::uint64_t digits10_iterate =
        (is_match ? (std::max)(boost::lexical_cast<std::uint64_t>(mr[1U]), std::uint64_t(0U))
                  : UINT64_C(0));

      if(print_progress)
      {
        std::cout << "Base-10 digits of iteration "
                  << std::right
                  << std::setw(3)
                  << k
                  << ": "
                  << std::right
                  << std::setw(12)
                  << digits10_iterate
                  << '\n';
      }

      // Test the approximate base-10 digits
      // of this iteration term.

      // If we have attained at least half or more
      // of the total desired digits with this
      // iteration, the calculation is finished
      // because the change from the next iteration will be
      // insignificantly small.
      BOOST_CONSTEXPR_OR_CONST std::uint64_t digits10_iterate_goal =
        static_cast<std::uint64_t>((static_cast<std::uint64_t>(std::numeric_limits<float_type>::digits10) + 1LL) / 2LL) + 16LL;

      if(digits10_iterate > digits10_iterate_goal)
      {
        break;
      }

      t  = val_pi;
      t += bB;
      t /= 4U;

      ss.str(std::string());
    }

    if(print_progress)
    {
      std::cout << "Iteration loop done, compute inverse" << '\n';
    }

    val_pi += bB;
    val_pi /= s;

    if(print_progress)
    {
      std::cout << "The pi calculation is done." << '\n';
    }
  }

  return val_pi;
}

template<typename float_type>
std::ostream& report_pi_timing(std::ostream& os, const float elapsed)
{
  return os << "=================================================" << '\n'
            << "Computed "
            << static_cast<std::uint64_t>(std::numeric_limits<float_type>::digits10 - 1)
            << " digits of pi.\n"
            << "Total computation time : "
            << std::fixed
            << std::setprecision(2)
            << elapsed
            << " seconds"
            << '\n'
            << "================================================="
            << '\n';
}

} } } // namespace pi::millions::detail

namespace pi { namespace millions {

template<typename float_type>
void print_pi(std::ostream& os)
{
  // Calculate the value of pi. When doing so, print the calculation
  // messages to the console. Use the clock function to obtain the
  // total time of the pi calculation.

  using local_time_point_type =
    std::chrono::high_resolution_clock::time_point;

  const local_time_point_type start = std::chrono::high_resolution_clock::now();
  detail::pi<float_type>(true);
  const local_time_point_type stop  = std::chrono::high_resolution_clock::now();

  // Evaluate the time that was required for the pi calculation.
  const float elapsed =
      static_cast<float>(std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count())
    / static_cast<float>(1000.0F);

  // Report the time of the pi calculation to the console.
  static_cast<void>(detail::report_pi_timing<float_type>(std::cout, elapsed));

  // Report the time of the pi calculation to the output stream.
  static_cast<void>(detail::report_pi_timing<float_type>(os, elapsed));

  // Report that we are writing the output file.
  std::cout << "Writing the output file." << '\n';

  // Pipe the value of pi into a stringstream object.
  std::stringstream ss;

  // Pipe the value of pi into a stringstream object with full precision.
  ss << std::fixed
     << std::setprecision(std::streamsize(std::numeric_limits<float_type>::digits10) - 1)
     << detail::pi<float_type>();

  // Extract the string value of pi.
  const std::string str_pi(ss.str());

  // Print pi using the following paramater-tunable format.

  // pi = 3.1415926535 8979323846 2643383279 5028841971 6939937510 : 50
  //        5820974944 5923078164 0628620899 8628034825 3421170679 : 100
  //        8214808651 3282306647 0938446095 5058223172 5359408128 : 150
  //        4811174502 8410270193 8521105559 6446229489 5493038196 : 200
  //        ...

  BOOST_CONSTEXPR_OR_CONST char* char_set_separator   = " ";
  BOOST_CONSTEXPR_OR_CONST char* char_group_separator = "\n";

  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_set   = 10U;
  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_line  = digits_per_set * 5U;
  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_group = digits_per_line * 10U;

  // The digits after the decimal point are grouped
  // in sets of digits_per_set with digits_per_line
  // digits per line. The running-digit count is reported
  // at the end of each line.

  // The char_set_separator character string is inserted
  // between sets of digits. Between groups of lines,
  // we insert a char_group_separator character string
  // (which likely might be selected as a newline).

  // For a simple verification of 1,000,000 digits,
  // for example, go to Wolfram Alpha and ask:
  //   1000000th digit of Pi.
  // This prints out 50 digits of pi in the neighborhood
  // of a million digits, with the millionth digit in bold.

  std::string::size_type pos;

  if(   ((pos = str_pi.find(char('3'), 0U)) != std::string::npos)
     && ((pos = str_pi.find(char('.'), 1U)) != std::string::npos)
     && ((pos = str_pi.find(char('1'), 1U)) != std::string::npos))
  {
    ;
  }
  else
  {
    pos = 0U;
  }

  os << "pi = " << str_pi.substr(0U, pos);

  const std::size_t digit_offset = pos;

  // Extract the digits after the decimal point in a loop.
  // Insert spaces, newlines and a running-digit count
  // in order to create a format for comfortable reading.

  bool all_output_streaming_is_finished = false;

  while(all_output_streaming_is_finished == false)
  {
    // Print a set of digits (i.e. having 10 digits per set).
    const std::string str_pi_substring(str_pi.substr(pos, digits_per_set));

    os << str_pi_substring << char_set_separator;

    pos += (std::min)(std::string::size_type(digits_per_set),
                      str_pi_substring.length());

    const std::size_t number_of_digits(pos - digit_offset);

    // Check if all output streaming is finished.
    all_output_streaming_is_finished = (pos >= str_pi.length());

    if(all_output_streaming_is_finished)
    {
      // Write the final digit count.
      // Break from the printing loop.
      // Flush the output stream with std::endl.

      os << ": " << number_of_digits << std::endl;
    }
    else
    {
      const bool this_line_is_finished =
        (std::size_t(number_of_digits % digits_per_line) == std::size_t(0U));

      if(this_line_is_finished)
      {
        // Print the running-digit count and start a new line.
        os << ": " << number_of_digits << std::endl;

        const bool this_group_of_lines_is_finished =
          (std::size_t(number_of_digits % digits_per_group) == std::size_t(0U));

        if(this_group_of_lines_is_finished)
        {
          // Insert a character (which might be a blank line)
          // after a group of lines.
          os << char_group_separator;
        }

        // Insert spaces at the start of the new line.
        os << "       ";
      }
    }
  }
}

} } // namespace pi::millions

int main()
{
  using float_type =
    boost::multiprecision::number<boost::multiprecision::gmp_float<1000001UL>,
                                  boost::multiprecision::et_off>;

  std::ofstream out("pi.out");

  if(out.is_open())
  {
    pi::millions::print_pi<float_type>(out);

    out.close();
  }

  return 0;
}


#pragma once

#include "EM.hpp"
#include <Eigen/Dense>
#include <cmath>

template<class Float, class C>
requires ContainerOf<C, Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> >
Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> weightedSum(
    const std::vector<Float>& a,
    const C& x)
{
    typedef Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> Vec;
    assert( a.size() == x.size() );
    int n = x.begin()->size();
    return std::transform_reduce(a.begin(), a.end(), x.begin(),
                                 (Vec)Vec::Zero(n));
}

template<class Float, int N>
class IndependentGaussian
        : public ProbabilityDistribution<Eigen::Matrix<Float, N, 1>, Float > {
private:
    typedef Eigen::Matrix<Float, N, 1> Vec;
public:
    size_t n;
    Vec deviation;
    Vec mean;

    IndependentGaussian(size_t _n = N) :
        n{_n}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
        init();
    }

    IndependentGaussian(const Vec& _deviation, const Vec& _mean) :
        n{(size_t)_deviation.size()},
        deviation{_deviation},
        mean{_mean}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Float operator()(const Vec& x) const{
        assert((size_t)x.size() == n);
        Float p = 1.0;
        Float s = 0.0;
        for(size_t j=0; j<n; j++){
            p *= one_two_pi/deviation(j);
            s += - (x[j]-mean[j])*(x[j]-mean[j])/2.0/deviation[j]/deviation[j];
        }
        return p*exp(s);
    }

    template <Container C>
    requires ContainerOf<C, Vec>
    void likelihoodEstimate(const std::vector<Float>& a,
                            const C& x)
    {
        Float sumA = std::reduce(a.begin(), a.end());
        if(sumA == 0.0){
            return;
        }
        mean = weightedSum(a, x)/sumA;

        Vec s2 = std::transform_reduce(
            a.begin(), a.end(), x.begin(),
            (Vec)Vec::Zero(n),
            std::plus<>(),
            [&](Float ai, const Vec& xi) -> Vec {
                return ai*(xi-mean).unaryExpr([&](auto xi_m){ return xi_m*xi_m; });
            });

        deviation = (s2/sumA).cwiseSqrt();
    }
private:
    static constexpr Float one_two_pi = 1.0/sqrt(2*M_PI);
    void init(){
        mean = Vec::Zero(n);
        deviation = Vec::Constant(n, 1.0);
    }
};

template<class Float, int N>
class Gaussian
        : public ProbabilityDistribution<Eigen::Matrix<Float, N, 1>, Float > {
private:
    typedef Eigen::Matrix<Float, N, 1> Vec;
    typedef Eigen::Matrix<Float, N, N> Mat;
    size_t n;
public:
    Vec mean;
    class : public Mat{
    public:
        Float constantFactor{ calculateConstantFactor() };
        void operator=(const Mat& _invCovariance){
            assert(_invCovariance.rows() == _invCovariance.cols() && "The inverse covariance matrix must be square");
            Mat::operator=(_invCovariance);
            constantFactor = calculateConstantFactor();
        }
    private:
        Float calculateConstantFactor(){
            return pow(one_two_pi, this->rows())*sqrt(this->determinant());
        }
    } invCovariance;

    Gaussian(size_t _n=N) :
        n{_n},
        mean{ Vec::Zero(n) },
        invCovariance{ Vec::Constant(n, 1.0).asDiagonal() }
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Gaussian(const Mat& _invCovariance, const Vec& _mean) :
        n{(size_t)_mean.size()},
        mean{_mean},
        invCovariance{_invCovariance}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Float operator()(const Vec& x) const{
        assert((size_t)x.size() == n);
        return invCovariance.constantFactor * exp(-0.5*(x-mean).dot(invCovariance*(x-mean)));
    }

    template <Container C>
    requires ContainerOf<C, Vec>
    void likelihoodEstimate(const std::vector<Float>& a,
                            const C& x)
    {
        Float sumA = std::reduce(a.begin(), a.end());
        if(sumA == 0.0){
            return;
        }
        mean = weightedSum(a, x)/sumA;

        Mat Cov = std::transform_reduce(
            a.begin(), a.end(), x.begin(),
            (Mat)Mat::Zero(n, n),
            std::plus<>(),
            [&](Float ai, const Vec& xi) -> Mat {
                return ai*(xi-mean)*(xi-mean).transpose();
            });
        //FIXME: what if there is no inverse?
        invCovariance = (Cov/sumA).inverse();
    }
private:
    static constexpr Float one_two_pi = 1.0/sqrt(2*M_PI);
};


#pragma once

/// @file

#include <type_traits>

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

/// neon namespace
namespace neon
{
/// Perform a floating point comparison using boost for large numbers and an absolute error for smaller ones

/// \tparam Floating point type
/// \param x Value 1
/// \param y Value 2
/// \param absolute_tolerance
/// \param relative_tolerance
template <class T>
auto is_approx(T const x,
               T const y,
               T const absolute_tolerance = 0.0,
               T const relative_tolerance = 1e-10) noexcept
    -> std::enable_if_t<std::is_floating_point<T>::value, bool>
{
    // References:
    // https://www.boost.org/doc/libs/1_67_0/libs/math/doc/html/math_toolkit/float_comparison.html
    // https://www.python.org/dev/peps/pep-0485/
    return boost::math::relative_difference(x, y) <= relative_tolerance
           || std::abs(x - y) <= absolute_tolerance;
}
}


#include "iostream"
#include <cmath>
#include <string>
#include "CoupledPde.hpp"
#include "gnuplot_i.hpp"
#include <omp.h>
// #include <boost/test/unit_test.hpp>

using namespace Eigen;
using namespace std;

double model_prob_1_rhs(double x){return 2.0;}
double model_prob_2_rhs(double x){return 34.0*sin(x);}
double model_prob_3_rhs(double x, double y){return exp(-(x*x+y*y));}
double uj0(double x){return 1*sin(x)+1;}

int main(int argc, char* argv[]) {
    // int nthreads, tid;
    // #pragma omp parallel
    // {
    //     /* Obtain thread number */
    //     tid = omp_get_thread_num();
    //     printf("Hello World from thread = %d\n", tid);

    //     /* Only master thread does this */
    //     if (tid == 0) {
    //         nthreads = omp_get_num_threads();
    //         printf("Number of threads = %d\n", nthreads);
    //     }
    // }

    SecondOrderOde ode_mp1(500, -5.0, 0.0, model_prob_1_rhs, 0.0, 15.0);
    SecondOrderOde ode_mp2(5, -5.0, 0.0, model_prob_1_rhs, 0.0, 15.0);
    BoundaryConditions bc_mp1;
    BoundaryConditions bc_mp2;
    bc_mp1.SetX0DirichletBc1D(0.15);
    bc_mp1.SetXNNeumannBc1D(0);
    bc_mp2.SetX0NeumannBc1D(1.0);
    bc_mp2.SetXNNeumannBc1D(0);
    // bc_mp1.SetX0NeumannBc1D(0);
    // bc_mp1.SetXNDirichletBc1D(0);
    // bc_mp1.SetXNNeumannBc1D(0);
    // bc_mp1.SetXNRobinBc1D(0);
    BvpPde oxygen(&ode_mp1, &bc_mp1,0.001, 1.0, 10.0, 128, uj0);
    BvpPde organic_mater_1(&ode_mp2, &bc_mp2,0.001, 1.0, 10.0, 128, uj0);
    // BvpOde oxygen(&ode_mp1, &bc_mp1, 10);
    oxygen.SetFilename("oxygen_results.dat");
    organic_mater_1.SetFilename("om1_results.dat");
    // oxygen.Solve();
    CoupledPde cpde(&oxygen, &organic_mater_1);
    cpde.SolveSystem();

    Gnuplot g1, g2;
    g1.set_style("lines").plot_xy(oxygen.mpGrid->xGrid,oxygen.solution,"differentiation");
    g2.set_style("lines").plot_xy(organic_mater_1.mpGrid->xGrid,organic_mater_1.solution,"differentiation");
    // g1.set_style("lines").plot_equation("0.5*x*(1-x)","exact solution");

    // Gnuplot g2;
    // SecondOrderOde ode_mp2(10.0, 5.0, 0.0, uj0, 0.0, 20.0);
    // BoundaryConditions bc_mp2;
    // bc_mp2.SetX0NeumannBc1D(10.0);
    // bc_mp2.SetXNDirichletBc1D(0.0);
    // BvpOde organic_mater_1(&ode_mp2, &bc_mp2, 64);
    // organic_mater_1.SetFilename("model_problem_results2.dat");
    // organic_mater_1.Solve();
    // g2.set_style("points").plot_xy(organic_mater_1.mpGrid->xGrid,organic_mater_1.solution);
    // g2.set_style("lines").plot_equation("(4*exp(x) + exp(-4*x) ) / (4*exp(pi)+exp(-4*pi))-5*sin(x)-3*cos(x)","exact solution");

    return 0;
}


/**
 * @file    pose_estimation_3d2d.cpp
 * @version v1.0.0
 * @date    Oct,27 2017
 * @author  jacob.lin
 */

#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 <chrono>

using namespace std;
using namespace cv;

/* 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) {
        cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2." << 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()) {
        cout << "img1 or img2 is no find." << 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);
    cout << "feature matches points total is " << matches.size() << 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);
    }
    cout << "3d-2d pairs: " << pts_3d.size() << 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公式转换为矩阵

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

    cout << "calling bundle adjustment" << 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 <= 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
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();    // 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(
        g2o::SE3Quat(
            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) {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        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) {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(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++;
    }

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

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


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#ifndef quantext_nadaraya_watson_regression_hpp
#define quantext_nadaraya_watson_regression_hpp

#include <ql/math/comparison.hpp>

#include <boost/make_shared.hpp>

/*! \file qle/math/nadarayawatson.hpp
    \brief Nadaraya-Watson regression
    \ingroup math
*/

namespace QuantExt {
using QuantLib::Real;
using QuantLib::Size;
namespace detail {

//! Regression impl
/*! \ingroup math
 */
class RegressionImpl {
public:
    virtual ~RegressionImpl() {}
    virtual void update() = 0;
    virtual Real value(Real x) const = 0;
    virtual Real standardDeviation(Real x) const = 0;
};

//! Nadaraya Watson impl
/*! \ingroup math
 */
template <class I1, class I2, class Kernel> class NadarayaWatsonImpl : public RegressionImpl {
public:
    /*! \pre the \f$ x \f$ values must be sorted.
        \pre kernel needs a Real operator()(Real x) implementation
    */
    NadarayaWatsonImpl(const I1& xBegin, const I1& xEnd, const I2& yBegin, const Kernel& kernel)
        : xBegin_(xBegin), xEnd_(xEnd), yBegin_(yBegin), kernel_(kernel) {}

    void update() {}

    Real value(Real x) const {

        Real tmp1 = 0.0, tmp2 = 0.0;

        for (Size i = 0; i < static_cast<Size>(xEnd_ - xBegin_); ++i) {
            Real tmp = kernel_(x - xBegin_[i]);
            tmp1 += yBegin_[i] * tmp;
            tmp2 += tmp;
        }

        return QuantLib::close_enough(tmp2, 0.0) ? 0.0 : tmp1 / tmp2;
    }

    Real standardDeviation(Real x) const {

        Real tmp1 = 0.0, tmp1b = 0.0, tmp2 = 0.0;

        for (Size i = 0; i < static_cast<Size>(xEnd_ - xBegin_); ++i) {
            Real tmp = kernel_(x - xBegin_[i]);
            tmp1 += yBegin_[i] * tmp;
            tmp1b += yBegin_[i] * yBegin_[i] * tmp;
            tmp2 += tmp;
        }

        return QuantLib::close_enough(tmp2, 0.0) ? 0.0 : std::sqrt(tmp1b / tmp2 - (tmp1 * tmp1) / (tmp2 * tmp2));
    }

private:
    I1 xBegin_, xEnd_;
    I2 yBegin_;
    Kernel kernel_;
};
} // namespace detail

//! Nadaraya Watson regression
/*! This implements the estimator

    \f[
    m(x) = \frac{\sum_i y_i K(x-x_i)}{\sum_i K(x-x_i)}
    \f]

    \ingroup math
*/
class NadarayaWatson {
public:
    /*! \pre the \f$ x \f$ values must be sorted.
        \pre kernel needs a Real operator()(Real x) implementation
    */
    template <class I1, class I2, class Kernel>
    NadarayaWatson(const I1& xBegin, const I1& xEnd, const I2& yBegin, const Kernel& kernel) {
        impl_ = boost::make_shared<detail::NadarayaWatsonImpl<I1, I2, Kernel> >(xBegin, xEnd, yBegin, kernel);
    }

    Real operator()(Real x) const { return impl_->value(x); }

    Real standardDeviation(Real x) const { return impl_->standardDeviation(x); }

private:
    boost::shared_ptr<detail::RegressionImpl> impl_;
};

} // namespace QuantExt

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/strategies/side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies:
    left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_triangle
{
public :

    // Template member function, because it is not always trivial
    // or convenient to explicitly mention the typenames in the
    // strategy-struct itself.

    // Types can be all three different. Therefore it is
    // not implemented (anymore) as "segment"

    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename coordinate_type<P1>::type,
                                typename coordinate_type<P2>::type
                            >::type,
                        typename coordinate_type<P>::type
                    >::type,
                CalculationType
            >::type coordinate_type;

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

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

        // Promote float->double, small int->int
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type const dx = sx2 - sx1;
        promoted_type const dy = sy2 - sy1;
        promoted_type const dpx = x - sx1;
        promoted_type const dpy = y - sy1;

        promoted_type const s
            = geometry::detail::determinant<promoted_type>
                (
                    dx, dy,
                    dpx, dpy
                );

        promoted_type const zero = promoted_type();
        return math::equals(s, zero) ? 0
            : s > zero ? 1
            : -1;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef side_by_triangle<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP


#include <glm/models/links/logit.hpp>

#include <armadillo>

using namespace arma;

logit_link::logit_link()
    : glm_link::glm_link( "logit" )
{
}

vec
logit_link::init_beta(const mat &X, const vec &y) const
{
    vec mu = (y + 0.5) / 2.0;
    vec eta = log( mu / ( 1.0 - mu ) );

    return pinv( X ) * eta;
}

vec
logit_link::mu(const arma::vec &eta) const
{
    return 1.0 / ( 1.0 + exp( -eta ) );
}

vec
logit_link::eta(const arma::vec &mu) const
{
    return log( mu / ( 1 -mu ) );
}

vec
logit_link::mu_eta(const arma::vec &mu) const
{
    return 1.0 / ( mu % ( 1.0 - mu ) );
}


/*
 *  Copyright (c) 2015-, Filippo Basso <bassofil@gmail.com>
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *     1. Redistributions of source code must retain the above copyright
 *        notice, this list of conditions and the following disclaimer.
 *     2. Redistributions in binary form must reproduce the above copyright
 *        notice, this list of conditions and the following disclaimer in the
 *        documentation and/or other materials provided with the distribution.
 *     3. Neither the name of the copyright holder(s) nor the
 *        names of its contributors may be used to endorse or promote products
 *        derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 *  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 *  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 *  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 *  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <Eigen/SVD>
#include <calibration_algorithms/plane_to_plane_calibration.h>

namespace unipd
{
namespace calib
{

Transform3
PlaneToPlaneCalibration::estimateTransform (const std::vector<std::pair<Plane3, Plane3>> & plane_pair_vector)
{
  const Size1 SIZE = plane_pair_vector.size();
  assert(SIZE >= 10);

  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> normals_1(SIZE, 3);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> normals_2(SIZE, 3);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> distances_1(SIZE);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> distances_2(SIZE);

  for (int i = 0; i < SIZE; ++i)
  {
    const Plane3 & plane_1 = plane_pair_vector[i].first;
    const Plane3 & plane_2 = plane_pair_vector[i].second;

    if (plane_1.offset() > 0)
    {
      normals_1.row(i) = -plane_1.normal();
      distances_1(i) = plane_1.offset();
    }
    else
    {
      normals_1.row(i) = plane_1.normal();
      distances_1(i) = -plane_1.offset();
    }

    if (plane_2.offset() > 0)
    {
      normals_2.row(i) = -plane_2.normal();
      distances_2(i) = plane_2.offset();
    }
    else
    {
      normals_2.row(i) = plane_2.normal();
      distances_2(i) = -plane_2.offset();
    }
  }

  Transform3 transform;

  Eigen::Matrix<Scalar, 3, 3> USV = normals_2.transpose() * normals_1;
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd;
  svd.compute(USV, Eigen::ComputeFullU | Eigen::ComputeFullV);
  transform.linear() = svd.matrixV() * svd.matrixU().transpose();

//  transform.translation() = (normals_1.transpose() * normals_1).inverse() * normals_1.transpose() * (distances_1 - distances_2);

  // Use QR decomposition
  Eigen::ColPivHouseholderQR<Eigen::Matrix<Scalar, Eigen::Dynamic, 3> > qr(normals_1.rows(), 3);
  qr.compute(normals_1);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> R = qr.matrixR().template triangularView<Eigen::Upper>();
  transform.translation() = (qr.colsPermutation() * R.transpose() * R * qr.colsPermutation().transpose()).inverse() * normals_1.transpose() * (distances_1 - distances_2);

  return transform;
}

} // namespace calib
} // namespace unipd


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

//CRS
void compressedSparseRow()
{
/*
    0 1 2 3 4 5 6 7 8
   ┌                 ┐
 0 |0 0 0 0 0 0 0 3 0|
 1 |0 0 8 0 0 1 0 0 0|
 2 |0 0 0 0 0 0 0 0 0|
 3 |4 0 0 0 0 0 0 0 0|
 4 |0 0 0 0 0 0 0 0 0|
 5 |0 0 2 0 0 0 0 0 0|
 6 |0 0 0 6 0 0 0 0 0|
 7 |0 9 0 0 5 0 0 0 0|
   └                 ┘
*/

    int rows=8;
    int cols=9;
    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> dense_mat;
    dense_mat.resize(rows,cols);

    dense_mat<<0, 0, 0, 0, 0, 0, 0, 3, 0,
                0, 0, 8, 0, 0, 1, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0,
                4, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 2, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 6, 0, 0, 0, 0, 0,
                0, 9, 0, 0, 5, 0, 0, 0, 0;

    //specify a tolerance:
    //sparse_mat = dense_mat.sparseView(epsilon,reference);
    Eigen::SparseMatrix<double,Eigen::RowMajor> sparse_mat = dense_mat.sparseView();

    std::cout<<"sparse matrix number of cols:"<<sparse_mat.cols()   <<std::endl;
    std::cout<<"sparse matrix numner of rows:"<<sparse_mat.rows()   <<std::endl;

    std::cout<<"sparse matrix values:"<<std::endl;
    auto valuePtr=sparse_mat.valuePtr();
    for(int i=0;i <sparse_mat.nonZeros();i++)
    {
        std::cout<<*(valuePtr++)  <<", ";
    }
    std::cout<< "\n";

    std::cout<<"COL_INDEX array size:"<<sparse_mat.innerSize()  <<std::endl;
    std::cout<<"COL_INDEX array :" <<std::endl;
    auto colIndexPtr=sparse_mat.innerIndexPtr();

    for(int i=0;i <sparse_mat.innerSize();i++)
    {
        std::cout<<*(colIndexPtr++)  <<", ";
    }
    std::cout<< "\n";

    std::cout<<"ROW_INDEX array size:"<<sparse_mat.outerSize()   <<std::endl;
    std::cout<<"ROW_INDEX array:" <<std::endl;
    auto rowIndexPtr=sparse_mat.outerIndexPtr();
    for(int i=0;i <sparse_mat.outerSize();i++)
    {
        std::cout<<*(rowIndexPtr++)  <<", ";
    }
    std::cout<< "\n ";

    std::cout<<"*********sparse matrix*********\n"<<sparse_mat  <<std::endl;

    std::cout<<"*********dense matrix*********\n"<<dense_mat  <<std::endl;

}

int main1()
{
    compressedSparseRow();
}

typedef Eigen::SparseMatrix<double> SpMat; // declares a column-major sparse matrix type of double
typedef Eigen::Triplet<double> T;//structure to hold a non zero as a triplet (i,j,value).

void buildProblem(std::vector<T>& coefficients, Eigen::VectorXd& b, int n){}
void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename){}
//https://eigen.tuxfamily.org/dox/classEigen_1_1Triplet.html
int main(int argc, char** argv)
{
  if(argc!=2) {
    std::cerr << "Error: expected one and only one argument.\n";
    return -1;
  }

  int n = 300;  // size of the image
  int m = n*n;  // number of unknows (=number of pixels)

  // Assembly:
  std::vector<T> coefficients;            // list of non-zeros coefficients
  Eigen::VectorXd b(m);                   // the right hand side-vector resulting from the constraints
  buildProblem(coefficients, b, n);

  SpMat A(m,m);
  A.setFromTriplets(coefficients.begin(), coefficients.end());

  // Solving:
  Eigen::SimplicialCholesky<SpMat> chol(A);  // performs a Cholesky factorization of A
  Eigen::VectorXd x = chol.solve(b);         // use the factorization to solve for the given right hand side

  // Export the result to a file:
  saveAsBitmap(x, n, argv[1]);

  return 0;
}


// Unit test for the makeGrid function in fvm_1d_functions.cpp
// This function calculates a cell-centered grid
// COMPLETE. This function works as intended. 2021/11/18

#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 makeGrid function." << \
    " -------------------------------------" << endl;

    // Set the input parameters to the makeGrid function
    // The goal is to make a cell centered grid such that the according nodal grid
    // has a left value of -1.5, a right boundary of 0.5, and a dx of 0.5.
    // This would give 4 cells within the domain with an additional 2 ghost cells on either side
    int totalNCells = 8;
    int numGhost = 2;
    double xLeft = -1.5;
    double dx = 0.5;

    // Set the exact values for the 1D grid
    ArrayXd x_Exact(8);
    x_Exact << -2.25, -1.75, -1.25, -.75, -.25, .25, .75, 1.25;

    // Calculate the values based on the function
    ArrayXd x = makeGrid(totalNCells, numGhost, xLeft, dx);

    // Set the acceptable tolerance. (Somewhat arbitrarily chosen)
    double TOL = dx*1e-14;

    // Determine if the two arrays are equal
    Array<bool, Dynamic, 1> boolArray = isEqualArray1D(x, x_Exact, TOL);

    // Print results
    if (boolArray.minCoeff() == 1) {
        cout << "Test of makeGrid is a success." << endl;
    }
    else {
        cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of makeGrid is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
    }

    cout << endl << endl;
}


//  Copyright 2020 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <iostream>
#include <benchmark/benchmark.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/gmp.hpp>

#include <gmpxx.h>

template <class Integer>
inline Integer factorial(unsigned n)
{
   Integer result = 1;
   for (unsigned k = 1; k <= n; ++k)
      result *= k;
   return result;
}

template <class Rational, class Integer = typename Rational::value_type>
inline Rational binomial(unsigned n, unsigned k)
{
   return Rational(factorial<Integer>(n), factorial<Integer>(k) * factorial<Integer>(n - k));
}

inline mpz_class pow(mpz_class i, unsigned p)
{
   mpz_class result;
   mpz_pow_ui(result.get_mpz_t(), i.get_mpz_t(), p);
   return result;
}

template <class Rational, class Integer = typename Rational::value_type>
Rational Bernoulli(unsigned m)
{
   Rational result = 0;

   for (unsigned k = 0; k <= m; ++k)
   {
      Rational inner = 0;
      for (unsigned v = 0; v <= k; ++v)
      {
         Rational term = binomial<Rational, Integer>(k, v) * Rational(pow(Integer(v), m), k + 1);
         if (v & 1)
            term = -term;
         inner += term;
      }
      result += inner;
   }
   return result;
}

template <class Rational, class Integer = typename Rational::value_type>
static void BM_bernoulli(benchmark::State& state)
{
   int m = state.range(0);
   for (auto _ : state)
   {
      benchmark::DoNotOptimize(Bernoulli<Rational, Integer>(m));
   }
}

BENCHMARK_TEMPLATE(BM_bernoulli, boost::multiprecision::cpp_rational)->DenseRange(50, 200, 4);
BENCHMARK_TEMPLATE(BM_bernoulli, boost::multiprecision::mpq_rational)->DenseRange(50, 200, 4);
BENCHMARK_TEMPLATE(BM_bernoulli, boost::multiprecision::number<boost::multiprecision::rational_adaptor<boost::multiprecision::gmp_int> >)->DenseRange(50, 200, 4);
BENCHMARK_TEMPLATE(BM_bernoulli, mpq_class, mpz_class)->DenseRange(50, 200, 4);

BENCHMARK_MAIN();


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

//[safe_prime

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/miller_rabin.hpp>
#include <iostream>
#include <iomanip>

int main()
{
   using namespace boost::random;
   using namespace boost::multiprecision;

   typedef cpp_int int_type;
   mt11213b base_gen(clock());
   independent_bits_engine<mt11213b, 256, int_type> gen(base_gen);
   //
   // We must use a different generator for the tests and number generation, otherwise
   // we get false positives.
   //
   mt19937 gen2(clock());

   for(unsigned i = 0; i < 100000; ++i)
   {
      int_type n = gen();
      if(miller_rabin_test(n, 25, gen2))
      {
         // Value n is probably prime, see if (n-1)/2 is also prime:
         std::cout << "We have a probable prime with value: " << std::hex << std::showbase << n << std::endl;
         if(miller_rabin_test((n-1)/2, 25, gen2))
         {
            std::cout << "We have a safe prime with value: " << std::hex << std::showbase << n << std::endl;
            return 0;
         }
      }
   }
   std::cout << "Ooops, no safe primes were found - probably a bad choice of seed values!" << std::endl;
   return 0;
}

//]


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::algorithm::log_shift.hpp                      //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_DETAIL_LOG_SHIFT_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_ALGORITHM_DETAIL_LOG_SHIFT_HPP_ER_2009
#include <boost/math/special_functions/log1p.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{
namespace detail{

    template<typename T>
    T log_shift(
        const T& x,
        const T& t
    ){
        return log(x+t);
    }

    template<typename T>
    T logit_shift(
        const T& p,
        const T& t
    ){
        // log(p/(1-p)) = log(p)-log(1-p)

        return log_shift(p,t) - boost::math::log1p(t - p);
    }

}// detail
}// data
}// survival
}// statistics
}// boost

#endif