//
// Created by Vlad Argunov on 03/01/2022.
//

#include "Simulation.h"

#include <Eigen/Dense>
#include <random>
#include <cmath>
#include <tuple>

Eigen::VectorXd Simulation::generate_uniform_variable(int n) {
        std::random_device rd;
        std::mt19937 gen(rd());
        Eigen::VectorXd uniform_numbers;
        uniform_numbers.resize(n);
        std::uniform_real_distribution<double> uniform(0.0, 1.0);
        for (int i = 0; i < n; ++i) {
            uniform_numbers(i) = uniform(gen);
        }
        return uniform_numbers;
}

Eigen::VectorXd Simulation::generate_normal_variable(int n, double mu, double sigma) {
    std::random_device rd;
    std::mt19937 gen(rd());
    Eigen::VectorXd normal_numbers;
    normal_numbers.resize(n);
    std::normal_distribution<> normal{mu,sigma};
    for (int i = 0; i < n; ++i) {
        normal_numbers(i) = normal(gen);
    }
    return normal_numbers;
}

Eigen::MatrixXd Simulation::generate_gbm(int n_processes, double tn, double stock_price, double mu, double sigma, int n_steps) {
//    Generates the matrix of Geometric brownian motions for each row and a time step for each column
    double time_step = tn / n_steps;
    Eigen::MatrixXd simulated_processes;
    Eigen::Index n_rows = n_processes;
    Eigen::Index n_cols = n_steps + 1;
    simulated_processes.resize(n_rows, n_cols);
    simulated_processes.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, stock_price);

    Eigen::VectorXd normal_rv;
    normal_rv.resize(n_processes);
    for (int col = 0; col < n_cols - 1; ++col) {
        normal_rv = generate_normal_variable(n_processes, 0, 1);
        for (int row = 0; row < n_rows; ++row) {
            simulated_processes(row, col + 1) = simulated_processes(row, col)
                    * exp(mu - 0.5 * sigma * sigma * time_step + sigma * sqrt(time_step) * normal_rv(row) );
        }

    }

    return simulated_processes;
}

std::tuple<Eigen::MatrixXd , Eigen::MatrixXd>
Simulation::generate_heston_process(int n_processes, double tn, double stock_price, double mu, double variance,
                                    double kappa, double theta, double sigma, double rho, int n_steps) {
    //    Generates the matrix of Heston processes for each row and a time step for each column
    // Here variance is the starting value_matrix of the volatility, theta is the long-term level,
    // kappa is the speed of convergence, and sigma is the volatility of volatility
    double time_step = tn / n_steps;
    Eigen::MatrixXd simulated_stock_processes, simulated_var_process;
    Eigen::Index n_rows = n_processes;
    Eigen::Index n_cols = n_steps + 1;

    simulated_stock_processes.resize(n_rows, n_cols);
    simulated_var_process.resize(n_rows, n_cols);

    simulated_stock_processes.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, stock_price);
    simulated_var_process.leftCols(1) = Eigen::MatrixXd::Constant(n_rows, 1, variance);

    Eigen::VectorXd normal_rv1, normal_rv2;
    normal_rv1.resize(n_processes);
    normal_rv2.resize(n_processes);
    for (int col = 0; col < n_cols - 1; ++col) {
        normal_rv1 = generate_normal_variable(n_processes, 0, 1);
        for (int row = 0; row < n_rows; ++row) {
            simulated_var_process(row, col + 1) = simulated_var_process(row, col) +
                                                  kappa * (theta - simulated_var_process(row, col)) * time_step
                                                  + sigma * sqrt(simulated_var_process(row, col) * time_step)
                                                  * (rho * normal_rv1(row) + sqrt(1 - rho * rho) * normal_rv2(row));

            simulated_stock_processes(row, col + 1) = simulated_stock_processes(row, col) * (1 + mu * time_step)
                    + simulated_stock_processes(row, col) * sqrt(simulated_var_process(row, col) * time_step) * normal_rv1(row);
        }

    }
    return {simulated_stock_processes, simulated_var_process};
}


#include <iostream>

#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"

struct Parabola {
    double value(const Eigen::VectorXd& x)
    {
      return x.dot(x);
    }

    Eigen::VectorXd gradient(const Eigen::VectorXd& x)
    {
      return 2 * x;
    }

    double initial_step_length(const Eigen::VectorXd& x, const Eigen::VectorXd& p)
    {
      double max_change = p.array().abs().maxCoeff();

      return 0.2 / max_change;
    }

    double change_acceptable(const Eigen::VectorXd& x, const Eigen::VectorXd& xp) {
      return (xp - x).array().abs().maxCoeff() - (0.2 + 1e-6);
    }
};

int main()
{
  Parabola parabola;
  Eigen::VectorXd x0 = Eigen::VectorXd::Constant(2, 1.0);
  LSLOpt::OptimizationParameters<double> params
      = LSLOpt::getOptimizationParameters<double>();
  LSLOpt::OstreamOutput output{LSLOpt::OutputLevel::Status, std::cerr};

  auto result = LSLOpt::lsl_bfgs(parabola, x0, params, output);

  std::cout << "STATUS " << result.status << std::endl;
  std::cout << "F      " << result.function_value << std::endl;
  std::cout << "X      " << result.x << std::endl;

  return 0;
}


#include "NMatrixOperations.h"

#include <Eigen/Dense>
#include <stdexcept>
#include <assert.h>
#include <string.h>
#include <vector>
#include <QDebug>

#define print(something) {std::stringstream ss; ss << something; qDebug() << ss.str().c_str(); }

// This was based on https://ef.gy/linear-algebra:normal-vectors-in-higher-dimensional-spaces
Eigen::VectorXd getNormalVector(Eigen::MatrixXd vectors) {
	assert(vectors.rows() == vectors.cols() + 1);
	const unsigned int N = vectors.rows();

	Eigen::MatrixXd pM = vectors.transpose();
	pM.conservativeResize(pM.rows() + 1, Eigen::NoChange);
	pM.row(N - 1) = Eigen::VectorXd::Zero(N);
	Eigen::MatrixXd baseVectors = Eigen::MatrixXd::Identity(N, N);

	Eigen::VectorXd result = Eigen::VectorXd::Zero(N);

	int signal = 1;
	for (unsigned int i = 0; i < N; i++) {
		Eigen::MatrixXd pS(N - 1, N - 1);

		for (unsigned int j = 0; j < (N - 1); j++) {
			pS.block(j, 0, 1, i) = pM.block(j, 0, 1, i);
			pS.block(j, i, 1, N - i - 1) = pM.block(j, i + 1, 1, N - i - 1);
		}

		result += signal * baseVectors.row(i) * pS.determinant();
		signal *= -1;
	}

	return result;
}

Eigen::MatrixXd translateMatrixN(int N, Eigen::VectorXd point) {
	assert(N > 0);

	Eigen::MatrixXd m = Eigen::MatrixXd::Identity(N + 1, N + 1);
	m.topRightCorner(N, 1) = point;
	return m;
}

// This was based on https://ef.gy/linear-algebra:perspective-projections
// However, he makes this in a different way than every other LookAt I could find
// Therefor it has been silightly adjusted to conform to the others
Eigen::MatrixXd lookAtMatrixN(const int N, Eigen::VectorXd from, Eigen::VectorXd to, Eigen::MatrixXd ups) {
	assert(N > 2);

	//print("LookAt Double Calculation:");
	//print("FROM:");
	//print(from.transpose());
	//print("TO:");
	//print(to.transpose());
	//print("UPS");
	//print(ups);

	Eigen::MatrixXd m(N, N);

	m.rightCols(1) = (to - from).normalized();

	int numLoops = 0;
	for (int currentColumn = N - 2; currentColumn > 0; currentColumn--) {
		Eigen::MatrixXd vectorsToCross(N, N - 1);
		int currentColumnOnVectorsToCross = 1;

		//First, cross product all ups, in order
		vectorsToCross.col(0) = ups.col(numLoops);
		for (int i = 1; i < currentColumn; i++) {
			vectorsToCross.col(currentColumnOnVectorsToCross) = ups.col(numLoops + i);
			currentColumnOnVectorsToCross++;
		}

		numLoops++;
		for (int i = 0; i < numLoops; i++) {
			vectorsToCross.col(currentColumnOnVectorsToCross) = m.col(currentColumn + i + 1);
			currentColumnOnVectorsToCross++;
		}

		//print("vectorsToCross\n" << vectorsToCross << "\n");

		auto normal = getNormalVector(vectorsToCross);
		//print("Normal:\n" << normal << "\n");
		//print("Normal Normalized:\n" << normal.normalized() << "\n");

		m.col(currentColumn) = normal.normalized();

		//print("Current Matrix:\n" << m << "\n");
	}

	m.col(0) = getNormalVector(m.rightCols(N - 1)).normalized();

	//print("m");
	//print(m);

	Eigen::MatrixXd temp = Eigen::MatrixXd::Identity(N + 1, N + 1);
	//m.leftCols(N - 1).rowwise().reverseInPlace();
	temp.topLeftCorner(N, N) = m;

	return temp.transpose();
}

Eigen::MatrixXd perspectiveMatrixN(const int N, double eye_radians_angle, double nearPlane, double farPlane, double aspectRatio) {
	assert(N > 2);

	if (N == 3) {
		Eigen::MatrixXd m = Eigen::MatrixXd::Zero(N + 1, N + 1);

		double f_tan = 1 / tan(eye_radians_angle / 2);
		m(0, 0) = f_tan / aspectRatio;
		m(1, 1) = f_tan;
		m(2, 2) = (nearPlane + farPlane) / (nearPlane - farPlane);
		m(2, 3) = -1.f;
		m(3, 2) = 2 * (nearPlane*farPlane) / (nearPlane - farPlane);

		return m.transpose();
	}
	else {
		Eigen::MatrixXd m = Eigen::MatrixXd::Identity(N + 1, N + 1);

		double f_tan = 1 / tan(eye_radians_angle / 2);
		m = m*f_tan;
		m(N - 1, N - 1) = 1;
		m(N, N) = 1;

		return m;
	}
}

// N > 3
Eigen::MatrixXd viewMatrixN(
	const int N,
	Eigen::VectorXd from,
	Eigen::VectorXd to,
	Eigen::MatrixXd ups,
	double eyeRadiansAngle,
	double nearPlane,
	double farPlane,
	double aspectRatio) {

	auto tr = translateMatrixN(N, -from);

	auto la = lookAtMatrixN(N, from, to, ups);

	auto pm = perspectiveMatrixN(N, eyeRadiansAngle, nearPlane, farPlane, aspectRatio);

	Eigen::MatrixXd result = pm * la * tr;

	//Axis direction correction
	Eigen::MatrixXd aux = Eigen::MatrixXd::Identity(N + 1, N + 1);
	//X correction
	aux(0, 0) = -1;
	//Z correction
	aux(2, 2) = N != 4 ? -1 : 1;
	result = aux * result;

	//Z correction

	return result;
}

//This assumes that point is an N+1 dimensional vector and that point(N) == 1
Eigen::VectorXd projectPointLosingDimension(Eigen::VectorXd point, Eigen::MatrixXd m) {
	//std::cout << "point\n" << point << "\n";

	Eigen::VectorXd pointWith1(point.rows() + 1);
	pointWith1.topRightCorner(point.rows(), 1) = point;
	pointWith1(point.rows()) = 1;

	//std::cout << "pointWith1\n" << pointWith1 << "\n";

	Eigen::VectorXd v = m * pointWith1;
	//std::cout << "v\n" << v << "\n";
	v = v / v(v.rows() - 2, 0);
	//std::cout << "v\n" << v << "\n";
	auto result = v.topLeftCorner(v.rows() - 1, 1);
	//std::cout << "result\n" << result << "\n";

	return result;
}

//This assumes that each point is an N+1 dimensional column in a matrix and that point(N) == 1
Eigen::MatrixXd projectPointsLosingDimension(Eigen::MatrixXd points, Eigen::MatrixXd m, bool usePerspective) {
	//std::cout << "point\n" << points << "\n";

	Eigen::MatrixXd pointWith1(points.rows() + 1, points.cols());
	pointWith1.topRightCorner(points.rows(), points.cols()) = points;
	pointWith1.row(points.rows()) = Eigen::VectorXd::Ones(points.cols());

	//std::cout << "pointWith1\n" << pointWith1 << "\n";

	Eigen::MatrixXd v = m * pointWith1;
	//std::cout << "v\n" << v << "\n";

	if (usePerspective) {
		for (int i = 0; i < v.cols(); i++) {
			if (v.col(i)(v.rows() - 2, 0) == 0 || v.col(i)(v.rows() - 2, 0) == -0) {
				v.col(i) = v.col(i) / (0.000001);
			} else {
				v.col(i) = v.col(i) / v.col(i)(v.rows() - 2, 0);
			}
		}
	}else{
		for (int i = 0; i < v.cols(); i++) {
			if (v.col(i)(v.rows() - 1, 0) == 0 || v.col(i)(v.rows() - 1, 0) == -0) {
				v.col(i) = v.col(i) / (0.000001);
			}
			else {
				v.col(i) = v.col(i) / v.col(i)(v.rows() - 1, 0);
			}
		}
	}

	Eigen::MatrixXd result = v.topRows(v.rows() - 2);

	return result;
}

