#include <NTL/RR.h>

#include "FactorBase.h"

#ifdef DEBUG
#include<iostream>
#endif

FactorBase::FactorBase(ZZ bound)
{
    setBound(bound);
}

bool FactorBase::factor(std::vector<long>& f, const ZZ& _n)const
{
    /*
     * Factorization of a number over a rational factor base.
     */
#ifdef DEBUG
    std::cerr<<"Value of factored num is: "<<_n<<std::endl;
#endif
    if(_n == 0)
    {
        return false;
    }
    f.resize(r.size());
    for(int i = 0; i < r.size(); ++i)
    {
        f[i] = 0;
    }
    ZZ n(_n);
    if(n < 0)
    {
        f[0] = 1;
        n = -n;
    }
    else
    {
        f[0] = 0;
    }
    ZZ q;
    for(long i=1; i<smallInd; ++i)
    {
        while(n % r[i]  == 0)
        {
#ifdef DEBUG
            std::cerr<<"N="<<n<<" is divisible by r="<<r[i]<<std::endl;
#endif
            n = n / r[i];
            ++f[i];
        }

        if(n == 1)
        {
            return true;
        }
    }

    if(n <= r[r.size()-1])
    {
        for(long i=0; i<r.size(); ++i)
        {
            if (n == r[i])
            {
                ++f[i];
                return true;
            }
        }
    }

    for(long i=smallInd; i<r.size(); ++i)
    {
        if(n % r[i] == 0)
        {
            n = n / r[i];
            ++f[i];
        }
        while(n % r[i]  == 0)
        {
            n = n / r[i];
            ++f[i];
        }
        if(n <= r[r.size()-1])
        {
            if (n == 1)
            {
                return true;
            }
            for(long j=i+1; j<r.size(); ++j)
            {
                if (n == r[j])
                {
                    ++f[j];
                    return true;
                }
            }
        }
    }
    return false;
}

FactorBase::FactorBase() {
    count = 0;
    smallInd = -1;
    r.reserve(0);
}

void FactorBase::setBound(ZZ bound) {
    /*
     * Generating a rational factor base with upper bound of bound.
     */
    ZZ q = ZZ(-1);
    ZZ elem;
    smallInd = 0;
#ifdef DEBUG
    std::cerr<<"Rational factor base elements are:\n";
#endif
    while(q<bound)
    {
#ifdef DEBUG
        std::cerr<<q<<" ";
#endif
        elem = q;
        r.push_back(elem);
        count++;
        q = NextPrime(q+1);
    }
    while(r[smallInd] < TruncToZZ(sqrt(conv<RR>(r[r.size()-1]))))
    {
        ++smallInd;
    }
#ifdef DEBUG
    std::cerr<<"\n";
#endif
}

unsigned long FactorBase::getSize() const
{
    return r.size();
}


/**
 * @file matode.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>

namespace MatODE {

/* SAM_LISTING_BEGIN_3 */
Eigen::MatrixXd eeulstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                         double h) {
  return Y0 + h * A * Y0;
}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
Eigen::MatrixXd ieulstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                         double h) {
  int n = A.rows();
  return (Eigen::MatrixXd::Identity(n, n) - h * A).partialPivLu().solve(Y0);
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
Eigen::MatrixXd impstep(const Eigen::MatrixXd& A, const Eigen::MatrixXd& Y0,
                        double h) {
  int n = A.rows();
  return (Eigen::MatrixXd::Identity(n, n) - h * 0.5 * A)
      .partialPivLu()
      .solve(Y0 + h * 0.5 * A * Y0);
}
/* SAM_LISTING_END_5 */

}  // namespace MatODE


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

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

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

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

// Free energy, energy, and specific heat of triangular lattice Ising model

#include <iomanip>
#include <iostream>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ising/mp_wrapper.hpp"
#include "triangular.hpp"

struct options {
  unsigned int prec;
  std::string Ja, Jb, Jc, Tmin, Tmax, dT;
  bool valid;
  options(unsigned int argc, char *argv[]) :
    prec(15), Ja("1"), Jb("1"), Jc("1"), valid(true) {
    if (argc == 1) { valid = false; return; }
    for (unsigned i = 1; i < argc; ++i) {
      switch (argv[i][0]) {
      case '-' :
        switch (argv[i][1]) {
        case 'p' :
          if (++i == argc) { valid = false; return; }
          prec = std::atoi(argv[i]); break;
        default :
          valid = false; return;
        }
        break;
      default :
        switch (argc - i) {
        case 1:
          Ja = Jb = Jc = "1";
          Tmin = Tmax = dT = argv[i];
          return;
        case 2:
          Ja = Jb = Jc = argv[i];
          Tmin = Tmax = dT = argv[i+1];
          return;
        case 4:
          Ja = argv[i];
          Jb = argv[i+1];
          Jc = argv[i+2];
          Tmin = Tmax = dT = argv[i+3];
          return;
        case 6:
          Ja = argv[i];
          Jb = argv[i+1];
          Jc = argv[i+2];
          Tmin = argv[i+3];
          Tmax = argv[i+4];
          dT = argv[i+5];
          return;
        default:
          valid = false; return;
        }
      }
    }
  }
};

template<typename T>
void calc(const options& opt) {
  using namespace ising::free_energy;
  typedef T real_t;
  real_t Ja = convert<real_t>(opt.Ja);
  real_t Jb = convert<real_t>(opt.Jb);
  real_t Jc = convert<real_t>(opt.Jc);
  real_t Tmin = convert<real_t>(opt.Tmin);
  real_t Tmax = convert<real_t>(opt.Tmax);
  real_t dT = convert<real_t>(opt.dT);
  if (Tmin > Tmax) throw(std::invalid_argument("Tmax should be larger than Tmin"));
  if (dT <= 0) throw(std::invalid_argument("dT should be positive"));
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "# lattice: triangular\n"
            << "# precision: " << std::numeric_limits<real_t>::digits10 << std::endl
            << "# Lx Ly Ja Jb Jc T 1/T F/N E/N C/N\n";
  for (auto t = Tmin; t < Tmax + 1e-4 * dT; t += dT) {
    auto beta = boost::math::differentiation::make_fvar<real_t, 2>(1 / t);
    auto f = triangular::infinite(Ja, Jb, Jc, beta);
    std::cout << "inf inf " << Ja << ' ' << Jb << ' ' << Jc << ' ' << t << ' ' << (1 / t) << ' '
              << free_energy(f, beta) << ' ' << energy(f, beta) << ' '
              << specific_heat(f, beta) << std::endl;
  }
}

int main(int argc, char **argv) {
  using namespace boost::multiprecision;
  options opt(argc, argv);
  if (!opt.valid) {
    std::cerr << "Usage: " << argv[0] << " [-p prec] T\n"
              << "       " << argv[0] << " [-p prec] J T\n"
              << "       " << argv[0] << " [-p prec] Ja Jb Jc T\n"
              << "       " << argv[0] << " [-p prec] Ja Jb Jc Tmin Tmax dT\n"; return 127;
  }
  if (opt.prec <= std::numeric_limits<float>::digits10) {
    calc<float>(opt);
  } else if (opt.prec <= std::numeric_limits<double>::digits10) {
    calc<double>(opt);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<cpp_dec_float_50>>::digits10) {
    calc<mp_wrapper<cpp_dec_float_50>>(opt);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<cpp_dec_float_100>>::digits10) {
    calc<mp_wrapper<cpp_dec_float_100>>(opt);
  } else {
    std::cerr << "Error: Required precision is too high\n"; return 127;
  }
}


#include <vector>
#include <random>
#include <cmath>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>

std::random_device rd;
std::mt19937 rng(rd());

int reed_frost(int n, int s0, double Q, double beta, double alpha, double k)
{
	std::binomial_distribution<int> outside(s0,1-Q);

	double gamma_mean = beta/pow(n,alpha);
	double gamma_shape = k;
	std::gamma_distribution<double> hazard(gamma_shape,
		gamma_mean/gamma_shape);

	int infected = outside(rd);
	int total_infected = infected;
	int susceptibles = s0-total_infected;

	while(infected!=0)
	{
		double cum_hazard=0.;
		for (int i = 0; i < infected; ++i)
		{
			double h = hazard(rd);
			cum_hazard += h;
		}
		double trans_prob = std::exp(-cum_hazard);
		std::binomial_distribution<int> next_gen(susceptibles,1-trans_prob);
		infected = next_gen(rd);
		total_infected += infected;
		susceptibles -= infected;
	}
	return total_infected;
}

int single_household(int n,std::vector<double> &params)
{

	/* Les paramètres sont dans l'ordre (cf Fraser et al. 2011) :

	- La proba d'échappement Q
	- La proba d'infection asymptomatique et infectieuse p_asx
	- La proba d'infection asymptomatique et non-infectieuse p_r
	- La proba d'échappement à une infection antérieure Q_prior
	- Hétérogénéité des infectivités k
	- Facteur de taille du domicile beta
	- Exposant de la taille du domicile alpha

	*/

	std::binomial_distribution<int> protection(n,params[2]);

	int prot = protection(rd);
	int susceptibles = n-prot;

	int prev_infected = reed_frost(n,susceptibles,params[3],params[5],params
		[6],params[4]);

	susceptibles -= prev_infected;

	int new_infected = reed_frost(n,susceptibles,params[0],params[5],params
		[6],params[4]);

	std::binomial_distribution<int> asympt(new_infected,params[1]);
	int asymptomatics = asympt(rd);
	int symptomatics = new_infected - asymptomatics;
	return symptomatics;
}

int main(int argc, char const *argv[])
{

	std::vector<double> test_params { 0.8,0,0,0.88,0.94,0.37,0.35 } ;
	std::vector<double> params=test_params;

	std::vector<int> test_numbers {244,1126,1342,1378,1017,680,434,
	  284,117,74,25,23,5,3,0,0,1 };

	int max_n = test_numbers.size();

	Eigen::MatrixXi table(max_n+1,max_n);

	for (int n = 0; n < max_n; ++n)
	{
		std::vector<int> symptomatics;
		int num_households = test_numbers[n];
		for (int i = 0; i < num_households; ++i)
		{
			int infected = single_household(n+1,params);
			symptomatics.push_back(infected);
		}

		for (int m = 0; m <= n+1; ++m)
		{
			int occurrences = std::count(symptomatics.begin(),
				symptomatics.end(),m);
			table(m,n) = occurrences;
		}
	}

	std::cout << table << std::endl;

	std::ofstream outfile("sim_matrix.txt");
	outfile << table;

	return 0;
}


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;
#define mod 10000000000000007

