#include <Eigen/LU>
#include <Eigen/QR>
#include <array>

#include <jrl-qp/test/kkt.h>
#include <jrl-qp/test/randomMatrices.h>
#include <jrl-qp/test/randomProblems.h>

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#define DOCTEST_CONFIG_SUPER_FAST_ASSERTS
#include "doctest/doctest.h"

using namespace jrl::qp::test;

// TEST_CASE("Random orthogonal matrices")
//{
//  const int N = 1000;
//  int positive = 0;
//  for (int i = 0; i < N; ++i)
//  {
//    int s = 5 + (10 * i) / N;
//    Eigen::MatrixXd Q = randOrtho(s);
//    FAST_REQUIRE_EQ(Q.rows(), Q.cols());
//    FAST_CHECK_EQ(Q.cols(), s);
//
//    double det = Q.lu().determinant();
//    FAST_CHECK_EQ(std::abs(det), doctest::Approx(1).epsilon(1e-8));
//    if (det > 0) ++positive;
//
//    FAST_CHECK_UNARY((Q.transpose() * Q).isIdentity(1e-8));
//    FAST_CHECK_UNARY((Q * Q.transpose()).isIdentity(1e-8));
//  }
//  FAST_CHECK_UNARY(429 <= positive && positive <= 571); //this can fails in less than 0.001% of cases.
//
//  for (int i = 0; i < N; ++i)
//  {
//    int s = 5 + (10 * i) / N;
//    Eigen::MatrixXd Q = randOrtho(s, true);
//    double det = Q.lu().determinant();
//    FAST_CHECK_EQ(det, doctest::Approx(1).epsilon(1e-8));
//  }
//}
//
// TEST_CASE("Random rank deficient matrices")
//{
//  std::array<Eigen::Index, 5> sizes = {10,25,50,75,100};
//  Eigen::Index n = 0;
//  double mean = 0;
//  double var = 0;
//  Precompute the required size for storing all coefficients
//  for (auto rows : sizes)
//  {
//    for (auto cols : sizes)
//    {
//      for (auto rank : sizes)
//      {
//        if (rank <= rows && rank <= cols)
//          n += rows * cols;
//      }
//    }
//  }
//  std::vector<double> val;
//  val.reserve(n);
//
//  Testing the rank of generated matrices
//  for (auto rows: sizes)
//  {
//    for (auto cols: sizes)
//    {
//      for (auto rank: sizes)
//      {
//        if (rank > rows || rank > cols) //we skip when rank > min(rows,cols)
//          continue;
//        Eigen::MatrixXd M = randn(rows, cols, rank);
//        FAST_CHECK_EQ(M.colPivHouseholderQr().rank(), rank);
//        std::copy(M.data(), M.data() + rows * cols, std::back_inserter(val));
//      }
//    }
//  }
//
//  Computing and testing mean and variance
//  for (int i = 0; i < n; ++i)
//  {
//    mean += val[i];
//  }
//  mean /= n;
//  for (int i = 0; i < n; ++i)
//  {
//    var += (val[i] - mean) * (val[i] - mean);
//  }
//  var /= n;
//
//  FAST_CHECK_EQ(std::abs(mean), doctest::Approx(0).epsilon(1e-2));
//  FAST_CHECK_EQ(var, doctest::Approx(1).epsilon(2.5e-2));
//}
//
// TEST_CASE("Random dependent matrices")
//{
//  std::vector<std::array<Eigen::Index, 6>> params = {
//    { 15, 5, 4, 7, 7, 9 },
//    { 15, 5, 4, 7, 5, 9 },
//    { 10, 5, 4, 7, 7, 9 },
//    { 7, 8, 4, 12, 6, 7 },
//    { 7, 8, 4, 12, 6, 6 } };
//  for (const auto& p: params)
//  {
//    auto [cols, rowsA, rankA, rowsB, rankB, rankAB] = p;
//    auto [A, B] = randDependent(cols, rowsA, rankA, rowsB, rankB, rankAB);
//    FAST_REQUIRE_EQ(A.rows(), rowsA);
//    FAST_REQUIRE_EQ(A.cols(), cols);
//    FAST_REQUIRE_EQ(B.rows(), rowsB);
//    FAST_REQUIRE_EQ(B.cols(), cols);
//    Eigen::MatrixXd M(rowsA+rowsB, cols); M << A, B;
//    FAST_CHECK_EQ(A.colPivHouseholderQr().rank(), rankA);
//    FAST_CHECK_EQ(B.colPivHouseholderQr().rank(), rankB);
//    FAST_CHECK_EQ(M.colPivHouseholderQr().rank(), rankAB);
//  }
//}