Eigen::MatrixXd rotateMatrixN(const unsigned int N, unsigned int axis1, unsigned int axis2, double radians_angle) {
	assert(axis1 != axis2);

	Eigen::MatrixXd rot_aa = Eigen::MatrixXd::Identity(N, N);

	rot_aa(axis1, axis1) = cos(radians_angle);
	rot_aa(axis1, axis2) = sin(radians_angle);
	rot_aa(axis2, axis1) = -sin(radians_angle);
	rot_aa(axis2, axis2) = cos(radians_angle);

	return rot_aa.transpose();
}

std::string generateNDimensionalShader(int n) {
	std::stringstream ss;
	ss << "#version 330 core\nlayout (location = 0) in double[" << n << "] position;\n";

	for (int i = n; i >= 3; i--) {
		ss << "uniform double m" << i << "[" << ((i + 1) * (i + 1)) << "];\n";
	}

	ss << "\nvoid main() {\n";

	ss << "	double[" << (n + 1) << "] positionWithOne;\n";
	ss << "	for (int i = 0; i < " << n << "; i++){\n";
	ss << "		positionWithOne[i] = position[i];\n";
	ss << "	}\n";
	ss << "	positionWithOne[" << n << "] = 1;\n\n";

	ss << "	double[" << (n + 1) << "] newPos" << n << ";\n";

	for (int i = n; i >= 3; i--) {
		ss << "	for (int i = 0; i <= " << i << "; i++) {\n";
		ss << "		double newVal = 0;\n";
		ss << "		for (int j = 0; j <= " << i << "; j++){\n";
		ss << "			newVal += m" << i << "[j * " << i << " + i] * positionWithOne[j];\n";
		ss << "		}\n";
		ss << "		newPos" << i << "[i] = newVal;\n";
		ss << "	}\n";
		ss << "	\n";
		if (i != 3) {
			ss << "	for (int i = 0; i < " << i << "; i++) {\n";
			ss << "		positionWithOne[i] = newPos" << i << "[i] / newPos" << i << "[" << (i - 1) << "];\n";
			ss << "	}\n";
			ss << "	\n";
			ss << "	double[" << i << "] newPos" << i - 1 << ";\n";
		}
	}

	ss << "	gl_Position.x = newPos3[0]/newPos3[3];\n";
	ss << "	gl_Position.y = newPos3[1]/newPos3[3];\n";
	ss << "	gl_Position.z = newPos3[2]/newPos3[3];\n";
	ss << "	gl_Position.w = newPos3[3]/newPos3[3];\n";
	ss << "}\n";

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

Eigen::MatrixXd makeVectorsHomogeneous(Eigen::MatrixXd points) {
	points.conservativeResize(points.rows() + 1, Eigen::NoChange);
	points.row(points.rows() - 1) = Eigen::VectorXd::Ones(points.cols());

	return points;
}

Eigen::MatrixXd makeMatrixHomogeneous(Eigen::MatrixXd matrix) {
	matrix.conservativeResize(matrix.rows() + 1, matrix.cols() + 1);
	matrix.row(matrix.rows() - 1) = Eigen::VectorXd::Zero(matrix.cols());
	matrix.col(matrix.cols() - 1) = Eigen::VectorXd::Zero(matrix.rows());
	matrix(matrix.rows() - 1, matrix.cols() - 1) = 1;

	return matrix;
}

#define print(something) {std::stringstream ss; ss << something; qDebug() << ss.str().c_str(); }

Eigen::MatrixXd rotateShapeToLowerDimension(Eigen::MatrixXd m) {
	const int D = m.rows();

	// First we move the first vertex of the shape to the origin
	m = translateMatrixN(D, -m.col(0).cast<double>()).cast<double>() * makeVectorsHomogeneous(m);
	m.conservativeResize(m.rows() - 1, Eigen::NoChange);

	//Then, we find a new base created from the points of the polytope
	//And also a vector that is simply the last dimension
	Eigen::MatrixXd newBase = Eigen::MatrixXd::Zero(D, D);
	int currentSizeBase = 1;;
	{
		Eigen::VectorXd aux = Eigen::VectorXd::Zero(D);
		aux(D - 1) = 1;
		newBase.col(0) = aux;

		for (int i = 0; i < m.cols() && currentSizeBase < 2; i++) {
			for (int j = i + 1; j < m.cols() && currentSizeBase < 2; j++) {
				Eigen::VectorXd newVectorOfBase = m.col(j) - m.col(i);
				newVectorOfBase.normalize();
				if (newVectorOfBase(0) < 0) {
					newVectorOfBase = newVectorOfBase * -1;
				}

				if (newVectorOfBase != newBase.col(0)) {
					newBase.col(currentSizeBase) = newVectorOfBase;
					currentSizeBase++;
				}

			}
		}
	}

	for (int i = 0; i < m.cols() && currentSizeBase < D; i++) {
		for (int j = i + 1; j < m.cols() && currentSizeBase < D; j++) {
			Eigen::VectorXd newVectorOfBase = m.col(j) - m.col(i);
			newVectorOfBase.normalize();
			if (newVectorOfBase(0) < 0) {
				newVectorOfBase = newVectorOfBase * -1;
			}

			Eigen::VectorXd x = newBase.colPivHouseholderQr().solve(newVectorOfBase);

			Eigen::VectorXd check = (newBase * x) - newVectorOfBase;
			double error = 0;
			for (int k = 0; k < x.size(); k++) {
				error += check(k) * check(k);
			}

			if (error > 1e-5f) {
				newBase.col(currentSizeBase) = newVectorOfBase;
				currentSizeBase++;
			}
		}
	}

	if (currentSizeBase != newBase.cols()) {
		for (int i = 0; i < newBase.rows(); i++) {
			if (newBase.row(i).isZero(1e-10)) {
				m.row(i) = m.row(m.rows() - 1);
				return m.topRows(D - 1);
			}
		}
	} else {
		//We set the coordinate we want to exclude as the last one
		for (int i = 0; i < D - 1; i++) {
			Eigen::VectorXd aux = newBase.col(i);
			newBase.col(i) = newBase.col(i + 1);
			newBase.col(i + 1) = aux;
		}

		//We change the base
		Eigen::MatrixXd transformationMatrix = newBase.inverse();
		Eigen::MatrixXd newVertices = transformationMatrix * m;

		//Return only the top rows, excluding the last one
		return newVertices.topRows(D - 1);
	}

	return Eigen::MatrixXd(0, 0);
}


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

#include <iostream>
#include <cstdlib>
#include <cstring>
#include <random>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

extern "C"
{
	double randValue(double max, double min) {
			return min + ((rand() / (double)RAND_MAX) * (max - min));
	}

	__declspec(dllexport) void cleanModel(double * model)
	{
		free(model);
	}

	__declspec(dllexport) double * lineaire_model(int nbInput)
	{
		double * model = (double *)malloc((nbInput + 1) * sizeof(double));
		model[0] = 1;

		for (int i = 1; i <= nbInput; ++i)
		{
			model[i] = randValue(1, -1);
		}
		return model;
	}

	__declspec(dllexport) double classifyRegression(double* model, double* input, int inputSize)
	{
		double sum = model[0];
		for (int i = 0; i < inputSize; ++i)
		{
			sum += model[i + 1] * input[i];
		}
		return sum;
	}

	__declspec(dllexport) void regression(double* model, double* valueSend, int size, int inputSize, double* waitValue)
	{
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> entry(size, inputSize + 1);
		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> waited(size, 1);

		for (int i = 0; i < size; ++i)
		{
			waited(i, 0) = *waitValue;
			++waitValue;
		}

		for (int i = 0; i < size; ++i)
		{
			entry(i, 0) = 1.0;
			for (int j = 1; j < inputSize + 1; ++j)
			{
				entry(i, j) = *valueSend;
				++valueSend;
			}
		}

		Eigen::MatrixXd transposedMatrix = entry.transpose();
		Eigen::MatrixXd inverseMatrix = (transposedMatrix*entry).inverse();
		Eigen::MatrixXd final = inverseMatrix*transposedMatrix;
		Eigen::MatrixXd weight = final*waited;

		for (int i = 0; i < weight.rows(); ++i)
		{
			*model = weight(i, 0);
			++model;
		}
	}
}


#include<iostream>
#include<vector>
#include<fstream>
#include <boost/math/special_functions/gamma.hpp>

const int X=0;
const int O2=1;
const int G=2;
const int Xy=3;
const int A=4;
const int B=5;

void get_rhs(std::vector<double>& rhs,std::vector<double> solnvec,double t,int nvars)
{
  // ------- MODEL CONSTANTS -------
  double y_xs = 0.009;
  double y_as = 1.01;
  double y_bs = 0.88;
  double y_os = 0.0467;

  double x_max = 11;
  double qs_max = 17;
  double o2_max = 0.214;

  double K_e = 0.0214;
  double K_s = 31;

  double alpha_s = 3;
  double beta_s = 12;
  double alpha_e = 1;
  double beta_e = 1e3;

  double kLa = 5;

  // calculate chi_i
  // A word of explanation about the form of the gamma incomplete call. This function
  // is usually expressed as g(a, x), where a and x are obligate positive. Therefore, we need
  // to restrict x to be no lower than 0. Additionally, we need to make sure we don't hit a
  // divide by zero error, so we condition use of the gamma function on the denominator being
  // non-zero.
  double chi_s = 1;
  double chi_e = 1;

  if (solnvec[Xy]>1e-8) {
    double sRatio = std::max(solnvec[G]/(solnvec[Xy]), 0.0);
    chi_s = boost::math::gamma_p(alpha_s, beta_s*sRatio);
  }

  if (solnvec[A]>1e-8) {
    double eRatio = std::max(solnvec[O2]/(solnvec[A]), 0.0);
    chi_e = boost::math::gamma_p(alpha_e, beta_e*eRatio);
  }

  double chi_p = 0.3;

  // calculate q_s
  double F_s = (solnvec[G] + solnvec[Xy])/(solnvec[G] + solnvec[Xy] + K_s);
  double F_e = (solnvec[O2] + solnvec[A]/beta_e)/(solnvec[O2] + solnvec[A]/beta_e + K_e);
  double q_s = qs_max*F_s*F_e;

  // calculate intermediate rates
  double rar = chi_p*y_as*q_s*solnvec[X];
  double rbr = (1-chi_p)*y_bs*q_s*solnvec[X];

  double our = -chi_e*y_os*q_s*solnvec[X];
  double otr = kLa *(o2_max - solnvec[O2]);
  double rae = -(1-chi_e)*y_as*q_s*solnvec[X];
  double rbe = -rae;

  // calculate final rates
  rhs[X] = y_xs*q_s*solnvec[X]*(1 - solnvec[X]/x_max);
  //rhs[O2] = our+otr;
  rhs[02] = 0;
  rhs[G] = -chi_s     *q_s*solnvec[X];
  rhs[Xy] = -(1-chi_s) *q_s*solnvec[X];
  rhs[A] = rar+rae;
  rhs[B] = rbr+rbe;

}

double get_our(std::vector<double> solnvec,double t,int nvars)
{
  // ------- MODEL CONSTANTS -------
  double y_os = 0.0467;

  double x_max = 11;
  double qs_max = 17;
  double o2_max = 0.214;

  double K_e = 0.0214;
  double K_s = 31;

  double alpha_s = 3;
  double beta_s = 12;
  double alpha_e = 1;
  double beta_e = 1e3;

  // calculate chi_i
  double chi_s = 1;
  double chi_e = 1;

  if (solnvec[Xy]>1e-8) {
    double sRatio = std::max(solnvec[G]/(solnvec[Xy]), 0.0);
    chi_s = boost::math::gamma_p(alpha_s, beta_s*sRatio);
  }

  if (solnvec[A]>1e-8) {
    double eRatio = std::max(solnvec[O2]/(solnvec[A]), 0.0);
    chi_e = boost::math::gamma_p(alpha_e, beta_e*eRatio);
  }

  double chi_p = 0.3;

  // calculate q_s
  double F_s = (solnvec[G] + solnvec[Xy])/(solnvec[G] + solnvec[Xy] + K_s);
  double F_e = (solnvec[O2] + solnvec[A]/beta_e)/(solnvec[O2] + solnvec[A]/beta_e + K_e);
  double q_s = qs_max*F_s*F_e;

  double our = -chi_e*y_os*q_s*solnvec[X];

  return our;
}

void advance(std::vector<double>& solnvec,int nvars,double t_now,double t_adv,double dt)
{
    double current_time=t_now;
    double final_time=t_now+t_adv;

    std::vector<double> rhs(nvars);
    std::vector<double> solnvec_n(nvars);

    while(current_time < final_time)
    {
        current_time += dt;

        //at current time level n
        solnvec_n=solnvec;

        //Doing RK23

        //stage 1
        get_rhs(rhs,solnvec,current_time,nvars);
        for(int i=0;i<nvars;i++)
        {
            solnvec[i] = solnvec_n[i] + 0.5*rhs[i]*dt;
        }

        //stage 2
        get_rhs(rhs,solnvec,current_time,nvars);
        for(int i=0;i<nvars;i++)
        {
            solnvec[i] = solnvec_n[i] + rhs[i]*dt;
        }
    }
}

int main()
{
    int nvars=6;
    std::vector<double> solnvec(nvars);

    double final_time=24.0;
    double advance_time=0.5;
    double current_time=0.0;
    double dt=advance_time/50.0;

    std::ofstream outfile("timehist.dat");

    //set initial conditions
    solnvec[X]  = 0.5;
    solnvec[O2] = 0.214;
    solnvec[G]  = 500.0;
    solnvec[Xy] = 250.0;
    solnvec[A]  = 0.0;
    solnvec[B]  = 0.0;

    //write initial condition
    outfile<<current_time<<"\t";
    for(int i=0;i<nvars;i++)
    {
        outfile<<solnvec[i]<<"\t";
    }
    outfile<<"\n";

    while(current_time < final_time)
    {
        current_time += advance_time;

        advance(solnvec,nvars,current_time,advance_time,dt);

        outfile<<current_time<<"\t";
        for(int i=0;i<nvars;i++)
        {
            outfile<<solnvec[i]<<"\t";
        }
        outfile<<"\n";
    }

    outfile.close();
    return(0);
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "xregPairedPointRegi3D3D.h"

#include <Eigen/Eigenvalues>

#include "xregAssert.h"
#include "xregRotUtils.h"
#include "xregPointCloudUtils.h"
#include "xregLandmarkMapUtils.h"

xreg::FrameTransform
xreg::PairedPointRegi3D3D(const Pt3List& pts1, const Pt3List& pts2, const CoordScalar scale)
{
  const size_type num_pts = pts1.size();
  xregASSERT(num_pts == pts2.size());

  FrameTransform xform = FrameTransform::Identity();

  // These calls are threaded
  const Pt3 cent1 = ComputeCentroid(pts1);
  const Pt3 cent2 = ComputeCentroid(pts2);

  // These calls are threaded
  const Pt3List pts1_prime = OffsetPoints(-cent1, pts1);
  const Pt3List pts2_prime = OffsetPoints(-cent2, pts2);

  CoordScalar s = scale;
  if (s <= 0)
  {
    s = 0;

    const CoordScalar s1 = SumOfNormsSquared(pts1_prime);  // threaded

    if (std::abs(s1) > 1.0e-6)
    {
      const CoordScalar s2 = SumOfNormsSquared(pts2_prime);  // threaded

      s = std::sqrt(s2 / s1);
    }
  }

  CoordScalar S[9];

  CoordScalar* S_dst = S;
  for (size_type i = 0; i < 3; ++i)
  {
    for (size_type j = 0; j < 3; ++j, ++S_dst)
    {
      // This call is threaded
      *S_dst = InnerProductAboutDimsOfPts(pts1_prime, pts2_prime, i, j);
    }
  }

  const CoordScalar S_xx = S[0];
  const CoordScalar S_xy = S[1];
  const CoordScalar S_xz = S[2];
  const CoordScalar S_yx = S[3];
  const CoordScalar S_yy = S[4];
  const CoordScalar S_yz = S[5];
  const CoordScalar S_zx = S[6];
  const CoordScalar S_zy = S[7];
  const CoordScalar S_zz = S[8];

  Mat4x4 N_matrix;
  N_matrix(0,0) = S_xx + S_yy + S_zz;
  N_matrix(0,1) = S_yz - S_zy;
  N_matrix(0,2) = S_zx - S_xz;
  N_matrix(0,3) = S_xy - S_yx;
  N_matrix(1,0) = N_matrix(0,1);
  N_matrix(1,1) = S_xx - S_yy - S_zz;
  N_matrix(1,2) = S_xy + S_yx;
  N_matrix(1,3) = S_zx + S_xz;
  N_matrix(2,0) = N_matrix(0,2);
  N_matrix(2,1) = N_matrix(1,2);
  N_matrix(2,2) = -S_xx + S_yy - S_zz;
  N_matrix(2,3) = S_yz + S_zy;
  N_matrix(3,0) = N_matrix(0,3);
  N_matrix(3,1) = N_matrix(1,3);
  N_matrix(3,2) = N_matrix(2,3);
  N_matrix(3,3) = -S_xx - S_yy + S_zz;

  Eigen::EigenSolver<Mat4x4> eig_dec(N_matrix, true);

  size_type max_index = 0;
  eig_dec.eigenvalues().real().maxCoeff(&max_index);
  xform.linear().matrix() = QuatToRotMat(eig_dec.eigenvectors().col(max_index).real());

  xform.linear().matrix() *= s;

  xform.matrix().block(0,3,3,1) = cent2 - (xform.linear() * cent1);

  return xform;
}

xreg::FrameTransform
xreg::PairedPointRegi3D3D(const LandMap3& pts1, const LandMap3& pts2, const CoordScalar scale)
{
  const auto corr_lists = CreateCorrespondencePointLists(pts1, pts2);

  return PairedPointRegi3D3D(std::get<0>(corr_lists), std::get<1>(corr_lists), scale);
}


//  (C) Copyright Nick Thompson 2021.
//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <cmath>
#include <cstdint>
#include <array>
#include <complex>
#include <tuple>
#include <iostream>
#include <vector>
#include <limits>
#include <boost/math/tools/color_maps.hpp>

#if !__has_include("lodepng.h")
 #error "lodepng.h is required to run this example."
#endif
#include "lodepng.h"
#include <iostream>
#include <string>
#include <vector>

// In lodepng, the vector is expected to be row major, with the top row
// specified first. Note that this is a bit confusing sometimes as it's more
// natural to let y increase moving *up*.
unsigned write_png(const std::string &filename,
                   const std::vector<std::uint8_t> &img, std::size_t width,
                   std::size_t height) {
  unsigned error = lodepng::encode(filename, img, width, height,
                                   LodePNGColorType::LCT_RGBA, 8);
  if (error) {
    std::cerr << "Error encoding png: " << lodepng_error_text(error) << "\n";
  }
  return error;
}

// Computes ab - cd.
// See: https://pharr.org/matt/blog/2019/11/03/difference-of-floats.html
template <typename Real>
inline Real difference_of_products(Real a, Real b, Real c, Real d)
{
    Real cd = c * d;
    Real err = std::fma(-c, d, cd);
    Real dop = std::fma(a, b, -cd);
    return dop + err;
}

template<typename Real>
auto fifth_roots(std::complex<Real> z)
{
    std::complex<Real> v = std::pow(z,4);
    std::complex<Real> dw = Real(5)*v;
    std::complex<Real> w = v*z - Real(1);
    return std::make_pair(w, dw);
}

template<typename Real>
auto g(std::complex<Real> z)
{
    std::complex<Real> z2 = z*z;
    std::complex<Real> z3 = z*z2;
    std::complex<Real> z4 = z2*z2;
    std::complex<Real> w = z4*(z4 + Real(15)) - Real(16);
    std::complex<Real> dw = Real(4)*z3*(Real(2)*z4 + Real(15));
    return std::make_pair(w, dw);
}

template<typename Real>
std::complex<Real> complex_newton(std::function<std::pair<std::complex<Real>,std::complex<Real>>(std::complex<Real>)> f, std::complex<Real> z)
{
    // f(x(1+e)) = f(x) + exf'(x)
    bool close = false;
    do
    {
        auto [y, dy] = f(z);
        z -= y/dy;
        close = (abs(y) <= 1.4*std::numeric_limits<Real>::epsilon()*abs(z*dy));
    } while(!close);
    return z;
}

template<typename Real>
class plane_pixel_map
{
public:
    plane_pixel_map(int64_t image_width, int64_t image_height, Real xmin, Real ymin)
    {
        image_width_ = image_width;
        image_height_ = image_height;
        xmin_ = xmin;
        ymin_ = ymin;
    }

    std::complex<Real> to_complex(int64_t i, int64_t j) const {
        Real x = xmin_ + 2*abs(xmin_)*Real(i)/Real(image_width_ - 1);
        Real y = ymin_ + 2*abs(ymin_)*Real(j)/Real(image_height_ - 1);
        return std::complex<Real>(x,y);
    }

    std::pair<int64_t, int64_t> to_pixel(std::complex<Real> z) const {
        Real x = z.real();
        Real y = z.imag();
        Real ii = (image_width_ - 1)*(x - xmin_)/(2*abs(xmin_));
        Real jj = (image_height_ - 1)*(y - ymin_)/(2*abs(ymin_));

        return std::make_pair(std::round(ii), std::round(jj));
    }

private:
    int64_t image_width_;
    int64_t image_height_;
    Real xmin_;
    Real ymin_;
};

int main(int argc, char** argv)
{
    using Real = double;
    using boost::math::tools::viridis;
    using std::sqrt;

    std::function<std::array<Real, 3>(Real)> color_map = viridis<Real>;
    std::string requested_color_map = "viridis";
    if (argc == 2) {
       requested_color_map = std::string(argv[1]);
       if (requested_color_map == "smooth_cool_warm") {
          color_map = boost::math::tools::smooth_cool_warm<Real>;
       }
       else if (requested_color_map == "plasma") {
          color_map = boost::math::tools::plasma<Real>;
       }
       else if (requested_color_map == "black_body") {
          color_map = boost::math::tools::black_body<Real>;
       }
       else if (requested_color_map == "inferno") {
          color_map = boost::math::tools::inferno<Real>;
       }
       else if (requested_color_map == "kindlmann") {
          color_map = boost::math::tools::kindlmann<Real>;
       }
       else if (requested_color_map == "extended_kindlmann") {
          color_map = boost::math::tools::extended_kindlmann<Real>;
       }
       else {
          std::cerr << "Could not recognize color map " << argv[1] << ".";
          return 1;
       }
    }
    constexpr int64_t image_width = 1024;
    constexpr int64_t image_height = 1024;
    constexpr const Real two_pi = 6.28318530718;

    std::vector<std::uint8_t> img(4*image_width*image_height, 0);
    plane_pixel_map<Real> map(image_width, image_height, Real(-2), Real(-2));

    for (int64_t j = 0; j < image_height; ++j)
    {
        std::cout << "j = " << j << "\n";
        for (int64_t i = 0; i < image_width; ++i)
        {
            std::complex<Real> z0 = map.to_complex(i,j);
            auto rt = complex_newton<Real>(g<Real>, z0);
            // The root is one of exp(2*pi*ij/5). Therefore, it can be classified by angle.
            Real theta = std::atan2(rt.imag(), rt.real());
            // Now theta in [-pi,pi]. Get it into [0,2pi]:
            if (theta < 0) {
                theta += two_pi;
            }
            theta /= two_pi;
            if (std::isnan(theta)) {
                std::cerr << "Theta is a nan!\n";
            }
            auto c = boost::math::tools::to_8bit_rgba(color_map(theta));
            int64_t idx = 4 * image_width * (image_height - 1 - j) + 4 * i;
            img[idx + 0] = c[0];
            img[idx + 1] = c[1];
            img[idx + 2] = c[2];
            img[idx + 3] = c[3];
        }
    }

    std::array<std::complex<Real>, 8> roots;
    roots[0] = -Real(1);
    roots[1] = Real(1);
    roots[2] = {Real(0), Real(1)};
    roots[3] = {Real(0), -Real(1)};
    roots[4] = {sqrt(Real(2)), sqrt(Real(2))};
    roots[5] = {sqrt(Real(2)), -sqrt(Real(2))};
    roots[6] = {-sqrt(Real(2)), -sqrt(Real(2))};
    roots[7] = {-sqrt(Real(2)), sqrt(Real(2))};

    for (int64_t k = 0; k < 8; ++k)
    {
        auto [ic, jc] = map.to_pixel(roots[k]);

        int64_t r = 7;
        for (int64_t i = ic - r; i < ic + r; ++i)
        {
            for (int64_t j = jc - r; j < jc + r; ++j)
            {
                if ((i-ic)*(i-ic) + (j-jc)*(j-jc) > r*r)
                {
                    continue;
                }
                int64_t idx = 4 * image_width * (image_height - 1 - j) + 4 * i;
                img[idx + 0] = 0;
                img[idx + 1] = 0;
                img[idx + 2] = 0;
                img[idx + 3] = 0xff;
            }
        }
    }

    // Requires lodepng.h
    // See: https://github.com/lvandeve/lodepng for download and compilation instructions
    write_png(requested_color_map + "_newton_fractal.png", img, image_width, image_height);
}


#include "problemes.h"
#include "chiffres.h"
#include "utilitaires.h"
#include "mpz_nombre.h"
#include "mpq_fraction.h"

#include <boost/range/adaptor/reversed.hpp>

ENREGISTRER_PROBLEME(65, "Convergents of e") {
    // The square root of 2 can be written as an infinite continued fraction.
    //
    // The infinite continued fraction can be written, √2 = [1;(2)], (2) indicates that 2 repeats ad infinitum. In a
    // similar way, √23 = [4;(1,3,1,8)].
    //
    // It turns out that the sequence of partial values of continued fractions for square roots provide the best rational
    // approximations. Let us consider the convergents for √2.
    //
    // Hence the sequence of the first ten convergents for √2 are:
    //
    // 1, 3/2, 7/5, 17/12, 41/29, 99/70, 239/169, 577/408, 1393/985, 3363/2378, ...
    // What is most surprising is that the important mathematical constant,
    // e = [2; 1,2,1, 1,4,1, 1,6,1 , ... , 1,2k,1, ...].
    //
    // The first ten terms in the sequence of convergents for e are:
    //
    // 2, 3, 8/3, 11/4, 19/7, 87/32, 106/39, 193/71, 1264/465, 1457/536, ...
    // The sum of digits in the numerator of the 10th convergent is 1+4+5+7=17.
    //
    // Find the sum of digits in the numerator of the 100th convergent of the continued fraction for e.
    std::vector<mpz_nombre> fraction_continue;
    fraction_continue.emplace_back(2);
    for (size_t n = 2; n < 101; n += 2) {
        fraction_continue.emplace_back(1);
        fraction_continue.emplace_back(n);
        fraction_continue.emplace_back(1);
    }
    fraction_continue.resize(99);
    mpq_fraction f(1);
    for (const auto &p: boost::adaptors::reverse(fraction_continue)) f = p + 1 / f;

    mpz_nombre resultat = f.numerateur().somme_chiffres();
    return resultat.to_string();
}


/*
 * Copyright 2012 Karsten Ahnert
 * Copyright 2012 Mario Mulansky
 *
 * 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 <iostream>
#include <fstream>
#include <utility>
#include "time.h"

#include <boost/numeric/odeint.hpp>
#include <boost/phoenix/phoenix.hpp>
#include <boost/numeric/mtl/mtl.hpp>

#include <boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp>

using namespace std;
using namespace boost::numeric::odeint;

namespace phoenix = boost::phoenix;

typedef mtl::dense_vector< double > vec_mtl4;
typedef mtl::compressed2D< double > mat_mtl4;

typedef boost::numeric::ublas::vector< double > vec_ublas;
typedef boost::numeric::ublas::matrix< double > mat_ublas;

// two systems defined 1 & 2 both are mostly sparse with the number of element variable
struct system1_mtl4
{

    void operator()( const vec_mtl4 &x , vec_mtl4 &dxdt , double t )
    {
        int size = mtl::size(x);

        dxdt[ 0 ] = -0.06*x[0];

        for (int i =1; i< size ; ++i){

            dxdt[ i ] = 4.2*x[i-1]-2.2*x[i]*x[i];
        }

    }
};

struct jacobi1_mtl4
{
    void operator()( const vec_mtl4 &x , mat_mtl4 &J , const double &t )
    {
        int size = mtl::size(x);
        mtl::matrix::inserter<mat_mtl4> ins(J);

        ins[0][0]=-0.06;

        for (int i =1; i< size ; ++i)
        {
            ins[i][i-1] = + 4.2;
            ins[i][i] = -4.2*x[i];
        }
    }
};

struct system1_ublas
{

    void operator()( const vec_ublas &x , vec_ublas &dxdt , double t )
    {
        int size = x.size();

        dxdt[ 0 ] = -0.06*x[0];

        for (int i =1; i< size ; ++i){

            dxdt[ i ] = 4.2*x[i-1]-2.2*x[i]*x[i];
        }

    }
};

struct jacobi1_ublas
{
    void operator()( const vec_ublas &x , mat_ublas &J , const double &t )
    {
        int size = x.size();
// mtl::matrix::inserter<mat_mtl4> ins(J);

        J(0,0)=-0.06;

        for (int i =1; i< size ; ++i){
//ins[i][0]=120.0*x[i];
            J(i,i-1) = + 4.2;
            J(i,i) = -4.2*x[i];

        }
    }
};

struct system2_mtl4
{

    void operator()( const vec_mtl4 &x , vec_mtl4 &dxdt , double t )
    {
        int size = mtl::size(x);

        for (int i =0; i< size/5 ; i+=5){

            dxdt[ i ] = -0.5*x[i];
            dxdt[i+1]= +25*x[i+1]*x[i+2]-740*x[i+3]*x[i+3]+4.2e-2*x[i];
            dxdt[i+2]= +25*x[i]*x[i]-740*x[i+3]*x[i+3];
            dxdt[i+3]= -25*x[i+1]*x[i+2]+740*x[i+3]*x[i+3];
            dxdt[i+4] = 0.250*x[i]*x[i+1]-44.5*x[i+3];

        }

    }
};

struct jacobi2_mtl4
{
    void operator()( const vec_mtl4 &x , mat_mtl4 &J , const double &t )
    {
        int size = mtl::size(x);
        mtl::matrix::inserter<mat_mtl4> ins(J);

        for (int i =0; i< size/5 ; i+=5){

            ins[ i ][i] = -0.5;
            ins[i+1][i+1]=25*x[i+2];
            ins[i+1][i+2] = 25*x[i+1];
            ins[i+1][i+3] = -740*2*x[i+3];
            ins[i+1][i] =+4.2e-2;

            ins[i+2][i]= 50*x[i];
            ins[i+2][i+3]= -740*2*x[i+3];
            ins[i+3][i+1] = -25*x[i+2];
            ins[i+3][i+2] = -25*x[i+1];
            ins[i+3][i+3] = +740*2*x[i+3];
            ins[i+4][i] = 0.25*x[i+1];
            ins[i+4][i+1] =0.25*x[i];
            ins[i+4][i+3]=-44.5;

        }
    }
};

struct system2_ublas
{

    void operator()( const vec_ublas &x , vec_ublas &dxdt , double t )
    {
        int size = x.size();
        for (int i =0; i< size/5 ; i+=5){

            dxdt[ i ] = -4.2e-2*x[i];
            dxdt[i+1]= +25*x[i+1]*x[i+2]-740*x[i+3]*x[i+3]+4.2e-2*x[i];
            dxdt[i+2]= +25*x[i]*x[i]-740*x[i+3]*x[i+3];
            dxdt[i+3]= -25*x[i+1]*x[i+2]+740*x[i+3]*x[i+3];
            dxdt[i+4] = 0.250*x[i]*x[i+1]-44.5*x[i+3];

        }

    }
};

struct jacobi2_ublas
{
    void operator()( const vec_ublas &x , mat_ublas &J , const double &t )
    {
        int size = x.size();

        for (int i =0; i< size/5 ; i+=5){

            J(i ,i) = -4.2e-2;
            J(i+1,i+1)=25*x[i+2];
            J(i+1,i+2) = 25*x[i+1];
            J(i+1,i+3) = -740*2*x[i+3];
            J(i+1,i) =+4.2e-2;

            J(i+2,i)= 50*x[i];
            J(i+2,i+3)= -740*2*x[i+3];
            J(i+3,i+1) = -25*x[i+2];
            J(i+3,i+2) = -25*x[i+1];
            J(i+3,i+3) = +740*2*x[i+3];
            J(i+4,i) = 0.25*x[i+1];
            J(i+4,i+1) =0.25*x[i];
            J(i+4,i+3)=-44.5;

        }

    }
};

void testRidiculouslyMassiveArray( int size )
{
    typedef boost::numeric::odeint::implicit_euler_mtl4 < double > mtl4stepper;
    typedef boost::numeric::odeint::implicit_euler< double > booststepper;

    vec_mtl4 x(size , 0.0);
    x[0]=1;

    double dt = 0.02;
    double endtime = 10.0;

    clock_t tstart_mtl4 = clock();
    size_t num_of_steps_mtl4 = integrate_const(
        mtl4stepper() ,
        make_pair( system1_mtl4() , jacobi1_mtl4() ) ,
        x , 0.0 , endtime , dt  );
    clock_t tend_mtl4 = clock() ;

    clog << x[0] << endl;
    clog << num_of_steps_mtl4 << " time elapsed: " << (double)(tend_mtl4-tstart_mtl4 )/CLOCKS_PER_SEC << endl;

    vec_ublas x_ublas(size , 0.0);
    x_ublas[0]=1;

    clock_t tstart_boost = clock();
    size_t num_of_steps_ublas = integrate_const(
        booststepper() ,
        make_pair( system1_ublas() , jacobi1_ublas() ) ,
        x_ublas , 0.0 , endtime , dt  );
    clock_t tend_boost = clock() ;

    clog << x_ublas[0] << endl;
    clog << num_of_steps_ublas << " time elapsed: " << (double)(tend_boost-tstart_boost)/CLOCKS_PER_SEC<< endl;

    clog << "dt_ublas/dt_mtl4 = " << (double)( tend_boost-tstart_boost )/( tend_mtl4-tstart_mtl4 ) << endl << endl;
    return ;
}

void testRidiculouslyMassiveArray2( int size )
{
    typedef boost::numeric::odeint::implicit_euler_mtl4 < double > mtl4stepper;
    typedef boost::numeric::odeint::implicit_euler< double > booststepper;

    vec_mtl4 x(size , 0.0);
    x[0]=100;

    double dt = 0.01;
    double endtime = 10.0;

    clock_t tstart_mtl4 = clock();
    size_t num_of_steps_mtl4 = integrate_const(
        mtl4stepper() ,
        make_pair( system1_mtl4() , jacobi1_mtl4() ) ,
        x , 0.0 , endtime , dt );

    clock_t tend_mtl4 = clock() ;

    clog << x[0] << endl;
    clog << num_of_steps_mtl4 << " time elapsed: " << (double)(tend_mtl4-tstart_mtl4 )/CLOCKS_PER_SEC << endl;

    vec_ublas x_ublas(size , 0.0);
    x_ublas[0]=100;

    clock_t tstart_boost = clock();
    size_t num_of_steps_ublas = integrate_const(
        booststepper() ,
        make_pair( system1_ublas() , jacobi1_ublas() ) ,
        x_ublas , 0.0 , endtime , dt  );

    clock_t tend_boost = clock() ;

    clog << x_ublas[0] << endl;
    clog << num_of_steps_ublas << " time elapsed: " << (double)(tend_boost-tstart_boost)/CLOCKS_PER_SEC<< endl;

    clog << "dt_ublas/dt_mtl4 = " << (double)( tend_boost-tstart_boost )/( tend_mtl4-tstart_mtl4 ) << endl << endl;
    return ;
}

int main( int argc , char **argv )
{
    std::vector< size_t > length;
    length.push_back( 8 );
    length.push_back( 16 );
    length.push_back( 32 );
    length.push_back( 64 );
    length.push_back( 128 );
    length.push_back( 256 );

    for( size_t i=0 ; i<length.size() ; ++i )
    {
        clog << "Testing with size " << length[i] << endl;
        testRidiculouslyMassiveArray( length[i] );
    }
    clog << endl << endl;

    for( size_t i=0 ; i<length.size() ; ++i )
    {
        clog << "Testing with size " << length[i] << endl;
        testRidiculouslyMassiveArray2( length[i] );
    }
}


#include "problemes.h"
#include "arithmetique.h"

#include <fstream>
#include <boost/algorithm/string.hpp>
#include <numeric>

typedef unsigned long long nombre;

ENREGISTRER_PROBLEME(42, "Coded triangle numbers") {
    // The nth term of the sequence of triangle numbers is given by, tn = ½n(n+1); so the first ten triangle numbers are:
    //
    //                                      1, 3, 6, 10, 15, 21, 28, 36, 45, 55, ...
    //
    // By converting each letter in a word to a number corresponding to its alphabetical position and adding these values
    // we form a word value. For example, the word value for SKY is 19 + 11 + 25 = 55 = t10. If the word value is a triangle
    // number then we shall call the word a triangle word.
    //
    // Using words.txt (right click and 'Save Link/Target As...'), a 16K text file containing nearly two-thousand common English words, how many are triangle words?
    std::ifstream ifs("data/p042_words.txt");
    std::string entree;
    ifs >> entree;
    std::vector<std::string> names;
    boost::split(names, entree, boost::is_any_of(","));

    std::set<nombre> triangle;
    for (nombre n = 1; n < 50; ++n)
        triangle.insert(n * (n + 1) / 2);

    nombre resultat = 0;
    for (const auto &name: names) {
        nombre score = std::accumulate(name.begin(), name.end(), 0ULL, [](const nombre &n, char c) {
            if (c != '"')
                return n + 1 + static_cast<nombre>(c - 'A');
            return n;
        });
        if (triangle.find(score) != triangle.end())
            ++resultat;
    }
    return std::to_string(resultat);
}


//
//  gmatrix9float.cpp
//  GCommon
//
//  Created by David Coen on 2011 06 01
//  Copyright Pleasure seeking morons 2011. All rights reserved.
//

#include "gmatrix9float.h"

#include "gvector3float.h"
#include "gmathmatrix.h"
#include "gmath.h"

#include <boost/swap.hpp>

#define DSC_INLINE_MATRIX_MUL

/*static*/ const GMatrix9Float GMatrix9Float::sIdentity(1.0F, 0.0F, 0.0F,
														  0.0F, 1.0F, 0.0F,
														  0.0F, 0.0F, 1.0F);

//constructors
GMatrix9Float::GMatrix9Float(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2
	)
{
	SetData(in_data_0_0, in_data_0_1, in_data_0_2,
		in_data_1_0, in_data_1_1, in_data_1_2,
		in_data_2_0, in_data_2_1, in_data_2_2
		);
	return;
}
GMatrix9Float::GMatrix9Float(const GR32* const in_data)
{
	for (GS32 index = 0; index < 9; ++index)
	{
		m_data[index] = in_data[index];
	}
	return;

}

GMatrix9Float::GMatrix9Float(const GMatrix9Float& in_src)
{
	(*this) = in_src;
	return;
}

GMatrix9Float::~GMatrix9Float()
{
	return;
}

//operators
const GMatrix9Float& GMatrix9Float::operator=(const GMatrix9Float& in_rhs)
{
	for (GS32 index = 0; index < 9; ++index)
	{
		m_data[index] = in_rhs.m_data[index];
	}
	return (*this);
}

//public methods
GMatrix9Float& GMatrix9Float::TransposeSelf()
{
	std::swap(m_0_1, m_1_0);
	std::swap(m_0_2, m_2_0);
	std::swap(m_1_2, m_2_1);

	return (*this);
}

const GMatrix9Float GMatrix9Float::ReturnInverse()const
{
	GMatrix9Float result;
	GMathMatrix<GR32>::Inverse4(&result.m_data[0], &m_data[0]);

	return result;
}

//public accessors
void GMatrix9Float::SetData(const GR32 in_data_0_0, const GR32 in_data_0_1, const GR32 in_data_0_2,
	const GR32 in_data_1_0, const GR32 in_data_1_1, const GR32 in_data_1_2,
	const GR32 in_data_2_0, const GR32 in_data_2_1, const GR32 in_data_2_2
	)
{
	m_0_0 = in_data_0_0;
	m_1_0 = in_data_1_0;
	m_2_0 = in_data_2_0;

	m_0_1 = in_data_0_1;
	m_1_1 = in_data_1_1;
	m_2_1 = in_data_2_1;

	m_0_2 = in_data_0_2;
	m_1_2 = in_data_1_2;
	m_2_2 = in_data_2_2;

	return;
}

const GVector3Float GMatrix9Float::GetAt()const
{
	const GVector3Float result(
		m_0_2,
		m_1_2,
		m_2_2
		);
	return result;
}

const GVector3Float GMatrix9Float::GetUp()const
{
	const GVector3Float result(
		m_0_1,
		m_1_1,
		m_2_1
		);
	return result;
}

//global operators
const GMatrix9Float operator*(const GMatrix9Float& in_lhs, const GMatrix9Float& in_rhs)
{
	GR32 value[9];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 3]) + (lhsData[ 2] * rhsData[ 6]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 7]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 8]);

	value[ 3] = (lhsData[ 3] * rhsData[ 0]) + (lhsData[ 4] * rhsData[ 3]) + (lhsData[ 5] * rhsData[ 6]);
	value[ 4] = (lhsData[ 3] * rhsData[ 1]) + (lhsData[ 4] * rhsData[ 4]) + (lhsData[ 5] * rhsData[ 7]);
	value[ 5] = (lhsData[ 3] * rhsData[ 2]) + (lhsData[ 4] * rhsData[ 5]) + (lhsData[ 5] * rhsData[ 8]);

	value[ 6] = (lhsData[ 6] * rhsData[ 0]) + (lhsData[ 7] * rhsData[ 3]) + (lhsData[ 8] * rhsData[ 6]);
	value[ 7] = (lhsData[ 6] * rhsData[ 1]) + (lhsData[ 7] * rhsData[ 4]) + (lhsData[ 8] * rhsData[ 7]);
	value[ 8] = (lhsData[ 6] * rhsData[ 2]) + (lhsData[ 7] * rhsData[ 5]) + (lhsData[ 8] * rhsData[ 8]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		3,
		3,
		3,
		&value[0]
		);