cpp_int pow(ll b, ll p){
    cpp_int res = 1, a = b;
    while(p){
        if(p & 1){
            res =  (res * a) % mod;
        }
        a = (a * a) % mod;
        a %= mod;
        p >>= 1;
    }
    return res;
}

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    int tc; cin >> tc;
    ll n, m, p, q;
    while(tc--){
        cin >> p >> q;
        n = (p*q)/2;
        m = p*q - n;
        cout << (pow(n, m-1)*pow(m, n-1)) % mod << "\n";
    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/Gridland Airports.cpp


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Properties.h"
#include "Utils/Geometry/AtomCollection.h"
#include "Utils/Geometry/ElementInfo.h"
#include <Eigen/Eigenvalues>

namespace Scine {
namespace Utils {
namespace Geometry {
namespace Properties {

std::vector<double> getMasses(const ElementTypeCollection& elements) {
  std::vector<double> masses;
  masses.reserve(elements.size());
  std::transform(elements.begin(), elements.end(), std::back_inserter(masses), ElementInfo::mass);
  return masses;
}

Position getCenterOfMass(const PositionCollection& positions, const std::vector<double>& masses) {
  Position P;
  P.setZero();
  double totalMass = 0;
  for (int i = 0; i < positions.rows(); ++i) {
    P += masses[i] * positions.row(i);
    totalMass += masses[i];
  }
  P /= totalMass;
  return P;
}

Position getCenterOfMass(const AtomCollection& structure) {
  auto masses = getMasses(structure.getElements());
  return getCenterOfMass(structure.getPositions(), masses);
}

Position getAveragePosition(const PositionCollection& positions) {
  return positions.colwise().sum() / positions.rows();
}

Eigen::Matrix3d calculateInertiaTensor(const PositionCollection& positions, const std::vector<double>& masses,
                                       const Position& centerOfMass) {
  double Ixx = 0, Iyy = 0, Izz = 0, Ixy = 0, Ixz = 0, Iyz = 0;
  for (int i = 0; i < positions.rows(); ++i) {
    double m = masses[i];
    auto dp = positions.row(i) - centerOfMass;
    double x = dp.x();
    double y = dp.y();
    double z = dp.z();
    Ixx += m * (y * y + z * z);
    Iyy += m * (x * x + z * z);
    Izz += m * (x * x + y * y);
    Ixy -= m * x * y;
    Ixz -= m * x * z;
    Iyz -= m * y * z;
  }
  Eigen::Matrix3d In;
  In << Ixx, Ixy, Ixz, Ixy, Iyy, Iyz, Ixz, Iyz, Izz;
  return In;
}

PrincipalMomentsOfInertia calculatePrincipalMoments(const PositionCollection& positions,
                                                    const std::vector<double>& masses, const Position& centerOfMass) {
  auto In = calculateInertiaTensor(positions, masses, centerOfMass);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
  es.compute(In);

  PrincipalMomentsOfInertia pmi;
  pmi.eigenvalues = es.eigenvalues();
  pmi.eigenvectors = es.eigenvectors();
  return pmi;
}

bool operator==(const PositionCollection& lhs, const PositionCollection& rhs) {
  return lhs.isApprox(rhs);
}

bool operator!=(const PositionCollection& lhs, const PositionCollection& rhs) {
  return !(lhs == rhs);
}

} /* namespace Properties */
} /* namespace Geometry */
} /* namespace Utils */
} /* namespace Scine */


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace boost::numeric::ublas;

int main()
{
	std::cout << "\nVector" << std::endl;
	// basic Vector Usage
	vector<double> v(3);
	for (unsigned i = 0; i < v.size(); ++ i)
	{
		v(i) = i;
	}
	std::cout << "vector \t\t" << v << std::endl;

	// unit_vector
	for (int i = 0; i < 3; ++ i)
	{
		unit_vector<double> v_unit(3, i);
		std::cout << "unit_vector \t" << v_unit << std::endl;
	}

	// zero_vector
	zero_vector<double> v_zero(3);
	std::cout << "zero_vector \t" << v_zero << std::endl;

	// scalr_vector
	scalar_vector<double> v_scalar(3,100);
	std::cout << "scalar_vector \t" << v_scalar << std::endl;

	std::cout << "\nMatrix" << std::endl;
	// basic matrix
	matrix<double> m(3, 3);
	for (unsigned i = 0; i < m.size1 (); ++ i)
	{
		for (unsigned j = 0; j < m.size2 (); ++ j)
		{
			m(i, j) = 3 * i + j;
		}
	}
	std::cout << "matrix \t\t" << m << std::endl;

	// identity matrix
	identity_matrix<double> m_identity(3);
    std::cout << "eye matrix\t" << m_identity << std::endl;

    // zero matrix
    zero_matrix<double> m_zero(3, 3);
    std::cout << "zero matrix\t" << m_zero << std::endl;

    // scalar matrix
    scalar_matrix<double> m_scalar(3, 3, 100);
    std::cout << "scalar matrix\t" << m_scalar << std::endl;

    // lower and upper triangular matrix
    triangular_matrix<double, lower> ml(3, 3);
    for (unsigned i = 0; i < ml.size1 (); ++ i)
        for (unsigned j = 0; j <= i; ++ j)
            ml(i, j) = 3 * i + j;
    std::cout << "lower triangular matrix\t" << ml << std::endl;

    triangular_matrix<double, upper> mu(3, 3);
    for (unsigned i = 0; i < mu.size1 (); ++ i)
        for (unsigned j = i; j < mu.size2 (); ++ j)
            mu(i, j) = 3 * i + j;
    std::cout << "upper triangular matrix\t" << mu << std::endl;

    // lower and upper symmetric matrix
    symmetric_matrix<double, lower> ml_symm(3, 3);
    for (unsigned i = 0; i < ml_symm.size1 (); ++ i)
        for (unsigned j = 0; j <= i; ++ j)
            ml_symm(i, j) = 3 * i + j;
    std::cout << "lower symmetric matrix\t" << ml_symm << std::endl;

    symmetric_matrix<double, upper> mu_symm(3, 3);
    for (unsigned i = 0; i < mu_symm.size1 (); ++ i)
        for (unsigned j = i; j < mu_symm.size2 (); ++ j)
            mu_symm(i, j) = 3 * i + j;
    std::cout << "upper symmetric matrix\t" << mu_symm << std::endl;

    // banded matrix
    banded_matrix<double> m_banded (3, 3, 1, 1);
    for (signed i = 0; i < signed (m_banded.size1 ()); ++ i)
        for (signed j = std::max (i - 1, 0); j < std::min (i + 2, signed (m_banded.size2 ())); ++ j)
            m_banded (i, j) = 3 * i + j;
    std::cout << "banded matrix\t" << m_banded << std::endl;
	return 0;
}


// This file implements the Levenberg-Marquardt method
// for pose projection matrix refinement
#include "marker.hpp"
#include <Eigen/Dense>
#include <cmath>
#include <limits>
#include <iostream>

#define ERROR_EPS       0.001
#define MAX_ITER        10

/////////////////////////////////////////////////////////////////
// objective: min(sum(p - K M q)^2)
// where
// K: 3x3 matrix
// M: 3x4 matrix
// q: 4x1 vector (obj points)
// p: 3x1 vector (img points)
// variable: M
/////////////////////////////////////////////////////////////////
// let's say an update of delta on M
// brings us closer to local minimum
// S = sum(p - K M q - J delta)^2
// where
// J: gradient of function (K M q) with respect to M
// take derivative of S with respect to delta and set to zero:
// (J^T J) delta = J^T (p - K M q)
// which is replaced to: (by Levenberg)
// (J^T J + lambda I) delta = J^T (p - K M q)
// where
// lambda: a damping factor adjusted each iteration
// finally, to make solution scale invariant:
// (J^T J + lambda diag(J^T J)) delta = J^T (p - K M q)
/////////////////////////////////////////////////////////////////

// refer to: https://github.com/opencv/opencv/blob/master/modules/calib3d/src/compat_ptsetreg.cpp#L121
// refer to book "Augmented Reality: Principles and Practice"
// refer to: https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
// refine pose matrix
void Marker::refinePoseM(
    const glm::mat3& cameraInvK,
    const std::vector<glm::vec2>& objPoints,
    const std::vector<glm::vec2>& imgPoints
)
{
    if(objPoints.size() < 4 || imgPoints.size() < 4)
        return;
    // camera matrix in eigen
    Eigen::Matrix3f Kinv;
    Kinv << cameraInvK[0][0], cameraInvK[1][0], cameraInvK[2][0],
            cameraInvK[0][1], cameraInvK[1][1], cameraInvK[2][1],
            cameraInvK[0][2], cameraInvK[1][2], cameraInvK[2][2];
    // pose matrix
    Eigen::Matrix<float, 3, 4> M;
    M << _poseM[0][0], _poseM[1][0], _poseM[2][0], _poseM[3][0],
         _poseM[0][1], _poseM[1][1], _poseM[2][1], _poseM[3][1],
         _poseM[0][2], _poseM[1][2], _poseM[2][2], _poseM[3][2];
    Eigen::Matrix<float, 3, 4> Mshaped = Eigen::MatrixXf::Constant(3, 4, 0.0f);
    // point matrix
    Eigen::Matrix4f objMatrix;
    objMatrix << objPoints[0].x, objPoints[1].x, objPoints[2].x, objPoints[3].x,
                 objPoints[0].y, objPoints[1].y, objPoints[2].y, objPoints[3].y,
                 0.0f,           0.0f,           0.0f,           0.0f,
                 1.0f,           1.0f,           1.0f,           1.0f;
    Eigen::Matrix<float, 3, 4> imgMatrix;
    imgMatrix << imgPoints[0].x, imgPoints[1].x, imgPoints[2].x, imgPoints[3].x,
                 imgPoints[0].y, imgPoints[1].y, imgPoints[2].y, imgPoints[3].y,
                 0.0f,           0.0f,           0.0f,           0.0f;
    // here we are minimizing sum(K^-1 p - M q)^2
    // if K M q = p, then M q = K^-1 p
    imgMatrix = Kinv * imgMatrix;
    // Jacobian
    // mxn matrix
    // m: number of points
    // n: number of parameters
    Eigen::Matrix<float, 12, 12> J = Eigen::MatrixXf::Constant(12, 12, 0.0f);
    // initialize Jacobian
    for(int i = 0; i < 4; i++)
    {
        // J(i+0, Eigen::seqN(0, 4)) = objMatrix.col(i);
        // J(i+4, Eigen::seqN(4, 4)) = objMatrix.col(i);
        // J(i+8, Eigen::seqN(8, 4)) = objMatrix.col(i);
        J(i*3+0, Eigen::seqN(0, 4)) = objMatrix.col(i);
        J(i*3+1, Eigen::seqN(4, 4)) = objMatrix.col(i);
        J(i*3+2, Eigen::seqN(8, 4)) = objMatrix.col(i);
    }
    Eigen::Matrix<float, 12, 12> JT = J.transpose();
    Eigen::Matrix<float, 12, 12> JTJ = JT * J;
    Eigen::Matrix<float, 12, 12> JTJdiag = Eigen::MatrixXf::Constant(12, 12, 0.0f);
    JTJdiag.diagonal() += JTJ.diagonal();
    // delta matrix
    Eigen::Vector<float, 12> delta = Eigen::VectorXf::Constant(12, 0.0f);
    // left and right hand side of equation
    Eigen::Matrix<float, 12, 12> eqLeft;
    Eigen::Vector<float, 12> eqRight;
    // damping factor (same as OpenCV)
    // lambda = 1.0 + exp(lambdaLog10 * log10val)
    const float log10val = std::log(10.0f);
    int lambdaLog10 = -3;
    // start iteration
    int iter = 0;
    float err = std::numeric_limits<float>::max();
    float prevErr = std::numeric_limits<float>::max();
    while(iter++ < MAX_ITER)
    {
        float lambda = 1.0f + std::exp(lambdaLog10 * log10val);
        // (J^T J + lambda diag(J^T J)) delta = J^T (p - K M q)
        eqLeft = JTJ + lambda * JTJdiag;
        Mshaped = imgMatrix - M * objMatrix;
        Eigen::Map<Eigen::VectorXf> flatten(Mshaped.data(), Mshaped.size());
        eqRight = JT * flatten;
        delta = eqLeft.colPivHouseholderQr().solve(eqRight);
        // update M
        M.row(0) += delta(Eigen::seqN(0, 4));
        M.row(1) += delta(Eigen::seqN(4, 4));
        M.row(2) += delta(Eigen::seqN(8, 4));
        // compute error
        prevErr = err;
        err = 0.0f;
        Eigen::Matrix<float, 3, 4> residual = imgMatrix - M * objMatrix;
        Eigen::Map<Eigen::VectorXf> residualVec(residual.data(), residual.size());
        for(int i = 0; i < 12; i++)
            err += residualVec[i] * residualVec[i];
        // std::cout << iter << "," << lambdaLog10 << "|" << err << std::endl;
        if(err <= ERROR_EPS) break;
        if(err > prevErr)
        {
            lambdaLog10 = std::min(lambdaLog10+1, 16);
            err = std::numeric_limits<float>::max();
            continue;
        }
        // update lambda
        lambdaLog10 = std::max(lambdaLog10-1, -16);
    }
    _err_LM = err;
    // std::cout << "LM err: " << err << "(" << iter << ")" << std::endl;
    // record new M and interpolate
    if(_poseMRefined[3][2] == 0.0f)
    {
        _poseMRefined = glm::mat4x3(
            glm::vec3(M.col(0)[0], M.col(0)[1], M.col(0)[2]),
            glm::vec3(M.col(1)[0], M.col(1)[1], M.col(1)[2]),
            glm::vec3(M.col(2)[0], M.col(2)[1], M.col(2)[2]),
            glm::vec3(M.col(3)[0], M.col(3)[1], M.col(3)[2])
        );
    }
    else
    {
        _poseMRefined = _poseM_interpolate * _poseMRefined +
            (1.0f - _poseM_interpolate) * glm::mat4x3(
            glm::vec3(M.col(0)[0], M.col(0)[1], M.col(0)[2]),
            glm::vec3(M.col(1)[0], M.col(1)[1], M.col(1)[2]),
            glm::vec3(M.col(2)[0], M.col(2)[1], M.col(2)[2]),
            glm::vec3(M.col(3)[0], M.col(3)[1], M.col(3)[2])
        );
    }
}


/*************************************************************************
	> File Name: main.cpp
	> Author: TAI Lei
	> Mail: ltai@ust.hk
	> Created Time: Thu Mar  7 19:39:14 2019
 ************************************************************************/

#include <iostream>
#include <random>
#include <cmath>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#define SIM_TIME 50.0
#define DT 0.1
#define PI 3.141592653

// x_{t+1} = F@x_{t}+B@u_t
Eigen::Vector4f motion_model(Eigen::Vector4f x, Eigen::Vector2f u){
  Eigen::Matrix4f F_;
  F_<<1.0,   0,   0,   0,
        0, 1.0,   0,   0,
        0,   0, 1.0,   0,
        0,   0,   0, 1.0;

  Eigen::Matrix<float, 4, 2> B_;
  B_<< DT * std::cos(x(2,0)),  0,
       DT * std::sin(x(2,0)),  0,
                        0.0,  DT,
                        1.0,  0.0;

  return F_ * x + B_ * u;
};

Eigen::Matrix4f jacobF(Eigen::Vector4f x, Eigen::Vector2f u){
  Eigen::Matrix4f jF_ = Eigen::Matrix4f::Identity();
  float yaw = x(2);
  float v = u(0);
  jF_(0,2) = -DT * v * std::sin(yaw);
  jF_(0,3) = DT * std::cos(yaw);
  jF_(1,2) = DT * v * std::cos(yaw);
  jF_(1,3) = DT * std::sin(yaw);
  return jF_;
};

//observation mode H
Eigen::Vector2f observation_model(Eigen::Vector4f x){
  Eigen::Matrix<float, 2, 4> H_;
  H_<< 1, 0, 0, 0,
       0, 1, 0, 0;
  return H_ * x;
};

Eigen::Matrix<float, 2, 4> jacobH(){
  Eigen::Matrix<float, 2, 4> jH_;
  jH_<< 1, 0, 0, 0,
        0, 1, 0, 0;
  return jH_;
};

void ekf_estimation(Eigen::Vector4f& xEst, Eigen::Matrix4f& PEst,
  Eigen::Vector2f z, Eigen::Vector2f u,
  Eigen::Matrix4f Q, Eigen::Matrix2f R){
    Eigen::Vector4f xPred = motion_model(xEst, u);
    Eigen::Matrix4f jF = jacobF(xPred, u);
    Eigen::Matrix4f PPred = jF * PEst * jF.transpose() + Q;

    Eigen::Matrix<float, 2, 4> jH = jacobH();
    Eigen::Vector2f zPred = observation_model(xPred);
    Eigen::Vector2f y = z - zPred;
    Eigen::Matrix2f S = jH * PPred * jH.transpose() + R;
    Eigen::Matrix<float, 4, 2> K = PPred * jH.transpose() * S.inverse();
    xEst = xPred + K * y;
    PEst = (Eigen::Matrix4f::Identity() - K * jH) * PPred;
};

cv::Point2i cv_offset(
    Eigen::Vector2f e_p, int image_width=2000, int image_height=2000){
  cv::Point2i output;
  output.x = int(e_p(0) * 100) + image_width/2;
  output.y = image_height - int(e_p(1) * 100) - image_height/3;
  return output;
};

void ellipse_drawing(
  cv::Mat bg_img, Eigen::Matrix2f pest, Eigen::Vector2f center,
  cv::Scalar ellipse_color=cv::Scalar(0, 0, 255)
){
  Eigen::EigenSolver<Eigen::Matrix2f> ces(pest);
  Eigen::Matrix2f e_value = ces.pseudoEigenvalueMatrix();
  Eigen::Matrix2f e_vector = ces.pseudoEigenvectors();

  double angle = std::atan2(e_vector(0, 1), e_vector(0, 0));
  cv::ellipse(
    bg_img,
    cv_offset(center, bg_img.cols, bg_img.rows),
    cv::Size(e_value(0,0)*1000, e_value(1,1)*1000),
    angle / PI * 180,
    0,
    360,
    ellipse_color,
    2,
    4);
};

int main(){
  float time=0.0;

  // control input
  Eigen::Vector2f u;
  u<<1.0, 0.1;

  // nosie control input
  Eigen::Vector2f ud;

  // observation z
  Eigen::Vector2f z;

  // dead reckoning
  Eigen::Vector4f xDR;
  xDR<<0.0,0.0,0.0,0.0;

  // ground truth reading
  Eigen::Vector4f xTrue;
  xTrue<<0.0,0.0,0.0,0.0;

  // Estimation
  Eigen::Vector4f xEst;
  xEst<<0.0,0.0,0.0,0.0;

  std::vector<Eigen::Vector4f> hxDR;
  std::vector<Eigen::Vector4f> hxTrue;
  std::vector<Eigen::Vector4f> hxEst;
  std::vector<Eigen::Vector2f> hz;

  Eigen::Matrix4f PEst = Eigen::Matrix4f::Identity();

  // Motional model covariance
  Eigen::Matrix4f Q = Eigen::Matrix4f::Identity();
  Q(0,0)=0.1 * 0.1;
  Q(1,1)=0.1 * 0.1;
  Q(2,2)=(1.0/180 * M_PI) * (1.0/180 * M_PI);
  Q(3,3)=0.1 * 0.1;

  // Observation model covariance
  Eigen::Matrix2f  R = Eigen::Matrix2f::Identity();
  R(0,0)=1.0;
  R(1,1)=1.0;

  // Motion model simulation error
  Eigen::Matrix2f Qsim = Eigen::Matrix2f::Identity();
  Qsim(0,0)=1.0;
  Qsim(1,1)=(30.0/180 * M_PI) * (30.0/180 * M_PI);

  // Observation model simulation error
  Eigen::Matrix2f Rsim = Eigen::Matrix2f::Identity();
  Rsim(0,0)=0.5 * 0.5;
  Rsim(1,1)=0.5 * 0.5;

  std::random_device rd{};
  std::mt19937 gen{rd()};
  std::normal_distribution<> gaussian_d{0,1};

  //for visualization
  cv::namedWindow("ekf", cv::WINDOW_NORMAL);
  int count = 0;

  while(time <= SIM_TIME){
    time += DT;

    ud(0) = u(0) + gaussian_d(gen) * Qsim(0,0);
    ud(1) = u(1) + gaussian_d(gen) * Qsim(1,1);

    xTrue = motion_model(xTrue, u);
    xDR = motion_model(xDR, ud);

    z(0) = xTrue(0) + gaussian_d(gen) * Rsim(0,0);
    z(1) = xTrue(1) + gaussian_d(gen) * Rsim(1,1);

    ekf_estimation(xEst, PEst, z, ud, Q, R);

    hxDR.push_back(xDR);
    hxTrue.push_back(xTrue);
    hxEst.push_back(xEst);
    hz.push_back(z);

    //visualization
    cv::Mat bg(3500,3500, CV_8UC3, cv::Scalar(255,255,255));
    for(unsigned int j=0; j<hxDR.size(); j++){

      // green groundtruth
      cv::circle(bg, cv_offset(hxTrue[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0,255,0), -1);

      // blue estimation
      cv::circle(bg, cv_offset(hxEst[j].head(2), bg.cols, bg.rows),
                 10, cv::Scalar(255,0,0), 5);

      // black dead reckoning
      cv::circle(bg, cv_offset(hxDR[j].head(2), bg.cols, bg.rows),
                 7, cv::Scalar(0, 0, 0), -1);
    }

    // red observation
    for(unsigned int i=0; i<hz.size(); i++){
      cv::circle(bg, cv_offset(hz[i], bg.cols, bg.rows),
               7, cv::Scalar(0, 0, 255), -1);
    }

    ellipse_drawing(bg, PEst.block(0,0,2,2), xEst.head(2));

    cv::imshow("ekf", bg);
    cv::waitKey(5);

    // std::string int_count = std::to_string(count);
    // cv::imwrite("./pngs/"+std::string(5-int_count.length(), '0').append(int_count)+".png", bg);

    count++;
  }
}


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

using Eigen::MatrixXd;

typedef Eigen::Matrix<float, 3, 3> MyMatrix33f;
typedef Eigen::Matrix<float, 3, 1> MyVector3f;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MyMatrix;

int main() {
  //Definitions
  MyMatrix33f a;
  MyVector3f v;
  MyMatrix mym(10, 15);
  MatrixXd m(2, 2);

  // Initializations
  a = MyMatrix33f::Zero();
  a = MyMatrix33f::Identity();
  v = MyVector3f::Random();

  // Assignments
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  a << 1,2,3,
       4,5,6,
       7,8,9;

  // Output
  std::cout << m << std::endl;

  // Mapping (shallow copy)
  int data[] = {1, 2, 3, 4};
  Eigen::Map<Eigen::RowVectorXi> r(data, 4);
  std::vector<float> data2 = {1,2,3,4,5,6,8,9};
  Eigen::Map<MyMatrix33f> n(data2.data());

  // Math
  // what are the types?
  auto x = Eigen::Matrix2d::Random();
  auto y = Eigen::Matrix2d::Random();
  auto res = x + y;
  auto res2 = x.array() * y.array(); // element wise multiplication
  // x = y + x;
  auto res3 = x.array() / y.array();
  //x += y;
  //r = x * y;  // matrix multi
  auto res4 = x.array() * 3;

}


/* Copyright (C) 2016-2018 Alibaba Group Holding Limited

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

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

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "orthogonal_initializer.h"

#include <string>
#include <cstring>
#include <cmath>
#include <memory>
#include <Eigen/SVD>
#include <Eigen/Dense>

#include "ps-plus/common/types.h"
#include "random/random.h"
#include "random/random_ops.h"
#include "normal_initializer.h"

using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;

namespace ps {
namespace initializer {

OrthogonalInitializer::OrthogonalInitializer(
    int64_t dim, int seed, float gain)
  : dim_(dim)
  , seed_(seed)
  , gain_(gain) {
  normal_initializer_.reset(new NormalInitializer(seed, 0.0, 1.0));
}

bool OrthogonalInitializer::Accept(DataType type) {
  if (type == DataType::kFloat ||
      type == DataType::kDouble) {
    return true;
  }

  return false;
}

void OrthogonalInitializer::Init(void* data,
                                 DataType type,
                                 size_t size) {
  if (size % dim_ != 0) {
    printf("error size\n");
    abort();
  }

  normal_initializer_->Init(data, type, size);
  int64_t row = size / dim_;
  int64_t col = dim_;
  if (type == DataType::kFloat) {
    Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> m((float*)data, row, col);
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinV | Eigen::ComputeThinU);
    Eigen::MatrixXf u = svd.matrixU() * gain_;
    Eigen::MatrixXf v = svd.matrixV() * gain_;
    if ((row == u.rows() && col == u.cols()) ||
        (row == u.cols() && col == u.rows())) {
      Eigen::MatrixXf trans_u = u.transpose();
      memcpy(data, trans_u.data(), size * sizeof(float));
    } else if ((row == v.rows() && col == v.cols()) ||
               (row == v.cols() && col == v.rows())) {
      memcpy(data, v.data(), size * sizeof(float));
    } else {
      printf("svd result shape[%d,%d|%d,%d] not match\n", u.rows(), u.cols(), v.rows(), v.cols());
      abort();
    }
  } else {
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> m((double*)data, row, col);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinV | Eigen::ComputeThinU);
    Eigen::MatrixXd u = svd.matrixU() * gain_;
    Eigen::MatrixXd v = svd.matrixV() * gain_;
    if ((row == u.rows() && col == u.cols()) ||
        (row == u.cols() && col == u.rows())) {
      Eigen::MatrixXd trans_u = u.transpose();
      memcpy(data, trans_u.data(), size * sizeof(double));
    } else if ((row == v.rows() && col == v.cols()) ||
               (row == v.cols() && col == v.rows())) {
      memcpy(data, v.data(), size * sizeof(double));
    } else {
      printf("svd result shape[%d,%d|%d,%d] not match\n", u.rows(), u.cols(), v.rows(), v.cols());
      abort();
    }
  }
}

Initializer* OrthogonalInitializer::Clone() {
  return new OrthogonalInitializer(
      dim_, seed_, gain_);
}

} //namespace initializer
} //ps


#include <Eigen/Dense>
#include "partitionlist.hpp"

using namespace Eigen;

template <class uint>
class AssemblyOp : public EigenBase< AssemblyOp<uint> > {
	private:
		const MatrixXd& A;
		const MatrixXd& B;
		const PartitionList<uint>& states;

	public:
		// eigen boilerplate
		typedef double Scalar;
		typedef double RealScalar;
		typedef int StorageIndex;
		enum {
			ColsAtCompileTime = Eigen::Dynamic,
			MaxColsAtCompileTime = Eigen::Dynamic,
			IsRowMajor = true
		};
		Index rows() const { return states.size(); }
		Index cols() const { return states.size(); }
		template <typename Rhs>
		Product< AssemblyOp<uint>, Rhs, AliasFreeProduct > operator*(const Eigen::MatrixBase<Rhs>& x) const {
			return Product< AssemblyOp<uint>, Rhs, AliasFreeProduct >(*this, x.derived());
		}

		AssemblyOp() {}
		AssemblyOp(const PartitionList<uint>& states_, const MatrixXd& A_, const MatrixXd& B_) : states(states_), A(A_), B(B_) {}
		//VectorXd operator*(const VectorXd& x) const { return this->apply(x); }

		VectorXd apply(const VectorXd& x) const {
			VectorXd b = VectorXd::Zero(x.size());

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;

			for (const auto& state : states) {
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						nbr = state;
						nbr.decrement(i);
						nbr.increment(j);
						nbr.increment(i-j);
						nbrIndex = states.indexOf(nbr);
						b(index) += B(j-1,i-j-1) * (double) n * (x(index) - x(nbrIndex));

						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							b(index) += A(i-1,j-1) * (double) (n * m) * (x(index) - x(nbrIndex));
						}
						else {
							b(index) += A(i-1,j-1) * (double) (n * m) * x(index);
						}
					}

					if (n > 1) {
						if (2*i <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(i);
							nbr.increment(2*i);
							nbrIndex = states.indexOf(nbr);
							b(index) += A(i-1,i-1) * (double) (n*(n-1)) * (x(index) - x(nbrIndex));
						}
						else {
							b(index) += A(i-1,i-1) * (double) (n*(n-1)) * x(index);
						}
					}

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							b(index) += A(i-1,j-1) * (double) (n * m) * (x(index) - x(nbrIndex));
						}
						else {
							b(index) += A(i-1,j-1) * (double) (n * m) * x(index);
						}
					}
				}

				++index;
			}
			return b;
		}

		VectorXd adjointApply(const VectorXd& x) const {
			VectorXd b = VectorXd::Zero(x.size());

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;
			double rate;

			for (const auto& state : states) {
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						nbr = state;
						nbr.decrement(i);
						nbr.increment(j);
						nbr.increment(i-j);
						nbrIndex = states.indexOf(nbr);
						rate = B(j-1,i-j-1) * (double) n * x(index);
						b(index) += rate;
						b(nbrIndex) -= rate;

						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							rate = A(i-1,j-1) * (double) (n * m) * x(index);
							b(index) += rate;
							b(nbrIndex) -= rate;
						}
						else {
							rate = A(i-1,j-1) * (double) (n * m) * x(index);
							b(index) += rate;
						}
					}

					if (n > 1) {
						if (2*i <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(i);
							nbr.increment(2*i);
							nbrIndex = states.indexOf(nbr);
							rate = A(i-1,i-1) * (double) (n*(n-1)) * x(index);
							b(index) += rate;
							b(nbrIndex) -= rate;
						}
						else {
							rate = A(i-1,i-1) * (double) (n*(n-1)) * x(index);
							b(index) += rate;
						}
					}

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							rate = A(i-1,j-1) * (double) (n * m) * x(index);
							b(index) += rate;
							b(nbrIndex) -= rate;
						}
						else {
							rate = A(i-1,j-1) * (double) (n * m) * x(index);
							b(index) += rate;
						}
					}
				}

				++index;
			}
			return b;
		}
/*
		Matrix<double,Dynamic,4> split_apply(const VectorXd& x) const {
			Matrix<double,Dynamic,4> b(x.size(),4);
			b.setConstant(0.0);

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;

			double rate = 0.0;
			for (const auto& state : states) {
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						nbr = state;
						nbr.decrement(i);
						nbr.increment(j);
						nbr.increment(i-j);
						nbrIndex = states.indexOf(nbr);

						rate = B(j-1,i-j-1) * (double) n;
						b(index,0) -= rate * x(nbrIndex);
						b(index,1) += rate * x(index);

						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						rate = A(i-1,j-1) * (double) (n * m);
						b(index,2) += rate * x(index);
						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							b(index,3) -= rate * x(nbrIndex);
						}
					}

					if (n > 1) {
						rate = A(i-1,i-1) * (double) (n*(n-1));
						b(index,2) += rate * x(index);
						if (2*i <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(i);
							nbr.increment(2*i);
							nbrIndex = states.indexOf(nbr);
							b(index,3) -= rate * x(nbrIndex);
						}
					}

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						rate = A(i-1,j-1) * (double) (n * m);
						b(index,2) += rate * x(index);
						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							b(index,3) -= rate * x(nbrIndex);
						}
					}
				}

				++index;
			}
			return b;
		}*/

		VectorXd solve_diagonal(const VectorXd& b) const {
			VectorXd x = b;

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;
			double currentRate, totalRate = 0.0;

			for (const auto& state : states) {
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						totalRate += B(j-1,i-j-1) * (double) n;

						// aggregate
						m = state.get(j);
						totalRate += A(i-1,j-1) * (double) (n * m);
					}

					totalRate += A(i-1,i-1) * (double) (n*(n-1));

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						totalRate += A(i-1,j-1) * (double) (n * m);
					}
				}
				x(index) /= totalRate;
				++index;
			}
			return x;
		}

		VectorXd solve_lower(const VectorXd& b, double omega) const {
			VectorXd x = b;

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;
			double currentRate, totalRate;

			for (const auto& state : states) {
				totalRate = 0.0;
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						nbr = state;
						nbr.decrement(i);
						nbr.increment(j);
						nbr.increment(i-j);
						nbrIndex = states.indexOf(nbr);
						currentRate = B(j-1,i-j-1) * (double) n;
						totalRate += currentRate;
						x(index) += currentRate * x(nbrIndex);

						// aggregate
						m = state.get(j);
						totalRate += A(i-1,j-1) * (double) (n * m);
					}

					totalRate += A(i-1,i-1) * (double) (n*(n-1));

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						totalRate += A(i-1,j-1) * (double) (n * m);
					}
				}
				x(index) /= totalRate/omega;
				++index;
			}
			return x;
		}

		VectorXd solve_upper(const VectorXd& b, double omega) const {
			VectorXd x = b;

			uint index = 0, nbrIndex, n, m, len = (*(states.cbegin())).getLength();
			Partition<uint> nbr;
			double currentRate, totalRate;

			index = states.size();
			auto stateit = states.end();
			while (index-->0) {
				--stateit;

				auto state = *stateit;
				totalRate = 0.0;
				for (size_t i = 1; i <= len; i++) { // size to react with
					n = state.get(i);
					if (n == 0) // make sure we can react
						continue;

					for (size_t j = 1; j <= i-1; j++) {
						// fragment
						totalRate += B(j-1,i-j-1) * (double) n;

						// aggregate
						m = state.get(j);
						currentRate = A(i-1,j-1) * (double) (n * m);
						totalRate += currentRate;

						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							x(index) += currentRate * x(nbrIndex);
						}
					}

					if (n > 1) {
						currentRate = A(i-1,i-1) * (double) (n*(n-1));
						totalRate += currentRate;
						if (2*i <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(i);
							nbr.increment(2*i);
							nbrIndex = states.indexOf(nbr);
							x(index) += currentRate * x(nbrIndex);
						}
					}

					for (size_t j = i+1; j <= len; j++) {
						// aggregate
						m = state.get(j);
						if (m == 0)
							continue;

						currentRate = A(i-1,j-1) * (double) (n * m);
						totalRate += currentRate;
						if (i+j <= len) {
							nbr = state;
							nbr.decrement(i);
							nbr.decrement(j);
							nbr.increment(i+j);
							nbrIndex = states.indexOf(nbr);
							x(index) += currentRate * x(nbrIndex);
						}
					}
				}
				x(index) /= totalRate/omega;
			}
			return x;
		}
};