void testRandomProblem(const RandomLeastSquare & pb)
{
  FAST_CHECK_UNARY(pb.wellFormed());
  Eigen::VectorXd mult(pb.l.size() + pb.f.size() + pb.xl.size());
  mult << pb.lambdaEq, pb.lambdaIneq, pb.lambdaBnd;
  FAST_CHECK_UNARY(testKKT(pb.x, mult, pb));
}

TEST_CASE("Random Least Squares")
{
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 3)));
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 3).nEq(2)));
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 0).nEq(2)));
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 3).nIneq(5)));
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 3).nIneq(5).nStrongActIneq(2)));
  testRandomProblem(randomProblem(ProblemCharacteristics(5, 3).nIneq(5).nStrongActIneq(4)));
  testRandomProblem(randomProblem({5, 3, 2, 5, 3, 0, 2, 0, 0, 0, false, false, false}));
  testRandomProblem(randomProblem({5, 3, 1, 5, 3, 0, 1, 0, 0, 0, false, false, false}));
}


#include <cap_gait/contrib/KalmanFilter.h>
#include <Eigen/LU>

using namespace margait_contrib;

KalmanFilter::KalmanFilter()
{
	lastX = 0;
	lastV = 0;
	smoothing = 0.1;
	timeStep = 0.010;
	x = 0;
	v = 0;
	a = 0;
	px = 0;
	pv = 0;
	pa = 0;

	// When the gain is adaptive, the smoothing config parameters don't make any difference.
	adaptiveGain = true;

	init();
}

// Prepares (resets) the noise matrices and the kalman gain, but preserves the current state.
// This is good for changing the smoothing on the fly without messing with the state estimate.
void KalmanFilter::init()
{
	// Process noise.
	Q = Eigen::Matrix3d::Identity();

	// Measurement noise.
	R = (0.001 + smoothing*100.0) * Eigen::Matrix3d::Identity();

	// Passive linear system dynamics matrix A.
	A << 1.0, timeStep,   0.0   ,
	     0.0,   1.0   , timeStep,
	     0.0,   0.0   ,   1.0   ;

	// State and measurement vectors.
	X << x, v, a;
	Z.setZero();

	// Other matrices
	H = I = K = Eigen::Matrix3d::Identity();
	P.setZero();
}

// Sets the smoothing parameter.
void KalmanFilter::setSmoothing(double s)
{
	smoothing = s;
	init();
}

// Sets the time interval between the updates. It's set to 10 milliseconds by default.
void KalmanFilter::setTimeStep(double t)
{
	timeStep = t;
	init();
}

// Resets the state to the provided parameters, but doesn't change the noise and the kalman gain.
// This is good to set the state estimate on the fly without messing with the Kalman gain.
void KalmanFilter::reset(double xx, double vv, double aa)
{
	x = xx;
	v = vv;
	a = aa;

	X << x, v, a;

	lastX = x;
	lastV = v;
}

// Updates the process. Provide a position measurement z.
void KalmanFilter::update(double z)
{
	double Zv = (z - lastX)/timeStep;
	Z << z, Zv, (Zv - lastV)/timeStep;
	lastX = Z(0);
	lastV = Z(1);

	if (adaptiveGain)
	{
		X = A*X;
		P = A*P*A.transpose() + Q;
		K = P*H.transpose() * (H*P*H.transpose() + R).inverse();
		X = X + K*(Z - H*X);
		P = (I - K*H)*P;
	}
	else
	{
		X = A*X;
		K = Q*H.transpose() * (H*Q*H.transpose() + R).inverse();
		X = X + K*(Z - H*X);
	}

	x = X(0);
	v = X(1);
	a = X(2);

	px = P(0,0);
	pv = P(1,1);
	pa = P(2,2);
}


