#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


#include <catch.hpp>

#include <sstream>

#include <boost/units/systems/si.hpp>

#include <UnitConvert.hpp>
#include <UnitConvert/GlobalUnitRegistry.hpp>

TEST_CASE("UnitRegisty Tests")
{
  using namespace UnitConvert;
  UnitRegistry ureg;
  CHECK(ureg.size() == 0);

  SECTION("Adding derived units from strings")
  {
    // add base units to the registry
    ureg.addBaseUnit<Dimension::Name::Length>("cm");
    ureg.addBaseUnit<Dimension::Name::Mass>("g");
    ureg.addBaseUnit<Dimension::Name::Time>("s");
    ureg.addBaseUnit<Dimension::Name::Temperature>("K");
    CHECK(ureg.size() == 4);

    // add some derived units
    ureg.addUnit("m = 100 cm");
    ureg.addUnit("1 in = 2.54 cm");
    ureg.addUnit("1 ft = 12 in");
    ureg.addUnit("1 J = 1 kg*m^2*s^-2");
    ureg.addUnit("1 W = 1 J/s");
    ureg.addUnit("1 cal = 4.184 J");
    ureg.addUnit("1 degC = K - 273.15");
    ureg.addUnit("C = K - 273.15");
    ureg.addUnit("delta_K = K");
    ureg.addUnit("delta_C = delta_K");

    Quantity<double> q;

    q = ureg.makeQuantity<double>(24, "cm");
    CHECK(q.to("m").value() == Approx(0.24));
    CHECK(q.to("cm").value() == Approx(24));
    CHECK(q.to("mcm").value() == Approx(24000));
    CHECK(q.to("mm").value() == Approx(240));
    CHECK(q.to_base_units().value() == Approx(24));

    q = ureg.makeQuantity<double>(10, "in");
    CHECK(q.to_base_units().value() == Approx(25.4));

    q = ureg.makeQuantity<double>(1, "J");
    // base units are g and cm so...
    // kg m^-2 s^-2   g/kg cm/m cm/m = 1000 100 100 g cm^-2 s^-2 = 10,000,000 g
    // cm^-2 s^-2
    CHECK(q.to_base_units().value() == Approx(1e7));

    q = ureg.makeQuantity<double>(2, "cal");
    CHECK(q.to_base_units().value() == Approx(2 * 4.184 * 1e7));

    q = ureg.makeQuantity<double>("100 degC");
    CHECK(q.to("K").value() == Approx(373.15));
    CHECK(q.to_base_units().value() == Approx(373.15));

    CHECK(q.unit().is_offset());
    CHECK(q.unit().offset() == Approx(-273.15));
    CHECK(!q.to("K").unit().is_offset());
    CHECK(q.to("K").unit().offset() == Approx(0));
    // to_base_units will return a quantity with an offset unit here,
    // but the offset will be zero.
    CHECK(q.to_base_units().unit().is_offset());
    CHECK(q.to_base_units().unit().offset() == Approx(0));

    CHECK(ureg.makeQuantity<double>("20 C").to("K").value() == Approx(293.15));
    CHECK(ureg.makeQuantity<double>("20 1/delta_C").to("1/K").value() ==
          Approx(20));
    CHECK(
        ureg.makeQuantity<double>("1 cal / g / delta_C").to("J/g/K").value() ==
        Approx(4.184));
  }

  SECTION("Loading derived units from stream")
  {
    // add base units to the registry
    ureg.addBaseUnit<Dimension::Name::Length>("cm");
    ureg.addBaseUnit<Dimension::Name::Mass>("g");
    ureg.addBaseUnit<Dimension::Name::Time>("s");

    std::string units =
        "m = 100 cm\n\n N = kg m / s^2\n # comment\nJ = N m\nW = J/s";
    std::stringstream in(units);
    ureg.loadUnits(in);

    CHECK(ureg.makeQuantity<double>(10, "g m^2 / s^3").to("W").value() == 0.01);
  }

  SECTION("Loading units from file")
  {
    ureg.loadUnits("unit_definitions.txt");

    CHECK(ureg.makeQuantity<double>(10, "g m^2 / s^3").to("W").value() == 0.01);
    CHECK(ureg.makeQuantity<double>(5, "m / s^2").to("gravity").value() ==
          Approx(5 / 9.80665));
    CHECK(ureg.makeQuantity<double>(5, "gravity").to("m/s^2").value() ==
          Approx(5 * 9.80665));

    // H2O is a unit for pressure. 1 H2O is 9806.65 Pa
    CHECK(ureg.makeQuantity<double>(5, "H2O").to("Pa/m").value() ==
          Approx(5 * 9806.65));
  }

  SECTION("Adding base units from strings")
  {
    ureg.addUnit("m = [L]");
    ureg.addUnit("g = [M]");
    ureg.addUnit("s = [T]");
    ureg.addUnit("K = [THETA]");
    ureg.addUnit("A = [I]");
    ureg.addUnit("mol = [N]");
    ureg.addUnit("cd = [J]");
    ureg.addUnit("rad = [1]");

    ureg.addUnit("100 cm = m");
    ureg.addUnit("in = 2.54 cm");
    ureg.addUnit("ft = 12 in");
    ureg.addUnit("J = 1 kg*m^2*s^-2");
    ureg.addUnit("W = 1 J/s");
    ureg.addUnit("cal = 4.184 J");

    CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));
    CHECK(ureg.makeQuantity<double>("2 m").to("in").value() ==
          Approx(200 / 2.54));
  }

  SECTION("Parsing errors")
  {
    CHECK_THROWS(ureg.getUnit("m"));
    CHECK_THROWS(ureg.makeUnit("m"));
    CHECK_THROWS(ureg.getUnit("[L]"));
    CHECK_NOTHROW(ureg.makeUnit("[L]"));
    CHECK_THROWS(ureg.makeQuantity<double>("10"));

  }
}