namespace Eigen {
namespace internal {
  template<typename uint>
  struct traits< AssemblyOp<uint> > :  public Eigen::internal::traits<Eigen::SparseMatrix<double> > {};
}
}

// Implementation of MatrixReplacement * Eigen::DenseVector though a specialization of internal::generic_product_impl:
namespace Eigen {
namespace internal {
  template<typename Rhs, typename uint>
  struct generic_product_impl<AssemblyOp<uint>, Rhs, SparseShape, DenseShape, GemvProduct> // GEMV stands for matrix-vector
  : generic_product_impl_base<AssemblyOp<uint>,Rhs,generic_product_impl<AssemblyOp<uint>,Rhs> >
  {
    typedef typename Product<AssemblyOp<uint>,Rhs>::Scalar Scalar;
    template<typename Dest>
    static void scaleAndAddTo(Dest& dst, const AssemblyOp<uint>& lhs, const Rhs& rhs, const Scalar& alpha)
    {
      // This method should implement "dst += alpha * lhs * rhs" inplace,
      // however, for iterative solvers, alpha is always equal to 1, so let's not bother about it.
      assert(alpha==Scalar(1) && "scaling is not implemented");
      // Here we could simply call dst.noalias() += lhs.my_matrix() * rhs,
      // but let's do something fancier (and less efficient):
      dst.noalias() += lhs.apply(rhs.matrix());
    }
  };
}
}