#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();
}


/////////////////////////////////////////
// Double integrator dynamics template //
/////////////////////////////////////////

#include <array>
#include <Eigen/Core>

#include "dynamic_system.hh"

// Foward declaration, REQUIRED for template code to work
class DoubleIntegrator;

// Template specialization of traits class to define the state, control and output type
template <> struct DynamicSystem_traits<DoubleIntegrator> {
    enum { POS=0,
           VEL,
           x_dim_};
    enum { ACCEL=0,
           u_dim_};
    using StateVector   = Eigen::Matrix<double, x_dim_, 1>;
    using ControlVector = Eigen::Matrix<double, u_dim_, 1>;
};

// Curiously recurring template pattern (CRTP) is used here
class DoubleIntegrator : public DynamicSystem <DoubleIntegrator>
{
public:
    inline static int x_dim() { return DynamicSystem_traits<DoubleIntegrator>::x_dim_;}
    inline static int u_dim() { return DynamicSystem_traits<DoubleIntegrator>::u_dim_;}

    //Here we use a functor to make it work with function calling that requires non-static functions
    struct
    {
        StateVector operator() (const StateVector & x, const ControlVector & u)
        {
            StateVector dxdt;
            dxdt[0] = x[1];
            dxdt[1] = u[0];
            return dxdt;
        }
    } continuous_dynamics_impl;
};


#include <iostream>
#include <type.hpp>

#include <Eigen/Eigen>

#include "Geometry/ParameterDesign/Bezier.hpp"
#include "imgui/implot.h"
#include "Visualization/Visualizer.h"

class BezierVisualizer :public Visualizer
{
protected:
	void AddPoint();
	void DragPoint();

	void draw(bool* p_open) override;

	std::vector<Point2d> points;
	bool updated = true;

	void evaluate_bezier()
	{
		BezierBernstein bezier(points);
		//Bezier bezier(points);
		for (int i = 0; i < Length; ++i)
		{
			xs[i] = bezier(ctr_points[i]).x();
			ys[i] = bezier(ctr_points[i]).y();
		}
		ctr_xs.resize(points.size());
		ctr_ys.resize(points.size());

		for (int i = 0; i < points.size(); ++i)
		{
			ctr_xs[i] = points[i].x();
			ctr_ys[i] = points[i].y();
		}
	}

public:
	BezierVisualizer() {
		for (int i = 0; i < Length; ++i)
		{
			ctr_points[i] = Float(i) / (Length - 1);
		}
	}
	const float Length = 1001;

	std::vector<float> ctr_points = std::vector<float>(Length);
	std::vector<float> xs = std::vector<float>(Length);
	std::vector<float> ys = std::vector<float>(Length);

	std::vector<float> ctr_xs = std::vector<float>(0);
	std::vector<float> ctr_ys = std::vector<float>(0);
};

void BezierVisualizer::AddPoint()
{
	if (ImGui::IsMouseClicked(ImGuiMouseButton_Right))
	{
		auto pos = ImPlot::GetPlotMousePos();
		points.emplace_back(pos.x, pos.y);
		updated = true;
	}
}

void BezierVisualizer::DragPoint()
{
	for (int i = 0; i < points.size(); ++i)
	{
		auto& point = points[i];
		updated |= ImPlot::DragPoint(("Point " + std::to_string(i)).c_str(), &point.x(), &point.y());
	}
}

void BezierVisualizer::draw(bool* p_open)
{
	if (updated)
	{
		evaluate_bezier();
		updated = false;
	}
	if (ImGui::BeginTabBar("Homework 2")) {
		if (ImGui::BeginTabItem("Interpolation"))
		{
			if (ImPlot::BeginPlot("Line Plot", "x", "f(x)", ImGui::GetContentRegionAvail(), ImPlotFlags_NoBoxSelect | ImPlotFlags_NoMenus)) {
				ImPlot::PlotLine("Bezier", &xs[0], &ys[0], Length);
				if (!ctr_xs.empty())
				{
					ImPlot::PlotLine("ControlBox", &ctr_xs[0], &ctr_ys[0], ctr_xs.size());
				}
				AddPoint();
				DragPoint();

				ImPlot::EndPlot();
			}

			ImGui::EndTabItem();
		}

		ImGui::EndTabBar();
	}
	ImGui::End();
}