TEST_CASE("Global Unit Registry Tests")
{
  using namespace UnitConvert;
  SECTION("First Usage")
  {
    UnitRegistry& ureg = getGlobalUnitRegistry();

    CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));
    CHECK(ureg.makeQuantity<double>("2 J").to("kg cm^2 / s^2").value() ==
          Approx(2. * 100 * 100));
    CHECK(ureg.makeQuantity<float>("0 degC").to("degF").value() == Approx(32));
  }

  SECTION("Second Usage")
  {
    UnitRegistry& ureg = getGlobalUnitRegistry();

    CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));
    CHECK(ureg.makeQuantity<double>("2 J").to("kg cm^2 / s^2").value() ==
          Approx(2. * 100 * 100));
    CHECK(ureg.makeQuantity<float>("0 degC").to("degF").value() == Approx(32));

  }

  SECTION("Obscure conversions")
  {
    UnitRegistry& ureg = getGlobalUnitRegistry();

    auto q = ureg.makeQuantity<float>("2 pound");
    CHECK(q.to("kg").value() == Approx(0.90718474));
    CHECK(q.to("electron_mass").value() == Approx(9.95879467317e+29));
    CHECK(q.to("carat").value() == Approx(4535.9237));
    CHECK(q.to("metric_ton").value() == Approx(0.00090718474));
    CHECK(q.to("bag").value() == Approx(0.0212765957447));
    CHECK(q.to("grain").value() == Approx(14000.0));
    CHECK(q.to("oz").value() == Approx(32));
    CHECK(q.to("short_ton").value() == Approx(0.001));

  }

}

TEST_CASE("UnitRegisty Adding Unit Tests")
{
  using namespace UnitConvert;
  UnitRegistry ureg;
  CHECK(ureg.size() == 0);

  ureg.addUnit("m = [L]");
  ureg.addUnit("cm = 0.01 m");

  CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));

  CHECK_THROWS(ureg.addUnit("cm = 10 m"));
  CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));

  ureg.existing_unit_policy = UnitRegistry::EXISTING_UNIT_POLICY::Warn;
  ureg.addUnit("cm = 10 m");
  CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));

  ureg.existing_unit_policy = UnitRegistry::EXISTING_UNIT_POLICY::Ignore;
  ureg.addUnit("cm = 10 m");
  CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(200));

  ureg.existing_unit_policy = UnitRegistry::EXISTING_UNIT_POLICY::Overwrite;
  ureg.addUnit("cm = 10 m");
  CHECK(ureg.makeQuantity<double>("2 m").to("cm").value() == Approx(0.2));

}


#include <iostream>
//
#include <polyvec/api.hpp>
#include <polyvec/curve-tracer/bezier_merging.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>
#include <polyvec/geometry/winding_number.hpp>
//
#include <Eigen/Core>
#include <Eigen/Geometry>

NAMESPACE_BEGIN()
::polyvec::BezierCurve build_bezier(const Eigen::Matrix<double, 2, 4> ctrl) {
  ::polyvec::BezierCurve bz;
  bz.set_control_points(ctrl);
  return bz;
};
NAMESPACE_END()