#include <cmath>
#include <vector>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

using std::vector;

double interest(double rate, int years)
{
    return pow(1 + rate, years);
}

double compound(double rate, int compound_amount, int years)
{
    double acutal_rate = rate / compound_amount;
    double result = pow(1 + acutal_rate, years * compound_amount);
    return result;
}

double continous_compound(double rate, int years)
{
    double rT = rate * years;
    return pow(M_E, rT);
}

double annuity(vector<double>payments_per_year)
{
    double increase = 0;
    for (auto&& amount : payments_per_year) {
        increase += amount;
    }
    return increase;
}

BOOST_PYTHON_MODULE(finance_formulas)
{
    using namespace boost::python;
    class_<vector<double> >("double_vector")
                .def(vector_indexing_suite<vector<double> >() );
    def("interest", interest);
    def("compound", compound);
    def("continous_compound", continous_compound);
    def("annuity", annuity);
}


// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/smartpointer.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/base/numbers.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

// STL
#include <array>
#include <fstream>
#include <iostream>
#include <cmath>

namespace Helmholtz
{
  using namespace dealii;

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class Solution : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;

    virtual Tensor<1, dim>
    gradient(const Point<dim> & p,
             const unsigned int component = 0) const override;
  };

  template <int dim>
  double Solution<dim>::value(const Point<dim> &p, const unsigned int) const
  {
    double return_value = 1;

    for (unsigned int i = 0; i<dim; ++i)
        	return_value *= cos(2*numbers::PI*p(i));

    return return_value;
  }

  template <>
  Tensor<1, 1> Solution<1>::gradient(const Point<1> &p,
                                         const unsigned int) const
  {
    Tensor<1, 1> return_value;

	return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0));

    return return_value;
  }

  template <>
  Tensor<1, 2> Solution<2>::gradient(const Point<2> &p,
                                         const unsigned int) const
  {
    Tensor<1, 2> return_value;

	return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1));
	return_value[1] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * sin(2*numbers::PI*p(1));

    return return_value;
  }

  template <>
    Tensor<1, 3> Solution<3>::gradient(const Point<3> &p,
                                           const unsigned int) const
    {
      Tensor<1, 3> return_value;

      return_value[0] = - 2 * numbers::PI * sin(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1)) * cos(2*numbers::PI*p(2));
      return_value[1] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * sin(2*numbers::PI*p(1)) * cos(2*numbers::PI*p(2));
      return_value[2] = - 2 * numbers::PI * cos(2*numbers::PI*p(0)) * cos(2*numbers::PI*p(1)) * sin(2*numbers::PI*p(2));

      return return_value;
    }

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override;
  };

  template <int dim>
  double RightHandSide<dim>::value(const Point<dim> &p,
                                   const unsigned int) const
  {
    double return_value = (1 + 4 * dim * std::pow(numbers::PI,2));

    for (unsigned int i = 0; i<dim; ++i)
    	return_value *= cos(2*numbers::PI*p(i));

    return return_value;
  }

  ///////////////////////////////////////////////////////
  ///////////////////////////////////////////////////////

  template <int dim>
  class HelmholtzSolver
  {
  public:
    HelmholtzSolver(const FiniteElement<dim> &fe);

    ~HelmholtzSolver();

    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void process_solution(const unsigned int cycle);
    void output_vtk(const unsigned int cycle);

    Triangulation<dim> triangulation;
    DoFHandler<dim>    dof_handler;

    SmartPointer<const FiniteElement<dim>> fe;

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> system_rhs;

    ConvergenceTable convergence_table;
  };

  template <int dim>
  HelmholtzSolver<dim>::HelmholtzSolver(const FiniteElement<dim> &fe)
    : dof_handler(triangulation)
    , fe(&fe)
  {}

  template <int dim>
  HelmholtzSolver<dim>::~HelmholtzSolver()
  {
    dof_handler.clear();
  }

  template <int dim>
  void HelmholtzSolver<dim>::setup_system()
  {
    dof_handler.distribute_dofs(*fe);
    DoFRenumbering::Cuthill_McKee(dof_handler);

    constraints.clear();
    DoFTools::make_hanging_node_constraints(dof_handler,
                                            constraints);

    Solution<dim>	exact_solution;

    VectorTools::interpolate_boundary_values(dof_handler,
                                               0,
                                               exact_solution,
                                               constraints);

    constraints.close();

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    constraints.condense(dsp);
    sparsity_pattern.copy_from(dsp);

    system_matrix.reinit(sparsity_pattern);

    solution.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());
  }

  template <int dim>
  void HelmholtzSolver<dim>::assemble_system()
  {
    QGauss<dim>     quadrature_formula(fe->degree + 1);

    const unsigned int n_q_points      = quadrature_formula.size();

    const unsigned int dofs_per_cell = fe->dofs_per_cell;

    FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
    Vector<double>     cell_rhs(dofs_per_cell);

    std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);

    FEValues<dim> fe_values(*fe,
                            quadrature_formula,
                            update_values | update_gradients |
                              update_quadrature_points | update_JxW_values);

    RightHandSide<dim>  right_hand_side;
    std::vector<double> rhs_values(n_q_points);

    for (const auto &cell : dof_handler.active_cell_iterators())
      {
        cell_matrix = 0.;
        cell_rhs    = 0.;

        fe_values.reinit(cell);

        right_hand_side.value_list(fe_values.get_quadrature_points(),
                                   rhs_values);

        for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
          for (unsigned int i = 0; i < dofs_per_cell; ++i)
            {
              for (unsigned int j = 0; j < dofs_per_cell; ++j)
                cell_matrix(i, j) +=
                  ((fe_values.shape_grad(i, q_point) *     // grad phi_i(x_q)
                      fe_values.shape_grad(j, q_point)     // grad phi_j(x_q)
                    +                                      //
                    fe_values.shape_value(i, q_point) *    // phi_i(x_q)
                      fe_values.shape_value(j, q_point)) * // phi_j(x_q)
                   fe_values.JxW(q_point));                // dx

              cell_rhs(i) += (fe_values.shape_value(i, q_point) * // phi_i(x_q)
                              rhs_values[q_point] *               // f(x_q)
                              fe_values.JxW(q_point));            // dx
            }

        cell->get_dof_indices(local_dof_indices);
        constraints.distribute_local_to_global(
                cell_matrix, cell_rhs, local_dof_indices, system_matrix, system_rhs);
      }
  }

  template <int dim>
  void HelmholtzSolver<dim>::solve()
  {
    SolverControl solver_control(1000, 1e-12);
    SolverCG<>    cg(solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve(system_matrix, solution, system_rhs, preconditioner);

    std::cout << "   " << solver_control.last_step()
                << " CG iterations needed to obtain convergence." << std::endl;

    constraints.distribute(solution);
  }

  template <int dim>
  void HelmholtzSolver<dim>::process_solution(const unsigned int cycle)
  {
	Solution<dim> exact_solution;

    Vector<float> difference_per_cell(triangulation.n_active_cells());
    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      exact_solution,
                                      difference_per_cell,
                                      QGauss<dim>(fe->degree + 1),
                                      VectorTools::L2_norm);
    const double L2_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::L2_norm);

    VectorTools::integrate_difference(dof_handler,
                                      solution,
                                      Solution<dim>(),
                                      difference_per_cell,
                                      QGauss<dim>(fe->degree + 1),
                                      VectorTools::H1_seminorm);
    const double H1_error =
      VectorTools::compute_global_error(triangulation,
                                        difference_per_cell,
                                        VectorTools::H1_seminorm);

    const unsigned int n_active_cells = triangulation.n_active_cells();
    const unsigned int n_dofs         = dof_handler.n_dofs();

    std::cout << "Cycle " << cycle << ':' << std::endl
              << "   Number of active cells:       " << n_active_cells
              << std::endl
              << "   Number of degrees of freedom: " << n_dofs << std::endl;

    convergence_table.add_value("cycle", cycle);
    convergence_table.add_value("cells", n_active_cells);
    convergence_table.add_value("dofs", n_dofs);
    convergence_table.add_value("L2", L2_error);
    convergence_table.add_value("H1", H1_error);
  }

  template <int dim>
  void HelmholtzSolver<dim>::output_vtk(const unsigned int cycle)
  {
	std::string vtk_filename
		= "solution-" + Utilities::int_to_string(dim,1) + "D";

	switch (fe->degree)
	  {
		case 1:
		  vtk_filename += "_q1";
		  break;
		case 2:
		  vtk_filename += "_q2";
		  break;
		default:
		  Assert(false, ExcNotImplemented());
	  }

	vtk_filename += "_cycle-" + Utilities::int_to_string(cycle,1);
	vtk_filename += ".vtk";

	std::ofstream output(vtk_filename);

	DataOut<dim> data_out;
	data_out.attach_dof_handler(dof_handler);
	data_out.add_data_vector(solution, "solution");

	data_out.build_patches(fe->degree);
	data_out.write_vtk(output);
}

  template <int dim>
  void HelmholtzSolver<dim>::run()
  {

    const unsigned int n_cycles = 6;

    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation, 0, 1);
            triangulation.refine_global(1);
          }
        else
        	triangulation.refine_global();

        setup_system();

        assemble_system();
        solve();

        process_solution(cycle);

        output_vtk(cycle);
      }

    convergence_table.set_precision("L2", 3);
    convergence_table.set_precision("H1", 3);

    convergence_table.set_scientific("L2", true);
    convergence_table.set_scientific("H1", true);

    convergence_table.set_tex_caption("cells", "\\# cells");
    convergence_table.set_tex_caption("dofs", "\\# dofs");
    convergence_table.set_tex_caption("L2", "$L^2$-error");
    convergence_table.set_tex_caption("H1", "$H^1$-error");

    convergence_table.set_tex_format("cells", "r");
    convergence_table.set_tex_format("dofs", "r");

    std::cout << std::endl;
    convergence_table.write_text(std::cout);

    // Write table into a LaTeX file.
    std::string error_filename
		= "error-" + Utilities::int_to_string(dim,1) + "D";

    switch (fe->degree)
      {
        case 1:
          error_filename += "_q1";
          break;
        case 2:
          error_filename += "_q2";
          break;
        default:
          Assert(false, ExcNotImplemented());
      }

    error_filename += ".tex";
    std::ofstream error_table_file(error_filename);

    convergence_table.write_tex(error_table_file);

    /////////////////

	convergence_table.add_column_to_supercolumn("cycle", "n cells");
	convergence_table.add_column_to_supercolumn("cells", "n cells");

	std::vector<std::string> new_order;
	new_order.emplace_back("n cells");
	new_order.emplace_back("H1");
	new_order.emplace_back("L2");
	convergence_table.set_column_order(new_order);

	convergence_table.evaluate_convergence_rates(
	  "L2", ConvergenceTable::reduction_rate);
	convergence_table.evaluate_convergence_rates(
	  "L2", ConvergenceTable::reduction_rate_log2);
	convergence_table.evaluate_convergence_rates(
	  "H1", ConvergenceTable::reduction_rate);
	convergence_table.evaluate_convergence_rates(
	  "H1", ConvergenceTable::reduction_rate_log2);

	std::cout << std::endl;
	convergence_table.write_text(std::cout);

	std::string conv_filename
		= "convergence-" + Utilities::int_to_string(dim,1) + "D";

	switch (fe->degree)
	  {
		case 1:
		  conv_filename += "_q1";
		  break;
		case 2:
		  conv_filename += "_q2";
		  break;
		default:
		  Assert(false, ExcNotImplemented());
	  }
	conv_filename += ".tex";

	std::ofstream table_file(conv_filename);
	convergence_table.write_tex(table_file);
  }

} // namespace Helmholtz

  ///////////////////////////////////////////
  ///////////////////////////////////////////
  ///////////////////////////////////////////