int main()
{
	BezierVisualizer visualizer;
	visualizer.RenderLoop();
}


/**
 * @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;
  }
}


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

#include <fcppt/math/matrix/componentwise_equal.hpp>
#include <fcppt/math/matrix/exponential_pade.hpp>
#include <fcppt/math/matrix/output.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <ostream>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(exponential_pade)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		double,
		3,
		3
	>
	matrix_type;

	matrix_type const t(
		fcppt::math::matrix::row(
			2.0, -1.0, 1.0
		),
		fcppt::math::matrix::row(
			0.0, 3.0, -1.0
		),
		fcppt::math::matrix::row(
			2.0, 1.0, 3.0
		)
	);

	double const epsilon =
		0.01;

	matrix_type const result(
	        fcppt::math::matrix::exponential_pade(
		        t
		)
	);

	std::cout
		<< result
		<< '\n';

	BOOST_CHECK(
		fcppt::math::matrix::componentwise_equal(
			result,
			matrix_type(
				fcppt::math::matrix::row(
					23.6045,-7.38906,23.6045
				),
				fcppt::math::matrix::row(
					-16.2155,14.7781,-23.6045
				),
				fcppt::math::matrix::row(
					30.9936,7.38906,30.9936
				)
			),
			epsilon
		)
	);
}


#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;
}


#include "../params.h"
#include "../sigmaplus_prover.h"
#include "../sigmaplus_verifier.h"

#include <boost/test/unit_test.hpp>

#include "../../test/fixtures.h"

BOOST_FIXTURE_TEST_SUITE(sigma_protocol_tests, ZerocoinTestingSetup200)

BOOST_AUTO_TEST_CASE(one_out_of_n)
{
    auto params = sigma::Params::get_default();
    std::size_t N = 16384;
    std::size_t n = params->get_n();
    std::size_t m = params->get_m();
    std::size_t index = 0;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for (std::size_t i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for (std::size_t i = 0; i < N; ++i) {
        if (i == index) {
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(uint64_t(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else {
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_CASE(one_out_of_n_batch)
{
    auto params = sigma::Params::get_default();
    const secp_primitives::Scalar zero(uint64_t(0));
    const std::size_t N = 16000; // n^m == 16384
    const std::size_t n = params->get_n();
    const std::size_t m = params->get_m();
    const std::vector<std::size_t> index = { 0, 1, 2, N - 1 };
    const std::vector<std::size_t> set_sizes = { N, N - 1, N - 1, 16 };
    const std::vector<secp_primitives::Scalar> serials = { zero, zero, zero, zero };
    const std::vector<bool> fPadding = { true, true, true, true };

    // Generators
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for (std::size_t i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g, h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;

    // All commitments
    for (std::size_t i = 0; i < N; ++i) {
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    // Known commitments
    std::vector<Scalar> r;
    r.resize(index.size());
    for (std::size_t i = 0; i < index.size(); i++) {
        r[i].randomize();
        commits[index[i]] = h_gens[0] * r[i];
    }

    // Build the proofs
    std::vector<sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement>> proofs;
    proofs.reserve(index.size());
    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    for (std::size_t i = 0; i < index.size(); i++) {
        sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);
        std::vector<secp_primitives::GroupElement> commits_(commits.begin() + N - set_sizes[i], commits.end());

        // Check commitment validity and prove
        BOOST_CHECK(h_gens[0] * r[i] == commits[index[i]]);
        BOOST_CHECK(h_gens[0] * r[i] == commits_[index[i] - (N - set_sizes[i])]);
        prover.proof(commits_, index[i] - (N - set_sizes[i]), r[i], true, proof);
        proofs.emplace_back(proof);

        // Test individual verification
        BOOST_CHECK(verifier.verify(commits_, proof, true));
        BOOST_CHECK(verifier.verify(commits, proof, true, set_sizes[i]));
    }

    // Test batch verification
    BOOST_CHECK(verifier.batch_verify(commits, serials, fPadding, set_sizes, proofs));

    // Invalidate the batch
    proofs[0] = proofs[1];
    BOOST_CHECK(!verifier.batch_verify(commits, serials, fPadding, set_sizes, proofs));
}

BOOST_AUTO_TEST_CASE(one_out_of_n_padding)
{
    auto params = sigma::Params::get_default();
    int N = 10000;
    int n = params->get_n();
    int m = params->get_m();
    int index = 9999;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(uint64_t(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }
    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    BOOST_CHECK(verifier.verify(commits, proof, true));

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proofNew(n, m);
    prover.proof(commits, 11111, r, true, proofNew);
    BOOST_CHECK(verifier.verify(commits, proofNew, true));
}

BOOST_AUTO_TEST_CASE(prove_and_verify_in_different_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        if(i == index){
            secp_primitives::GroupElement c;
            secp_primitives::Scalar zero(uint64_t(0));
            c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
            commits.push_back(c);

        }
        else{
            commits.push_back(secp_primitives::GroupElement());
            commits[i].randomize();
        }
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);

    // Add more commit
    secp_primitives::GroupElement c;
    secp_primitives::Scalar zero(uint64_t(0));
    c = sigma::SigmaPrimitives<secp_primitives::Scalar,secp_primitives::GroupElement>::commit(g, zero, h_gens[0], r);
    commits.push_back(c);

    BOOST_CHECK(!verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_CASE(prove_coin_out_of_index)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();

    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, commits.size(), r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_CASE(prove_coin_not_in_set)
{
    auto params = sigma::Params::get_default();
    int N = 16384;
    int n = params->get_n();
    int m = params->get_m();
    int index = 0;
    secp_primitives::GroupElement g;
    g.randomize();
    std::vector<secp_primitives::GroupElement> h_gens;
    h_gens.resize(n * m);
    for(int i = 0; i < n * m; ++i ){
        h_gens[i].randomize();
    }
    secp_primitives::Scalar r;
    r.randomize();
    sigma::SigmaPlusProver<secp_primitives::Scalar,secp_primitives::GroupElement> prover(g,h_gens, n, m);

    std::vector<secp_primitives::GroupElement> commits;
    for(int i = 0; i < N; ++i){
        commits.push_back(secp_primitives::GroupElement());
        commits[i].randomize();
    }

    sigma::SigmaPlusProof<secp_primitives::Scalar,secp_primitives::GroupElement> proof(n, m);

    prover.proof(commits, index, r, true, proof);

    sigma::SigmaPlusVerifier<secp_primitives::Scalar,secp_primitives::GroupElement> verifier(g, h_gens, n, m);
    BOOST_CHECK(!verifier.verify(commits, proof, true));
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <fstream>

const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t", "\n");

int main()
{
	// Initialization of vectors and matrices
    Eigen::VectorXd v(3);
	Eigen::MatrixXd matA(3,3);

	v << 0, 1, 2;
	matA << v, v, v;

	std::ofstream output;
	output.open("bin/Aufgabe1.txt", std::ofstream::out | std::ofstream::trunc);
	output << "Matrix matA:\n" << matA.format(CSVFormat) << std::endl;
	output.close();

    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])
        );
    }
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/digamma.hpp>

TEST(AgradRev,falling_factorial_var_double) {
  double a(2);
  AVAR b(4.0);
  AVAR f = stan::math::falling_factorial(b,a);
  EXPECT_FLOAT_EQ(12, f.val());

  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(0, g[0]);
  EXPECT_FLOAT_EQ((boost::math::digamma(5) - boost::math::digamma(3))
                  * 12.0, g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::falling_factorial(4.0 + eps, 2.0)
                  - stan::math::falling_factorial(4.0 - eps, 2.0))
                  / (2 * eps), g[1]);
}

TEST(AgradRev, falling_factorial_exceptions) {
  double a = 1;
  AVAR b(-3.0);
  EXPECT_THROW(stan::math::falling_factorial(b,a), std::domain_error);
  EXPECT_THROW(stan::math::falling_factorial(b,b), std::domain_error);
}

TEST(AgradRev, falling_factorial_double_var) {
  double a(5);
  AVAR b(4.0);
  AVAR f = stan::math::falling_factorial(a,b);
  EXPECT_FLOAT_EQ(120.0, f.val());
  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(0, g[0]);
  EXPECT_FLOAT_EQ(boost::math::digamma(2) * 120.0, g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::falling_factorial(5.0, 4.0 + eps)
                  - stan::math::falling_factorial(5.0, 4.0 - eps))
                  / (2 * eps), g[1]);
}
TEST(AgradRev, falling_factorial_var_var) {
  AVAR a(6.0);
  AVAR b(4.0);
  AVAR f = stan::math::falling_factorial(a,b);
  EXPECT_FLOAT_EQ(360.0, f.val());
  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ((boost::math::digamma(7) - boost::math::digamma(3))
                  * 360.0, g[0]);
  EXPECT_FLOAT_EQ(boost::math::digamma(3) * 360.0, g[1]);

  double eps = 1e-6;
  EXPECT_FLOAT_EQ((stan::math::falling_factorial(6.0 + eps, 4.0)
                  - stan::math::falling_factorial(6.0 - eps, 4.0))
                  / (2 * eps), g[0]);
  EXPECT_FLOAT_EQ((stan::math::falling_factorial(6.0, 4.0 + eps)
                  - stan::math::falling_factorial(6.0, 4.0 - eps))
                  / (2 * eps), g[1]);
}

struct falling_factorial_fun {
  template <typename T0, typename T1>
  inline
  typename stan::return_type<T0,T1>::type
  operator()(const T0& arg1,
             const T1& arg2) const {
    return falling_factorial(arg1,arg2);
  }
};

TEST(AgradRev, falling_factorial_nan) {
  falling_factorial_fun falling_factorial_;
  test_nan(falling_factorial_,4.0,1.0,false,true);

}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a(2);
  AVAR b(4.0);
  test::check_varis_on_stack(stan::math::falling_factorial(b,a));
  test::check_varis_on_stack(stan::math::falling_factorial(b,2));
  test::check_varis_on_stack(stan::math::falling_factorial(4,a));
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the kernel ridge regression
    object from the dlib C++ Library.

    This example creates a simple set of data to train on and then shows
    you how to use the kernel ridge regression tool to find a good decision
    function that can classify examples in our data set.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 13
    from the origin are labeled +1 and all other points are labeled
    as -1.  All together, the dataset will contain 10201 sample points.

*/