NAMESPACE_BEGIN(polyvectest)
NAMESPACE_BEGIN(BezierMerging)

int potrace_params(int, char**) {
  // Creat the points and then test the mapping
  Eigen::Matrix<double, 2, 3> points0;
  Eigen::Matrix<double, 2, 4> yy;
  Eigen::Matrix2d A;
  Eigen::Vector2d b;
  Eigen::Matrix2d Amanu;
  Eigen::Vector2d bmanu;
  polyvec::VtkCurveWriter writer;

  points0.col(0) << 2, 10;
  points0.col(1) << 5, 7;
  points0.col(2) << 4, 12;

  //
  // First Just Create a bezier and draw it
  //
  const double alpha_manu = 0.8;
  const double beta_manu = 0.3;

  yy.col(0) = points0.col(0);
  yy.col(1) = ::polyvec::Num::lerp<Eigen::Vector2d>(points0.col(0),
                                                    points0.col(1), alpha_manu);
  yy.col(2) = ::polyvec::Num::lerp<Eigen::Vector2d>(points0.col(2),
                                                    points0.col(1), beta_manu);
  yy.col(3) = points0.col(2);

  writer.add_polyline(points0);
  writer.add_point(yy.col(0));
  writer.add_point(yy.col(1));
  writer.add_point(yy.col(2));
  writer.add_point(yy.col(3));
  writer.add_polyline(build_bezier(yy).get_tesselation2());
  writer.dump("test_dump/potrace_params_00.vtk");
  writer.clear();

  //
  // Now create the potrace parameterization
  //
  ::polyfit::BezierMerging::PointParameters point_params{yy};
  assert_break(point_params.is_potracable());
  {
    double alpha, beta;
    Eigen::Vector2d oo;
    bool is;
    point_params.is_potracable(is, oo, alpha, beta);
    assert_break(is == true);
    assert_break(std::abs(alpha - alpha_manu) < 1e-8);
    assert_break(std::abs(beta - beta_manu) < 1e-8);
    assert_break((oo - points0.col(1)).norm() < 1e-8);
  }
  ::polyfit::BezierMerging::PotraceParameters potrace_params =
      point_params.as_potrace_params();

  writer.add_polyline(
      build_bezier(potrace_params.get_yy().control_points).get_tesselation2());
  writer.dump("test_dump/potrace_params_01.vtk");
  writer.clear();

  //
  // Now draw the closest equiparam bezier
  //

  writer.add_polyline(
      build_bezier(potrace_params.equiparamed().get_yy().control_points)
          .get_tesselation2());
  writer.dump("test_dump/potrace_params_02.vtk");
  writer.clear();

  //
  // Now check that the area works
  //
  Eigen::Matrix2Xd tess;
  tess = build_bezier(yy).get_tesselation2();
  double area_tess;
  {
    bool is_ccw;
    polyvec::WindingNumber::compute_orientation(tess, is_ccw, area_tess);
  }
  double area_analytic = potrace_params.get_areay();
  double area_eqp_analytic = potrace_params.equiparamed().get_areay();

  printf("Testing area \n");
  printf("areas: tess: %.6f, ana: %.6f, anaeq: %.6f \n", area_tess,
         area_analytic, area_eqp_analytic);

  //
  // Now check that the tangent things works
  //
  printf("Testing inverse tangent solver \n");
  {
    ::polyvec::BezierCurve bzcurve = build_bezier(yy);
    for (int i = 1; i < 99 ; ++i) {
      double t_solved;
      bool success;

      const double t = 1. / 99 * i;
      const Eigen::Vector2d tan = bzcurve.dposdt(t).normalized();
      potrace_params.t_for_tangent(tan, t_solved, success);

      assert_break( success );
      assert_break( std::abs(t-t_solved) < 1e-10 );
    }
  }

  return EXIT_FAILURE;
}

NAMESPACE_END(BezierMerging)
NAMESPACE_END(polyvectest)


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

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "rips_complex"
#include <boost/test/unit_test.hpp>

#include <cmath>  // float comparison
#include <limits>
#include <string>
#include <vector>
#include <algorithm>    // std::max

#include <gudhi/Rips_complex.h>
#include <gudhi/Sparse_rips_complex.h>
// to construct Rips_complex from a OFF file of points
#include <gudhi/Points_off_io.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/distance_functions.h>
#include <gudhi/reader_utils.h>
#include <gudhi/Unitary_tests_utils.h>