#endif
	return GMatrix9Float(&value[0]);
}

const GVector3Float operator*(const GVector3Float& in_lhs, const GMatrix9Float& in_rhs)
{
	GR32 value[3];
#ifdef DSC_INLINE_MATRIX_MUL
	const GR32* const lhsData = in_lhs.GetData();
	const GR32* const rhsData = in_rhs.GetData();
	value[ 0] = (lhsData[ 0] * rhsData[ 0]) + (lhsData[ 1] * rhsData[ 3]) + (lhsData[ 2] * rhsData[ 6]);
	value[ 1] = (lhsData[ 0] * rhsData[ 1]) + (lhsData[ 1] * rhsData[ 4]) + (lhsData[ 2] * rhsData[ 7]);
	value[ 2] = (lhsData[ 0] * rhsData[ 2]) + (lhsData[ 1] * rhsData[ 5]) + (lhsData[ 2] * rhsData[ 8]);
#else
	GMathMatrix<GR32>::MatrixMul(
		in_lhs.GetData(),
		in_rhs.GetData(),
		3,
		1,
		3,
		&value[0]
		);
#endif
	return GVector3Float(value[0], value[1], value[2]);
}

const GMatrix9Float& operator*=(GMatrix9Float& in_lhs, const GMatrix9Float& in_rhs)
{
	in_lhs = (in_lhs * in_rhs);
	return in_lhs;
}