#include <iostream>
#include <dlib/svm.h>

using namespace std;
using namespace dlib;

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_krr_classification_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.   (Note that if you wanted
    // more than 2 features in this vector you can simply change the 2 to something else.
    // Or if you don't know how many features you want until runtime then you can put a 0
    // here and use the matrix.set_size() member function)
    typedef matrix<double, 2, 1> sample_type;

    // This is a typedef for the type of kernel we are going to use in this example.
    // In this case I have selected the radial basis kernel that can operate on our
    // 2D sample_type objects
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Now we make objects to contain our samples and their respective labels.
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Now let's put some data into our samples and labels objects.  We do this
    // by looping over a bunch of points and labeling them according to their
    // distance from the origin.
    for (double r = -20; r <= 20; r += 0.4)
    {
        for (double c = -20; c <= 20; c += 0.4)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 13 from the origin
            if (sqrt((double)r*r + c*c) <= 13)
                labels.push_back(+1);
            else
                labels.push_back(-1);

        }
    }

    cout << "samples generated: " << samples.size() << endl;
    cout << "  number of +1 samples: " << sum(mat(labels) > 0) << endl;
    cout << "  number of -1 samples: " << sum(mat(labels) < 0) << endl;

    // Here we normalize all the samples by subtracting their mean and dividing by their standard deviation.
    // This is generally a good idea since it often heads off numerical stability problems and also
    // prevents one large feature from smothering others.  Doing this doesn't matter much in this example
    // so I'm just doing this here so you can see an easy way to accomplish this with
    // the library.
    vector_normalizer<sample_type> normalizer;
    // let the normalizer learn the mean and standard deviation of the samples
    normalizer.train(samples);
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = normalizer(samples[i]);

    // here we make an instance of the krr_trainer object that uses our kernel type.
    krr_trainer<kernel_type> trainer;

    // The krr_trainer has the ability to perform leave-one-out cross-validation.
    // It does this to automatically determine the regularization parameter.  Since
    // we are performing classification instead of regression we should be sure to
    // call use_classification_loss_for_loo_cv().  This function tells it to measure
    // errors in terms of the number of classification mistakes instead of mean squared
    // error between decision function output values and labels.
    trainer.use_classification_loss_for_loo_cv();

    // Now we loop over some different gamma values to see how good they are.
    cout << "\ndoing leave-one-out cross-validation" << endl;
    for (double gamma = 0.000001; gamma <= 1; gamma *= 5)
    {
        // tell the trainer the parameters we want to use
        trainer.set_kernel(kernel_type(gamma));

        // loo_values will contain the LOO predictions for each sample.  In the case
        // of perfect prediction it will end up being a copy of labels.
        std::vector<double> loo_values;
        trainer.train(samples, labels, loo_values);

        // Print gamma and the fraction of samples correctly classified during LOO cross-validation.
        const double classification_accuracy = mean_sign_agreement(labels, loo_values);
        cout << "gamma: " << gamma << "     LOO accuracy: " << classification_accuracy << endl;
    }

    // From looking at the output of the above loop it turns out that a good value for
    // gamma for this problem is 0.000625.  So that is what we will use.
    trainer.set_kernel(kernel_type(0.000625));
    typedef decision_function<kernel_type> dec_funct_type;
    typedef normalized_function<dec_funct_type> funct_type;

    // Here we are making an instance of the normalized_function object.  This object provides a convenient
    // way to store the vector normalization information along with the decision function we are
    // going to learn.
    funct_type learned_function;
    learned_function.normalizer = normalizer;  // save normalization information
    learned_function.function = trainer.train(samples, labels); // perform the actual training and save the results

    // print out the number of basis vectors in the resulting decision function
    cout << "\nnumber of basis vectors in our learned_function is "
         << learned_function.function.basis_vectors.size() << endl;

    // Now let's try this decision_function on some samples we haven't seen before.
    // The decision function will return values >= 0 for samples it predicts
    // are in the +1 class and numbers < 0 for samples it predicts to be in the -1 class.
    sample_type sample;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    // We can also train a decision function that reports a well conditioned probability
    // instead of just a number > 0 for the +1 class and < 0 for the -1 class.  An example
    // of doing that follows:
    typedef probabilistic_decision_function<kernel_type> probabilistic_funct_type;
    typedef normalized_function<probabilistic_funct_type> pfunct_type;

    // The train_probabilistic_decision_function() is going to perform 3-fold cross-validation.
    // So it is important that the +1 and -1 samples be distributed uniformly across all the folds.
    // calling randomize_samples() will make sure that is the case.
    randomize_samples(samples, labels);

    pfunct_type learned_pfunct;
    learned_pfunct.normalizer = normalizer;
    learned_pfunct.function = train_probabilistic_decision_function(trainer, samples, labels, 3);
    // Now we have a function that returns the probability that a given sample is of the +1 class.

    // print out the number of basis vectors in the resulting decision function.
    // (it should be the same as in the one above)
    cout << "\nnumber of basis vectors in our learned_pfunct is "
         << learned_pfunct.function.decision_funct.basis_vectors.size() << endl;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    // Another thing that is worth knowing is that just about everything in dlib is serializable.
    // So for example, you can save the learned_pfunct object to disk and recall it later like so:
    serialize("saved_function.dat") << learned_pfunct;

    // Now let's open that file back up and load the function object it contains.
    deserialize("saved_function.dat") >> learned_pfunct;

}