// Type definitions
using Point = std::vector<double>;
using Simplex_tree = Gudhi::Simplex_tree<>;
using Filtration_value = Simplex_tree::Filtration_value;
using Rips_complex = Gudhi::rips_complex::Rips_complex<Simplex_tree::Filtration_value>;
using Sparse_rips_complex = Gudhi::rips_complex::Sparse_rips_complex<Simplex_tree::Filtration_value>;
using Distance_matrix = std::vector<std::vector<Filtration_value>>;

BOOST_AUTO_TEST_CASE(RIPS_DOC_OFF_file) {
  // ----------------------------------------------------------------------------
  //
  // Init of a Rips complex from a OFF file
  //
  // ----------------------------------------------------------------------------
  std::string off_file_name("alphacomplexdoc.off");
  double rips_threshold = 12.0;
  std::cout << "========== OFF FILE NAME = " << off_file_name << " - Rips threshold=" <<
      rips_threshold << "==========" << std::endl;

  Gudhi::Points_off_reader<Point> off_reader(off_file_name);
  Rips_complex rips_complex_from_file(off_reader.get_point_cloud(), rips_threshold, Gudhi::Euclidean_distance());

  const int DIMENSION_1 = 1;
  Simplex_tree st;
  rips_complex_from_file.create_complex(st, DIMENSION_1);
  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION_1);

  const int NUMBER_OF_VERTICES = 7;
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 18);

  // Check filtration values of vertices is 0.0
  for (auto f_simplex : st.skeleton_simplex_range(0)) {
    BOOST_CHECK(st.filtration(f_simplex) == 0.0);
  }

  // Check filtration values of edges
  for (auto f_simplex : st.skeleton_simplex_range(DIMENSION_1)) {
    if (DIMENSION_1 == st.dimension(f_simplex)) {
      std::vector<Point> vp;
      std::cout << "vertex = (";
      for (auto vertex : st.simplex_vertex_range(f_simplex)) {
        std::cout << vertex << ",";
        vp.push_back(off_reader.get_point_cloud().at(vertex));
      }
      std::cout << ") - distance =" << Gudhi::Euclidean_distance()(vp.at(0), vp.at(1)) <<
          " - filtration =" << st.filtration(f_simplex) << std::endl;
      BOOST_CHECK(vp.size() == 2);
      GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), Gudhi::Euclidean_distance()(vp.at(0), vp.at(1)));
    }
  }

  const int DIMENSION_2 = 2;
  Simplex_tree st2;
  rips_complex_from_file.create_complex(st2, DIMENSION_2);
  std::cout << "st2.dimension()=" << st2.dimension() << std::endl;
  BOOST_CHECK(st2.dimension() == DIMENSION_2);

  std::cout << "st2.num_vertices()=" << st2.num_vertices() << std::endl;
  BOOST_CHECK(st2.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st2.num_simplices()=" << st2.num_simplices() << std::endl;
  BOOST_CHECK(st2.num_simplices() == 23);

  Simplex_tree::Filtration_value f01 = st2.filtration(st2.find({0, 1}));
  Simplex_tree::Filtration_value f02 = st2.filtration(st2.find({0, 2}));
  Simplex_tree::Filtration_value f12 = st2.filtration(st2.find({1, 2}));
  Simplex_tree::Filtration_value f012 = st2.filtration(st2.find({0, 1, 2}));
  std::cout << "f012= " << f012 << " | f01= " << f01 << " - f02= " << f02 << " - f12= " << f12 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f012, std::max(f01, std::max(f02,f12)));

  Simplex_tree::Filtration_value f45 = st2.filtration(st2.find({4, 5}));
  Simplex_tree::Filtration_value f56 = st2.filtration(st2.find({5, 6}));
  Simplex_tree::Filtration_value f46 = st2.filtration(st2.find({4, 6}));
  Simplex_tree::Filtration_value f456 = st2.filtration(st2.find({4, 5, 6}));
  std::cout << "f456= " << f456 << " | f45= " << f45 << " - f56= " << f56 << " - f46= " << f46 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f456, std::max(f45, std::max(f56,f46)));

  const int DIMENSION_3 = 3;
  Simplex_tree st3;
  rips_complex_from_file.create_complex(st3, DIMENSION_3);
  std::cout << "st3.dimension()=" << st3.dimension() << std::endl;
  BOOST_CHECK(st3.dimension() == DIMENSION_3);

  std::cout << "st3.num_vertices()=" << st3.num_vertices() << std::endl;
  BOOST_CHECK(st3.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st3.num_simplices()=" << st3.num_simplices() << std::endl;
  BOOST_CHECK(st3.num_simplices() == 24);

  Simplex_tree::Filtration_value f123 = st3.filtration(st3.find({1, 2, 3}));
  Simplex_tree::Filtration_value f013 = st3.filtration(st3.find({0, 1, 3}));
  Simplex_tree::Filtration_value f023 = st3.filtration(st3.find({0, 2, 3}));
  Simplex_tree::Filtration_value f0123 = st3.filtration(st3.find({0, 1, 2, 3}));
  std::cout << "f0123= " << f0123 << " | f012= " << f012 << " - f123= " << f123 << " - f013= " << f013 <<
      " - f023= " << f023 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f0123, std::max(f012, std::max(f123, std::max(f013, f023))));

}