/*
  from _Mathematics for computer graphics p.171
 mapping a pair of vectors onto another pair, u,x being one pair, a,y being another of unit vectors at same angle
 u.x = a.y = cos(\), with sin(\) != 0
 a roation matrix sending u,x to a,y given by
               a
 M = [u v w ][ b ]
               c
 d = | x * u | = | y * a | = | sin(\) |
 if 90deg = u to x, cos(\) = 0, sin(\) = 1
 v = ( x * u ) / d
 b = ( y * a ) / d
 w = ( x - u( cos(\) ) ) / d
 c = ( y - a( cos(\) ) ) / d
*/
const GMatrix9Float GMatrix9FloatConstructAtUp(
	const GVector3Float& in_targetAt,
	const GVector3Float& in_targetUp,
	const GVector3Float& in_baseAt,
	const GVector3Float& in_baseUp
	)
{
	GMatrix9Float returnMatrix;

	const GVector3Float crossBaseUpAt = CrossProduct(in_baseUp, in_baseAt);
	const GVector3Float crossTargetUpAt = CrossProduct(in_targetUp, in_targetAt);

	return GMatrix9Float(
		(in_baseAt.m_x * in_targetAt.m_x) + (crossBaseUpAt.m_x * crossTargetUpAt.m_x) + (in_baseUp.m_x * in_targetUp.m_x),
		(in_baseAt.m_y * in_targetAt.m_x) + (crossBaseUpAt.m_y * crossTargetUpAt.m_x) + (in_baseUp.m_y * in_targetUp.m_x),
		(in_baseAt.m_z * in_targetAt.m_x) + (crossBaseUpAt.m_z * crossTargetUpAt.m_x) + (in_baseUp.m_z * in_targetUp.m_x),

		(in_baseAt.m_x * in_targetAt.m_y) + (crossBaseUpAt.m_x * crossTargetUpAt.m_y) + (in_baseUp.m_x * in_targetUp.m_y),
		(in_baseAt.m_y * in_targetAt.m_y) + (crossBaseUpAt.m_y * crossTargetUpAt.m_y) + (in_baseUp.m_y * in_targetUp.m_y),
		(in_baseAt.m_z * in_targetAt.m_y) + (crossBaseUpAt.m_z * crossTargetUpAt.m_y) + (in_baseUp.m_z * in_targetUp.m_y),

		(in_baseAt.m_x * in_targetAt.m_z) + (crossBaseUpAt.m_x * crossTargetUpAt.m_z) + (in_baseUp.m_x * in_targetUp.m_z),
		(in_baseAt.m_y * in_targetAt.m_z) + (crossBaseUpAt.m_y * crossTargetUpAt.m_z) + (in_baseUp.m_y * in_targetUp.m_z),
		(in_baseAt.m_z * in_targetAt.m_z) + (crossBaseUpAt.m_z * crossTargetUpAt.m_z) + (in_baseUp.m_z * in_targetUp.m_z)
		);
}