#include <ctime>
#include <gtest/gtest.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include "slick/math/so2.h"
#include "slick/test/unittest/util.h"

using namespace std;
using namespace Eigen;
using namespace slick;

// constructors tests ==========================================================
template <typename T>
void DefaultConstructor_Test() {
  // for double precision
  SO2Group<T> rot;
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), rot.get_matrix(),
                     Gap<T>());
}

TEST(SO2Test, DefaultConstructor) {
  DefaultConstructor_Test<double>();
  DefaultConstructor_Test<float>();
}

template <typename T>
void FromMatrixConstructor_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);
  SO2Group<T> rot(mrot);
  EXPECT_MATRIX_NEAR(mrot, rot.get_matrix(), Gap<T>());
}

TEST(SO2Test, FromMatrixConstructor) {
  FromMatrixConstructor_Test<double>();
  FromMatrixConstructor_Test<float>();
}

template <typename T>
void FromAngleConstructor_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);
  SO2Group<T> rot(angle);
  EXPECT_MATRIX_NEAR(mrot, rot.get_matrix(), Gap<T>());
}

TEST(SO2Test, FromAngleConstructor) {
  FromAngleConstructor_Test<double>();
  FromAngleConstructor_Test<float>();
}

template <typename T>
void FromListInitializerConstructor_Test() {
  SO2Group<T> rot{1, 0, 0, 1};
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), rot.get_matrix(),
                     Gap<T>());

  EXPECT_NEAR(0.0, rot.ln(), Gap<T>());
}