using Vector_of_points = std::vector<Point>;

bool is_point_in_list(Vector_of_points points_list, Point point) {
  for (auto& point_in_list : points_list) {
    if (point_in_list == point) {
      return true;  // point found
    }
  }
  return false;  // point not found
}

class Custom_square_euclidean_distance {
 public:
  template< typename Point >
  auto operator()(const Point& p1, const Point& p2) -> typename Point::value_type {
    auto it1 = p1.begin();
    auto it2 = p2.begin();
    typename Point::value_type dist = 0.;
    for (; it1 != p1.end(); ++it1, ++it2) {
      typename Point::value_type tmp = (*it1) - (*it2);
      dist += tmp*tmp;
    }
    return dist;
  }
};

BOOST_AUTO_TEST_CASE(Rips_complex_from_points) {
  // ----------------------------------------------------------------------------
  // Init of a list of points
  // ----------------------------------------------------------------------------
  Vector_of_points points;
  std::vector<double> coords = { 0.0, 0.0, 0.0, 1.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 0.0, 0.0, 1.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 0.0, 1.0, 0.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 1.0, 0.0, 0.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));

  // ----------------------------------------------------------------------------
  // Init of a Rips complex from the list of points
  // ----------------------------------------------------------------------------
  Rips_complex rips_complex_from_points(points, 2.0, Custom_square_euclidean_distance());

  std::cout << "========== Rips_complex_from_points ==========" << std::endl;
  Simplex_tree st;
  const int DIMENSION = 3;
  rips_complex_from_points.create_complex(st, DIMENSION);

  // Another way to check num_simplices
  std::cout << "Iterator on Rips complex simplices in the filtration order, with [filtration value]:" << std::endl;
  int num_simplices = 0;
  for (auto f_simplex : st.filtration_simplex_range()) {
    num_simplices++;
    std::cout << "   ( ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << ") -> " << "[" << st.filtration(f_simplex) << "] ";
    std::cout << std::endl;
  }
  BOOST_CHECK(num_simplices == 15);
  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 15);

  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION);
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == 4);

  for (auto f_simplex : st.filtration_simplex_range()) {
    std::cout << "dimension(" << st.dimension(f_simplex) << ") - f = " << st.filtration(f_simplex) << std::endl;
    switch (st.dimension(f_simplex)) {
      case 0:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.0);
        break;
      case 1:
      case 2:
      case 3:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 2.0);
        break;
      default:
        BOOST_CHECK(false);  // Shall not happen
        break;
    }
  }
}

BOOST_AUTO_TEST_CASE(Sparse_rips_complex_from_points) {
  // This is a clone of the test above
  // ----------------------------------------------------------------------------
  // Init of a list of points
  // ----------------------------------------------------------------------------
  Vector_of_points points;
  std::vector<double> coords = { 0.0, 0.0, 0.0, 1.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 0.0, 0.0, 1.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 0.0, 1.0, 0.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));
  coords = { 1.0, 0.0, 0.0, 0.0 };
  points.push_back(Point(coords.begin(), coords.end()));

  // ----------------------------------------------------------------------------
  // Init of a Rips complex from the list of points
  // ----------------------------------------------------------------------------
  // .001 is small enough that we get a deterministic result matching the exact Rips
  Sparse_rips_complex sparse_rips(points, Custom_square_euclidean_distance(), .001);

  std::cout << "========== Sparse_rips_complex_from_points ==========" << std::endl;
  Simplex_tree st;
  const int DIMENSION = 3;
  sparse_rips.create_complex(st, DIMENSION);

  // Another way to check num_simplices
  std::cout << "Iterator on Rips complex simplices in the filtration order, with [filtration value]:" << std::endl;
  int num_simplices = 0;
  for (auto f_simplex : st.filtration_simplex_range()) {
    num_simplices++;
    std::cout << "   ( ";
    for (auto vertex : st.simplex_vertex_range(f_simplex)) {
      std::cout << vertex << " ";
    }
    std::cout << ") -> " << "[" << st.filtration(f_simplex) << "] ";
    std::cout << std::endl;
  }
  BOOST_CHECK(num_simplices == 15);
  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 15);

  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION);
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == 4);

  for (auto f_simplex : st.filtration_simplex_range()) {
    std::cout << "dimension(" << st.dimension(f_simplex) << ") - f = " << st.filtration(f_simplex) << std::endl;
    switch (st.dimension(f_simplex)) {
      case 0:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 0.0);
        break;
      case 1:
      case 2:
      case 3:
        GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), 2.0);
        break;
      default:
        BOOST_CHECK(false);  // Shall not happen
        break;
    }
  }
}