/*
  http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
*/
const GMatrix9Float GMatrix9FloatConstructAxisAngle(const GVector3Float& in_axis, const GR32 in_angleRad)
{
	const GVector3Float localAxis = Normalise(in_axis);
	const GR32 axis_x = localAxis.m_x;
	const GR32 axis_y = localAxis.m_y;
	const GR32 axis_z = localAxis.m_z;

	const GR32 c = GMath::Cos( in_angleRad );
	const GR32 s = GMath::Sin( in_angleRad );
	const GR32 t = 1.0F - c;

	const GR32 tmp1_01 = axis_x * axis_y * t;
	const GR32 tmp2_01 = axis_z * s;

	const GR32 tmp1_02 = axis_x * axis_z * t;
	const GR32 tmp2_02 = axis_y * s;

	const GR32 tmp1_21 = axis_y * axis_z * t;
	const GR32 tmp2_21 = axis_x * s;

	return GMatrix9Float(
		c + axis_x * axis_x * t, tmp1_01 - tmp2_01, tmp1_02 + tmp2_02,
		tmp1_01 + tmp2_01, c + axis_y * axis_y * t, tmp1_21 - tmp2_21,
		tmp1_02 - tmp2_02, tmp1_21 + tmp2_21, c + axis_z * axis_z * t
		);
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

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

#include <cmath>
#include <fstream>
#include <iostream>

using namespace dealii;

//! Generate a hypercube, and output it as an svg file.
void first_grid(Triangulation<2> &triangulation)
{
  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

  std::ofstream out("grid-1.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.svg" << std::endl;
}

//! Generate a locally refined hyper_shell, and output it as an svg file.
void second_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);

  // triangulation.reset_manifold(0);

  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.svg" << std::endl;
}