TEST(SO2Test, FromListInitializerConstructor) {
  FromListInitializerConstructor_Test<double>();
  FromListInitializerConstructor_Test<float>();
}

// so2 specific functions ======================================================
template <typename T>
void Ln_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand()) / T(RAND_MAX) * 2 * M_PI;
  SO2Group<T> rot(angle);
  auto est = rot.ln();
  est = (est > 0) ? est : 2 * M_PI + est;
  EXPECT_NEAR(angle, est, Gap<T>());
}

TEST(SO2Test, Ln) {
  Ln_Test<double>();
  Ln_Test<float>();
}

template <typename T>
void Inverse_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());
  SO2Group<T> rot(angle);
  Eigen::Matrix<T, 2, 2> m_inv = rot.get_matrix().inverse();
  EXPECT_MATRIX_NEAR(m_inv, rot.inverse().get_matrix(), Gap<T>());
}

TEST(SO2Test, Inverse) {
  Inverse_Test<double>();
  Inverse_Test<float>();
}

template <typename T>
void SO2RightHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot1;
  mrot1 = Eigen::Rotation2D<T>(angle);
  SO2Group<T> rot1(angle);

  std::srand(std::time(0));  // use current time as seed for random generator
  angle = T(std::rand());
  Eigen::Matrix<T, 2, 2> mrot2;
  mrot2 = Eigen::Rotation2D<T>(angle);
  SO2Group<T> rot2(angle);

  SO2Group<T> rot(2.0);
  EXPECT_MATRIX_NEAR(mrot1 * mrot2, (rot1 * rot2).get_matrix(), Gap<T>());
}