BOOST_AUTO_TEST_CASE(Rips_doc_csv_file) {
  // ----------------------------------------------------------------------------
  //
  // Init of a Rips complex from a OFF file
  //
  // ----------------------------------------------------------------------------
  std::string csv_file_name("full_square_distance_matrix.csv");
  double rips_threshold = 12.0;
  std::cout << "========== CSV FILE NAME = " << csv_file_name << " - Rips threshold=" <<
      rips_threshold << "==========" << std::endl;

  Distance_matrix distances = Gudhi::read_lower_triangular_matrix_from_csv_file<Filtration_value>(csv_file_name);
  Rips_complex rips_complex_from_file(distances, rips_threshold);

  const int DIMENSION_1 = 1;
  Simplex_tree st;
  rips_complex_from_file.create_complex(st, DIMENSION_1);
  std::cout << "st.dimension()=" << st.dimension() << std::endl;
  BOOST_CHECK(st.dimension() == DIMENSION_1);

  const int NUMBER_OF_VERTICES = 7;
  std::cout << "st.num_vertices()=" << st.num_vertices() << std::endl;
  BOOST_CHECK(st.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st.num_simplices()=" << st.num_simplices() << std::endl;
  BOOST_CHECK(st.num_simplices() == 18);

  // Check filtration values of vertices is 0.0
  for (auto f_simplex : st.skeleton_simplex_range(0)) {
    BOOST_CHECK(st.filtration(f_simplex) == 0.0);
  }

  // Check filtration values of edges
  for (auto f_simplex : st.skeleton_simplex_range(DIMENSION_1)) {
    if (DIMENSION_1 == st.dimension(f_simplex)) {
      std::vector<Simplex_tree::Vertex_handle> vvh;
      std::cout << "vertex = (";
      for (auto vertex : st.simplex_vertex_range(f_simplex)) {
        std::cout << vertex << ",";
        vvh.push_back(vertex);
      }
      std::cout << ") - filtration =" << st.filtration(f_simplex) << std::endl;
      BOOST_CHECK(vvh.size() == 2);
      GUDHI_TEST_FLOAT_EQUALITY_CHECK(st.filtration(f_simplex), distances[vvh.at(0)][vvh.at(1)]);
    }
  }

  const int DIMENSION_2 = 2;
  Simplex_tree st2;
  rips_complex_from_file.create_complex(st2, DIMENSION_2);
  std::cout << "st2.dimension()=" << st2.dimension() << std::endl;
  BOOST_CHECK(st2.dimension() == DIMENSION_2);

  std::cout << "st2.num_vertices()=" << st2.num_vertices() << std::endl;
  BOOST_CHECK(st2.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st2.num_simplices()=" << st2.num_simplices() << std::endl;
  BOOST_CHECK(st2.num_simplices() == 23);

  Simplex_tree::Filtration_value f01 = st2.filtration(st2.find({0, 1}));
  Simplex_tree::Filtration_value f02 = st2.filtration(st2.find({0, 2}));
  Simplex_tree::Filtration_value f12 = st2.filtration(st2.find({1, 2}));
  Simplex_tree::Filtration_value f012 = st2.filtration(st2.find({0, 1, 2}));
  std::cout << "f012= " << f012 << " | f01= " << f01 << " - f02= " << f02 << " - f12= " << f12 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f012, std::max(f01, std::max(f02,f12)));

  Simplex_tree::Filtration_value f45 = st2.filtration(st2.find({4, 5}));
  Simplex_tree::Filtration_value f56 = st2.filtration(st2.find({5, 6}));
  Simplex_tree::Filtration_value f46 = st2.filtration(st2.find({4, 6}));
  Simplex_tree::Filtration_value f456 = st2.filtration(st2.find({4, 5, 6}));
  std::cout << "f456= " << f456 << " | f45= " << f45 << " - f56= " << f56 << " - f46= " << f46 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f456, std::max(f45, std::max(f56,f46)));

  const int DIMENSION_3 = 3;
  Simplex_tree st3;
  rips_complex_from_file.create_complex(st3, DIMENSION_3);
  std::cout << "st3.dimension()=" << st3.dimension() << std::endl;
  BOOST_CHECK(st3.dimension() == DIMENSION_3);

  std::cout << "st3.num_vertices()=" << st3.num_vertices() << std::endl;
  BOOST_CHECK(st3.num_vertices() == NUMBER_OF_VERTICES);

  std::cout << "st3.num_simplices()=" << st3.num_simplices() << std::endl;
  BOOST_CHECK(st3.num_simplices() == 24);

  Simplex_tree::Filtration_value f123 = st3.filtration(st3.find({1, 2, 3}));
  Simplex_tree::Filtration_value f013 = st3.filtration(st3.find({0, 1, 3}));
  Simplex_tree::Filtration_value f023 = st3.filtration(st3.find({0, 2, 3}));
  Simplex_tree::Filtration_value f0123 = st3.filtration(st3.find({0, 1, 2, 3}));
  std::cout << "f0123= " << f0123 << " | f012= " << f012 << " - f123= " << f123 << " - f013= " << f013 <<
      " - f023= " << f023 << std::endl;
  GUDHI_TEST_FLOAT_EQUALITY_CHECK(f0123, std::max(f012, std::max(f123, std::max(f013, f023))));

}