//! Create an L-shaped domain with one global refinement, and write it on
// `third_grid.vtk`.  Refine the L-shaped mesh adaptively around the re-entrant
// corner three times (after the global refinement you already did), but with a
// twist: refine all cells with the distance between the center of the cell and
// re-entrant corner is smaller than 1/3.
void third_grid(Triangulation<2> &tria)
{
  // Insert code here
  // last year code in comments
  /* Triangulation<2> tr1,tr2,tr3,tr_final;
   const Point<2> p1(0, 0);
   const Point<2> p2(1,2);
   const Point<2> p3(2,1);
   const Point<2> p4(1,0);
   const Point<2> p5(1,1);
   const Point<2> p6(0,1);
   GridGenerator::hyper_rectangle(tr1,p1,p5);
   GridGenerator::hyper_rectangle(tr2,p6,p2);
   GridGenerator::hyper_rectangle(tr3,p4,p3);
   //they cannot be refined!
   GridGenerator::merge_triangulations ({&tr1,&tr2,&tr3},tr_final);*/
  GridGenerator::hyper_L(tria);
  tria.refine_global(1);
  const Point<2> p5(0, 0);
  for (unsigned int step = 0; step < 3; ++step)
    {
      // Active cells are those that are not further refined
      // we need to mark cells for refinement
      for (auto &cell : tria.active_cell_iterators())
        {
          const double distance_from_corner = cell->center().distance({0, 0});
          // choose whatever refinement condition
          if (distance_from_corner < 2.0 / 3.0)
            {
              cell->set_refine_flag();
            }
        }
      // refine global calls this function too
      tria.execute_coarsening_and_refinement();
    } // for steps loop

  // tr_final.refine_global(2); //we want a nice picture :)

  std::ofstream file_var("grid-3.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(tria, file_var);
  std::cout << "Grid written to grid-3.vtk" << std::endl;
}

//! Returns a tuple with number of levels, number of cells, number of active
// cells. Test this with all of  your meshes.
std::tuple<unsigned int, unsigned int, unsigned int>
get_info(const Triangulation<2> &tria)
{
  // Insert code here
  return std::make_tuple(tria.n_levels(),
                         tria.n_cells(),
                         tria.n_active_cells());
}

void
torus_grid()
{
  Triangulation<2, 3> tria;
  GridGenerator::torus(tria, 2, 1);
  tria.refine_global(2);

  std::ofstream out("grid-torus.vtk");
  GridOut       grid_out;
  grid_out.write_vtk(tria, out);
  std::cout << "Grid written to grid-torus.vtk" << std::endl;
}

int
main()
{
  Triangulation<2> triangulation;
  first_grid(triangulation);
  triangulation.clear();
  second_grid(triangulation);
  triangulation.clear();
  third_grid(triangulation);

  torus_grid();
}


#include <dvs_mosaic/poisolve/laplace.h>
#include <dvs_mosaic/reconstruction.h>

#include <boost/multi_array.hpp>
#include <glog/logging.h>

namespace poisson
{

void reconstructBrightnessFromGradientMap(const cv::Mat& grad_map,
                                                cv::Mat* map_reconstructed)
{
  CHECK_EQ(grad_map.type(), CV_32FC2);
  CHECK_GT(grad_map.cols, 0);
  CHECK_GT(grad_map.rows, 0);
  const cv::Size img_size = grad_map.size();

  // Compute the right hand side of Poisson eq.
  // F = dgx/dx + dgy/dy and put it into a boost::multi_array
  const size_t height = img_size.height;
  const size_t width = img_size.width;
  boost::multi_array<double,2> M(boost::extents[height][width]);
  boost::multi_array<double,2> F(boost::extents[height][width]);

  // Compute right hand side (rhs) using one-sided finite differences
  for(size_t i=0; i < height-1; ++i)
  {
    for(size_t j=0; j < width-1; ++j)
    {
      F[i][j] = double( grad_map.at<cv::Vec2f>(i,j+1)[0] - grad_map.at<cv::Vec2f>(i,j)[0]
                      + grad_map.at<cv::Vec2f>(i+1,j)[1] - grad_map.at<cv::Vec2f>(i,j)[1] );
    }
  }
  F[height-1][width-1] = 0.0;

  // Solve for M_xx + M_yy = F using Poisson solver,
  // with constant intensity (zero) boundary conditions
  const double gradient_on_boundary = 0.0;
  pde::poisolve(M, F, 1.0, 1.0, 1.0, 1.0,
                gradient_on_boundary,
                pde::types::boundary::Neumann,false);

  // Fill in output variable
  *map_reconstructed = cv::Mat(img_size, CV_32FC1);
  // FILL IN ...

}

}


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

using namespace boost::numeric::ublas;

int main()
{
	// op(Matrix)
	matrix<std::complex<double> > m (3, 3);
	for (unsigned i = 0; i < m.size1 (); ++ i)
		for (unsigned j = 0; j < m.size2 (); ++ j)
			m (i, j) = std::complex<double> (3 * i + j, 3 * i + j);

	std::cout << "minus \t" << - m << std::endl;
	std::cout << "conj \t" << conj (m) << std::endl;
	std::cout << "real \t" << real (m) << std::endl;
	std::cout << "imag \t" << imag (m) << std::endl;
	std::cout << "trans \t" << trans (m) << std::endl;
	std::cout << "herm \t" << herm (m) << std::endl;

	// Matrix +/- Matrix
	matrix<double> m1 (3, 3), m2 (3, 3);
	for (unsigned i = 0; i < std::min (m1.size1 (), m2.size1 ()); ++ i)
		for (unsigned j = 0; j < std::min (m1.size2 (), m2.size2 ()); ++ j)
			m1 (i, j) = m2 (i, j) = 3 * i + j;

	std::cout << "add \t" << m1 + m2 << std::endl;
	std::cout << "subt \t" << m1 - m2 << std::endl;

	// scalar * Matrix
	matrix<double> mm (3, 3);
	for (unsigned i = 0; i < mm.size1 (); ++ i)
		for (unsigned j = 0; j < mm.size2 (); ++ j)
			mm (i, j) = 3 * i + j;

	std::cout << "2 * matrix \t" << 2.0 * mm << std::endl;
	std::cout << "matrix * 2 \t" << mm * 2.0 << std::endl;

	// Matrix * Vector
	matrix<double> m_ (3, 3);
	vector<double> v (3);
	for (unsigned i = 0; i < std::min (m_.size1 (), v.size ()); ++ i) {
		for (unsigned j = 0; j < m_.size2 (); ++ j)
			m_ (i, j) = 3 * i + j;
		v (i) = i;
	}

	std::cout << "matrix * vector (A * b) \t" << prod (m_, v) << std::endl;
	std::cout << "vector * matrix (A' * b)\t" << prod (v, m_) << std::endl;

	// Matrix * Matrix
	matrix<double> m11 (3, 3), m22 (3, 3);
	for (unsigned i = 0; i < std::min (m11.size1 (), m22.size1 ()); ++ i)
		for (unsigned j = 0; j < std::min (m11.size2 (), m22.size2 ()); ++ j)
			m11 (i, j) = m22 (i, j) = 3 * i + j;

	std::cout << "matrix * matrix (A * B) \t" << prod (m11, m22) << std::endl;
	return 0;
}


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

#include "naive_bayes.cpp"
#include "eigen_space.cpp"

#define PI 3.141592653589793238462643383279502884L
#define REGULARIZATION_FACTOR 0.5
#define IDEAL_NUM_PRINCIPLE_COMPONENTS 16

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

const std::string TRAINING_FILE_NAME = "data/zip.train"; // 7291 items
const std::string TEST_FILE_NAME = "data/zip.test"; // 2007 items

std::vector<ClassificationObject>
ReadData(
  const std::string& fileName,
  const unsigned featureDim,
  const unsigned numItems) {

  std::ifstream fin;
  fin.clear(); fin.open(fileName.c_str());

  if (!fin.good()) {
    throw std::exception();
  }

  std::vector<ClassificationObject> data; data.reserve(numItems);

  for (unsigned i = 0; i < numItems; ++i) {
    ClassificationObject classificationObject;
    double dummy;
    fin >> dummy;
    classificationObject.label = (unsigned) dummy;

    classificationObject.features.resize(featureDim);
    for (unsigned j = 0; j < featureDim; ++j) {
      fin >> classificationObject.features(j);
    }
    data.push_back(classificationObject);
  }

  return data;
}

Eigen::MatrixXi
PerformClassifications(const std::vector<unsigned>& labelSet,
  const std::vector<ClassInfo>& classSummaries,
  const std::vector<ClassificationObject>& testObjects) {

  Eigen::MatrixXi confusionMatrix(labelSet.size(), labelSet.size());
  confusionMatrix.setZero(labelSet.size(), labelSet.size());

  unsigned i = 1;
  unsigned onePercent = testObjects.size() / 100;
  for (const ClassificationObject& object : testObjects) {
    if (i % onePercent == 0) {
      std::cout << i << "%% processed." << std::endl;
    }
    i++;

    unsigned classifiedAs = ClassifyObject(object, classSummaries);
    confusionMatrix(object.label, classifiedAs)++;
  }

  return confusionMatrix;
}

Eigen::MatrixXi
PerformPcaClassifications(const std::vector<unsigned>& labelSet,
  const std::vector<ReducedDimClassInfo>& classSummaries,
  const std::vector<ClassificationObject>& testObjects) {

  Eigen::MatrixXi confusionMatrix(labelSet.size(), labelSet.size());
  confusionMatrix.setZero(labelSet.size(), labelSet.size());

  unsigned i = 1;
  unsigned onePercent = testObjects.size() / 100;
  for (const ClassificationObject& object : testObjects) {
    if (i % onePercent == 0) {
      std::cout << (double)i * 100.0 / testObjects.size() << "% processed." << std::endl;
    }
    i++;

    unsigned classifiedAs = PcaClassifyObject(object, classSummaries);
    confusionMatrix(object.label, classifiedAs)++;
  }

  return confusionMatrix;
}

int
main(const int argc, const char** argv) {
  std::cout << "Reading Data..." << std::endl;
  std::vector<ClassificationObject> trainingData = ReadData(
    TRAINING_FILE_NAME,
    256,
    7291
  );
  std::vector<ClassificationObject> testData = ReadData(
    TEST_FILE_NAME,
    256,
    2007
  );

  std::cout << "Training Models..." << std::endl;
  std::vector<ClassInfo> classSummaries; classSummaries.reserve(10);
  for (unsigned label = 0; label <= 9; ++label) {
    classSummaries.push_back(ComputeClassInfo(trainingData, label, 256));
  }

  std::cout << "Training Reduced Dimensionality Models..." << std::endl;
  std::vector<ReducedDimClassInfo> reducedClassSummaries; reducedClassSummaries.reserve(10);
  for (unsigned label = 0; label <= 9; ++label) {
    reducedClassSummaries.push_back(
      ComputeClassInfoInPcaDimensionReducedSpace(
        trainingData,
        classSummaries[label],
        16
      )
    );
  }

  std::cout << "Classifying Objects..." << std::endl;
  Eigen::MatrixXi confusionMatrix = PerformPcaClassifications(
    {0,1,2,3,4,5,6,7,8,9},
    reducedClassSummaries,
    testData
  );

  std::cout << "Results:" << std::endl;
  std::cout << confusionMatrix << std::endl;

	return 0;
}


#include "camsim.hpp"

#include "gtsam/inference/Symbol.h"
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/make_shared.hpp>

using namespace gtsam;
using namespace gtsam::noiseModel;
using symbol_shorthand::X;

/**
 * Unary factor on the unknown pose, resulting from measuring the projection of
 * a known 3D point in the image
 */
class ResectioningFactor_xxx : public NoiseModelFactor1<Pose3>
{
  typedef NoiseModelFactor1<Pose3> Base;

  Cal3_S2::shared_ptr K_; ///< camera's intrinsic parameters
  Point3 P_;              ///< 3D point on the calibration rig
  Point2 p_;              ///< 2D measurement of the 3D point

public:

  /// Construct factor given known point P and its projection p
  ResectioningFactor_xxx(const SharedNoiseModel &model, const Key &key,
                         const Cal3_S2::shared_ptr &calib, const Point2 &p, const Point3 &P) :
    Base(model, key), K_(calib), P_(P), p_(p)
  {
  }

  /// evaluate the error
  virtual Vector evaluateError(const Pose3 &pose, boost::optional<Matrix &> H =
  boost::none) const
  {
    SimpleCamera camera(pose, *K_);
    return camera.project(P_, H, boost::none, boost::none) - p_;
  }
};

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/

int gtsam_resection()
{
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(5., .5));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(55, 45), Point3(10, 10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(45, 45), Point3(-10, 10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(45, 55), Point3(-10, -10, 0));
  graph.emplace_shared<ResectioningFactor_xxx>(measurementNoise, X(1), calib,
                                               Point2(55, 55), Point3(10, -10, 0));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
//                 Pose3(Rot3(1, 1, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));
                 Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  graph.print("full graph");

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  auto camera_f_marker = result.at<Pose3>(X(1));
  std::cout << camera_f_marker.rotation() << camera_f_marker.rotation().xyz() << std::endl;

  std::cout.precision(2);
  Marginals marginals(graph, result);
  std::cout << "x1 covariance:\n" << marginals.marginalCovariance(X(1)) << std::endl;

  std::cout << "initial error = " << graph.error(initial) << std::endl;
  std::cout << "final error = " << graph.error(result) << std::endl;

  return EXIT_SUCCESS;
}


// Copyright (c) 2018-2019, University of Bremen, M. Farzalipour Tabriz
// Copyrights licensed under the 2-Clause BSD License.
// See the accompanying LICENSE.txt file for terms.

#pragma once
#include <armadillo>
#include "arma_io.hpp"
#include <fftw3.h>
#include "general_io.hpp"
#include "spline.hpp"

#define PI datum::pi

//These functions extend the functionality of the included Armadillo library by providing:
// (arma,iostream) << and >> operators for reading from iostreams to armadillo types and vice versa
// fft/ifft functions for cube files: Wrappers for FFTW with MATLAB/Octave scaling convention
//				in FFT functions the forward FFT scales by 1, and the reverse scales by 1/N.
// 3D ndgrid and 3D meshgrid
// 3D spline interpolation
// 3D shift: by number of the elements along one axis or with a relative 3D shift vector
// irowvec to matrix/cube size conversion
// scalar triple product

using namespace arma;

// a (very inefficient) 3D spline interpolation!
cube interp3(const rowvec& x, const rowvec& y, const rowvec& z, const cube& v, const rowvec& xi, const rowvec& yi, const rowvec& zi);
cube interp3(const cube& v, const rowvec& xi, const rowvec& yi, const rowvec& zi);

//Rectangular grid in 3D space
tuple<cube, cube, cube> ndgrid(const rowvec& v1, const rowvec& v2, const rowvec& v3);

//3D meshgrid
tuple<cube, cube, cube> meshgrid(const rowvec& v1, const rowvec& v2, const rowvec& v3);

//shifts a cube by a relative 3D vector [0 1]
cube shift(cube cube_in, rowvec3 shifts);

//Planar average of a cube in the defined direction
//direction: 0,1,2 > x,y,z
vec planar_average(const uword& direction, const cube& cube_in);

//1D FFT of complex data.
//no normalization for forward FFT
cx_vec fft(cx_vec X);

//1D FFT of real data.
//no normalization for forward FFT
cx_vec fft(vec X);

//3D FFT of complex data.
//no normalization for forward FFT
cx_cube fft(cx_cube X);

//3D FFT of real data.
//no normalization for forward FFT
cx_cube fft(cube X);

//1D inverse FFT of complex data.
//normalized by N = X.n_elem
cx_vec ifft(cx_vec X);

//3D inverse FFT of complex data.
//normalized by N = X.n_elem
cx_cube ifft(cx_cube X);

//returns a cube size object from the values inside a vector
SizeCube as_size(const urowvec3& vec);

//returns a matrix size object from the values inside a vector
SizeMat as_size(const urowvec2& vec);

//element-wise fmod
mat fmod(mat mat_in, const double& denom) noexcept;

//element-wise positive fmod
mat fmod_p(mat mat_in, const double& denom) noexcept;

//positive fmod
double fmod_p(double num, const double& denom) noexcept;

//just a simple square! May cause overflows!!
inline double square(const double& input) noexcept {
	return input * input;
}

//Poisson solver in 3D with anisotropic dielectric profiles
//diel is the N*3 matrix of variations in dielectric tensor elements in direction normal to the surface
cx_cube poisson_solver_3D(const cx_cube& rho, mat diel, rowvec3 lengths, uword normal_direction);

//generate a copy of the cube with the elements shifted by N positions along:
//dim=0: each row
//dim=1: each column
//dim=2: each slice
template <typename T>
Cube<T> shift(const Cube<T>& A, const sword& N, const uword& dim) {
	Cube<T> B(arma::size(A));
	const auto index_init = regspace<uvec>(0, A.n_elem - 1);
	imat sub_shift = conv_to<imat>::from(ind2sub(arma::size(A), index_init));
	const uword size = arma::size(A)(dim);
	sub_shift.row(dim).for_each([&N, &size](sword& i) noexcept {
		i += N;
		while (i < 0) i += size;
		i = i % size;
	});

	B(sub2ind(arma::size(A), conv_to<umat>::from(sub_shift))) = A(index_init);

	return B;
}

//Undo a fftshift
template <typename T>
Row<T> ifftshift(const Row<T> &A) {
	return shift(A, -1 * (A.n_elem / 2));
}

//Undo a fftshift
template <typename T>
Cube<T> ifftshift(Cube<T> A) {
	for (uword i = 0; i < 3; ++i) {
		A = shift(A, -1 * (arma::size(A)(i) / 2), i);
	}

	return A;
}

//returns the size of a cube as a rowvec
template<typename T>
urowvec3 SizeVec(const Cube<T>& c) {
	const SizeCube size = arma::size(c);
	return urowvec({ size(0), size(1), size(2) });
}

//returns the size of a matrix as a rowvec
template<typename T>
urowvec2 SizeVec(const Mat<T>& c) {
	const SizeMat size = arma::size(c);
	return urowvec({ size(0), size(1) });
}

//sign of the val as -1/0/+1
template <typename T> int sgn(T val) {
	return (T(0) < val) - (val < T(0));
}


#pragma once

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/biscale_mesh.hpp>
#include <crest/geometry/triangle.hpp>
#include <crest/quadrature/triquad.hpp>
#include <crest/util/eigen_extensions.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace crest
{
    namespace detail
    {
        /**
         * Returns the coefficients of the three linear Lagrange basis functions
         * associated with the triangle.
         *
         * Given basis function l_i for i = 0, 1, 2, then
         * l_i = a_i x + b_i y + c_i
         * where
         *      [ a_0   a_1   a_2 ]
         * B =  [ b_0   b_1   b_2 ]
         *      [ c_0   c_1   c_2 ]
         * @param triangle
         * @return
         */
        template <typename Scalar>
        Eigen::Matrix<Scalar, 3, 3> basis_coefficients_for_triangle(const Triangle<Scalar> & triangle)
        {
            constexpr Scalar one = static_cast<Scalar>(1.0);
            Eigen::Matrix<Scalar, 3, 3> X;
            X <<
              triangle.a.x, triangle.a.y, one,
                    triangle.b.x, triangle.b.y, one,
                    triangle.c.x, triangle.c.y, one;

            return X.partialPivLu().solve(Eigen::Matrix<Scalar, 3, 3>::Identity());
        };
        /**
         * The interpolation of a fine-scale finite element space in the (possibly discontinuous) space of
         * affine functions on a coarse finite element space can be computed by the relation
         * Py = Bx, where y represent the weights in the affine coarse space and x represents the weights
         * in the fine-scale finite element space. This function computes B.
         * @param coarse
         * @param fine
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> build_affine_interpolator_rhs(
                const BiscaleMesh<Scalar, int> & mesh)
        {
            std::vector<Eigen::Triplet<Scalar>> triplets;

            for (int coarse_index = 0; coarse_index < mesh.coarse_mesh().num_elements(); ++coarse_index)
            {
                // The basis functions p_h in the affine space can be expressed as
                // p_h(x, y) = a x + b y + c
                // for some coefficients a, b, c.
                const auto coarse_triangle = mesh.coarse_mesh().triangle_for(coarse_index);
                const Eigen::Matrix<Scalar, 3, 3> coarse_coeff = basis_coefficients_for_triangle(coarse_triangle);
                for (const auto fine_index : mesh.descendants_for(coarse_index))
                {
                    const auto vertex_indices = mesh.fine_mesh().elements()[fine_index].vertex_indices;
                    const auto fine_triangle = mesh.fine_mesh().triangle_for(fine_index);
                    const Eigen::Matrix<Scalar, 3, 3> fine_coeff = basis_coefficients_for_triangle(fine_triangle);

                    // Local coarse basis function i
                    for (size_t i = 0; i < 3; ++i)
                    {
                        // Local fine basis function j
                        for (size_t j = 0; j < 3; ++j)
                        {
                            const auto vertex_index = vertex_indices[j];
                            const auto product = [&](auto x, auto y)
                            {
                                const auto coarse_basis_value =
                                        coarse_coeff(0, i) * x + coarse_coeff(1, i) * y + coarse_coeff(2, i);
                                const auto fine_basis_value =
                                        fine_coeff(0, j) * x + fine_coeff(1, j) * y + fine_coeff(2, j);
                                return coarse_basis_value * fine_basis_value;
                            };
                            const auto inner_product = triquad<2>(product,
                                                                  fine_triangle.a,
                                                                  fine_triangle.b,
                                                                  fine_triangle.c);
                            const auto row = 3 * coarse_index + i;
                            triplets.push_back(Eigen::Triplet<Scalar>(row, vertex_index, inner_product));
                        }
                    }
                }
            }

            const auto num_dof_affine_space = 3 * mesh.coarse_mesh().num_elements();
            Eigen::SparseMatrix<Scalar> B(num_dof_affine_space, mesh.fine_mesh().num_vertices());
            B.setFromTriplets(triplets.cbegin(), triplets.cend());
            return B;
        };

        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> build_affine_interpolator_lhs_inverse(
                const IndexedMesh<Scalar, int> & coarse)
        {
            const auto num_dof_affine_space = 3 * coarse.num_elements();
            Eigen::SparseMatrix<Scalar> P(num_dof_affine_space, num_dof_affine_space);

            // P will be block diagonal with 3x3 blocks, so we can reserve space in advance.
            P.reserve(Eigen::VectorXi::Constant(num_dof_affine_space, 3));

            Eigen::Matrix<Scalar, 3, 3> P_ref;
            P_ref << 2.0 / 24.0, 1.0 / 24.0, 1.0 / 24.0,
                    1.0 / 24.0, 2.0 / 24.0, 1.0 / 24.0,
                    1.0 / 24.0, 1.0 / 24.0, 2.0 / 24.0;

            for (int t = 0; t < coarse.num_elements(); ++t)
            {
                const auto triangle = coarse.triangle_for(t);
                const auto determinant = static_cast<Scalar>(2.0) * crest::area(triangle);
                assert(determinant != static_cast<Scalar>(0));
                Eigen::Matrix<Scalar, 3, 3> P_local = determinant * P_ref.template cast<Scalar>();
                P_local = P_local.inverse().eval();

                for (int j = 0; j < 3; ++j)
                {
                    for (int i = 0; i < 3; ++i)
                    {
                        const auto row = 3 * t + i;
                        const auto col = 3 * t + j;
                        P.insert(row, col) = P_local(i, j);
                    }
                }
            }

            return P;
        };

        /**
         * Given a coarse and fine mesh where the coarse mesh is a subset of the fine mesh (when interpreted as a forest),
         * build a matrix that interpolates functions in the fine finite element space in the
         * (possibly discontinuous) space of affine functions on the coarse space.
         * @param coarse
         * @param fine
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> affine_interpolator(
                const BiscaleMesh<Scalar, int> & mesh)
        {
            const Eigen::SparseMatrix<Scalar> B = build_affine_interpolator_rhs(mesh);
            const Eigen::SparseMatrix<Scalar> P = build_affine_interpolator_lhs_inverse(mesh.coarse_mesh());
            return P * B;
        };

        template <typename Scalar>
        std::vector<unsigned int> count_vertex_occurrences(const IndexedMesh<Scalar, int> & mesh)
        {
            auto count = std::vector<unsigned int>(mesh.num_vertices(), static_cast<Scalar>(0));
            for (const auto & element : mesh.elements())
            {
                for (const auto & v : element.vertex_indices)
                {
                    ++count[v];
                }
            }
            return count;
        }

        /**
         * Given a triangulation T, returns a matrix of dimensions N(T) x [3 * card(T)] which maps functions in the
         * (possibly discontinuous) affine space P_1 to functions in the standard linear finite element space
         * on the mesh.
         * Here, N(T) denotes the number of vertices in T, and card(T) denotes the number of elements in T.
         * @param mesh
         * @return
         */
        template <typename Scalar>
        Eigen::SparseMatrix<Scalar> nodal_average_interpolator(const IndexedMesh<Scalar, int> & mesh)
        {
            const auto occurrences = count_vertex_occurrences(mesh);

            // Reserve space for 1 non-zero per column, since each basis function in the affine space
            // maps to exactly one vertex in the standard finite element space.
            const auto num_dof_affine_space = 3 * mesh.num_elements();
            Eigen::SparseMatrix<Scalar> J(mesh.num_vertices(), num_dof_affine_space);
            J.reserve(Eigen::VectorXi::Constant(num_dof_affine_space, 1));

            for (int t = 0; t < mesh.num_elements(); ++t)
            {
                const auto vertices = mesh.elements()[t].vertex_indices;

                for (size_t v = 0; v < 3; ++v)
                {
                    const auto col = 3 * t + v;
                    const auto vertex_index = vertices[v];
                    const auto cardinality = occurrences[vertex_index];
                    J.insert(vertex_index, col) = static_cast<Scalar>(1.0) / static_cast<Scalar>(cardinality);
                }
            }

            return J;
        };
    }

    template <typename Scalar>
    Eigen::SparseMatrix<Scalar> quasi_interpolator(const BiscaleMesh<Scalar, int> & mesh)
    {
        const auto P = detail::affine_interpolator(mesh);
        const auto J = detail::nodal_average_interpolator(mesh.coarse_mesh());
        return J * P;
    }

}


/**
 * @file kalmen.hpp
 * @author Vahid Bastani
 *
 * Implementation of Kalman filter
 */
#ifndef SSMPACK_FILTER_KALMAN_HPP
#define SSMPACK_FILTER_KALMAN_HPP

#include "ssmkit/distribution/gaussian.hpp"
#include "ssmkit/distribution/conditional.hpp"
#include "ssmkit/process/markov.hpp"
#include "ssmkit/process/memoryless.hpp"
#include "ssmkit/process/hierarchical.hpp"
#include "ssmkit/filter/recursive_bayesian_base.hpp"
#include <armadillo>

namespace ssmkit {
namespace filter {

using process::Hierarchical;
using process::Markov;
using process::Memoryless;
using distribution::Conditional;
using distribution::Gaussian;

/** Kalman filter
 */
template <class STA_MAP, class OBS_MAP>
class Kalman
    : public RecursiveBayesianBase<Kalman<STA_MAP, OBS_MAP>> {

 public:
  //! Type of process object
  using TProcess =
      Hierarchical<Markov<Gaussian, STA_MAP, Gaussian>,
                   Memoryless<Gaussian, OBS_MAP>>;
  //! Type of the posterior state \f$(\hat{\mathbf{x}}, \hat{\mathbf{P}})\f$
  using TCompeleteState =
      std::tuple<arma::vec, arma::mat>;

 private:
  //! The process object
  TProcess process_;
  //! The state transition matrix \f$\mathbf{F}\f$
  const arma::mat &dyn_mat_;
  //! The measurement matrix \f$\mathbf{H}\f$
  const arma::mat &mes_mat_;
  //! The covariance of dynamic noise \f$\mathbf{Q}\f$
  const arma::mat &dyn_cov_;
  //! The covariance of measurement noise \f$\mathbf{R}\f$
  const arma::mat &mes_cov_;
  //! The corrected state vector \f$\mathbf{x}_{t|t}\f$
  arma::vec state_vec_;
  //! The corrected state covariance \f$\mathbf{P}_{t|t}\f$
  arma::mat state_cov_;
  //! The predicted state vector \f$\mathbf{x}_{t|t-1}\f$
  arma::vec p_state_vec_;
  //! The predicted state covariance \f$\mathbf{P}_{t|t-1}\f$
  arma::mat p_state_cov_;

 public:
  /** Construct a Kalman filter
   *
   * Construct a Kalman filter with parameters taken from \p process argument.
   */
  Kalman(const TProcess &process)
      : process_(process),
        dyn_mat_(
            process_.template getProcess<0>().getCPDF().getParamMap().transfer),
        mes_mat_(
            process_.template getProcess<1>().getCPDF().getParamMap().transfer),
        dyn_cov_(process_.template getProcess<0>()
                     .getCPDF()
                     .getParamMap()
                     .covariance),
        mes_cov_(process_.template getProcess<1>()
                     .getCPDF()
                     .getParamMap()
                     .covariance) {}

  /** Prediction
   *
   * Performs the prediction step.
   *
   * \f{equation}{\hat{\mathbf{x}}_{t|t-1} = \mathbf{F}\hat{\mathbf{x}}_{t-1|t-1}+u_d(y^d_1,
   * \cdots, y^d_{N_d}) \f}
   * \f{equation}{\mathbf{P}_{t|t-1} = \mathbf{F}\mathbf{P}_{t-1|t-1}\mathbf{F}^T+\mathbf{Q}\f}
   *
   * @param args... Control variables \f$y^d_1, \cdots, y^d_{N_d}\f$ of the dynamic process, if any.
   */
  template <class... TArgs>
  void predict(const TArgs &... args) {
    // use the map function to pass controls, avoiding control definition
    // is move used here??
    p_state_vec_ =
        std::get<0>(process_.template getProcess<0>().getCPDF().getParamMap()(
            state_vec_, args...));
    p_state_cov_ = dyn_mat_ * state_cov_ * dyn_mat_.t() + dyn_cov_;
  }

  /** Correction
   *
   * Performs correction step.
   *
   * \f{equation}{\tilde{\mathbf{z}}_t=\mathbf{z}_t-\mathbf{H}\hat{\mathbf{x}}_{t|t-1}-u_m(y^m_1, \cdots, y^m_{N_m})\f}
   * \f{equation}{\mathbf {S}_t=\mathbf{H}\mathbf{P}_{t|t-1}\mathbf{H}^T+\mathbf{R} \f}
   * \f{equation}{\mathbf{K}_t=\mathbf{P}_{t|t-1}\mathbf{H}^T\mathbf{S}_t^{-1} \f}
   * \f{equation}{\hat{\mathbf{x}}_{t|t}=\hat{\mathbf{x}}_{t|t-1}+\mathbf{K}_{t}\tilde{\mathbf{z}}_t \f}
   * \f{equation}{\mathbf{P}_{t|t}=(I-\mathbf{K}_t\mathbf{H})\mathbf{P}_{t|t-1}\f}
   *
   * @param measurement Measurement vector \f$\mathbf{z}_t\f$.
   * @param args... Control variables \f$y^m_1, \cdots, y^m_{N_m}\f$ of the measurement process, if any.
   * @return Estimated state \f$(\hat{\mathbf{x}}_{t|t}, \mathbf{P}_{t|t})\f$
   */
  template <class... TArgs>
  TCompeleteState correct(const arma::vec &measurement,
                          const TArgs &... args) {
    arma::vec inovation =
        measurement -
        std::get<0>(process_.template getProcess<1>().getCPDF().getParamMap()(
            p_state_vec_, args...));

    arma::mat inovation_cov = mes_mat_ * p_state_cov_ * mes_mat_.t() + mes_cov_;
    arma::mat kalman_gain =
        p_state_cov_ * mes_mat_.t() * arma::inv_sympd(inovation_cov);

    state_vec_ = p_state_vec_ + kalman_gain * inovation;
    state_cov_ = p_state_cov_ - kalman_gain * mes_mat_ * p_state_cov_;
    return std::make_tuple(state_vec_, state_cov_);
  }
  /** Initialization
   *
   * @return Initial state \f$(\hat{\mathbf{x}}_{0|0}, \mathbf{P}_{0|0})\f$
   */
  TCompeleteState initialize() {
    state_vec_ = process_.template getProcess<0>().getInitialPDF().getMean();
    state_cov_ =
        process_.template getProcess<0>().getInitialPDF().getCovariance();
    return std::make_tuple(state_vec_, state_cov_);
  }
};

template <class STA_MAP, class OBS_MAP>
Kalman<STA_MAP, OBS_MAP> makeKalman(
    Hierarchical<Markov<Gaussian, STA_MAP, Gaussian>,
                 Memoryless<Gaussian, OBS_MAP>> process) {
  return Kalman<STA_MAP, OBS_MAP>(process);
}

} // namespace filter
} // namespace ssmkit

#endif // SSMPACK_FILTER_KALMAN_HPP


//
// Created by kellerberrin on 5/08/18.
//

#include "kel_exec_env.h"
#include "kel_distribution.h"

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/hypergeometric.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/negative_binomial.hpp>

namespace bm = boost::math;
namespace kel = kellerberrin;

/////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Distribution functions use boost special functions.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::NormalDistribution::pdf(double x, double mean, double std_dev) {

  static const double inv_sqrt_2pi = 0.3989422804014327;

  double a = (x - mean) / std_dev;

  return (inv_sqrt_2pi / std_dev) * std::exp(-0.5 * a * a);

}

