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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// 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_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP

#include <random>
#include <random>

#include <boost/geometry/algorithms/equals.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution
{

template
<
    typename Point,
    typename DomainGeometry,
    int Dim
>
struct uniform_inverse_transform_sampling
{};

template
<
    typename Point,
    typename DomainGeometry
>
struct uniform_inverse_transform_sampling<Point, DomainGeometry, 2>
{
    uniform_inverse_transform_sampling(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_inverse_transform_sampling const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        Point out;
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type computation_type;
        std::uniform_real_distribution<computation_type> lon_dist(
            get_as_radian<0, 0>(d),
            get_as_radian<1, 0>(d));
        set_from_radian<0>(out, lon_dist(g));

        coordinate_type lat1 = get_as_radian<0, 1>(d);
        coordinate_type lat2 = get_as_radian<1, 1>(d);
        coordinate_type x1   = (1.0 - std::cos(lat1)) / 2,
                        x2   = (1.0 - std::cos(lat2)) / 2;
        std::uniform_real_distribution<computation_type> x_dist(
            std::min(x1, x2),
            std::max(x1, x2));
        coordinate_type x = x_dist(g);
        set_from_radian<1>(out, std::acos(1.0 - 2.0 * x));
        return out;
    }
    void reset(DomainGeometry const&) {};
};

template
<
    typename Point,
    typename DomainGeometry
>
struct uniform_inverse_transform_sampling<Point, DomainGeometry, 3>
{
    uniform_inverse_transform_sampling(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                uniform_inverse_transform_sampling const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }
    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        uniform_inverse_transform_sampling<Point, DomainGeometry, 2> helper(d);
        Point out = helper.apply(g, d);
        typedef typename coordinate_type<Point>::type coordinate_type;
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type computation_type;
        coordinate_type r1 = get<0, 2>(d);
        coordinate_type r2 = get<1, 2>(d);
        std::uniform_real_distribution<computation_type>
            r_dist( r1 * r1 * r1 , r2 * r2 * r2 );
        set<2>(out, std::cbrt(r_dist(g)));
        return out;
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry
>
struct default_strategy
<
    Point,
    DomainGeometry,
    box_tag,
    single_tag, //There are no MultiBoxes right now
    2,
    spherical_tag
> : public uniform_inverse_transform_sampling<Point, DomainGeometry, 2> {
    typedef uniform_inverse_transform_sampling<Point, DomainGeometry, 2> base;
    using base::base;
};

template
<
    typename Point,
    typename DomainGeometry
>
struct default_strategy
<
    Point,
    DomainGeometry,
    box_tag,
    single_tag, //There are no MultiBoxes right now
    3,
    spherical_tag
> : public uniform_inverse_transform_sampling<Point, DomainGeometry, 3> {
    typedef uniform_inverse_transform_sampling<Point, DomainGeometry, 3> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_SPHERICAL_UNIFORM_INVERSE_TRANSFORM_SAMPLING_HPP


#include "mex.h"
#include <Eigen/Dense>
#include "../radialpose.h"
#include "../misc/ransac_estimator.h"
#include <RansacLib/ransac.h>

using namespace radialpose;

void print_usage() {
	mexPrintf("[R,t,f,params] = ransac_radialpose_mex(x,X,solver,tol,[min_iter],[max_iter]);\n");
	mexPrintf(" Solvers:\n");
	mexPrintf("  1 - D(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  2 - D(2,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  3 - D(3,0) - 5p -- Larsson et al.  ICCV 2019  (Minimal)\n");
	mexPrintf("  4 - D(3,3) - 8p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  5 - U(1,0) - 5p -- Larsson et al.  ICCV 2019\n");
	mexPrintf("  6 - U(0,1) - 4p -- Larsson et al.  ICCV 2017  (Minimal, Non-planar)\n");
	mexPrintf("  7 - U(0,1) - 4p -- Bujnak et al.   ACCV 2010  (Minimal, Non-planar)\n");
	mexPrintf("  8 - U(0,1) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf("  9 - U(0,2) - 5p -- Kukelova et al. ICCV 2013\n");
	mexPrintf(" 10 - U(0,3) - 5p -- Kukelova et al. ICCV 2013  (Minimal)\n");
	mexPrintf(" 11 - U(0,1) - 4p -- Oskarsson       arxiv 2018 (Minimal, Planar)\n");
	mexPrintf(" 12 - N/A    - 5p -- Kukelova et al. ICCV 2013  (Minimal, 1D Radial)\n\n");
}

void save_pose(int nlhs, mxArray *plhs[], Camera pose) {
	int n_sols = 1;
	int n_params = 0;
	if (n_sols > 0)
		n_params = pose.dist_params.size();

	if (nlhs >= 1) {
		plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
		double *p = mxGetPr(plhs[0]);
		for (int j = 0; j < 9; ++j)
			p[j] = pose.R(j);
	}
	if (nlhs >= 2) {
		plhs[1] = mxCreateDoubleMatrix(3, 1, mxREAL);
		double *p = mxGetPr(plhs[1]);
		for (int j = 0; j < 3; ++j)
			p[j] = pose.t(j);
	}
	if (nlhs >= 3) {
		plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
		double *p = mxGetPr(plhs[2]);
		*p = pose.focal;

	}
	if (nlhs >= 4) {
		plhs[3] = mxCreateDoubleMatrix(n_params, 1, mxREAL);
		double *p = mxGetPr(plhs[3]);
		for (int j = 0; j < n_params; ++j)
			p[j] = pose.dist_params[j];
	}
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs < 4 || nrhs > 7) {
		print_usage();
		mexErrMsgTxt("Incorrect number of input arguments.");
	}
	if (nlhs > 4) {
		print_usage();
		mexErrMsgTxt("Wrong number of output arguments.");
	}

	if (mxGetM(prhs[0]) != 2) {
		print_usage();
		mexErrMsgTxt("First input must be 2 x N matrix.");
	}
	if (mxGetM(prhs[1]) != 3) {
		print_usage();
		mexErrMsgTxt("Second input must be 3 x N matrix.");
	}
	if (mxGetN(prhs[0]) != mxGetN(prhs[1])) {
		print_usage();
		mexErrMsgTxt("Not the same number of 2D points and 3D points.");
	}

	double tol = 5.0;
	if (nrhs >= 4) {
		tol = mxGetScalar(prhs[3]);
	}

	ransac_lib::LORansacOptions options;
	options.squared_inlier_threshold_ = tol * tol;

	options.final_least_squares_ = true;

	if (nrhs >= 5)
		options.min_num_iterations_ = static_cast<int>(mxGetScalar(prhs[4]));
	if (nrhs >= 6)
		options.max_num_iterations_ = static_cast<int>(mxGetScalar(prhs[5]));

	double damp_factor = 0.0;
	if (nrhs >= 7) {
		damp_factor = mxGetScalar(prhs[6]);
	}

	Eigen::Matrix<double, 2, Eigen::Dynamic> x = Eigen::Map<Eigen::Matrix<double, 2, Eigen::Dynamic>>(mxGetPr(prhs[0]), 2, mxGetN(prhs[0]));
	Eigen::Matrix<double, 3, Eigen::Dynamic> X = Eigen::Map<Eigen::Matrix<double, 3, Eigen::Dynamic>>(mxGetPr(prhs[1]), 3, mxGetN(prhs[1]));
	int solver_idx = static_cast<int>(mxGetScalar(prhs[2]));

	ransac_lib::RansacStatistics ransac_stats;
	int inliers = 0;
	Camera best_model;
	best_model.R = Eigen::Matrix3d::Identity();
	best_model.t = Eigen::Vector3d::Zero();

	if (solver_idx == 1) { // D(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<1, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<1, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 2) { // D(2,0)
		radialpose::larsson_iccv19::Solver<2, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<2, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<2, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 3) { // D(3,0)
		radialpose::larsson_iccv19::Solver<3, 0, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<3, 0, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<3, 0, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 4) { // D(3,3)
		radialpose::larsson_iccv19::Solver<3, 3, true> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<3, 3, true>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<3, 3, true>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 5) { // U(1,0)
		radialpose::larsson_iccv19::Solver<1, 0, false> estimator;
		estimator.damp_factor = damp_factor;
		radialpose::RansacEstimator<larsson_iccv19::Solver<1, 0, false>> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv19::Solver<1, 0, false>>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 6) { // U(0,1)
		radialpose::larsson_iccv17::NonPlanarSolver estimator;
		radialpose::RansacEstimator<larsson_iccv17::NonPlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<larsson_iccv17::NonPlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 7) {  // U(0,1)
		radialpose::bujnak_accv10::NonPlanarSolver estimator;
		radialpose::RansacEstimator<bujnak_accv10::NonPlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<bujnak_accv10::NonPlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 8) {  // U(0,1)
		radialpose::kukelova_iccv13::Solver estimator(1);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 9) {  // U(0,2)
		radialpose::kukelova_iccv13::Solver estimator(2);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 10) { // U(0,3)
		radialpose::kukelova_iccv13::Solver estimator(3);
		radialpose::RansacEstimator<kukelova_iccv13::Solver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Solver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 11) {  // U(0,1)
		radialpose::oskarsson_arxiv18::PlanarSolver estimator;
		radialpose::RansacEstimator<oskarsson_arxiv18::PlanarSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<oskarsson_arxiv18::PlanarSolver>> lomsac;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else if (solver_idx == 12) {  // N/A
		radialpose::kukelova_iccv13::Radial1DSolver estimator;
		radialpose::RansacEstimator<kukelova_iccv13::Radial1DSolver> solver(x, X, estimator);
		ransac_lib::LocallyOptimizedMSAC<Camera,
			std::vector<Camera>,
			RansacEstimator<kukelova_iccv13::Radial1DSolver>> lomsac;
		solver.use_local_opt = false;
		inliers = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);

	} else {
		print_usage();
		mexErrMsgTxt("Solver NYI.\n");
	}

	save_pose(nlhs, plhs, best_model);

}


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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip8

#include <boost/math/distributions/negative_binomial.hpp>

using namespace boost::math;
using namespace boost::math::policies;

typedef negative_binomial_distribution<
      double,
      policy<discrete_quantile<integer_round_nearest> >
   > dist_type;

// Lower quantile rounded up:
double x = quantile(dist_type(20, 0.3), 0.05);
// Upper quantile rounded down:
double y = quantile(complement(dist_type(20, 0.3), 0.05));

//]

#include <iostream>

int main()
{
   std::cout << x << " " << y << std::endl;
}


/*
 * Copyright (c) 2021. Kun Huang.
 * This Source Code Form is subject to the terms of the Apache License, v. 2.0.
 */

//
// Created by huangkun on 2021/7/9.
//

#ifndef OPENGV2_UTILITY_HPP
#define OPENGV2_UTILITY_HPP

#include <functional>
#include <numeric>
#include <unordered_map>

#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <nanoflann.hpp>
//#include <cereal/cereal.hpp>

namespace opengv2 {
    template<typename EigenMatrixType>
    using vectorofEigenMatrix = std::vector<EigenMatrixType, Eigen::aligned_allocator<EigenMatrixType>>;

    template<typename T>
    struct EigenMatrixCompare {
        inline bool operator()(const T &lhs, const T &rhs) const {
            return lhs.norm() < rhs.norm();
        }
    };

    /**
     * @brief Hash function for Eigen matrix and vector.
     * The code is from `hash_combine` function of the Boost library. See
     * http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine .
     */
    template<typename T>
    struct EigenMatrixHash : std::unary_function<T, size_t> {
        std::size_t operator()(T const &matrix) const {
            // Note that it is oblivious to the storage order of Eigen matrix (column- or
            // row-major). It will give you the same hash value for two different matrices if they
            // are the transpose of each other in different storage order.
            size_t seed = 0;
            for (size_t i = 0; i < matrix.size(); ++i) {
                auto elem = *(matrix.data() + i);
                seed ^= std::hash<typename T::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            }
            return seed;
        }
    };

    /**
     * @brief Generic functor base for use with the Eigen-nonlinear optimization
     * toolbox. Please refer to the Eigen-documentation for further information.
     */
    template<typename Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
    struct EigenOptimizationFunctor {
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        EigenOptimizationFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

        EigenOptimizationFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }

        int values() const { return m_values; }

        // you should define that in the subclass :
        // void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
    };

    struct pair_hash {
        template<class T1, class T2>
        std::size_t operator()(const std::pair<T1, T2> &pair) const {
            return std::hash<T1>()(pair.first) ^ std::hash<T2>()(pair.second);
        }
    };

    /**
     * @brief Fit normal distribution.
     * @tparam T
     * @param v
     * @param mean
     * @param stdev
     */
    template<class T>
    inline void fitNormal(const std::vector<T> &v, T &mean, T &stdev) {
        double sum = std::accumulate(v.begin(), v.end(), 0.0);
        mean = sum / v.size();

        std::vector<T> diff(v.size());
        std::transform(v.begin(), v.end(), diff.begin(), [mean](T x) { return x - mean; });
        double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
        stdev = std::sqrt(sq_sum / (v.size() - 1));
    }
}

namespace nanoflann {
    template<typename T>
    struct SO3DataSetAdaptor {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        explicit SO3DataSetAdaptor(const opengv2::vectorofEigenMatrix<Eigen::Quaternion<T>> &Q_set) : QSet(Q_set) {}

        const opengv2::vectorofEigenMatrix<Eigen::Quaternion<T>> &QSet;

        // Must return the number of data points
        inline size_t kdtree_get_point_count() const { return QSet.size(); }

        // Returns the dim'th component of the idx'th point in the class:
        // Since this is inlined and the "dim" argument is typically an immediate value, the
        //  "if/else's" are actually solved at compile time.
        inline T kdtree_get_pt(const size_t idx, const size_t dim) const {
            return QSet[idx].coeffs()[dim];
        }

        // Optional bounding-box computation: return false to default to a standard bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
        //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
        template<class BBOX>
        bool kdtree_get_bbox(BBOX & /* bb */) const { return false; }
    };

    template<typename num_t>
    using SO3_KDTree = KDTreeSingleIndexAdaptor<SO3_Adaptor<num_t, SO3DataSetAdaptor<num_t>>, SO3DataSetAdaptor<num_t>, 4>;
}

/*namespace cereal {
    template<class Archive, class Derived>
    inline
    typename std::enable_if<
            traits::is_output_serializable < BinaryData < typename Derived::Scalar>, Archive>::value, void>

    ::type
    save(Archive &ar, Eigen::PlainObjectBase<Derived> const &m) {
        typedef Eigen::PlainObjectBase<Derived> ArrT;
        if (ArrT::RowsAtCompileTime == Eigen::Dynamic) ar(m.rows());
        if (ArrT::ColsAtCompileTime == Eigen::Dynamic) ar(m.cols());
        ar(binary_data(m.data(), m.size() * sizeof(typename Derived::Scalar)));
    }

    template<class Archive, class Derived>
    inline
    typename std::enable_if<
            traits::is_input_serializable < BinaryData < typename Derived::Scalar>, Archive>::value, void>

    ::type
    load(Archive &ar, Eigen::PlainObjectBase<Derived> &m) {
        typedef Eigen::PlainObjectBase<Derived> ArrT;
        Eigen::Index rows = ArrT::RowsAtCompileTime, cols = ArrT::ColsAtCompileTime;
        if (rows == Eigen::Dynamic) ar(rows);
        if (cols == Eigen::Dynamic) ar(cols);
        m.resize(rows, cols);
        ar(binary_data(m.data(), static_cast<std::size_t>(rows * cols * sizeof(typename Derived::Scalar))));
    }

    template<class Archive, class Scalar, int Options>
    inline void serialize(Archive &ar, ::Eigen::Quaternion<Scalar, Options> &quat) {
        ar(make_nvp("w", quat.w()), make_nvp("x", quat.x()), make_nvp("y", quat.y()), make_nvp("z", quat.z()));
    }
}*/

#endif //OPENGV2_UTILITY_HPP


// Header
#include "Translate.hpp"

// Boost
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

namespace QuadProgMm {
  /* From QuadProg++:

  The problem is in the expression:

  min 0.5 * x G x + g0 x
  s.t.
      CE^T x + ce0 = 0
      CI^T x + ci0 >= 0

   The matrix and vectors dimensions are as follows:
       G: n * n
      g0: n

      CE: n * p
     ce0: p

      CI: n * m
     ci0: m

       x: n
  */
  Translation::Translation(const std::set<Variable>& variables_, int p, int m) :
    Translation(
      std::vector<Variable>(variables_.begin(), variables_.end()),
      variables_.size(),
      p,
      m
    )
  {}

  Translation::Translation(const std::vector<Variable>& variables_, int n, int p, int m) :
    variables(variables_),
    G(n, n), g0(n), g00(0),
    CE(n, p), ce0(p),
    CI(n, m), ci0(m)
  {}

  void translate(
    const std::vector<Variable>& variables,
    const QuadraticExpression& q,
    quadprogpp::Matrix<double>& G,
    quadprogpp::Vector<double>& g0,
    double& g00
  ) {
    int index1 = 0;
    foreach(Variable v1, variables) {
      int index2 = 0;
      foreach(Variable v2, variables) {
        const double coeff = q.getQuadraticCoefficient(v1, v2);
        if (index1 == index2) {
          G[index1][index1] = 2 * coeff;
        } else {
          G[index1][index2] = coeff;
          G[index2][index1] = coeff;
        }
        ++index2;
      }
      ++index1;
    }

    int index = 0;
    foreach(Variable v, variables) {
      g0[index] = q.getLinearCoefficient(v);
      ++index;
    }

    g00 = q.getConstantCoefficient();
  }

  void translate(
    const std::vector<Variable>& variables,
    const LinearExpression& l,
    quadprogpp::Matrix<double>& CIE,
    quadprogpp::Vector<double>& cie0,
    int indexIE
  ) {
    int index = 0;
    foreach(Variable v, variables) {
      CIE[index][indexIE] = l.getLinearCoefficient(v);
      ++index;
    }

    cie0[indexIE] = l.getConstantCoefficient();
  }

  Translation translate(const QuadraticExpression& q, const std::vector<Constraint>& constraints) {
    int p = 0;
    int m = 0;
    std::set<Variable> variables = q.getVariables();
    foreach(Constraint c, constraints) {
      switch(c.getType()) {
        case Constraint::ZERO:
          ++p;
          break;
        case Constraint::POSITIVE:
          ++m;
          break;
      }
      const std::set<Variable> constraintVariables = c.getLinearExpression().getVariables();
      variables.insert(constraintVariables.begin(), constraintVariables.end());
    }

    Translation t(variables, p, m);

    translate(t.variables, q, t.G, t.g0, t.g00);

    int indexE = 0;
    int indexI = 0;
    foreach(Constraint c, constraints) {
      const LinearExpression& l = c.getLinearExpression();

      switch(c.getType()) {
        case Constraint::ZERO:
          translate(t.variables, l, t.CE, t.ce0, indexE);
          ++indexE;
          break;
        case Constraint::POSITIVE:
          translate(t.variables, l, t.CI, t.ci0, indexI);
          ++indexI;
          break;
      }
    }

    return t;
  }
}


#pragma once
#include <random>
#include <bitset>
#include <fstream>
#include <ios>
#include <string>
#include <cassert>
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <tbb/tbb.h>

#include <nlohmann/json.hpp>

#include "Utilities/type_traits.hpp"
#include "Utilities/Utility.hpp"
#include "Serializers/SerializeEigen.hpp"

namespace yannq
{
//! \ingroup Machines
//! RBM machine
template<typename T>
class CorrelatedRBM
{
	static_assert(std::is_floating_point<T>::value || is_complex_type<T>::value, "T must be floating or complex");
public:
	using Scalar = T;
	using RealScalar = typename yannq::remove_complex<T>::type;

	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

	using RealVector = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;

	using DataT = std::tuple<Eigen::VectorXi, Vector>;

private:
	uint32_t n_; //# of qubits
	uint32_t m_; //# of hidden units

	bool useBias_;

	Eigen::SparseMatrix<int> correl_;

	Matrix W_; //W should be m by (n + k)
	Vector a_; //a is length n + k
	Vector b_; //b is length m

public:
	/* \param n number of qubits
	 * \param m number of hidden units
	 * \param correl n \times k matrix. For each column, 1 is given at the location of
	 * a qubit involving a correlation. C_k = \prod_{l, correl(l,k)=1} s_l
	 * */
	CorrelatedRBM(uint32_t n, uint32_t m,
			const Eigen::SparseMatrix<int>& correl, bool useBias = true) noexcept
		: n_(n), m_(m), useBias_(useBias), correl_{correl},
		W_(m,n+correl.cols()), a_(n+correl.cols()), b_(m)
	{
		correl_.makeCompressed();
		a_.setZero();
		b_.setZero();
		W_.setZero();
	}

	CorrelatedRBM() noexcept
		: useBias_{true}
	{
	}

	template<typename U, std::enable_if_t<std::is_convertible_v<U, T> && !std::is_same_v<U, T>, int> = 0>
	CorrelatedRBM(const CorrelatedRBM<U>& rhs)
		: n_{rhs.getN()}, m_{rhs.getM()}, useBias_{rhs.useBias()}, correl_{rhs.correl_}
	{
		W_ = rhs.getW().template cast<T>();
		a_ = rhs.getA().template cast<T>();
		b_ = rhs.getB().template cast<T>();
	}

	CorrelatedRBM(const CorrelatedRBM& rhs) /* noexcept */ = default;
	CorrelatedRBM(CorrelatedRBM&& rhs) /* noexcept */ = default;

	template<typename U, std::enable_if_t<std::is_convertible_v<U, T> && !std::is_same_v<U, T>, int> = 0>
	CorrelatedRBM& operator=(const CorrelatedRBM<U>& rhs)
	{
		n_ = rhs.n_;
		m_ = rhs.m_;
		useBias_ = rhs.useBias_;
		correl_ = rhs.correl_;

		W_ = rhs.W_.template cast<T>();
		a_ = rhs.a_.template cast<T>();
		b_ = rhs.b_.template cast<T>();

		return *this;
	}

	CorrelatedRBM& operator=(const CorrelatedRBM& rhs) /* noexcept */ = default;
	CorrelatedRBM& operator=(CorrelatedRBM&& rhs) /* noexcept */ = default;

	template<typename U, std::enable_if_t<std::is_convertible_v<T, U>, int> = 0>
	CorrelatedRBM<U> cast() const
	{
		CorrelatedRBM<U> res(n_, m_, correl_, useBias_);
		res.setA(a_.template cast<U>());
		res.setB(b_.template cast<U>());
		res.setW(W_.template cast<U>());
		return res;
	}

	nlohmann::json desc() const
	{
		return nlohmann::json
		{
			{"name", "CorrelatedRBM"},
			{"useBias", useBias_},
			{"k", correl_.rows()},
			{"n", n_},
			{"m", m_}
		};
	}

	inline uint32_t getN() const
	{
		return n_;
	}
	inline uint32_t getM() const
	{
		return m_;
	}

	inline uint32_t getK() const
	{
		return correl_.cols();
	}

	inline uint32_t getDim() const
	{
		auto k = getK();
		if(useBias_)
			return (n_ + k)*m_ + n_ + k + m_;
		else
			return (n_ + k)*m_;
	}

	inline bool useBias() const
	{
		return useBias_;
	}

	Eigen::VectorXi calcCorrel(const Eigen::VectorXi& sigma) const
	{
		Eigen::VectorXi c = Eigen::VectorXi::Ones(getK());
		for(uint32_t l = 0; l < getK(); ++l) //cols (outer)
		{
			for(Eigen::SparseMatrix<int>::InnerIterator it(correl_, l); it; ++it)
			{
				c(l) *= sigma(it.row());
			}
		}
		return c;
	}

	/**
	 * \param ss is sigma plus correl
	 */
	inline Vector calcTheta(const Eigen::VectorXi& ss) const
	{
		assert(ss.size() == n_ + getK());
		Vector s = ss.cast<T>();
		return W_*s + b_;
	}

	void setUseBias(bool newBias)
	{
		useBias_ = newBias;
	}

	void resize(uint32_t n, uint32_t m)
	{
		n_ = n;
		m_ = m;

		a_.resize(n);
		b_.resize(m + getK());
		W_.resize(m + getK(),n);

		if(!useBias_)
		{
			a_.setZero();
			b_.setZero();
		}
	}

	void conservativeResize(uint32_t newM)
	{
		Vector newB = Vector::Zero(newM);
		newB.head(m_) = b_;

		Matrix newW = Matrix::Zero(newM, n_ + getK());
		newW.topRows(m_) = W_;

		m_ = newM;
		b_ = std::move(newB);
		W_ = std::move(newW);
	}

	void setCorrel(const Eigen::SparseMatrix<int>& correl)
	{
		correl_ = correl;
	}

	void setW(const Eigen::Ref<const Matrix>& m)
	{
		assert(m.rows() == W_.rows() && m.cols() == W_.cols());
		W_ = m;
	}

	void setA(const VectorConstRef& A)
	{
		assert(A.size() == a_.size());
		if(!useBias_)
			return ;
		a_ = A;
	}

	void setB(const VectorConstRef& B)
	{
		assert(B.size() == b_.size());
		if(!useBias_)
			return ;
		b_ = B;
	}

	inline const T& W(uint32_t j, uint32_t i) const
	{
		return W_.coeff(j,i);
	}
	inline const T& A(uint32_t i) const
	{
		return a_.coeff(i);
	}
	inline const T& B(uint32_t j) const
	{
		return b_.coeff(j);
	}

	inline T& W(uint32_t j, uint32_t i)
	{
		return W_.coeffRef(j,i);
	}
	inline T& A(uint32_t i)
	{
		return a_.coeffRef(i);
	}
	inline T& B(uint32_t j)
	{
		return b_.coeffRef(j);
	}

	Eigen::SparseMatrix<int> getCorrel() const
	{
		return correl_;
	}

	const Matrix& getW() const & { return W_; }
	Matrix getW() && { return std::move(W_); }

	const Vector& getA() const & { return a_; }
	Vector getA() && { return std::move(a_); }

	const Vector& getB() const & { return b_; }
	Vector getB() && { return std::move(b_); }

	//! update Bias A by adding v
	void updateA(const VectorConstRef& v)
	{
		assert(useBias_);
		a_ += v;
	}
	//! update Bias B by adding v
	void updateB(const VectorConstRef& v)
	{
		assert(useBias_);
		b_ += v;
	}
	//! update the weight W by adding m
	void updateW(const Eigen::Ref<const Matrix>& m)
	{
		W_ += m;
	}

	//! update all parameters.
	void updateParams(const VectorConstRef& m)
	{
		assert(m.size() == getDim());
		uint32_t k = getK();
		W_ += Eigen::Map<const Matrix>(m.data(), m_, n_ + k);
		if(!useBias_)
			return ;
		a_ += Eigen::Map<const Vector>(m.data() + m_*(n_ + k), n_ + k);
		b_ += Eigen::Map<const Vector>(m.data() + m_*(n_ + k) + n_ + k, m_);
	}

	Vector getParams() const
	{
		const auto k = getK();
		Vector res(getDim());
		res.head((n_+k)*m_) = Eigen::Map<const Vector>(W_.data(), W_.size());
		if(!useBias_)
			return res;

		res.segment((n_ + k)*m_, n_ + k) = a_;
		res.segment((n_ + k)*m_ + n_ + k, m_) = b_;
		return res;
	}

	void setParams(const VectorConstRef& r)
	{
		assert(r.size() == getDim());
		const auto k = getK();
		Eigen::Map<Vector>(W_.data(), W_.size()) = r.head((n_ + k)*m_);
		if(!useBias_)
			return ;
		a_ = r.segment((n_+k)*m_, n_ + k);
		b_ = r.segment((n_+k)*m_ + n_ + k, m_);
	}

	bool hasNaN() const
	{
		return a_.hasNaN() || b_.hasNaN() || W_.hasNaN();
	}

	/* When T is real type */
	template <typename RandomEngine, class U=T,
            	std::enable_if_t < !is_complex_type<U>::value, int > = 0 >
	void initializeRandom(RandomEngine& re, T sigma = 1e-3)
	{
		const auto k = getK();
		std::normal_distribution<T> nd{0, sigma};
		if(useBias_)
		{
			for(uint32_t i = 0u; i < n_ + k; i++)
			{
				a_.coeffRef(i) = nd(re);
			}
			for(uint32_t i = 0u; i < m_; i++)
			{
				b_.coeffRef(i) = nd(re);
			}
		}
		for(uint32_t j = 0u; j < n_ + k; j++)
		{
			for(uint32_t i = 0u; i < m_; i++)
			{
				W_.coeffRef(i, j) = nd(re);
			}
		}
	}

	/* When T is complex type */
	template <typename RandomEngine, class U=T,
               std::enable_if_t < is_complex_type<U>::value, int > = 0 >
	void initializeRandom(RandomEngine& re, typename remove_complex<T>::type sigma = 1e-3)
	{
		const auto k = getK();
		std::normal_distribution<typename remove_complex<T>::type> nd{0, sigma};

		if(useBias_)
		{
			for(uint32_t i = 0u; i < n_ + k; i++)
			{
				a_.coeffRef(i) = T{nd(re), nd(re)};
			}
			for(uint32_t i = 0u; i < m_; i++)
			{
				b_.coeffRef(i) = T{nd(re), nd(re)};
			}
		}
		for(uint32_t j = 0; j < n_ + k; j++)
		{
			for(uint32_t i = 0u; i < m_; i++)
			{
				W_.coeffRef(i, j) = T{nd(re), nd(re)};
			}
		}
	}

	bool operator==(const CorrelatedRBM<T>& rhs) const
	{
		if(n_ != rhs.n_ || m_ != rhs.m_ || getK() != rhs.getK())
			return false;
		if((correl_ - rhs.corre_).norm() > 1e-6)
			return false;
		if(useBias_)
			return (a_ == rhs.a_) && (b_ == rhs.b_) && (W_ == rhs.W_);
		else
			return (W_ == rhs.W_);
	}

	std::tuple<Eigen::VectorXi, Vector> makeData(const Eigen::VectorXi& sigma) const
	{
		Eigen::VectorXi ss(n_ + getK());
		ss.head(n_) = sigma;
		ss.tail(getK()) = calcCorrel(sigma);
		return std::make_tuple(ss, calcTheta(ss));
	}

	T logCoeff(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		using std::cosh;

		Vector ss = std::get<0>(t).template cast<T>();
		T s = a_.transpose()*ss;
		for(uint32_t j = 0u; j < m_; j++)
		{
			s += logCosh(std::get<1>(t).coeff(j));
		}
		return s;
	}

	T coeff(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		using std::cosh;

		Vector ss = std::get<0>(t).template cast<T>();
		T s = a_.transpose()*ss;
		T p = exp(s) * std::get<1>(t).array().cosh().prod();
		return p;
	}

	Vector logDeriv(const std::tuple<Eigen::VectorXi, Vector>& t) const
	{
		Vector res(getDim());
		const auto k = getK();

		Vector tanhs = std::get<1>(t).array().tanh();
		Vector ss = std::get<0>(t).template cast<Scalar>();

		for(uint32_t i = 0u; i < n_ + k; i++)
		{
			res.segment(i*m_, m_) = ss(i)*tanhs;
		}
		if(!useBias_)
			return res;
		res.segment((n_+k)*m_, n_ + k) = ss;
		res.segment((n_+k)*m_ + n_ + k, m_) = tanhs;
		return res;
	}
};

template<typename T>
typename CorrelatedRBM<T>::Vector getPsi(const CorrelatedRBM<T>& qs, bool normalize)
{
	const uint32_t n = qs.getN();
	typename CorrelatedRBM<T>::Vector psi(1u<<n);
	tbb::parallel_for(0u, (1u << n),
		[n, &qs, &psi](uint32_t idx)
	{
		auto s = toSigma(n, idx);
		psi(idx) = qs.coeff(qs.makeData(s));
	});
	if(normalize)
		psi.normalize();
	return psi;
}

template<typename T, typename Iterable> //Iterable must be random access iterable
typename CorrelatedRBM<T>::Vector getPsi(const CorrelatedRBM<T>& qs, Iterable&& basis, bool normalize)
{
	const uint32_t n = qs.getN();
	typename CorrelatedRBM<T>::Vector psi(basis.size());

	tbb::parallel_for(std::size_t(0u), basis.size(),
		[n, &qs, &psi, &basis](std::size_t idx)
	{
		auto s = toSigma(n, basis[idx]);
		psi(idx) = qs.coeff(qs.makeData(s));
	});
	if(normalize)
		psi.normalize();
	return psi;
}
}//namespace yannq


/*
The MIT License

Copyright (c) 2020-2022 Zhepei Wang
                        Hongkai Ye

Permission is hereby granted, free of charge, to any person obtaining a
copy of this software and associated documentation files (the "Software"),
to deal in the Software without restriction, including without limitation
the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the
Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef ROOT_FINDER_HPP
#define ROOT_FINDER_HPP

#define _USE_MATH_DEFINES
#include <cfloat>
#include <cmath>
#include <set>
#include <Eigen/Eigen>

namespace RootFinderParam
{
    constexpr size_t highestOrder = 64;
}

namespace RootFinderPriv
{

    inline int polyMod(double *u, double *v, double *r, int lu, int lv)
    // Modulus of u(x)/v(x)
    // The leading coefficient of v, i.e., v[0], must be 1.0 or -1.0
    // The length of u, v, and r are lu, lv, and lu, respectively
    {
        int orderu = lu - 1;
        int orderv = lv - 1;

        memcpy(r, u, lu * sizeof(double));

        if (v[0] < 0.0)
        {
            for (int i = orderv + 1; i <= orderu; i += 2)
            {
                r[i] = -r[i];
            }
            for (int i = 0; i <= orderu - orderv; i++)
            {
                for (int j = i + 1; j <= orderv + i; j++)
                {
                    r[j] = -r[j] - r[i] * v[j - i];
                }
            }
        }
        else
        {
            for (int i = 0; i <= orderu - orderv; i++)
            {
                for (int j = i + 1; j <= orderv + i; j++)
                {
                    r[j] = r[j] - r[i] * v[j - i];
                }
            }
        }

        int k = orderv - 1;
        while (k >= 0 && fabs(r[orderu - k]) < DBL_EPSILON)
        {
            r[orderu - k] = 0.0;
            k--;
        }

        return (k <= 0) ? 1 : (k + 1);
    }

    inline double polyEval(double *p, int len, double x)
    // Evaluate the polynomial p(x), which has len coefficients
    // Note: Horner scheme should not be employed here !!!
    // Horner scheme has bad numerical stability despite of its efficiency.
    // These errors are particularly troublesome for root-finding algorithms.
    // When the polynomial is evaluated near a zero, catastrophic
    // cancellation (subtracting two nearby numbers) is guaranteed to occur.
    // Therefore, Horner scheme may slow down some root-finding algorithms.
    {
        double retVal = 0.0;

        if (len > 0)
        {
            if (fabs(x) < DBL_EPSILON)
            {
                retVal = p[len - 1];
            }
            else if (x == 1.0)
            {
                for (int i = len - 1; i >= 0; i--)
                {
                    retVal += p[i];
                }
            }
            else
            {
                double xn = 1.0;

                for (int i = len - 1; i >= 0; i--)
                {
                    retVal += p[i] * xn;
                    xn *= x;
                }
            }
        }

        return retVal;
    }

    inline std::set<double> solveCub(double a, double b, double c, double d)
    // Calculate all roots of a*x^3 + b*x^2 + c*x + d = 0
    {
        std::set<double> roots;

        constexpr double cos120 = -0.50;
        constexpr double sin120 = 0.866025403784438646764;

        if (fabs(d) < DBL_EPSILON)
        {
            // First solution is x = 0
            roots.insert(0.0);

            // Converting to a quadratic equation
            d = c;
            c = b;
            b = a;
            a = 0.0;
        }

        if (fabs(a) < DBL_EPSILON)
        {
            if (fabs(b) < DBL_EPSILON)
            {
                // Linear equation
                if (fabs(c) > DBL_EPSILON)
                    roots.insert(-d / c);
            }
            else
            {
                // Quadratic equation
                double discriminant = c * c - 4.0 * b * d;
                if (discriminant >= 0)
                {
                    double inv2b = 1.0 / (2.0 * b);
                    double y = sqrt(discriminant);
                    roots.insert((-c + y) * inv2b);
                    roots.insert((-c - y) * inv2b);
                }
            }
        }
        else
        {
            // Cubic equation
            double inva = 1.0 / a;
            double invaa = inva * inva;
            double bb = b * b;
            double bover3a = b * (1.0 / 3.0) * inva;
            double p = (3.0 * a * c - bb) * (1.0 / 3.0) * invaa;
            double halfq = (2.0 * bb * b - 9.0 * a * b * c + 27.0 * a * a * d) * (0.5 / 27.0) * invaa * inva;
            double yy = p * p * p / 27.0 + halfq * halfq;

            if (yy > DBL_EPSILON)
            {
                // Sqrt is positive: one real solution
                double y = sqrt(yy);
                double uuu = -halfq + y;
                double vvv = -halfq - y;
                double www = fabs(uuu) > fabs(vvv) ? uuu : vvv;
                double w = (www < 0) ? -pow(fabs(www), 1.0 / 3.0) : pow(www, 1.0 / 3.0);
                roots.insert(w - p / (3.0 * w) - bover3a);
            }
            else if (yy < -DBL_EPSILON)
            {
                // Sqrt is negative: three real solutions
                double x = -halfq;
                double y = sqrt(-yy);
                double theta;
                double r;
                double ux;
                double uyi;
                // Convert to polar form
                if (fabs(x) > DBL_EPSILON)
                {
                    theta = (x > 0.0) ? atan(y / x) : (atan(y / x) + M_PI);
                    r = sqrt(x * x - yy);
                }
                else
                {
                    // Vertical line
                    theta = M_PI / 2.0;
                    r = y;
                }
                // Calculate cube root
                theta /= 3.0;
                r = pow(r, 1.0 / 3.0);
                // Convert to complex coordinate
                ux = cos(theta) * r;
                uyi = sin(theta) * r;
                // First solution
                roots.insert(ux + ux - bover3a);
                // Second solution, rotate +120 degrees
                roots.insert(2.0 * (ux * cos120 - uyi * sin120) - bover3a);
                // Third solution, rotate -120 degrees
                roots.insert(2.0 * (ux * cos120 + uyi * sin120) - bover3a);
            }
            else
            {
                // Sqrt is zero: two real solutions
                double www = -halfq;
                double w = (www < 0.0) ? -pow(fabs(www), 1.0 / 3.0) : pow(www, 1.0 / 3.0);
                // First solution
                roots.insert(w + w - bover3a);
                // Second solution, rotate +120 degrees
                roots.insert(2.0 * w * cos120 - bover3a);
            }
        }
        return roots;
    }

    inline int solveResolvent(double *x, double a, double b, double c)
    // Solve resolvent eqaution of corresponding Quartic equation
    // The input x must be of length 3
    // Number of zeros are returned
    {
        double a2 = a * a;
        double q = (a2 - 3.0 * b) / 9.0;
        double r = (a * (2.0 * a2 - 9.0 * b) + 27.0 * c) / 54.0;
        double r2 = r * r;
        double q3 = q * q * q;
        double A, B;
        if (r2 < q3)
        {
            double t = r / sqrt(q3);
            if (t < -1.0)
            {
                t = -1.0;
            }
            if (t > 1.0)
            {
                t = 1.0;
            }
            t = acos(t);
            a /= 3.0;
            q = -2.0 * sqrt(q);
            x[0] = q * cos(t / 3.0) - a;
            x[1] = q * cos((t + M_PI * 2.0) / 3.0) - a;
            x[2] = q * cos((t - M_PI * 2.0) / 3.0) - a;
            return 3;
        }
        else
        {
            A = -pow(fabs(r) + sqrt(r2 - q3), 1.0 / 3.0);
            if (r < 0.0)
            {
                A = -A;
            }
            B = (0.0 == A ? 0.0 : q / A);

            a /= 3.0;
            x[0] = (A + B) - a;
            x[1] = -0.5 * (A + B) - a;
            x[2] = 0.5 * sqrt(3.0) * (A - B);
            if (fabs(x[2]) < DBL_EPSILON)
            {
                x[2] = x[1];
                return 2;
            }

            return 1;
        }
    }

    inline std::set<double> solveQuartMonic(double a, double b, double c, double d)
    // Calculate all roots of the monic quartic equation:
    // x^4 + a*x^3 + b*x^2 + c*x +d = 0
    {
        std::set<double> roots;

        double a3 = -b;
        double b3 = a * c - 4.0 * d;
        double c3 = -a * a * d - c * c + 4.0 * b * d;

        // Solve the resolvent: y^3 - b*y^2 + (ac - 4*d)*y - a^2*d - c^2 + 4*b*d = 0
        double x3[3];
        int iZeroes = solveResolvent(x3, a3, b3, c3);

        double q1, q2, p1, p2, D, sqrtD, y;

        y = x3[0];
        // Choosing Y with maximal absolute value.
        if (iZeroes != 1)
        {
            if (fabs(x3[1]) > fabs(y))
            {
                y = x3[1];
            }
            if (fabs(x3[2]) > fabs(y))
            {
                y = x3[2];
            }
        }

        // h1 + h2 = y && h1*h2 = d  <=>  h^2 - y*h + d = 0    (h === q)

        D = y * y - 4.0 * d;
        if (fabs(D) < DBL_EPSILON) //In other words: D == 0
        {
            q1 = q2 = y * 0.5;
            // g1 + g2 = a && g1 + g2 = b - y   <=>   g^2 - a*g + b - y = 0    (p === g)
            D = a * a - 4.0 * (b - y);
            if (fabs(D) < DBL_EPSILON) //In other words: D == 0
            {
                p1 = p2 = a * 0.5;
            }
            else
            {
                sqrtD = sqrt(D);
                p1 = (a + sqrtD) * 0.5;
                p2 = (a - sqrtD) * 0.5;
            }
        }
        else
        {
            sqrtD = sqrt(D);
            q1 = (y + sqrtD) * 0.5;
            q2 = (y - sqrtD) * 0.5;
            // g1 + g2 = a && g1*h2 + g2*h1 = c   ( && g === p )  Krammer
            p1 = (a * q1 - c) / (q1 - q2);
            p2 = (c - a * q2) / (q1 - q2);
        }

        // Solve the quadratic equation: x^2 + p1*x + q1 = 0
        D = p1 * p1 - 4.0 * q1;
        if (fabs(D) < DBL_EPSILON)
        {
            roots.insert(-p1 * 0.5);
        }
        else if (D > 0.0)
        {
            sqrtD = sqrt(D);
            roots.insert((-p1 + sqrtD) * 0.5);
            roots.insert((-p1 - sqrtD) * 0.5);
        }

        // Solve the quadratic equation: x^2 + p2*x + q2 = 0
        D = p2 * p2 - 4.0 * q2;
        if (fabs(D) < DBL_EPSILON)
        {
            roots.insert(-p2 * 0.5);
        }
        else if (D > 0.0)
        {
            sqrtD = sqrt(D);
            roots.insert((-p2 + sqrtD) * 0.5);
            roots.insert((-p2 - sqrtD) * 0.5);
        }

        return roots;
    }

    inline std::set<double> solveQuart(double a, double b, double c, double d, double e)
    // Calculate the quartic equation: a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
    // All coefficients can be zero
    {
        if (fabs(a) < DBL_EPSILON)
        {
            return solveCub(b, c, d, e);
        }
        else
        {
            return solveQuartMonic(b / a, c / a, d / a, e / a);
        }
    }

    inline std::set<double> eigenSolveRealRoots(const Eigen::VectorXd &coeffs, double lbound, double ubound, double tol)
    // Calculate roots of coeffs(x) inside (lbound, rbound) by computing eigen values of its companion matrix
    // Complex roots with magnitude of imaginary part less than tol are considered real
    {
        std::set<double> rts;

        int order = (int)coeffs.size() - 1;
        Eigen::VectorXd monicCoeffs(order + 1);
        monicCoeffs << 1.0, coeffs.tail(order) / coeffs(0);

        Eigen::MatrixXd companionMat(order, order);
        companionMat.setZero();
        companionMat(0, order - 1) = -monicCoeffs(order);
        for (int i = 1; i < order; i++)
        {
            companionMat(i, i - 1) = 1.0;
            companionMat(i, order - 1) = -monicCoeffs(order - i);
        }
        Eigen::VectorXcd eivals = companionMat.eigenvalues();
        double real;
        int eivalsNum = eivals.size();
        for (int i = 0; i < eivalsNum; i++)
        {
            real = eivals(i).real();
            if (eivals(i).imag() < tol && real > lbound && real < ubound)
                rts.insert(real);
        }

        return rts;
    }

    inline double numSignVar(double x, double **sturmSeqs, int *szSeq, int len)
    // Calculate the number of sign variations of the Sturm sequences at x
    // The i-th sequence with size szSeq[i] stored in sturmSeqs[i][], 0 <= i < len
    {
        double y, lasty;
        int signVar = 0;
        lasty = polyEval(sturmSeqs[0], szSeq[0], x);
        for (int i = 1; i < len; i++)
        {
            y = polyEval(sturmSeqs[i], szSeq[i], x);
            if (lasty == 0.0 || lasty * y < 0.0)
            {
                ++signVar;
            }
            lasty = y;
        }

        return signVar;
    };

    inline void polyDeri(double *coeffs, double *dcoeffs, int len)
    // Calculate the derivative poly coefficients of a given poly
    {
        int horder = len - 1;
        for (int i = 0; i < horder; i++)
        {
            dcoeffs[i] = (horder - i) * coeffs[i];
        }
        return;
    }

    template <typename F, typename DF>
    inline double safeNewton(const F &func, const DF &dfunc,
                             const double &l, const double &h,
                             const double &tol, const int &maxIts)
    // Safe Newton Method
    // Requirements: f(l)*f(h)<=0
    {
        double xh, xl;
        double fl = func(l);
        double fh = func(h);
        if (fl == 0.0)
        {
            return l;
        }
        if (fh == 0.0)
        {
            return h;
        }
        if (fl < 0.0)
        {
            xl = l;
            xh = h;
        }
        else
        {
            xh = l;
            xl = h;
        }

        double rts = 0.5 * (xl + xh);
        double dxold = fabs(xh - xl);
        double dx = dxold;
        double f = func(rts);
        double df = dfunc(rts);
        double temp;
        for (int j = 0; j < maxIts; j++)
        {
            if ((((rts - xh) * df - f) * ((rts - xl) * df - f) > 0.0) ||
                (fabs(2.0 * f) > fabs(dxold * df)))
            {
                dxold = dx;
                dx = 0.5 * (xh - xl);
                rts = xl + dx;
                if (xl == rts)
                {
                    break;
                }
            }
            else
            {
                dxold = dx;
                dx = f / df;
                temp = rts;
                rts -= dx;
                if (temp == rts)
                {
                    break;
                }
            }

            if (fabs(dx) < tol)
            {
                break;
            }

            f = func(rts);
            df = dfunc(rts);
            if (f < 0.0)
            {
                xl = rts;
            }
            else
            {
                xh = rts;
            }
        }

        return rts;
    }

    inline double shrinkInterval(double *coeffs, int numCoeffs, double lbound, double ubound, double tol)
    // Calculate a single zero of poly coeffs(x) inside [lbound, ubound]
    // Requirements: coeffs(lbound)*coeffs(ubound) < 0, lbound < ubound
    {
        double *dcoeffs = new double[numCoeffs - 1];
        polyDeri(coeffs, dcoeffs, numCoeffs);
        auto func = [&coeffs, &numCoeffs](double x)
        { return polyEval(coeffs, numCoeffs, x); };
        auto dfunc = [&dcoeffs, &numCoeffs](double x)
        { return polyEval(dcoeffs, numCoeffs - 1, x); };
        constexpr int maxDblIts = 128;
        double rts = safeNewton(func, dfunc, lbound, ubound, tol, maxDblIts);
        delete[] dcoeffs;
        return rts;
    }

    inline void recurIsolate(double l, double r, double fl, double fr, int lnv, int rnv,
                             double tol, double **sturmSeqs, int *szSeq, int len,
                             std::set<double> &rts)
    // Isolate all roots of sturmSeqs[0](x) inside interval (l, r) recursively and store them in rts
    // Requirements: fl := sturmSeqs[0](l) != 0, fr := sturmSeqs[0](r) != 0, l < r,
    //               lnv != rnv, lnv = numSignVar(l), rnv = numSignVar(r)
    //               sturmSeqs[0](x) must have at least one root inside (l, r)
    {
        int nrts = lnv - rnv;
        double fm;
        double m;

        if (nrts == 0)
        {
            return;
        }
        else if (nrts == 1)
        {
            if (fl * fr < 0)
            {
                rts.insert(shrinkInterval(sturmSeqs[0], szSeq[0], l, r, tol));
                return;
            }
            else
            {
                // Bisect when non of above works
                int maxDblIts = 128;

                for (int i = 0; i < maxDblIts; i++)
                {
                    // Calculate the root with even multiplicity
                    if (fl * fr < 0)
                    {
                        rts.insert(shrinkInterval(sturmSeqs[1], szSeq[1], l, r, tol));
                        return;
                    }

                    m = (l + r) / 2.0;
                    fm = polyEval(sturmSeqs[0], szSeq[0], m);

                    if (fm == 0 || fabs(r - l) < tol)
                    {
                        rts.insert(m);
                        return;
                    }
                    else
                    {
                        if (lnv == numSignVar(m, sturmSeqs, szSeq, len))
                        {
                            l = m;
                            fl = fm;
                        }
                        else
                        {
                            r = m;
                            fr = fm;
                        }
                    }
                }

                rts.insert(m);
                return;
            }
        }
        else if (nrts > 1)
        {
            // More than one root exists in the interval
            int maxDblIts = 128;

            int mnv;
            int bias = 0;
            bool biased = false;
            for (int i = 0; i < maxDblIts; i++)
            {
                bias = biased ? bias : 0;
                if (!biased)
                {
                    m = (l + r) / 2.0;
                }
                else
                {
                    m = (r - l) / pow(2.0, bias + 1.0) + l;
                    biased = false;
                }
                mnv = numSignVar(m, sturmSeqs, szSeq, len);

                if (fabs(r - l) < tol)
                {
                    rts.insert(m);
                    return;
                }
                else
                {
                    fm = polyEval(sturmSeqs[0], szSeq[0], m);
                    if (fm == 0)
                    {
                        bias++;
                        biased = true;
                    }
                    else if (lnv != mnv && rnv != mnv)
                    {
                        recurIsolate(l, m, fl, fm, lnv, mnv, tol, sturmSeqs, szSeq, len, rts);
                        recurIsolate(m, r, fm, fr, mnv, rnv, tol, sturmSeqs, szSeq, len, rts);
                        return;
                    }
                    else if (lnv == mnv)
                    {
                        l = m;
                        fl = fm;
                    }
                    else
                    {
                        r = m;
                        fr = fm;
                    }
                }
            }

            rts.insert(m);
            return;
        }
    };

    inline std::set<double> isolateRealRoots(const Eigen::VectorXd &coeffs, double lbound, double ubound, double tol)
    // Calculate roots of coeffs(x) inside (lbound, rbound) leveraging Sturm theory
    // Requirement: leading coefficient must be nonzero
    //              coeffs(lbound) != 0, coeffs(rbound) != 0, lbound < rbound
    {
        std::set<double> rts;

        // Calculate monic coefficients
        int order = (int)coeffs.size() - 1;
        Eigen::VectorXd monicCoeffs(order + 1);
        monicCoeffs << 1.0, coeffs.tail(order) / coeffs(0);

        // Calculate Cauchy’s bound for the roots of a polynomial
        double rho_c = 1 + monicCoeffs.tail(order).cwiseAbs().maxCoeff();

        // Calculate Kojima’s bound for the roots of a polynomial
        Eigen::VectorXd nonzeroCoeffs(order + 1);
        nonzeroCoeffs.setZero();
        int nonzeros = 0;
        double tempEle;
        for (int i = 0; i < order + 1; i++)
        {
            tempEle = monicCoeffs(i);
            if (fabs(tempEle) >= DBL_EPSILON)
            {
                nonzeroCoeffs(nonzeros++) = tempEle;
            }
        }
        nonzeroCoeffs = nonzeroCoeffs.head(nonzeros).eval();
        Eigen::VectorXd kojimaVec = nonzeroCoeffs.tail(nonzeros - 1).cwiseQuotient(nonzeroCoeffs.head(nonzeros - 1)).cwiseAbs();
        kojimaVec.tail(1) /= 2.0;
        double rho_k = 2.0 * kojimaVec.maxCoeff();

        // Choose a sharper one then loosen it by 1.0 to get an open interval
        double rho = std::min(rho_c, rho_k) + 1.0;

        // Tighten the bound to search in
        lbound = std::max(lbound, -rho);
        ubound = std::min(ubound, rho);

        // Build Sturm sequence
        int len = monicCoeffs.size();
        double sturmSeqs[(RootFinderParam::highestOrder + 1) * (RootFinderParam::highestOrder + 1)];
        int szSeq[RootFinderParam::highestOrder + 1] = {0}; // Explicit ini as zero (gcc may neglect this in -O3)
        double *offsetSeq[RootFinderParam::highestOrder + 1];
        int num = 0;

        for (int i = 0; i < len; i++)
        {
            sturmSeqs[i] = monicCoeffs(i);
            sturmSeqs[i + 1 + len] = (order - i) * sturmSeqs[i] / order;
        }
        szSeq[0] = len;
        szSeq[1] = len - 1;
        offsetSeq[0] = sturmSeqs + len - szSeq[0];
        offsetSeq[1] = sturmSeqs + 2 * len - szSeq[1];

        num += 2;

        bool remainderConstant = false;
        int idx = 0;
        while (!remainderConstant)
        {
            szSeq[idx + 2] = polyMod(offsetSeq[idx],
                                     offsetSeq[idx + 1],
                                     &(sturmSeqs[(idx + 3) * len - szSeq[idx]]),
                                     szSeq[idx], szSeq[idx + 1]);
            offsetSeq[idx + 2] = sturmSeqs + (idx + 3) * len - szSeq[idx + 2];

            remainderConstant = szSeq[idx + 2] == 1;
            for (int i = 1; i < szSeq[idx + 2]; i++)
            {
                offsetSeq[idx + 2][i] /= -fabs(offsetSeq[idx + 2][0]);
            }
            offsetSeq[idx + 2][0] = offsetSeq[idx + 2][0] > 0.0 ? -1.0 : 1.0;
            num++;
            idx++;
        }

        // Isolate all distinct roots inside the open interval recursively
        recurIsolate(lbound, ubound,
                     polyEval(offsetSeq[0], szSeq[0], lbound),
                     polyEval(offsetSeq[0], szSeq[0], ubound),
                     numSignVar(lbound, offsetSeq, szSeq, len),
                     numSignVar(ubound, offsetSeq, szSeq, len),
                     tol, offsetSeq, szSeq, len, rts);

        return rts;
    }

} // namespace RootFinderPriv

namespace RootFinder
{

    inline Eigen::VectorXd polyConv(const Eigen::VectorXd &lCoef, const Eigen::VectorXd &rCoef)
    // Calculate the convolution of lCoef(x) and rCoef(x)
    {
        Eigen::VectorXd result(lCoef.size() + rCoef.size() - 1);
        result.setZero();
        for (int i = 0; i < result.size(); i++)
        {
            for (int j = 0; j <= i; j++)
            {
                result(i) += (j < lCoef.size() && (i - j) < rCoef.size()) ? (lCoef(j) * rCoef(i - j)) : 0;
            }
        }

        return result;
    }

    // // This function needs FFTW 3 and only performs better when the scale is large
    // inline Eigen::VectorXd polyConvFFT(const Eigen::VectorXd &lCoef, const Eigen::VectorXd &rCoef)
    // // Calculate the convolution of lCoef(x) and rCoef(x) using FFT
    // // This function is fast when orders of both poly are larger than 100
    // {
    //     int paddedLen = lCoef.size() + rCoef.size() - 1;
    //     int complexLen = paddedLen / 2 + 1;
    //     Eigen::VectorXd result(paddedLen);
    //     double *rBuffer = fftw_alloc_real(paddedLen);
    //     // Construct FFT plan and buffers
    //     fftw_complex *cForwardBuffer = fftw_alloc_complex(complexLen);
    //     fftw_complex *cBackwardBuffer = fftw_alloc_complex(complexLen);
    //     fftw_plan forwardPlan = fftw_plan_dft_r2c_1d(paddedLen, rBuffer, cForwardBuffer,
    //                                                  FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    //     fftw_plan backwardPlan = fftw_plan_dft_c2r_1d(paddedLen, cBackwardBuffer, rBuffer,
    //                                                   FFTW_ESTIMATE | FFTW_DESTROY_INPUT);
    //     // Pad lCoef by zeros
    //     int len = lCoef.size();
    //     for (int i = 0; i < len; i++)
    //     {
    //         rBuffer[i] = lCoef(i);
    //     }
    //     for (int i = len; i < paddedLen; i++)
    //     {
    //         rBuffer[i] = 0.0;
    //     }
    //     // Compute fft(pad(lCoef(x)) and back it up
    //     fftw_execute(forwardPlan);
    //     memcpy(cBackwardBuffer, cForwardBuffer, sizeof(fftw_complex) * complexLen);
    //     // Pad rCoef by zeros
    //     len = rCoef.size();
    //     for (int i = 0; i < len; i++)
    //     {
    //         rBuffer[i] = rCoef(i);
    //     }
    //     for (int i = len; i < paddedLen; i++)
    //     {
    //         rBuffer[i] = 0.0;
    //     }
    //     // Compute fft(pad(rCoef(x))
    //     fftw_execute(forwardPlan);
    //     // Compute fft(pad(lCoef(x)).fft(pad(rCoef(x))
    //     double real, imag;
    //     for (int i = 0; i < complexLen; i++)
    //     {
    //         real = cBackwardBuffer[i][0];
    //         imag = cBackwardBuffer[i][1];
    //         cBackwardBuffer[i][0] = real * cForwardBuffer[i][0] -
    //                                 imag * cForwardBuffer[i][1];
    //         cBackwardBuffer[i][1] = imag * cForwardBuffer[i][0] +
    //                                 real * cForwardBuffer[i][1];
    //     }
    //     // Compute ifft(fft(pad(lCoef(x)).fft(pad(rCoef(x)))
    //     fftw_execute(backwardPlan);
    //     // Recover the original intensity
    //     double intensity = 1.0 / paddedLen;
    //     for (int i = 0; i < paddedLen; i++)
    //     {
    //         result(i) = rBuffer[i] * intensity;
    //     }
    //     // Destruct FFT plan and buffers
    //     fftw_destroy_plan(forwardPlan);
    //     fftw_destroy_plan(backwardPlan);
    //     fftw_free(rBuffer);
    //     fftw_free(cForwardBuffer);
    //     fftw_free(cBackwardBuffer);
    //     return result;
    // }

    inline Eigen::VectorXd polySqr(const Eigen::VectorXd &coef)
    // Calculate self-convolution of coef(x)
    {
        int coefSize = coef.size();
        int resultSize = coefSize * 2 - 1;
        int lbound, rbound;
        Eigen::VectorXd result(resultSize);
        double temp;
        for (int i = 0; i < resultSize; i++)
        {
            temp = 0;
            lbound = i - coefSize + 1;
            lbound = lbound > 0 ? lbound : 0;
            rbound = coefSize < (i + 1) ? coefSize : (i + 1);
            rbound += lbound;
            if (rbound & 1) //faster than rbound % 2 == 1
            {
                rbound >>= 1; //faster than rbound /= 2
                temp += coef(rbound) * coef(rbound);
            }
            else
            {
                rbound >>= 1; //faster than rbound /= 2
            }

            for (int j = lbound; j < rbound; j++)
            {
                temp += 2.0 * coef(j) * coef(i - j);
            }
            result(i) = temp;
        }

        return result;
    }

    inline double polyVal(const Eigen::VectorXd &coeffs, double x,
                          bool numericalStability = true)
    // Evaluate the polynomial at x, i.e., coeffs(x)
    // Horner scheme is faster yet less stable
    // Stable one should be used when coeffs(x) is close to 0.0
    {
        double retVal = 0.0;
        int order = (int)coeffs.size() - 1;

        if (order >= 0)
        {
            if (fabs(x) < DBL_EPSILON)
            {
                retVal = coeffs(order);
            }
            else if (x == 1.0)
            {
                retVal = coeffs.sum();
            }
            else
            {
                if (numericalStability)
                {
                    double xn = 1.0;

                    for (int i = order; i >= 0; i--)
                    {
                        retVal += coeffs(i) * xn;
                        xn *= x;
                    }
                }
                else
                {
                    int len = coeffs.size();

                    for (int i = 0; i < len; i++)
                    {
                        retVal = retVal * x + coeffs(i);
                    }
                }
            }
        }

        return retVal;
    }

    inline int countRoots(const Eigen::VectorXd &coeffs, double l, double r)
    // Count the number of distinct roots of coeffs(x) inside (l, r), leveraging Sturm theory
    // Boundary values, i.e., coeffs(l) and coeffs(r), must be nonzero
    {
        int nRoots = 0;

        int originalSize = coeffs.size();
        int valid = originalSize;
        for (int i = 0; i < originalSize; i++)
        {
            if (fabs(coeffs(i)) < DBL_EPSILON)
            {
                valid--;
            }
            else
            {
                break;
            }
        }

        if (valid > 0 && fabs(coeffs(originalSize - 1)) > DBL_EPSILON)
        {
            Eigen::VectorXd monicCoeffs(valid);
            monicCoeffs << 1.0, coeffs.segment(originalSize - valid + 1, valid - 1) / coeffs(originalSize - valid);

            // Build the Sturm sequence
            int len = monicCoeffs.size();
            int order = len - 1;
            double sturmSeqs[(RootFinderParam::highestOrder + 1) * (RootFinderParam::highestOrder + 1)];
            int szSeq[RootFinderParam::highestOrder + 1] = {0}; // Explicit ini as zero (gcc may neglect this in -O3)
            int num = 0;

            for (int i = 0; i < len; i++)
            {
                sturmSeqs[i] = monicCoeffs(i);
                sturmSeqs[i + 1 + len] = (order - i) * sturmSeqs[i] / order;
            }
            szSeq[0] = len;
            szSeq[1] = len - 1;
            num += 2;

            bool remainderConstant = false;
            int idx = 0;
            while (!remainderConstant)
            {
                szSeq[idx + 2] = RootFinderPriv::polyMod(&(sturmSeqs[(idx + 1) * len - szSeq[idx]]),
                                                         &(sturmSeqs[(idx + 2) * len - szSeq[idx + 1]]),
                                                         &(sturmSeqs[(idx + 3) * len - szSeq[idx]]),
                                                         szSeq[idx], szSeq[idx + 1]);
                remainderConstant = szSeq[idx + 2] == 1;
                for (int i = 1; i < szSeq[idx + 2]; i++)
                {
                    sturmSeqs[(idx + 3) * len - szSeq[idx + 2] + i] /= -fabs(sturmSeqs[(idx + 3) * len - szSeq[idx + 2]]);
                }
                sturmSeqs[(idx + 3) * len - szSeq[idx + 2]] /= -fabs(sturmSeqs[(idx + 3) * len - szSeq[idx + 2]]);
                num++;
                idx++;
            }

            // Count numbers of sign variations at two boundaries
            double yl, lastyl, yr, lastyr;
            lastyl = RootFinderPriv::polyEval(&(sturmSeqs[len - szSeq[0]]), szSeq[0], l);
            lastyr = RootFinderPriv::polyEval(&(sturmSeqs[len - szSeq[0]]), szSeq[0], r);
            for (int i = 1; i < num; i++)
            {
                yl = RootFinderPriv::polyEval(&(sturmSeqs[(i + 1) * len - szSeq[i]]), szSeq[i], l);
                yr = RootFinderPriv::polyEval(&(sturmSeqs[(i + 1) * len - szSeq[i]]), szSeq[i], r);
                if (lastyl == 0.0 || lastyl * yl < 0.0)
                {
                    ++nRoots;
                }
                if (lastyr == 0.0 || lastyr * yr < 0.0)
                {
                    --nRoots;
                }
                lastyl = yl;
                lastyr = yr;
            }
        }

        return nRoots;
    }

    inline std::set<double> solvePolynomial(const Eigen::VectorXd &coeffs, double lbound, double ubound, double tol, bool isolation = true)
    // Calculate roots of coeffs(x) inside (lbound, rbound)
    //
    // Closed-form solutions are employed for reduced_order < 5
    // isolation = true:
    //                    Sturm' theory and some geometrical property are employed to bracket each root
    //                    Safe-Newton is employed to shrink the interval efficiently
    // isolation = false:
    //                    Eigen values of polynomial companion matrix are calculated
    //
    // Requirement: leading coefficient must be nonzero
    //              coeffs(lbound) != 0, coeffs(rbound) != 0, lbound < rbound
    {
        std::set<double> rts;

        int valid = coeffs.size();
        for (int i = 0; i < coeffs.size(); i++)
        {
            if (fabs(coeffs(i)) < DBL_EPSILON)
            {
                valid--;
            }
            else
            {
                break;
            }
        }

        int offset = 0;
        int nonzeros = valid;
        if (valid > 0)
        {
            for (int i = 0; i < valid; i++)
            {
                if (fabs(coeffs(coeffs.size() - i - 1)) < DBL_EPSILON)
                {
                    nonzeros--;
                    offset++;
                }
                else
                {
                    break;
                }
            }
        }

        if (nonzeros == 0)
        {
            rts.insert(INFINITY);
            rts.insert(-INFINITY);
        }
        else if (nonzeros == 1 && offset == 0)
        {
            rts.clear();
        }
        else
        {
            Eigen::VectorXd ncoeffs(std::max(5, nonzeros));
            ncoeffs.setZero();
            ncoeffs.tail(nonzeros) << coeffs.segment(coeffs.size() - valid, nonzeros);

            if (nonzeros <= 5)
            {
                rts = RootFinderPriv::solveQuart(ncoeffs(0), ncoeffs(1), ncoeffs(2), ncoeffs(3), ncoeffs(4));
            }
            else
            {
                if (isolation)
                {
                    rts = RootFinderPriv::isolateRealRoots(ncoeffs, lbound, ubound, tol);
                }
                else
                {
                    rts = RootFinderPriv::eigenSolveRealRoots(ncoeffs, lbound, ubound, tol);
                }
            }

            if (offset > 0)
            {
                rts.insert(0.0);
            }
        }

        for (auto it = rts.begin(); it != rts.end();)
        {
            if (*it > lbound && *it < ubound)
            {
                it++;
            }
            else
            {
                it = rts.erase(it);
            }
        }

        return rts;
    }

} // namespace RootFinder

#endif