#ifdef GUDHI_DEBUG
BOOST_AUTO_TEST_CASE(Rips_create_complex_throw) {
  // ----------------------------------------------------------------------------
  //
  // Init of a Rips complex from a OFF file
  //
  // ----------------------------------------------------------------------------
  std::string off_file_name("alphacomplexdoc.off");
  double rips_threshold = 12.0;
  std::cout << "========== OFF FILE NAME = " << off_file_name << " - Rips threshold=" <<
      rips_threshold << "==========" << std::endl;

  Gudhi::Points_off_reader<Point> off_reader(off_file_name);
  Rips_complex rips_complex_from_file(off_reader.get_point_cloud(), rips_threshold, Gudhi::Euclidean_distance());

  Simplex_tree stree;
  std::vector<int> simplex = {0, 1, 2};
  stree.insert_simplex_and_subfaces(simplex);
  std::cout << "Check exception throw in debug mode" << std::endl;
  // throw excpt because stree is not empty
  BOOST_CHECK_THROW (rips_complex_from_file.create_complex(stree, 1), std::invalid_argument);
}
#endif


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


/**
 * @file : cartesian_product.h
 * @author : Vikas Dhiman
 * @date: Wed July 11 2013
 */

#pragma once

#include <boost/function.hpp>
#include <boost/iterator/filter_iterator.hpp>
#include <boost/type_traits.hpp>
#include <boost/utility/result_of.hpp>
#include <vector>

#include <boost/lambda/lambda.hpp>
namespace bl = boost::lambda;

namespace occgrid {

/**
 * @brief: Produces a cartesian product of assignments to a Iterator of
 * variable nodes
 *
 * @type : InputIterator : A type of Iterator of VariableNodes
 * @type : PossibleValueIter : A type of Iterator of VariableNodes
 */
template<typename InputIterator, typename SampleSpaceMap>
class CartesianProduct {
private:
  typedef typename std::iterator_traits<InputIterator>::value_type Vnode;
  typedef typename SampleSpaceMap::value_type::first_type PossibleValueIter;
  typedef typename std::iterator_traits<PossibleValueIter>::value_type Value;

  // Nodes with possible assignments
	const InputIterator nodes_begin_;
  const InputIterator nodes_end_;

  const SampleSpaceMap& cdmap_;

  // A vector of value iterators pointing to the current value of each node
  std::vector< PossibleValueIter > current_val_;

  // last_iteration_
  bool last_iteration_;

public:
  CartesianProduct(
      InputIterator nodes_begin,
      InputIterator nodes_end,
      SampleSpaceMap cdmap)
    : nodes_begin_(nodes_begin),
    nodes_end_(nodes_end),
    cdmap_(cdmap),
    current_val_(),
    last_iteration_(false)
    {
      // Initialize all values
      for (InputIterator it = nodes_begin_; it != nodes_end_; ++it) {
        current_val_.push_back(get(cdmap_, *it).first);
      }
    }