double kel::GammaDistribution::pdf(double x) const {

  return bm::pdf(bm::gamma_distribution<>(_a, _b), x);

}

double kel::GammaDistribution::cdf(double x) const {

  return bm::cdf(bm::gamma_distribution<>(_a, _b), x);

}

double kel::GammaDistribution::quantile(double p) const {

  return bm::quantile(bm::gamma_distribution<>(_a, _b), p);

}

double kel::BetaDistribution::logInverseBetaFunction(double a, double b) {

  assert(a > 0.0);
  assert(b > 0.0);

  double log_gamma = bm::lgamma<double>(a + b) - bm::lgamma<double>(a) - bm::lgamma<double>(b);

  return log_gamma;

}

double kel::BetaDistribution::logPartialPdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a >= 0);
  assert(b >= 0);

  double ret =  (b - 1) * std::log(1 - x) + (a - 1) * std::log(x);

  return ret;

}

double kel::BetaDistribution::logPdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a > 0);
  assert(b > 0);

  double ret = logInverseBetaFunction(a, b) + logPartialPdf(x, a, b);

  return ret;

}

double kel::BetaDistribution::pdf(double x, double a, double b) {

  assert(x >= 0 && x <= 1);
  assert(a > 0);
  assert(b > 0);

  double p = bm::tgamma<double>(a + b) / (bm::tgamma<double>(a) * bm::tgamma<double>(b));
  double q = std::pow(1 - x, b - 1) * std::pow(x, a - 1);
  return p * q;

}