int main()
{
  const unsigned int dim = 1;

  try
    {
      using namespace dealii;

      {
        std::cout << "Solving with Q1 elements in " << dim << "D" << std::endl
                  << std::endl
                  << "=============================="
                  << std::endl
                  << std::endl;

        FE_Q<dim>             fe(1);
        Helmholtz::HelmholtzSolver<dim> helmholtz_problem_2d(fe);

        helmholtz_problem_2d.run();

        std::cout << std::endl;
      }

      {
        std::cout << "Solving with Q2 elements in " << dim << "D" << std::endl
                  << "==============================" << std::endl
                  << std::endl;

        FE_Q<dim>             fe(2);
        Helmholtz::HelmholtzSolver<dim> helmholtz_problem_2d(fe);

        helmholtz_problem_2d.run();

        std::cout << std::endl;
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#ifndef _EIGEN_SPACE_
#define _EIGEN_SPACE_ 1

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <exception>
#include <algorithm>

#include "naive_bayes.cpp"

#define _l_ std::cout<<__LINE__<<std::endl;

typedef struct {
  Eigen::VectorXd longMean;
  Eigen::MatrixXd transformationMatrix;
  ClassInfo ci;
} ReducedDimClassInfo;

bool
EigenvectorCompare(
    const std::pair<double, Eigen::VectorXd>& p,
    const std::pair<double, Eigen::VectorXd>& q) {

  return p.first < q.first;
}

Eigen::MatrixXd
GetSortedEigenvectors(const Eigen::MatrixXd& matrix) {
  Eigen::EigenSolver<Eigen::MatrixXd> eigenSolver;
  eigenSolver.compute(matrix, true);

  std::vector<std::pair<double, Eigen::VectorXd>> sortedEigenvectors;
  for (unsigned i = 0; i < eigenSolver.eigenvalues().size(); ++i) {
    if (eigenSolver.eigenvalues().imag()(i) == 0) {
      sortedEigenvectors.push_back(
        {
          eigenSolver.eigenvalues().real()(i),
          eigenSolver.eigenvectors().col(i).real()
        }
      );
    }
  }
  std::sort(sortedEigenvectors.begin(), sortedEigenvectors.end(), EigenvectorCompare);

  Eigen::MatrixXd rowMatrixOfSortedEigenvectors;
  rowMatrixOfSortedEigenvectors.resizeLike(matrix);

  for (unsigned i = 0; i < matrix.cols(); ++i) {
	if (sortedEigenvectors[i].second.rows() == rowMatrixOfSortedEigenvectors.rows()) {
 	   rowMatrixOfSortedEigenvectors.col(i) = sortedEigenvectors[i].second;
	} else {
		rowMatrixOfSortedEigenvectors.col(i).setZero();
	}
  }

  return rowMatrixOfSortedEigenvectors.transpose();
}

ReducedDimClassInfo
ComputeClassInfoInPcaDimensionReducedSpace(
    const std::vector<ClassificationObject>& data,
    const ClassInfo& cartesianClassInfo,
    const unsigned reducedDimensionality) {

  // create the space transformation matrix
  unsigned originalDimensionality = cartesianClassInfo.mean.size();
  Eigen::MatrixXd eigenspaceTransformMatrix(
    reducedDimensionality,
    originalDimensionality
  );

  eigenspaceTransformMatrix =
    (GetSortedEigenvectors(cartesianClassInfo.covariance)).topLeftCorner(
      reducedDimensionality,
      originalDimensionality
    );

  // transform all of the data to the new space
  std::vector<ClassificationObject> reducedData;
  reducedData.reserve(data.size());
  for (unsigned i = 0; i < data.size(); ++i) {
    ClassificationObject temp;
    temp.label = data[i].label;

    temp.features.resize(reducedDimensionality);
    temp.features = eigenspaceTransformMatrix * data[i].features;

    reducedData.push_back(temp);
  }

  // compute the new class info
  ReducedDimClassInfo info;
  info.longMean = cartesianClassInfo.mean;
  info.transformationMatrix = eigenspaceTransformMatrix;
  info.ci = ComputeClassInfo(
    reducedData,
    cartesianClassInfo.label,
    reducedDimensionality
  );

  return info;
}

unsigned
PcaClassifyObject(const ClassificationObject& object, const std::vector<ReducedDimClassInfo>& classSummaries) {
  unsigned mostLikelyClass = classSummaries.front().ci.label;
  double highestProbability = 0.0;

  for (const ReducedDimClassInfo& classSummary : classSummaries) {
    Eigen::VectorXd scaledFeatureVector = object.features - classSummary.longMean;
    Eigen::VectorXd reducedFeatureVector = classSummary.transformationMatrix * scaledFeatureVector;
    double probability = GaussianPdf(reducedFeatureVector, classSummary.ci) * classSummary.ci.prior;
    if (probability > highestProbability) {
      mostLikelyClass = classSummary.ci.label;
      highestProbability = probability;
    }
  }

std::cout<<highestProbability<<std::endl;

  return mostLikelyClass;
}

#endif //_EIGEN_SPACE_


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <ScatterDraw/ScatterDraw.h>
#include <Eigen/Eigen.h>
#include <Functions4U/Functions4U.h>
#include <STEM4U/Mooring.h>
#include <plugin/png/png.h>

using namespace Upp;

void TestMooring(bool test) {
	UppLog() << "\n\nCatenary mooring demo";

	double rho_m = 346.7;		// Kg/m
	double rho_m3 = 7850;
	double rho_water = 1025;

	double BL = 1.49E9;			// Mooring line break load
	double moorlen = 490;

	double xanchorvessel = 480;
	double zanchor = 0;
	double zvessel = 54;

	double Fhanchorvessel, Fvanchor, Fvvessel, xonfloor;
	Vector<double> x, z;
	MooringStatus status;

	Catenary(moorlen, xanchorvessel, zanchor, zvessel, xonfloor);

	double moorlen2;
	CatenaryGetLen(xonfloor, xanchorvessel, zanchor, zvessel, moorlen2);
	VERIFY(EqualDecimals(moorlen, moorlen2, 4));

	CatenaryGetLen(0.1*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(484.4677, moorlen2, 4));
	CatenaryGetLen(0.15*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(484.7264, moorlen2, 4));
	CatenaryGetLen(0.2*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.0166, moorlen2, 4));
	CatenaryGetLen(0.25*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.3445, moorlen2, 4));
	CatenaryGetLen(0.3*xanchorvessel, xanchorvessel, 0, zvessel, moorlen2);
	VERIFY(EqualDecimals(485.7176, moorlen2, 4));

	status = Catenary(rho_m, rho_m3, rho_water, moorlen, BL, xanchorvessel, zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel, xonfloor, x, z, 10);
	UppLog() << "\nStatus is: " << MooringStatusStr(status);
	VERIFY(status == CATENARY_ON_FLOOR);
	VERIFY(EqualDecimals(xonfloor, 292.5933, 4));
	VERIFY(EqualDecimals(Fhanchorvessel, 987138.9765, 4));
	VERIFY(EqualDecimals(Fvvessel, 583737.6409, 4));

	double Fanchor = sqrt(sqr(Fhanchorvessel) + sqr(Fvanchor));
	double anganchor = atan2(Fvanchor, Fhanchorvessel)*180./M_PI;
	double Fvessel = sqrt(sqr(Fhanchorvessel) + sqr(Fvvessel));
	double angvessel = atan2(Fvvessel, Fhanchorvessel)*180./M_PI;
	UppLog() << Format("\nFhanchorvessel=%.2f, Fvanchor=%.2f, Fanchor=%.2f, ang_anchor=%.1f grad\nFvvessel=%.2f, Fvessel=%.2f, ang_vessel=%.1f, xonfloor=%.1f",
		Fhanchorvessel, -Fvanchor, Fanchor, anganchor, -Fvvessel, Fvessel, angvessel, xonfloor);
	VERIFY(FormatF(angvessel, 4) == "30.5976");

	UppLog() << "\nX\tY";
	for (int i = 0; i < x.size(); ++i)
		UppLog() << Format("\n%.2f\t%.4f", x[i], z[i]);
	VERIFY(FormatF(z[7], 4) == "9.8116");

	if (!test) {
		int num = 100;
		Vector<Vector<double>> xx(5), zz(5);
		String dir = AppendFileNameX(GetDesktopFolder(), "STEM4U_Demo");
		RealizeDirectory(dir);
		ScatterDraw scatter;
		scatter.SetLabelX("Distance from anchor to vessel [m]").
				SetLabelY("Height from floor to vessel [m]").
				SetLeftMargin(70).SetBottomMargin(50).SetSize(Size(1000, 400)).
				SetLegendAnchor(ScatterDraw::LEFT_TOP);
		{
			scatter.RemoveAllSeries().SetTitle("Shifting the vessel to the anchor");
			for (int iz = 0; iz < 5; ++iz) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel-iz*15, zanchor, zvessel, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Xanchorvessel:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						xanchorvessel-iz*15, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor xanchorvessel.png"), scatter.GetImage());
		}
		{
			scatter.RemoveAllSeries().SetTitle("Raising the vessel");
			for (int iz = 0; iz < 5; iz++) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel, zanchor, zvessel+iz*10, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Zvessel:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						zvessel+iz*10, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor zvessel.png"), scatter.GetImage());
		}
		{
			scatter.RemoveAllSeries().SetTitle("Raising the anchor");
			for (int iz = 0; iz < 5; iz++) {
				Catenary(rho_m, rho_m3, rho_water, moorlen, BL,
					xanchorvessel, zanchor+iz*30, zvessel, Fhanchorvessel, Fvanchor, Fvvessel,
					xonfloor, xx[iz], zz[iz], num);
				String legend = Format("Zanchor:%.0f, Fx:%.0f, Fzanchor:%.0f, Fzvessel:%.0f, xonfloor:%.1f",
						zanchor+iz*30, Fhanchorvessel/1000., Fvanchor/1000., Fvvessel/1000., xonfloor);
				scatter.AddSeries(xx[iz], zz[iz]).Legend(legend).NoMark();
				scatter.AddSeries(&zz[iz][0], 1, xx[iz][0], 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
				scatter.AddSeries(&zz[iz].Top(), 1, xx[iz].Top(), 0).NoSeriesLegend().MarkStyle<CircleMarkPlot>().MarkWidth(15);
			}
			scatter.ZoomToFit(true, true);
			PNGEncoder().SaveFile(AppendFileNameX(dir, "Moor zanchor.png"), scatter.GetImage());
		}
	}
}


#include <limbus/OpenglWindow.hpp>
#include <limbus/Timer.hpp>
#include <limbus/opengl.h>
#include <pingo/SpriteBuffer.hpp>
#include <pingo/Texture.hpp>

#include <iostream>
#include <complex>
#include <cmath>
#include <boost/thread.hpp>

class FractalRenderer
{
public:
	struct Color
	{
		Color() {}
		Color(double r, double g, double b) : r(r), g(g), b(b) {}
		double r, g, b;

		Color operator * (double factor)
		{
			return Color(r*factor, g*factor, b*factor);
		}

		Color operator + (const Color& other)
		{
			return Color(r + other.r, g + other.g, b + other.b);
		}
	};

	struct Job
	{
		size_t y_start;
		size_t y_count;
		double scale;
		std::complex<double> offset;
	};

	static const int max_iterations = 1000;
	static const int max_colors = 50;

	FractalRenderer(unsigned char* texture_data, Color* color_ramp, double texture_width, double texture_height)
		: texture_data(texture_data), color_ramp(color_ramp), texture_width(texture_width), texture_height(texture_height), job_done(false), running(true)
	{
	}

	typedef boost::lock_guard<boost::mutex> lock_guard;

	void setJob(Job new_job)
	{
		current_job = new_job;
		lock_guard lock(job_mutex);
		job_done = false;
	}

	bool jobDone()
	{
		lock_guard lock(job_mutex);
		return job_done;
	}

	void stop()
	{
		lock_guard lock(running_mutex);
		running = false;
	}

	void operator () ()
	{
		bool _running;

		do
		{
			bool done;
			{
				lock_guard lock(job_mutex);
				done = job_done;
			}

			if (!done)
			{
				for (size_t _y = current_job.y_start; _y < current_job.y_start + current_job.y_count; ++_y)
				{
					for (size_t _x = 0; _x < (size_t)texture_width; ++_x)
					{
						std::complex<double> c(((double)_x / texture_width) * 2.0 - 1.0,
											   ((double)_y / texture_height) * 2.0 - 1.0);

						c *= current_job.scale;
						c += current_job.offset;

						std::complex<double> z(0.0, 0.0);

						double q = (z.real() - 1/4)*(z.real() - 1/4) + z.imag()*z.imag();
						bool inside_cardioid = q * (q + (z.real() - 1/4)) < (1/4)*z.imag()*z.imag();
						bool inside_period2_bulb = (z.real() + 1)*(z.real() + 1) + z.imag()*z.imag() < 1/16;

						size_t i = 0;
						if (!inside_cardioid || !inside_period2_bulb)
						{
							while (i < max_iterations && z.real()*z.real() + z.imag()*z.imag() < 2.0*2.0)
							{
								z = z*z + c;
								i += 1;
							}
						}
						else
						{
							i = max_iterations;
						}

						if (z.real()*z.real() + z.imag()*z.imag() < 2.0*2.0 || i == max_iterations)
							WritePixel(_x, _y, -1, 0, 0.0);
						else
						{
							double s = i + (log(log((double)max_iterations)) - log(log(abs(z))))/log(2.0);

							int first = (int)s;
							int second = first + 1;
							double factor = s - first;
							WritePixel(_x, _y, first, second, factor);
						}
					}
				}

				lock_guard lock(job_mutex);
				job_done = true;
			}
			else
			{
				boost::thread::sleep(boost::get_system_time() + boost::posix_time::milliseconds(1));
			}

			lock_guard lock(running_mutex);
			_running = running;
		} while (_running);
	}

private:
	FractalRenderer(FractalRenderer& other);

	Job current_job;
	bool job_done;
	bool running;

	boost::mutex job_mutex, running_mutex;

	double texture_width;
	double texture_height;

	unsigned char* texture_data;
	Color* color_ramp;

	void WritePixel(size_t x, size_t y, int first, int second, double factor)
	{
		size_t offset = y * (size_t)texture_width + x;

		Color color;
		if (first == -1)
			color = Color(0, 0, 0);
		else
			color = color_ramp[second % (max_colors*2)] * factor + color_ramp[first % (max_colors*2)] * (1.0 - factor);

		texture_data[offset * 3 + 0] = (unsigned char)(color.r * 255.0);
		texture_data[offset * 3 + 1] = (unsigned char)(color.g * 255.0);
		texture_data[offset * 3 + 2] = (unsigned char)(color.b * 255.0);
	}
};

class Application : public Limbus::OpenglWindow::EventHandler
{
	bool running;

	void onClose(Limbus::OpenglWindow& self)
	{
		running = false;
	}

	Limbus::OpenglWindow window;
	Pingo::SpriteBuffer* sprite_buffer;
	Pingo::Texture* texture;
	unsigned char* texture_data;

	FractalRenderer::Color color_ramp[FractalRenderer::max_colors*2];

public:
	void run()
	{
		running = true;
		window.setCaption("Fractal Demo");
		window.addEventHandler(this);
		window.setWidth(256);
		window.setHeight(256);
		window.create();

		glClearColor( 0.0f, 0.0f, 0.0f, 0.0f );
		glDisable( GL_DEPTH_TEST );
		glEnable( GL_BLEND );
		glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
		glEnable( GL_TEXTURE_2D );

		glMatrixMode( GL_PROJECTION );
		glLoadIdentity();
		glOrtho( 0.0f, window.getWidth(), window.getHeight(), 0.0f, -100.0f, 100.0f );
		glMatrixMode( GL_MODELVIEW );
		glLoadIdentity();

		texture_data = new unsigned char[window.getWidth() * window.getHeight() * 3];

		texture = new Pingo::Texture();
		texture->loadFromMemory(texture_data, window.getWidth(), window.getHeight(), 3);
		sprite_buffer = new Pingo::SpriteBuffer(texture, 1, true);

		sprite_buffer->setWritable(true);
		sprite_buffer->setRectangle(0, 0.0f, 0.0f, (float)window.getWidth(), (float)window.getHeight());
		sprite_buffer->setColor(0, 1.0f, 1.0f, 1.0f, 1.0f);
		sprite_buffer->setTextureRectangle(0, 0.0f, 0.0f, (float)window.getWidth(), (float)window.getHeight());
		sprite_buffer->setWritable(false);

		for (size_t i = 0; i < FractalRenderer::max_colors; ++i)
		{
			float factor = (float)i / FractalRenderer::max_colors;
			float inverse_factor = (float)(FractalRenderer::max_colors - i) / FractalRenderer::max_colors;

			color_ramp[i] = FractalRenderer::Color(sqrt(sqrt(factor)), factor, inverse_factor * 0.5f);
			color_ramp[FractalRenderer::max_colors*2 - 1 - i] = color_ramp[i];
		}

		FractalRenderer worker(texture_data, color_ramp, (double)window.getWidth(), (double)window.getHeight());
		FractalRenderer worker2(texture_data, color_ramp, (double)window.getWidth(), (double)window.getHeight());

		FractalRenderer::Job job;
		job.offset = std::complex<double>(+0.001643721971153, +0.822467633298876);
		job.scale = 2.0;
		job.y_start = 0;
		job.y_count = window.getHeight() / 2;

		FractalRenderer::Job job2;
		job2.offset = std::complex<double>(+0.001643721971153, +0.822467633298876);
		job2.scale = 2.0;
		job2.y_start = window.getHeight() / 2;
		job2.y_count = window.getHeight() - job2.y_start;

		worker.setJob(job);
		worker2.setJob(job2);

		boost::thread worker_thread(boost::ref(worker));
		boost::thread worker_thread2(boost::ref(worker2));

		Limbus::Timer timer;
		while (running)
		{
			window.pollEvents();
			double elapsed = 1.0f + timer.getElapsed() * 0.005;

			if (worker.jobDone() && worker2.jobDone())
			{
				texture->update(texture_data);

				job.scale = 1.0 / (elapsed * (1.0 / job.scale));
				if (job.scale < 0.00000000001)
					job.scale = 0.00000000001;
				worker.setJob(job);

				job2.scale = job.scale;
				worker2.setJob(job2);
			}

			sprite_buffer->draw(0, 1, 0, 0);
			window.swapBuffers();
		}

		worker.stop();
		worker2.stop();
		worker_thread.join();
		worker_thread2.join();

		delete sprite_buffer;
		delete texture;
		delete [] texture_data;
	}
};

void main()
{
	Application app;
	app.run();
}


/*
 * Copyright (c) 2015, Jonathan Ventura
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef POLYNOMIAL_INTERNAL_HPP
#define POLYNOMIAL_INTERNAL_HPP

#include <Eigen/Core>
#include <vector>
#include <queue>

namespace polynomial
{
    namespace Internal
    {

        template<int a,int b> struct max{ static const int value=(a<b)?b:a; };
        template<int a,int b> struct min{ static const int value=(a>b)?b:a; };

        template <int length>
        inline
        Eigen::Map< Eigen::Matrix<double,1,length> > vecmap(double *data)
        {
            return Eigen::Map< Eigen::Matrix<double,1,length> >(data);
        }

        template <int length>
        inline
        Eigen::Map< const Eigen::Matrix<double,1,length> > vecmap(const double *data)
        {
            return Eigen::Map< const Eigen::Matrix<double,1,length> >(data);
        }

        template <int deg1,int deg2,int k>
        struct
        _PolyConv
        {
            static void
            compute( double *coefout, const double *coef1, const double *coef2 )
            {
                coefout[k] =
                (
                 vecmap<min<k,deg1>::value-max<0,k-deg2>::value+1>
                 (
                  coef1+max<0,k-deg2>::value
                  )
                 .array()
                 *
                 vecmap<min<k,deg1>::value-max<0,k-deg2>::value+1>
                 (
                  coef2+k-min<k,deg1>::value
                  ).reverse().array()
                 ).sum();
                _PolyConv<deg1,deg2,k-1>::compute( coefout, coef1, coef2 );
            }
        };

        template <int deg1,int deg2>
        struct
        _PolyConv<deg1,deg2,-1>
        {
            static void
            compute( double *coefout, const double *coef1, const double *coef2 )
            {
            }
        };

        template <int deg1,int deg2>
        struct
        PolyConv
        {
            static void
            compute( Eigen::Matrix<double,deg1+deg2+1,1> &coefout, const Eigen::Matrix<double,deg1+1,1> &coef1, const Eigen::Matrix<double,deg2+1,1> &coef2 )
            {
                _PolyConv<deg1,deg2,deg1+deg2>::compute(coefout.data(),coef1.data(),coef2.data());
            }
        };

        template <>
        struct
        PolyConv<Eigen::Dynamic,Eigen::Dynamic>
        {
            static void
            compute( Eigen::VectorXd &coefout, const Eigen::VectorXd &coef1, const Eigen::VectorXd &coef2 )
            {
                int deg1 = coef1.rows()-1;
                int deg2 = coef2.rows()-1;

                for ( int k = deg1+deg2; k >= 0; k-- )
                {
                    coefout[k] =
                    (
                     coef1.block(
                                 std::max(0,k-deg2),0,
                                 std::min(k,deg1)-std::max(0,k-deg2)+1,1
                                 ).array()
                     *
                     coef2.block(
                                 k-std::min(k,deg1),0,
                                 std::min(k,deg1)-std::max(0,k-deg2)+1,1
                                 ).reverse().array()
                     ).sum();
                }
            }
        };

        template<int deg,int k>
        struct
        _PolyVal
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return coef[k] + x*_PolyVal<deg,k-1>::compute(coef,x);
            }
        };

        template<int deg>
        struct
        _PolyVal<deg,0>
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return coef[0];
            }
        };

        template<int deg>
        struct
        PolyVal
        {
            static double
            compute( const Eigen::Matrix<double,deg+1,1> &coef, const double &x )
            {
                return Internal::_PolyVal<deg,deg>::compute( coef, x );
            }
        };

        template<>
        struct
        PolyVal<Eigen::Dynamic>
        {
            static double
            compute( const Eigen::VectorXd &coef, const double &x )
            {
                int deg = coef.rows()-1;
                double val = 0;
                for ( int i = 0; i <= deg; i++ ) val = coef(i) + x*val;
                return val;
            }
        };

        template<int deg>
        struct BisectionMethod
        {
            static double compute(const Eigen::Matrix<double,deg+1,1> &coef, const double lower, const double upper )
            {
                double a = lower;
                double fa = PolyVal<deg>::compute(coef,a);
                char fapos = (fa>0);
                double b = upper;
                double c = 0.5*(a+b);

                for ( int i = 0; i < 24; i++ )
                {
                    const double fc = PolyVal<deg>::compute(coef,c);
                    const char fcpos = (fc>0);
                    if ( fcpos == fapos )
                    {
                        a = c;
                        fa = fc;
                    } else {
                        b = c;
                    }
                    c = 0.5*(a+b);
                }

                return c;
            }
        };

        template<>
        struct BisectionMethod<Eigen::Dynamic>
        {
            static double compute(const Eigen::VectorXd &coef, const double lower, const double upper )
            {
                double a = lower;
                double fa = PolyVal<Eigen::Dynamic>::compute(coef,a);
                char fapos = (fa>0);
                double b = upper;
                double c = 0.5*(a+b);

                for ( int i = 0; i < 100; i++ )
                {
                    const double fc = PolyVal<Eigen::Dynamic>::compute(coef,c);
                    const char fcpos = (fc>0);
                    if ( fcpos == fapos )
                    {
                        a = c;
                        fa = fc;
                    } else {
                        b = c;
                    }
                    c = 0.5*(a+b);
                }

                return c;
            }
        };

        template<int deg>
        struct SturmChain
        {
            Eigen::Matrix<double,deg+1,1> coef;
            SturmChain<deg-1> next;
        };

        template<>
        struct SturmChain<0>
        {
            Eigen::Matrix<double,1,1> coef;
        };

        template<int degin,int degout,int k>
        struct _GetSturmChain
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return _GetSturmChain<degin-1,degout,k-1>::compute(sc.next);
            }
        };

        template<int degin,int degout>
        struct _GetSturmChain<degin,degout,0>
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return sc;
            }
        };

        template<int degin,int degout>
        struct GetSturmChain
        {
            static SturmChain<degout>& compute( SturmChain<degin> &sc )
            {
                return _GetSturmChain<degin,degout,degin-degout>::compute(sc);
            }
        };

        template <int deg,int k>
        struct _ModPoly
        {
            static const void compute( SturmChain<deg> &sc )
            {
                const Eigen::Matrix<double,k+3,1> &u( GetSturmChain<deg,k+2>::compute(sc).coef );
                const Eigen::Matrix<double,k+2,1> &v( GetSturmChain<deg,k+1>::compute(sc).coef );
                Eigen::Matrix<double,k+3,1> r = u;
                const double s = v(0)/fabs(v(0));
                r(k+2) *= s;
                const int blocklength = k+1;
                r.block(1,0,blocklength,1) = s * r.block(1,0,blocklength,1) - r(0) * v.block(1,0,blocklength,1);
                r.block(2,0,blocklength,1) = s * r.block(2,0,blocklength,1) - r(1) * v.block(1,0,blocklength,1);
                Eigen::Matrix<double,k+1,1> &rout( GetSturmChain<deg,k>::compute(sc).coef );
                rout = r.block(2,0,k+1,1);
                double f = -fabs(rout(0));
                rout /= f;
                _ModPoly<deg,k-1>::compute(sc);
            }
        };

        template <int deg>
        struct _ModPoly<deg,0>
        {
            static const void compute( SturmChain<deg> &sc )
            {
                const Eigen::Matrix<double,3,1> &u( GetSturmChain<deg,2>::compute(sc).coef );
                const Eigen::Matrix<double,2,1> &v( GetSturmChain<deg,1>::compute(sc).coef );
                Eigen::Matrix<double,3,1> r = u;
                const double s = v(0)/fabs(v(0));
                r(2) *= s;
                r(1) = s * r(1) - r(0) * v(1);
                r(2) = s * r(2) - r(1) * v(1);
                GetSturmChain<deg,0>::compute(sc).coef[0] = -r(2);
            }
        };

        template <int deg>
        struct ModPoly
        {
            static const void compute( SturmChain<deg> &sc )
            {
                _ModPoly<deg,deg-2>::compute(sc);
            }
        };

        template <int deg>
        struct _SignChanges
        {
            static const void compute( SturmChain<deg> &sc, const double x, const double lf, int &count )
            {
                double f = PolyVal<deg>::compute( sc.coef, x );
                count += (f*lf<0);
                _SignChanges<deg-1>::compute( sc.next, x, f, count );
            }
        };

        template <>
        struct _SignChanges<0>
        {
            static const void compute( SturmChain<0> &sc, const double x, const double lf, int &count )
            {
                double f = sc.coef[0];
                count += (f*lf<0);
            }
        };

        template <int deg>
        struct SignChanges
        {
            static const void compute( SturmChain<deg> &sc, const double x, int &count )
            {
                double f = PolyVal<deg>::compute( sc.coef, x );
                count = 0;
                _SignChanges<deg-1>::compute( sc.next, x, f, count );
            }
        };

        template <>
        struct SignChanges<0>
        {
            static const void compute( SturmChain<0> &sc, const double x, int &count )
            {
                count = 0;
            }
        };

        struct SturmInterval
        {
            double lb, ub;
            int sclb, scub;
            SturmInterval(const double _lb, const double _ub, const int _sclb, const int _scub)
            : lb(_lb), ub(_ub), sclb(_sclb), scub(_scub) { }
        };

        struct RootInterval
        {
            double lb, ub;
            RootInterval(const double _lb, const double _ub)
            : lb(_lb), ub(_ub) { }
        };

        template<int deg>
        class SturmRootFinder
        {
            void bisection(const double lb, const double ub, std::vector<double> &roots)
            {
                int sclb = 0;
                int scub = 0;
                SignChanges<deg>::compute(sc,lb,sclb);
                SignChanges<deg>::compute(sc,ub,scub);
                std::queue<SturmInterval> intervals;
                intervals.push( SturmInterval(lb,ub,sclb,scub) );

                std::vector<RootInterval> root_intervals;

                while ( !intervals.empty() )
                {
                    SturmInterval interval = intervals.front();
                    intervals.pop();

                    // get num roots in interval
                    int n = interval.sclb-interval.scub;

                    if ( n == 0 )
                    {

                    }
                    else if ( n == 1 )
                    {
                        root_intervals.push_back( RootInterval(interval.lb,interval.ub) );
                    }
                    else if ( fabs(interval.ub-interval.lb)>1e-15 )
                    {
                        double m = (interval.lb+interval.ub)*0.5;

                        // get sign changes at m
                        int scm = 0;
                        SignChanges<deg>::compute(sc,m,scm);

                        // add [lb,m] interval
                        intervals.push( SturmInterval(interval.lb,m,interval.sclb,scm) );

                        // add [m,ub] interval
                        intervals.push( SturmInterval(m,interval.ub,scm,interval.scub) );
                    }
                }

                roots.resize(root_intervals.size());
                for ( int i = 0; i < root_intervals.size(); i++ )
                {
                    roots[i] = BisectionMethod<deg>::compute(sc.coef,root_intervals[i].lb,root_intervals[i].ub);
                }
            }
        public:
            SturmChain<deg> sc;

            SturmRootFinder( const Eigen::Matrix<double,deg+1,1> &coef )
            {
                sc.coef = coef;
                sc.next.coef = coef.head(deg);
                double f = fabs(sc.coef[0] * deg);
                for ( int i = 0; i < deg; i++ ) sc.next.coef[i] *= (deg-i)/f;
                ModPoly<deg>::compute(sc);
            }

            void realRoots(const double lb, const double ub, std::vector<double> &roots)
            {
                bisection(lb,ub,roots);
            }
        };

        template<>
        class SturmRootFinder<Eigen::Dynamic>
        {
            std::vector<Eigen::VectorXd> sc;

            Eigen::VectorXd modpoly( const Eigen::VectorXd &u, const Eigen::VectorXd &v )
            {
                int udeg = u.rows()-1;
                Eigen::VectorXd r = u;
                double s = v(0)/fabs(v(0));
                r(udeg) *= s;
                int blocklength = udeg-1;
                r.block(1,0,blocklength,1) = s * r.block(1,0,blocklength,1) - r(0) * v.block(1,0,blocklength,1);
                r.block(2,0,blocklength,1) = s * r.block(2,0,blocklength,1) - r(1) * v.block(1,0,blocklength,1);
                return r.block(2,0,blocklength,1);
            }

            int signchanges(double x)
            {
                int deg = sc[0].rows()-1;
                Eigen::VectorXd f(deg+1);
                for ( int i = 0; i < deg+1; i++ ) f[i] = PolyVal<Eigen::Dynamic>::compute(sc[i],x);
                int n = 0;
                // NB: not skipping zeros here
                for ( int i = 1; i < deg+1; i++ ) n += (f[i]*f[i-1]<0);
                return n;
            }

            void bisection(const double lb, const double ub, std::vector<double> &roots)
            {
                int sclb = signchanges(lb);
                int scub = signchanges(ub);
                std::queue<SturmInterval> intervals;
                intervals.push( SturmInterval(lb,ub,sclb,scub) );

                std::vector<RootInterval> root_intervals;

                while ( !intervals.empty() )
                {
                    SturmInterval interval = intervals.front();
                    intervals.pop();

                    // get num roots in interval
                    int n = interval.sclb-interval.scub;

                    if ( n == 0 )
                    {

                    }
                    else if ( n == 1 )
                    {
                        root_intervals.push_back( RootInterval(interval.lb,interval.ub) );
                    }
                    else if ( fabs(interval.ub-interval.lb)>1e-15 )
                    {
                        double m = (interval.lb+interval.ub)*0.5;

                        // get sign changes at m
                        int scm = signchanges(m);

                        // add [lb,m] interval
                        intervals.push( SturmInterval(interval.lb,m,interval.sclb,scm) );

                        // add [m,ub] interval
                        intervals.push( SturmInterval(m,interval.ub,scm,interval.scub) );
                    }
                }

                roots.resize(root_intervals.size());
                for ( int i = 0; i < root_intervals.size(); i++ )
                {
                    roots[i] = BisectionMethod<Eigen::Dynamic>::compute(sc[0],root_intervals[i].lb,root_intervals[i].ub);
                }
            }
        public:
            SturmRootFinder( const Eigen::VectorXd &coef )
            {
                int deg = coef.rows()-1;
                sc.resize(deg+1);

                sc[0] = coef;

                double f = fabs(sc[0](0) * deg);
                sc[1] = sc[0].block(0,0,deg,1);
                for ( int i = 0; i < deg; i++ ) sc[1][i] *= (deg-i)/f;

                for ( int i = 2; i < deg+1; i++ )
                {
                    sc[i] = modpoly(sc[i-2],sc[i-1]);
                    if ( i != deg )
                    {
                        double f = -fabs(sc[i](0));
                        sc[i] /= f;
                    }
                    else
                    {
                        sc[i] = -sc[i];
                    }
                }
            }

            void realRoots(const double lb, const double ub, std::vector<double> &roots)
            {
                bisection(lb,ub,roots);
            }
        };

        template<int deg>
        struct RootFinder
        {
            static void compute( const Eigen::Matrix<double,deg+1,1> &p, std::vector<double> &roots )
            {
                Eigen::PolynomialSolver<double,deg> ps;
                ps.compute(p.reverse());
                ps.realRoots(roots);
            }
        };

        template<>
        struct RootFinder<Eigen::Dynamic>
        {
            static void compute( const Eigen::Matrix<double,Eigen::Dynamic,1> &p, std::vector<double> &roots )
            {
                Eigen::PolynomialSolver<double,Eigen::Dynamic> ps;
                ps.compute(p.reverse());
                ps.realRoots(roots);
            }
        };

        template<>
        struct RootFinder<2>
        {
            static void compute( const Eigen::Matrix<double,3,1> &p, std::vector<double> &roots )
            {
                const double discrim = p[1]*p[1] - 4.*p[0]*p[2];
                if ( discrim < 0 )
                {
                    roots.clear();
                }
                else if ( discrim == 0 )
                {
                    roots.resize(1);
                    roots[0] = -p[1] / ( 2. * p[0] );
                }
                else
                {
                    const double twoa = 2. * p[0];
                    const double sqrt_discrim = sqrt(discrim);
                    roots.resize(2);
                    roots[0] = (-p[1] + sqrt_discrim) / twoa;
                    roots[1] = (-p[1] - sqrt_discrim) / twoa;
                }
            }
        };

    } // end namespace Internal

} // end namespace Polynomial

#endif