  /// Return the next assignment
  // TODO: We don't need to accept a property map type, we can just do by
  // transformed iterator over a map object
  template<typename PropertyMap>
  bool next(PropertyMap &assign) {
    bool carry_over = true;
    typedef typename std::vector< PossibleValueIter >::iterator val_it_it;
    val_it_it node_val_it_it(current_val_.begin());
    InputIterator node_it(nodes_begin_);
    for(;(node_val_it_it != current_val_.end())
        && (node_it != nodes_end_);
        ++ node_val_it_it, ++ node_it)
    {
      PossibleValueIter &node_val_it = *node_val_it_it;
      const Vnode &node = *node_it;
      PossibleValueIter poss_val_end(get(cdmap_, node).second);
      Value val = *node_val_it;
      //std::cout << node << " -> " << val << std::endl;
      assign[node] = val;

      if (carry_over)
        node_val_it ++;

      if (node_val_it == poss_val_end) {
        // loop around and carry over the carry_over
        node_val_it = get(cdmap_, node).first;
        // values exhausted carry over to next loop
        carry_over = true;
      } else {
        // nothing to carry over to next loop
        carry_over = false;
      }
    }
    // If we complete the loop that means we have exhausted all possible cases
    bool is_last_iteration = last_iteration_;
    last_iteration_ = (carry_over);
    return (! is_last_iteration);
  }
};

template<typename Real,
  //typename UnaryFunction,
  typename InputIterator,
  typename SampleSpaceMap,
  typename Assignment
  >
// typename UnaryFunction::result_type
Real
summaryOf(
		const boost::function<Real (const Assignment&)> &func,
    //UnaryFunction &func,
		InputIterator dependent_nodes_begin,
		InputIterator dependent_nodes_end,
    const SampleSpaceMap& cdmap,
		const typename std::iterator_traits<InputIterator>::value_type &x,
    //const typename Assignment::value_type &xv
    const typename SampleSpaceMap::value_type::first_type::value_type &xv
    )
{
  BOOST_AUTO_TPL(fi_begin,
      boost::make_filter_iterator(
        (x != bl::_1), dependent_nodes_begin, dependent_nodes_end));
  BOOST_AUTO_TPL(fi_end,
      boost::make_filter_iterator(
        (x != bl::_1), dependent_nodes_end, dependent_nodes_end));
  typedef BOOST_TYPEOF_TPL(fi_begin) filter_iterator;

  CartesianProduct<filter_iterator, SampleSpaceMap> poss_assign(fi_begin, fi_end, cdmap);
  Real summary(0);
  Assignment assign;
  assign[x] = xv;
  while (poss_assign.next(assign)) {
    Real fa = func(assign);
    summary += fa;
  }
  return summary;
}

template<typename Real,
  //typename UnaryFunction,
  typename InputIterator,
  typename SampleSpaceMap,
  typename Assignment
  >
// typename UnaryFunction::result_type
Real
maxOf(
		const boost::function<Real (const Assignment&)> &func,
    //UnaryFunction &func,
		InputIterator dependent_nodes_begin,
		InputIterator dependent_nodes_end,
    const SampleSpaceMap& cdmap,
		const typename std::iterator_traits<InputIterator>::value_type &x,
    //const typename Assignment::value_type &xv
    const typename SampleSpaceMap::value_type::first_type::value_type &xv
    )
{
  // typedef typename boost::remove_reference<
  //   typename UnaryFunction::argument_type>::type const_assignment_type;
  // typedef typename boost::remove_const<const_assignment_type>::type Assignment;
  // typedef typename UnaryFunction::result_type Real;
  typedef typename std::iterator_traits<InputIterator>::value_type Vnode;

  // filter iterator to get all neighbors except var
  BOOST_AUTO_TPL(fi_begin,
      boost::make_filter_iterator(
        (x != bl::_1), dependent_nodes_begin, dependent_nodes_end));
  BOOST_AUTO_TPL(fi_end,
      boost::make_filter_iterator(
        (x != bl::_1), dependent_nodes_end, dependent_nodes_end));
  typedef BOOST_TYPEOF_TPL(fi_begin) filter_iterator;

  CartesianProduct<filter_iterator, SampleSpaceMap> poss_assign(fi_begin, fi_end, cdmap);
  Real mx(0);
  Assignment assign;
  assign[x] = xv;
  while (poss_assign.next(assign)) {
    Real fa = func(assign);
    using std::max;
    mx = max(mx, fa);
  }
  return mx;
}
} // namespace occgrid