TEST(SO2Test, SO2RightHandMulOperator) {
  SO2RightHandMulOperator_Test<double>();
  SO2RightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixRightHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);

  SO2Group<T> rot(angle);

  Eigen::Matrix<T, 2, 2> mrand = Eigen::Matrix<T, 2, 2>::Random();
  EXPECT_MATRIX_NEAR(mrot * mrand, rot * mrand, Gap<T>());
}

TEST(SO2Test, MatrixRightHandMulOperator) {
  MatrixRightHandMulOperator_Test<double>();
  MatrixRightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixLeftHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);

  SO2Group<T> rot(angle);

  Eigen::Matrix<T, 2, 2> mrand = Eigen::Matrix<T, 2, 2>::Random();
  EXPECT_MATRIX_NEAR(mrand * mrot, mrand * rot, Gap<T>());
}

TEST(SO2Test, MatrixLeftHandMulOperator) {
  MatrixLeftHandMulOperator_Test<double>();
  MatrixLeftHandMulOperator_Test<float>();
}

int main(int argc, char** argv) {
  std::vector<char*> vars(argc+1);
  for (int i = 0; i < argc; ++i)
    vars[i] = argv[i];
  char ca[50];
  sprintf(ca, "--gtest_repeat=%d", NSAMPLES);
  vars[argc] = ca;
  argc++;
  ::testing::InitGoogleTest(&argc, &vars.front());
  return RUN_ALL_TESTS();
}


/*************************************************************************
	> 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);
}