double kel::BetaDistribution::mean(double a, double b) {

  assert(a > 0);
  assert(b > 0);

  return a / (a + b);

}

double kel::BetaDistribution::var(double a, double b) {

  assert(a > 0);
  assert(b > 0);

  return (a * b) / ((a + b) * (a + b) * (a + b + 1.0));

}

double kel::BetaDistribution::mode(double a, double b) {

  assert(a > 1);
  assert(b > 1);

  return (a - 1.0) / (a + b - 2.0);

}

double kel::BetaBinomialDistribution::pdf(size_t n, size_t k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0);
  assert(beta > 0);

  double coeff = bm::binomial_coefficient<double>(n, k);

  double a1 = static_cast<double>(k) + alpha;
  double b1 = static_cast<double>(n-k) + beta;

  double p = bm::beta<double>(a1, b1);
  double q = bm::beta<double>(alpha, beta);

  return coeff * (p / q);

}

double kel::BetaBinomialDistribution::partialPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0);
  assert(beta > 0);

  double a1 = k + alpha;
  double b1 = n - k + beta;

  double p = bm::beta<double>(a1, b1);
  double q = bm::beta<double>(alpha, beta);

  return (p / q);

}

double kel::BetaBinomialDistribution::logPartialPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0.0);
  assert(beta > 0.0);

  double r = n - k;
  double a1 = k + alpha;
  double b1 = r + beta;

  double beta_n = bm::lgamma<double>(a1) + bm::lgamma<double>(b1) - bm::lgamma<double>(a1 + b1);
  double beta_d = bm::lgamma<double>(alpha) + bm::lgamma<double>(beta) - bm::lgamma<double>(alpha + beta);
  double prob = beta_n - beta_d;

  return prob;

}

double kel::BetaBinomialDistribution::logPdf(double n, double k, double alpha, double beta) {

  assert(k <= n);
  assert(alpha > 0.0);
  assert(beta > 0.0);

  double r = n - k;
  double a1 = k + alpha;
  double b1 = r + beta;

  double beta_coeff = bm::lgamma<double>(r + 1.0) + bm::lgamma<double>(k + 1.0) - bm::lgamma<double>(n + 2.0);
  double inverse_log_binonimal_coeff = std::log(n + 1.0) + beta_coeff;
  double beta_n = bm::lgamma<double>(a1) + bm::lgamma<double>(b1) - bm::lgamma<double>(a1 + b1);
  double beta_d = bm::lgamma<double>(alpha) + bm::lgamma<double>(beta) - bm::lgamma<double>(alpha + beta);
  double prob = beta_n - beta_d - inverse_log_binonimal_coeff;

  return prob;

}

// .first is alpha, .second is beta. The raw moments are calculated from the observations and used to calculate alpha and beta.
[[nodiscard]] std::pair<double, double> kel::BetaBinomialDistribution::methodOfMoments(const std::vector<size_t>& observations, size_t n_trials) {

  // calculate the first moment
  size_t obs_sum = std::accumulate(observations.begin(), observations.end(), static_cast<size_t>(0));
  const double m1 = static_cast<double>(obs_sum) / static_cast<double>(observations.size());

  // calculate the 2nd moment
  auto sqr_lambda = [](size_t accumulate, size_t obs)->size_t { return accumulate + (obs * obs); };
  size_t sqr_sum = std::accumulate(observations.begin(), observations.end(), static_cast<size_t>(0), sqr_lambda);
  const double m2 = static_cast<double>(sqr_sum) / static_cast<double>(observations.size());

  const double n = static_cast<double>(n_trials);

  const double a_numer = (n * m1) - m2;
  const double ab_denom = n * ((m2 / m1) - m1 - 1) + m1;
  const double a = a_numer / ab_denom;

  const double b_numer = (n - m1) * (n - (m2/m1));
  const double b = b_numer / ab_denom;

  return {a, b};

}

double kel::BinomialDistribution::pdf(size_t n, size_t k, double prob_success) {

  assert(k <= n and k >= 0);
  assert(prob_success >= 0 and prob_success <= 1.0);

  double coeff = bm::binomial_coefficient<double>(n, k);

  double p = std::pow(prob_success, static_cast<double>(k));

  double q = std::pow((1.0 - prob_success), static_cast<double>(n - k));

  return coeff * p * q;

}

double kel::BinomialDistribution::cdf(size_t n, double k, double prob_success) {

  assert(k <= n and k >= 0);
  assert(prob_success >= 0 and prob_success <= 1.0);

  size_t integer_k = std::floor(k);
  double sum_pdf = 0.0;

  for (size_t index = 0; index <= integer_k; ++index) {

    sum_pdf += cdf(n, index, prob_success);

  }

  return sum_pdf;

}

kel::HypergeometricDistribution::HypergeometricDistribution(size_t pop_successes_K, size_t sample_size_n, size_t population_N) {

  if (pop_successes_K > population_N) {

    ExecEnv::log().warn("HypergeometricDistribution::HypergeometricDistribution; Population Successes K:{} exceeds population size :{}",
                         pop_successes_K, population_N);
    pop_successes_K = population_N;
  }

  if (sample_size_n > population_N) {

    ExecEnv::log().warn("HypergeometricDistribution::HypergeometricDistribution; Sample size n:{} exceeds population size :{}",
                         sample_size_n, population_N);
    sample_size_n = population_N;
  }

  pop_successes_K_ = pop_successes_K;
  sample_size_n_ = sample_size_n;
  population_N_ = population_N;

}

double kel::HypergeometricDistribution::pdf(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::pdf; r_successes k:{} exceeds upper limit :{}",
                         successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::pdf; r_successes k:{} below lower limit :{}",
                         successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::pdf(hypergeometric, successes_k);

}

double kel::HypergeometricDistribution::cdf(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution(N:{},K:{},n:{},k:{})::cdf; r_successes k exceeds upper limit :{}",
                        population_N_, pop_successes_K_, sample_size_n_, successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution(N:{},K:{},n:{},k:{})::cdf; r_successes k exceeds upper limit :{}",
                        population_N_, pop_successes_K_, sample_size_n_, successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::cdf(hypergeometric, successes_k);

}

double kel::HypergeometricDistribution::quantile(size_t successes_k) const {

  if (successes_k > upperSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::quantile; r_successes k:{} exceeds upper limit :{}",
                         successes_k, upperSuccesses_k());
    successes_k = upperSuccesses_k();

  }

  if (successes_k < lowerSuccesses_k()) {

    ExecEnv::log().warn("HypergeometricDistribution::quantile; r_successes k:{} below lower limit :{}",
                         successes_k, lowerSuccesses_k());
    successes_k = lowerSuccesses_k();

  }

  bm::hypergeometric_distribution hypergeometric(pop_successes_K_, sample_size_n_, population_N_);

  return bm::quantile(hypergeometric, successes_k);

}

size_t kel::HypergeometricDistribution::lowerSuccesses_k() const {

  int64_t lower_success = static_cast<int64_t>(sample_size_n_ + pop_successes_K_) - static_cast<int64_t>(population_N_);
  return static_cast<size_t>(std::max<int64_t>(0, lower_success));

}

double kel::HypergeometricDistribution::upperSingleTailTest(size_t test_value_k) const {

  if (test_value_k > 0) {

    test_value_k = test_value_k - 1;

  }

  return 1.0 - cdf(test_value_k);

}

double kel::HypergeometricDistribution::lowerSingleTailTest(size_t test_value_k) const {

  return cdf(test_value_k);

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// The Poisson distribution. Uses boost for implementation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::Poisson::pdf(size_t count) const {

  return bm::pdf(bm::poisson_distribution<>(lambda_), count);

}

double kel::Poisson::cdf(size_t count) const  {

  return bm::cdf(bm::poisson_distribution<>(lambda_), count);

}

size_t kel::Poisson::quantile(double quantile) const {

  return bm::quantile(bm::poisson_distribution<>(lambda_), quantile);

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// The Negative Binomial distribution. Uses boost for implementation.
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////

double kel::NegativeBinomial::pdf(size_t count) const {

  return bm::pdf(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), count);

}

double kel::NegativeBinomial::cdf(size_t count) const {

  return bm::cdf(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), count);

}

size_t kel::NegativeBinomial::quantile(double quantile) const {

  return bm::quantile(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_), quantile);

}

double kel::NegativeBinomial::mean() const {

  return bm::mean(bm::negative_binomial_distribution<>(r_successes_, p_prob_success_));

}