//GlobalFunctions.cpp

//Modification Date: 6/22/15

#include <boost/math/distributions/normal.hpp>
#include <cmath>
#include <stdio.h>
#include <iostream>
#include <vector>
#include "GlobalFunctions.hpp"
using namespace std;

double CallPrice(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return (S * exp((b - r)*T) * N(d1)) - (K * exp(-r * T)* N(d2));
}

double CallPricePCP(double P, double S, double K, double T, double r, double b) {
	return P + S*exp((b-r)*T) - K*exp(-r*T);
}

double PutPrice(double S, double K, double T, double r, double sig, double b){
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return (K * exp(-r*T) * N(-d2)) - (S * exp((b - r) * T) * N(-d1));
}

double PutPricePCP(double C, double S, double K, double T, double r, double b) {
	return C - S*exp((b - r)*T) + K*exp(-r*T);
}

std::vector<double> MeshArray(double LowerLimit, double UpperLimit, int Num)			//N - amount of steps
{
	vector<double> mesh;
	mesh.reserve(Num+1);
	double h = (UpperLimit - LowerLimit) / Num;

	for (double x = LowerLimit; x <= UpperLimit; x += h)
	{
		mesh.push_back(x);
	}
	return mesh;
}

void PrintVector (const vector<double>& V)
{
	for (vector<double>::const_iterator it = V.begin(); it != V.end(); ++it)
		cout << *it << ", ";
}

double N(double x) {
	boost::math::normal_distribution<> myNormal(0.0, 1.0);
	return boost::math::cdf(myNormal, x);
}

double n(double x) {
	boost::math::normal_distribution<> myNormal(0.0, 1.0);
	return boost::math::pdf(myNormal, x);
}

double CallDelta(double S, double K, double T, double r, double sig, double b)
{
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return exp((b - r)*T) * N(d1);
}

double PutDelta(double S, double K, double T, double r, double sig, double b)
{
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return exp((b - r)*T) * (N(d1) - 1.0);
}

double GammaGF(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return n(d1)*exp((b - r)*T) / (S*tmp);
}

double VegaGF(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;

	return S*sqrt(T)*exp((b - r)*T)*n(d1);
}

double CallTheta(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -S*sig*exp((b - r)*T)*n(d1) / (2 * sqrt(T)) - (b - r)*S*exp((b - r)*T)*N(d1) - r*K*exp(-r*T)*N(d2);
}

double PutTheta(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -S*sig*exp((b - r)*T)*n(d1) / (2 * sqrt(T)) - (b - r)*S*exp((b - r)*T)*N(-d1) + r*K*exp(-r*T)*N(-d2);
}

double CallRho(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return 0.01*K*T*exp(- r*T)*N(d2);
}

double PutRho(double S, double K, double T, double r, double sig, double b) {
	double tmp = sig * sqrt(T);

	double d1 = (log(S / K) + (b + (sig*sig)*0.5) * T) / tmp;
	double d2 = d1 - tmp;

	return -0.01*K*T*exp(- r*T)*N(-d2);
}

double CallDelta(double S, double h, double K, double T, double r, double sig, double b) {		//Delta Aproximation  using divided difference
	return (CallPrice(S + h, K, T, r, sig, b) - CallPrice(S - h, K, T, r, sig, b)) / (2 * h);
}

double PutDelta(double S, double h, double K, double T, double r, double sig, double b) {		//Delta Aproximation  using divided difference
	return (PutPrice(S + h, K, T, r, sig, b) - PutPrice(S - h, K, T, r, sig, b)) / (2 * h);
}

double GammaGF(double S, double h, double K, double T, double r, double sig, double b) {		//Gamma Aproximation  using divided difference
	return (CallPrice(S + h, K, T, r, sig, b) - 2 * CallPrice(S, K, T, r, sig, b) + CallPrice(S - h, K, T, r, sig, b)) / (h * h);
}

double PerpetualCall(double S, double K, double r, double sig, double b)
{ // Dividend q = r - b

	double sig2 = sig*sig;								//sig to the second power defined for convenience
	double fac = b / sig2 - 0.5; fac *= fac;			//fac to the second power defined for convenience
	double y1 = 0.5 - b / sig2 + sqrt(fac + 2.0*r / sig2);

	if (1.0 == y1)								//no need to calculate the function if y1 equals to 1,
		return S;

	double fac2 = ((y1 - 1.0)*S) / (y1 * K);
	double c = K * pow(fac2, y1) / (y1 - 1.0);

	return c;
}

double PerpetualPut(double S, double K, double r, double sig, double b)
{
	double sig2 = sig*sig;
	double fac = b / sig2 - 0.5; fac *= fac;
	double y2 = 0.5 - b / sig2 - sqrt(fac + 2.0*r / sig2);

	if (0.0 == y2)
		return S;

	double fac2 = ((y2 - 1.0)*S) / (y2 * K);
	double p = K * pow(fac2, y2) / (1.0 - y2);

	return p;
}


#include <Eigen/Core>
#include <iostream>
#include <mathtoolbox/rbf-interpolation.hpp>
#include <random>
#include <vector>

using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

namespace
{
    std::random_device                     seed;
    std::default_random_engine             engine(seed());
    std::uniform_real_distribution<double> uniform_dist(-1.0, 1.0);

    double CalcFunction(const Vector2d& x) { return std::sin(10.0 * x(0)) + std::sin(10.0 * x(1)); }
} // namespace

int main()
{
    // Generate scattered data (in this case, 500 data points in a 2-dimensional space)
    constexpr int    number_of_samples = 500;
    constexpr double noise_intensity   = 0.1;
    Eigen::MatrixXd  X(2, number_of_samples);
    Eigen::VectorXd  y(number_of_samples);
    for (int i = 0; i < number_of_samples; ++i)
    {
        X.col(i) = Vector2d(uniform_dist(engine), uniform_dist(engine));
        y(i)     = CalcFunction(X.col(i)) + noise_intensity * uniform_dist(engine);
    }

    // Define interpolation settings
    const auto     kernel             = mathtoolbox::ThinPlateSplineRbfKernel();
    constexpr bool use_regularization = true;

    // Instantiate an interpolator
    mathtoolbox::RbfInterpolator rbf_interpolator(kernel);

    // Set data
    rbf_interpolator.SetData(X, y);

    // Calculate internal weights with or without regularization
    rbf_interpolator.CalcWeights(use_regularization);

    // Calculate and print interpolated values on randomly sampled points in CSV format
    constexpr int number_of_test_samples = 100;
    std::cout << "x(0),x(1),y" << std::endl;
    for (int i = 0; i < number_of_test_samples; ++i)
    {
        const Vector2d x = Vector2d(uniform_dist(engine), uniform_dist(engine));
        const double   y = rbf_interpolator.CalcValue(x);

        std::cout << x(0) << "," << x(1) << "," << y << std::endl;
    }

    return 0;
}


/* Functions-3.hpp (exercise 1.5.3)
Description:
	* Functions for exercise 1.5.3.
Functions:
	*tuple<double,double,double,double,double> GetStatisticalPRoperties(const Container&): calculate the mean, mean deviation, range, variance and standard deviation of passed dataset stored in STL container.
	*Type median(const Container<Type, Alloc>&): calculate median of dataset stored in passed sorted STL container.
	*Type mode(const Container<Type, Alloc>&): calculate the mode of dataset stored in passed STL container. If multimodal, then return the smallest mode.
*/

#ifndef FUNCTIONS3_HPP
#define FUNCTIONS3_HPP

#ifndef _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
	#define _SILENCE_ALL_CXX17_DEPRECATION_WARNINGS
#endif

#include <boost/numeric/ublas/vector.hpp>
#include <boost/range/numeric.hpp>
#include <memory>
#include <numeric>
#include <set>
#include <tuple>
#include "Exception.hpp"
#include "SizeExcept.hpp"

/* 1.5.3: */
// a) Create a function that returns statistical properties of dataset implemented in a boost::vector:
template<typename Container>
std::tuple<double, double, double, double, double> GetStatisticalProperties(const Container &values)
{
	std::size_t numElements = values.size();
	double mean, meanDev = 0, range, var, stdDev;
	if (numElements > 0)
	{
		// Calculate mean:
		mean = std::accumulate(values.begin(), values.end(), 0.0L, [&](double first, double second) { return first + second; }) / (double)numElements;
		// Calculate mean deviation:
		std::for_each(values.begin(), values.end(), [&](double element) { meanDev += std::abs(element - mean) / (double)numElements; });
		// meanDev = std::accumulate(values.begin(), values.end(), 0.0L, [&](double first, double second) { return std::abs(first - mean) + second; }) / (double)numElements;
		// Calculate range:
		range = (double) *std::max_element(values.begin(), values.end()) - (double) *std::min_element(values.begin(), values.end());
		// Calculate the variance (E[(x - mu)^2] = E(x^2) - E(x)^2).
		var = boost::inner_product(values, values, 0.0L) / (double)numElements - mean * mean;
		// Calculate the standard deviation:
		stdDev = std::sqrt(var);
	}
	else
	{
		throw Exceptions::SizeExcept("There must be at least one element in the passed vector.");
	}
	return std::make_tuple<double, double, double, double, double>(std::move(mean), std::move(meanDev), std::move(range), std::move(var), std::move(stdDev));
}
// b) Create a function that returns median and mode of dataset stored in boost::vector:
template<template<typename T, typename> class Container, class Type, class Alloc = std::allocator<Type>>
Type median(const Container<Type, Alloc> &values)											/* Get median of elements in passed sorted container. */
{
	if (values.size() % 2 == 0)
	{
		return values[values.size() / 2]  / 2.0 + values[values.size() / 2 + 1] / 2.0;
	}
	else
	{
		return values[values.size() / 2];
	}
}

template<template<typename, typename> class Container, class Type, class Alloc = std::allocator<Type>>
Type mode(const Container<Type, Alloc> &values)												/* Get mode of elements in passed container. */
{
	std::map<Type, std::size_t> uniqueCounts;
	std::set<Type> modeSet;
	std::size_t modeIndex = 0, maxFreq = 0;
	for (auto elem = values.begin(); elem != values.end(); elem++)
	{
		// Add element to unique list if not present or increment frequency by 1:
		if (uniqueCounts.find(*elem) != uniqueCounts.end())
		{
			uniqueCounts[*elem]++;
		}
		else
		{
			uniqueCounts.insert(std::pair<Type, std::size_t>(*elem, 1));
		}
	}
	if (uniqueCounts.size())
	{
		// Find the maximum frequency:
		std::for_each(uniqueCounts.begin(), uniqueCounts.end(),
		[&](auto elem)
		{
			std::size_t currFreq = std::get<1>(elem);
			if (maxFreq < currFreq)
			{
				maxFreq = currFreq;
			}
		});
		// Put all uniques that share the maximum frequency into the mode-set:
		std::for_each(uniqueCounts.begin(), uniqueCounts.end(),
		[&](auto elem)
		{
			if (std::get<1>(elem) == maxFreq)
			{
				modeSet.emplace(std::get<0>(elem));
			}
		});
		// Return the first element in the set (std::set is in ascending order by default so first element is the smallest mode):
		return *modeSet.begin();
	}
	else
	{
		throw Exceptions::SizeExcept("There must be at least one element in container to calculate the mode. ");
	}
}

#endif

Advanced C++ Course/Benjamin Rutan HW 1 Submission/1.5/1.5/Functions-3.hpp


#include "Classes.h"
#include <Eigen/Dense>
#include <math.h>
using namespace Eigen;

int Wireframe::normalise() {
    double sumx = 0.0;
    double sumy = 0.0;
    double sumz = 0.0;
    for(int i = 0; i < this->vertices.size(); i++) {
        sumx += this->vertices[i].x;
        sumy += this->vertices[i].y;
        sumz += this->vertices[i].z;
    }
    sumx = sumx/(this->vertices.size());
    sumy = sumy/(this->vertices.size());
    sumz = sumz/(this->vertices.size());
    for(int i = 0; i < this->edges.size(); i++) {
        this->edges[i].p1.x -= sumx;
        this->edges[i].p1.y -= sumy;
        this->edges[i].p1.z -= sumz;
        this->edges[i].p2.x -= sumx;
        this->edges[i].p2.y -= sumy;
        this->edges[i].p2.z -= sumz;
    }
    return 0;
}

Wireframe* Wireframe::projectFrame() {
    ///
    /// Project the frame on xz plane
    ///
    double plane[4] = {0,0,1,0};
    Wireframe* projected;
    projected = new Wireframe;
    for(int i = 0; i < this->edges.size(); i++) {
        Edge edge;
        edge.p1 = edges[i].p1.projectPoint(plane);
        edge.p2 = edges[i].p2.projectPoint(plane);
        projected->edges.push_back(edge);
    }
    return projected;
}

int Wireframe::rotateFrame(int type) {
    ///
    /// Rotate the Wireframe by ten degrees
    ///
    Matrix3d rot;
    double theta = 0.2;
    double costheta = cos(theta);
    double sintheta = sin(theta);
    if(type==1) {
        rot << 1, 0, 0,
            0, costheta, sintheta,
            0, -sintheta, costheta;
    }
    if(type==2) {
        rot << costheta, 0, -sintheta,
            0, 1, 0,
            sintheta, 0, costheta;
    }
    if(type==3) {
        rot << 1, 0, 0,
            0, costheta, -sintheta,
            0, sintheta, costheta;
    }
    if(type==4) {
        rot << costheta, 0, sintheta,
            0, 1, 0,
            -sintheta, 0, costheta;
    }
    for(int i = 0; i < this->edges.size(); i++) {
        Vector3d p1(edges[i].p1.x,edges[i].p1.y,edges[i].p1.z);
        Vector3d p2(edges[i].p2.x,edges[i].p2.y,edges[i].p2.z);
        Vector3d p1new = rot*p1;
        Vector3d p2new = rot*p2;
        edges[i].p1.setCoordinates(p1new.x(),p1new.y(),p1new.z());
        edges[i].p2.setCoordinates(p2new.x(),p2new.y(),p2new.z());
    }
    return 0;
}


#pragma once

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

class EKF {

public:

    EKF(const Eigen::MatrixXd& P0,
        const Eigen::MatrixXd& Q,
        const Eigen::MatrixXd& R,
        const float& dt ) : initialized(false), P_post(P0), P_prio(P0), Q(Q), R(R), dt(dt)
    {}

    void init(const Eigen::VectorXd& x0) {
        x_hat_prio = x0;
        x_hat_post = x0;

        I = Eigen::MatrixXd::Identity(x0.size(), x0.size());

        initialized = true;
    }

    void prediction_update(const Eigen::VectorXd& U) {
        if(!initialized) {
            throw std::runtime_error("Filter is not initialized!");
        }

        // Use the motion model to predict a-priori estimate
        this->x_hat_prio = this->motion_model(this->x_hat_post, U, this->dt);
        Eigen::MatrixXd jF = this->jacobian_F(this->x_hat_prio, U, this->dt);
        P_prio = jF * P_post * jF.transpose() + Q;
    }

    void innovation_update(const Eigen::VectorXd& Z) {
        Eigen::MatrixXd jH = this->jacobian_H(this->x_hat_prio);
        Eigen::VectorXd z_predict = this->observation_model(this->x_hat_prio);

        Eigen::MatrixXd K = P_prio * jH.transpose() * (jH * P_prio * jH.transpose() + R).inverse();
        this->x_hat_post = this->x_hat_prio + K * (Z - z_predict);
        P_post = (I - K * jH) * P_prio;
    }

    // Return the current state
    Eigen::VectorXd get_state() {return x_hat_post;};

    // Return the current state error covariance
    Eigen::MatrixXd get_P() {return P_post;};

    // is the filter initialized?
    bool initialized;

    // // Motion model (user defined)
    std::function<Eigen::VectorXd(const Eigen::VectorXd& x_hat_post,
                                  const Eigen::VectorXd& u_measured,
                                  const float& dt)> motion_model;

    // Observation model (user defined)
    std::function<Eigen::VectorXd(const Eigen::VectorXd&)> observation_model;

    // Jacobian of the motion model
    std::function<Eigen::MatrixXd(const Eigen::VectorXd& x_hat_prio,
                                  const Eigen::VectorXd& u_measured,
                                  const float& dt)> jacobian_F;

    // Jacobian of the observation model
    std::function<Eigen::MatrixXd(const Eigen::VectorXd& x_hat_prio)> jacobian_H;

private:
    // Estimated states
    Eigen::VectorXd x_hat_prio, x_hat_post;

    float dt;

    Eigen::MatrixXd P_post, P_prio, Q, R, I;

};


#pragma once

#include <armadillo>
#include "estructura_capas_red.cpp"

using namespace std;
using namespace arma;

namespace ic {

vec sigmoid(const vec& v)
{
	return 2 / (1 + exp(-0.5 * v)) - 1;
}

vector<vec> salidaMulticapa(const vector<mat>& pesos,
                            const vec& patron)
{
	// Este multicapa va a tener salida lineal sólo en la capa de salida.
	// En las demás capas se va a usar salida sigmoidea.
	vector<vec> ySalidas;

	// Calculo de la salida para la primer capa
	{
		vec v = pesos[0] * join_vert(vec{-1}, patron); // agrega entrada correspondiente al sesgo

		if (pesos.size() != 1) { // Si hay más de una capa, a la salida de la primera
			v = sigmoid(v);      // hay que aplicarle sigmoidea.
		}

		ySalidas.push_back(v);
	}

	// Calculo de las salidas para las demas capas
	for (unsigned int i = 1; i < pesos.size(); ++i) {
		vec v = pesos[i] * join_vert(vec{-1}, ySalidas[i - 1]); // agrega entrada correspondiente al sesgo

		if (i != pesos.size() - 1) { // Para todas las capas menos la última, usar salida sigmoidea.
			v = sigmoid(v);
		}

		ySalidas.push_back(v);
	}

	return ySalidas;
}

double errorCuadraticoMulticapa(const vector<mat>& pesos,
                                const mat& patrones,
                                const mat& salidaDeseada)
{
	double errorCuadraticoTotal = 0;

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());
		const vec salidaRed = ySalidas.back();

		const double errorCuadraticoPatron = sum(pow(salidaDeseada.row(n).t() - salidaRed, 2));
		errorCuadraticoTotal += errorCuadraticoPatron;
	}

	return errorCuadraticoTotal;
}

double errorRelativoPromedioMulticapa(const vector<mat>& pesos,
                                      const mat& patrones,
                                      const mat& salidaDeseada)
{
	double sumaErroresRelativos = 0;

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());
		const vec salidaRed = ySalidas.back();

		double sumaParcial = 0;

		for (unsigned int i = 0; i < salidaDeseada.n_cols; ++i) {
			const double errorRelativoPatron = abs(salidaRed(i) - salidaDeseada(n, i)) / abs(salidaDeseada(n, i));
			sumaParcial += errorRelativoPatron;
		}

		// Promedio del error absoluto a lo largo de todas las salidas
		// de este patrón.
		sumaErroresRelativos += sumaParcial / salidaDeseada.n_cols;
	}

	// Promedio del error a lo largo de todos los patrones
	return sumaErroresRelativos / patrones.n_rows * 100;
}

vector<mat> epocaMulticapa(const mat& patrones,
                           const mat& salidaDeseada,
                           double tasaAprendizaje,
                           double inercia,
                           vector<mat> pesos)
{
	//Entrenamiento
	vector<mat> deltaWOld;
	for (unsigned int i = 0; i < pesos.size(); ++i)
		deltaWOld.push_back(zeros(pesos[i].n_rows, pesos[i].n_cols));

	for (unsigned int n = 0; n < patrones.n_rows; ++n) {
		// Calcular las salidas para cada capa
		const vector<vec> ySalidas = salidaMulticapa(pesos, patrones.row(n).t());

		// Calculo del error
		const vec error = salidaDeseada.row(n).t() - ySalidas.back();

		// Calculo retropropagacion
		// Calculo de gradiente error local instantaneo
		vector<vec> delta;
		// Tenemos tantos vectores de deltas como capas
		delta.resize(ySalidas.size());

		// Delta de ultima capa
		delta[delta.size() - 1] = error;
		// Deltas de las capas anteriores
		for (int i = ySalidas.size() - 2; i >= 0; --i) {
			// No participan los pesos correspondientes al sesgo en el cálculo de los deltas
			const mat pesosAux = pesos[i + 1].tail_cols(pesos[i + 1].n_cols - 1);
			delta[i] = pesosAux.t() * delta[i + 1];
		}

		// Actualizacion de pesos de todas las capas menos la primera.
		// Si la red tiene una sola capa, no se entra acá.
		for (int i = pesos.size() - 1; i >= 1; --i) {
			const mat deltaWnuevo = tasaAprendizaje
			                            * delta[i]
			                            * join_horiz(vec{-1}, ySalidas[i - 1].t())
			                        + inercia * deltaWOld[i];
			pesos[i] += deltaWnuevo;
			deltaWOld[i] = deltaWnuevo;
		}

		// Actualización de pesos de la primer capa
		const mat deltaW = tasaAprendizaje
		                       * delta[0]
		                       * join_horiz(vec{-1}, patrones.row(n))
		                   + inercia * deltaWOld[0];
		pesos[0] += deltaW;
		deltaWOld[0] = deltaW;
	}

	return pesos;
} // fin funcion Epoca

tuple<vector<mat>, double, int> entrenarMulticapa(const EstructuraCapasRed& estructura,
                                                  const mat& datos,
                                                  int nEpocas,
                                                  double tasaAprendizaje,
                                                  double inercia,
                                                  double tolErrorRelativoPromedio,
                                                  bool monitoreo = false)
{
    mat partEntrenamiento;
    mat partMonitoreo;

    if (monitoreo) {
        const int nEntrenamiento = datos.n_rows * 0.9;
        partEntrenamiento = datos.head_rows(nEntrenamiento);
        partMonitoreo = datos.tail_rows(datos.n_rows - nEntrenamiento);
    }
    else {
        partEntrenamiento = datos;
    }

	const int nSalidas = estructura(estructura.n_elem - 1);
    const int nEntradas = partEntrenamiento.n_cols - nSalidas;
	const int nCapas = estructura.n_elem;
	// Vamos a tener tantas columnas en salidaDeseada
	// como neuronas en la capa de salida
    const mat salidaDeseada = partEntrenamiento.tail_cols(nSalidas);
	// Extender la matriz de patrones con la entrada correspondiente al umbral
    const mat patrones = partEntrenamiento.head_cols(nEntradas);

	// Inicializar pesos y tasa de error
	vector<mat> pesos;

	// La primer matriz matriz de pesos tiene tantas filas como neuronas en la primer capa
	// y tantas columnas como componentes tiene la entrada, más la entrada correspondiente
	// al sesgo.
	pesos.push_back(randu(estructura(0), nEntradas + 1) - 0.5);

	for (int i = 1; i < nCapas; ++i) {
		// Las siguientes matrices de pesos tienen tantas filas como neuronas en dicha capa
		// y tantas columnas como entradas a esa capa, que van a ser las salidas de
		// la capa anterior mas la entrada correspondiente al sesgo.
		// Las salidas de la capa anterior es igual al nro de neuronas en la capa anterior.
		pesos.push_back(randu(estructura(i), estructura(i - 1) + 1) - 0.5);
	}

	double errorRelativoPromedio;
    double menorErrorMonitoreo = numeric_limits<double>::max();
    vector<mat> mejoresPesos = pesos;
    //    int mejorEpoca = 0;

	// Ciclo de las epocas
	int epoca = 1;
	for (; epoca <= nEpocas; ++epoca) {
		// Ciclo para una época
		pesos = epocaMulticapa(patrones,
		                       salidaDeseada,
		                       tasaAprendizaje,
		                       inercia,
		                       pesos);

		errorRelativoPromedio = errorRelativoPromedioMulticapa(pesos, patrones, salidaDeseada);
        //        cout << epoca << " Error cuadratico entrenamiento: "
        //             << errorCuadraticoMulticapa(pesos, patrones, salidaDeseada) << endl;

        if (monitoreo) {
            const double errorMonitoreo = errorCuadraticoMulticapa(pesos,
                                                                   partMonitoreo.head_cols(nEntradas),
                                                                   partMonitoreo.tail_cols(nSalidas));
            //            cout << "Error cuadratico monitoreo: " << errorMonitoreo << endl;

            if (errorMonitoreo < menorErrorMonitoreo) {
                menorErrorMonitoreo = errorMonitoreo;
                mejoresPesos = pesos;
                //                mejorEpoca = epoca;
            }
        }

        if (errorRelativoPromedio <= tolErrorRelativoPromedio)
            break;
    }
    // Fin ciclo (epocas)

    // Si el bucle anterior no cortó por tolerancia de error,
    // el for va a incrementar la variable una vez de más.
    if (epoca > nEpocas)
        epoca = nEpocas;

    //    cout << "Mejor epoca: " << mejorEpoca << endl;

    if (monitoreo)
        return make_tuple(mejoresPesos, errorRelativoPromedio, epoca);
    else
        return make_tuple(pesos, errorRelativoPromedio, epoca);
}

struct ParametrosMulticapa {
	EstructuraCapasRed estructuraRed;
	int nEpocas;
	double tasaAprendizaje;
	double inercia;
	double toleranciaError;
};
}

istream& operator>>(istream& is, ic::ParametrosMulticapa& parametros)
{
	// Formato:
	// estructura: [3 2 1]
	// n_epocas: 200
	// tasa_entrenamiento: 0.1
	// inercia: 0.5
	// parametro_sigmoidea: 1
	// tolerancia_error: 5
	string str;

	// No chequeamos si la etiqueta de cada línea está bien o no. No nos importa
	is >> str >> parametros.estructuraRed
	    >> str >> parametros.nEpocas
	    >> str >> parametros.tasaAprendizaje
	    >> str >> parametros.inercia
	    >> str >> parametros.toleranciaError;

	// Control básico de valores de parámetros
	if (parametros.nEpocas <= 0
	    || parametros.tasaAprendizaje <= 0
	    || parametros.toleranciaError <= 0
	    || parametros.toleranciaError >= 100)
		is.clear(ios::failbit);

	return is;
}


/**
 * \file RobertBristowJohnsonFilter.hxx
 */

#include "RobertBristowJohnsonFilter.h"

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType>
  RobertBristowJohnsonLowPassCoefficients<DataType>::RobertBristowJohnsonLowPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - cosw0)/2 / (1 + alpha);
    coefficients_in[1] = (1 - cosw0) / (1 + alpha);
    coefficients_in[0] = (1 - cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonHighPassCoefficients<DataType>::RobertBristowJohnsonHighPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_in[1] = -(1 + cosw0) / (1 + alpha);
    coefficients_in[0] = (1 + cosw0) / 2 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassCoefficients<DataType>::RobertBristowJohnsonBandPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = Q * alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = - Q * alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPass2Coefficients<DataType>::RobertBristowJohnsonBandPass2Coefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPass2Coefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = alpha / (1 + alpha);
    coefficients_in[1] = 0;
    coefficients_in[0] = -alpha / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPass2Coefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPass2Coefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPass2Coefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandStopCoefficients<DataType>::RobertBristowJohnsonBandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandStopCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = 1 / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1 / (1 + alpha);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandStopCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandStopCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandStopCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonAllPassCoefficients<DataType>::RobertBristowJohnsonAllPassCoefficients(gsl::index nb_channels)
    :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonAllPassCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 - alpha) / (1 + alpha);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha);
    coefficients_in[0] = 1;
    coefficients_out[1] = 2 * cosw0 / (1 + alpha);
    coefficients_out[0] = (alpha - 1) / (1 + alpha);
  }

  template <typename DataType_>
  void RobertBristowJohnsonAllPassCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonAllPassCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonAllPassCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template<typename DataType>
  RobertBristowJohnsonBandPassPeakCoefficients<DataType>::RobertBristowJohnsonBandPassPeakCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);

    coefficients_in[2] = (1 + alpha * gain) / (1 + alpha / gain);
    coefficients_in[1] = -2 * cosw0 / (1 + alpha / gain);
    coefficients_in[0] = (1 - alpha * gain) / (1 + alpha / gain);
    coefficients_out[1] = 2 * cosw0 / (1 + alpha / gain);
    coefficients_out[0] = (alpha / gain - 1) / (1 + alpha / gain);
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonBandPassPeakCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonLowShelvingCoefficients<DataType>::RobertBristowJohnsonLowShelvingCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);
    CoeffDataType d = (gain + 1) + (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) - (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = 2 * gain * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = 2 * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonLowShelvingCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonLowShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonLowShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  RobertBristowJohnsonHighShelvingCoefficients<DataType>::RobertBristowJohnsonHighShelvingCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template <typename DataType>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType>::setup()
  {
    Parent::setup();

    CoeffDataType w0 = 2 * boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate;
    CoeffDataType cosw0 = std::cos(w0);
    CoeffDataType alpha = std::sin(w0) / (2 * Q);
    CoeffDataType d = (gain + 1) - (gain - 1) * cosw0 + 2 * std::sqrt(gain) * alpha;

    coefficients_in[2] = gain * ((gain + 1) + (gain - 1) * cosw0 + 2 * sqrt(gain) * alpha) / d;
    coefficients_in[1] = -2 * gain * ((gain - 1) + (gain + 1) * cosw0) / d;
    coefficients_in[0] = gain * ((gain + 1) + (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
    coefficients_out[1] = -2 * ((gain - 1) - (gain + 1) * cosw0) / d;
    coefficients_out[0] = -((gain + 1) - (gain - 1) * cosw0 - 2 * sqrt(gain) * alpha) / d;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_Q() const
  {
    return Q;
  }

  template <typename DataType_>
  void RobertBristowJohnsonHighShelvingCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if (gain <= 0)
    {
      throw std::out_of_range("gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template <typename DataType_>
  typename RobertBristowJohnsonHighShelvingCoefficients<DataType_>::CoeffDataType RobertBristowJohnsonHighShelvingCoefficients<DataType_>::get_gain() const
  {
    return gain;
  }
}


// MomentTools.cpp
//
// Breannan Smith
// Last updated: 09/14/2015

#include "MomentTools.h"

#include <iostream>

#include <Eigen/Eigenvalues>

static void diagonalizeInertiaTensor( const Matrix3s& I, Matrix3s& R0, Vector3s& I0 )
{
  // Inertia tensor should by symmetric
  assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  // Inertia tensor should have positive determinant
  assert( I.determinant() > 0.0 );

  // Compute the eigenvectors and eigenvalues of the input matrix
  const Eigen::SelfAdjointEigenSolver<Matrix3s> es{ I };

  // Check for errors
  if( es.info() == Eigen::NumericalIssue )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NumericalIssue" << std::endl;
  }
  else if( es.info() == Eigen::NoConvergence )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::NoConvergence" << std::endl;
  }
  else if( es.info() == Eigen::InvalidInput )
  {
    std::cerr << "Warning, failed to compute eigenvalues of inertia tensor due to Eigen::InvalidInput" << std::endl;
  }
  assert( es.info() == Eigen::Success );

  // Save the eigenvectors and eigenvalues
  I0 = es.eigenvalues();
  assert( ( I0.array() > 0.0 ).all() );
  assert( I0.x() <= I0.y() );
  assert( I0.y() <= I0.z() );
  R0 = es.eigenvectors();
  assert( fabs( fabs( R0.determinant() ) - 1.0 ) <= 1.0e-6 );

  // Ensure that we have an orientation preserving transform
  if( R0.determinant() < 0.0 )
  {
    R0.col( 0 ) *= -1.0;
  }
}

namespace MomentTools
{

// TODO: most of this function can be vectorized
void computeMoments( const Matrix3Xsc& vertices, const Matrix3Xuc& indices, scalar& mass, Vector3s& I, Vector3s& center, Matrix3s& R )
{
  assert( ( indices.array() < unsigned( vertices.cols() ) ).all() );

  constexpr scalar oneDiv6{ 1.0 / 6.0 };
  constexpr scalar oneDiv24{ 1.0 / 24.0 };
  constexpr scalar oneDiv60{ 1.0 / 60.0 };
  constexpr scalar oneDiv120{ 1.0 / 120.0 };

  // order:  1, x, y, z, x^2, y^2, z^2, xy, yz, zx
  VectorXs integral{ VectorXs::Zero( 10 ) };

  for( int i = 0; i < indices.cols(); ++i )
  {
    // Copy the vertices of triangle i
    const Vector3s v0{ vertices.col( indices( 0, i ) ) };
    const Vector3s v1{ vertices.col( indices( 1, i ) ) };
    const Vector3s v2{ vertices.col( indices( 2, i ) ) };

    // Compute a normal for the current triangle
    const Vector3s N{ ( v1 - v0 ).cross( v2 - v0 ) };

    // Compute the integral terms
    scalar tmp0{ v0.x() + v1.x() };
    scalar tmp1{ v0.x() * v0.x() };
    scalar tmp2{ tmp1 + v1.x() * tmp0 };
    const scalar f1x{ tmp0 + v2.x() };
    const scalar f2x{ tmp2 + v2.x() * f1x };
    const scalar f3x{ v0.x() * tmp1 + v1.x() * tmp2 + v2.x() * f2x };
    const scalar g0x{ f2x + v0.x() * ( f1x + v0.x() ) };
    const scalar g1x{ f2x + v1.x() * ( f1x + v1.x() ) };
    const scalar g2x{ f2x + v2.x() * ( f1x + v2.x() ) };

    tmp0 = v0.y() + v1.y();
    tmp1 = v0.y() * v0.y();
    tmp2 = tmp1 + v1.y() * tmp0;
    const scalar f1y{ tmp0 + v2.y() };
    const scalar f2y{ tmp2 + v2.y() * f1y };
    const scalar f3y{ v0.y() * tmp1 + v1.y() * tmp2 + v2.y() * f2y };
    const scalar g0y{ f2y + v0.y() * ( f1y + v0.y() ) };
    const scalar g1y{ f2y + v1.y() * ( f1y + v1.y() ) };
    const scalar g2y{ f2y + v2.y() * ( f1y + v2.y() ) };

    tmp0 = v0.z() + v1.z();
    tmp1 = v0.z()*v0.z();
    tmp2 = tmp1 + v1.z()*tmp0;
    const scalar f1z{ tmp0 + v2.z() };
    const scalar f2z{ tmp2 + v2.z() * f1z };
    const scalar f3z{ v0.z() * tmp1 + v1.z() * tmp2 + v2.z() * f2z };
    const scalar g0z{ f2z + v0.z() * ( f1z + v0.z() ) };
    const scalar g1z{ f2z + v1.z() * ( f1z + v1.z() ) };
    const scalar g2z{ f2z + v2.z() * ( f1z + v2.z() ) };

    // Update integrals
    integral(0) += N.x() * f1x;
    integral(1) += N.x() * f2x;
    integral(2) += N.y() * f2y;
    integral(3) += N.z() * f2z;
    integral(4) += N.x() * f3x;
    integral(5) += N.y() * f3y;
    integral(6) += N.z() * f3z;
    integral(7) += N.x() * ( v0.y() * g0x + v1.y() * g1x + v2.y() * g2x );
    integral(8) += N.y() * ( v0.z() * g0y + v1.z() * g1y + v2.z() * g2y );
    integral(9) += N.z() * ( v0.x() * g0z + v1.x() * g1z + v2.x() * g2z );
  }

  integral(0) *= oneDiv6;
  integral(1) *= oneDiv24;
  integral(2) *= oneDiv24;
  integral(3) *= oneDiv24;
  integral(4) *= oneDiv60;
  integral(5) *= oneDiv60;
  integral(6) *= oneDiv60;
  integral(7) *= oneDiv120;
  integral(8) *= oneDiv120;
  integral(9) *= oneDiv120;

  // Mass
  mass = integral(0);

  // Center of mass
  center = Vector3s{ integral(1), integral(2), integral(3) } / mass;

  // Inertia relative to world origin
  R(0,0) = integral(5) + integral(6);
  R(0,1) = -integral(7);
  R(0,2) = -integral(9);
  R(1,0) = R(0,1);
  R(1,1) = integral(4) + integral(6);
  R(1,2) = -integral(8);
  R(2,0) = R(0,2);
  R(2,1) = R(1,2);
  R(2,2) = integral(4) + integral(5);

  // Comptue the inertia relative to the center of mass
  R(0,0) -= mass * ( center.y() * center.y() + center.z() * center.z() );
  R(0,1) += mass * center.x() * center.y();
  R(0,2) += mass * center.z() * center.x();
  R(1,0) = R(0,1);
  R(1,1) -= mass * ( center.z() * center.z() + center.x() * center.x() );
  R(1,2) += mass * center.y() * center.z();
  R(2,0) = R(0,2);
  R(2,1) = R(1,2);
  R(2,2) -= mass * ( center.x() * center.x() + center.y() * center.y() );

  // Diagonalize the inertia tensor
  Matrix3s R0;
  diagonalizeInertiaTensor( R, R0, I );
  // Check that we actually diagonalized the inertia tensor
  assert( ( R0 * I.asDiagonal() * R0.transpose() - R ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  assert( ( R0.transpose() * R * R0 - Matrix3s{ I.asDiagonal() } ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  R = R0;

  // All inertias should be positive
  assert( ( I.array() > 0.0 ).all() );
  // Check that we have an orthonormal transformation
  assert( ( R * R.transpose() - Matrix3s::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
  assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
}

}


/**
 * @file tests/pca_test.cpp
 * @author Ajinkya Kale
 * @author Marcus Edel
 *
 * Test file for PCA class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/pca/pca.hpp>
#include <mlpack/methods/pca/decomposition_policies/exact_svd_method.hpp>
#include <mlpack/methods/pca/decomposition_policies/quic_svd_method.hpp>
#include <mlpack/methods/pca/decomposition_policies/randomized_svd_method.hpp>
#include <mlpack/methods/pca/decomposition_policies/randomized_block_krylov_method.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

BOOST_AUTO_TEST_SUITE(PCATest);

using namespace arma;
using namespace mlpack;
using namespace mlpack::pca;
using namespace mlpack::distribution;

/*
 * Compare the output of the our PCA implementation with Armadillo's using the
 * specified decomposition policy.
 */
template<typename DecompositionPolicy>
void ArmaComparisonPCA(
    const bool scaleData = false,
    const DecompositionPolicy& decomposition = DecompositionPolicy())
{
  arma::mat coeff, coeff1, score, score1;
  arma::vec eigVal, eigVal1;

  arma::mat data = arma::randu<arma::mat>(3, 1000);

  PCA<DecompositionPolicy> pcaType(scaleData, decomposition);
  pcaType.Apply(data, score1, eigVal1, coeff1);

  princomp(coeff, score, eigVal, trans(data));

  // Verify the PCA results based on the eigenvalues.
  for (size_t i = 0; i < eigVal.n_elem; i++)
  {
    if (eigVal[i] == 0.0)
      BOOST_REQUIRE_SMALL(eigVal1[i], 1e-15);
    else
      BOOST_REQUIRE_CLOSE(eigVal[i], eigVal1[i], 0.0001);
  }
}

/*
 * Test that dimensionality reduction with PCA works the same way MATLAB does
 * (which should be correct!) using the specified decomposition policy.
 */
template<typename DecompositionPolicy>
void PCADimensionalityReduction(
    const bool scaleData = false,
    const DecompositionPolicy& decomposition = DecompositionPolicy())
{
  // Fake, simple dataset.  The results we will compare against are from MATLAB.
  mat data("1 0 2 3 9;"
           "5 2 8 4 8;"
           "6 7 3 1 8");

  // Now run PCA to reduce the dimensionality.
  size_t trial = 0;
  bool success = false;
  double varRetained = 0.0;
  while (trial < 3 && !success)
  {
    // In some cases the LU decomposition may fail.
    try
    {
      PCA<DecompositionPolicy> p(scaleData, decomposition);
      varRetained = p.Apply(data, 2); // Reduce to 2 dimensions.
      success = true;
    }
    catch (std::logic_error&) { }

    ++trial;
  }

  BOOST_REQUIRE_EQUAL(success, true);

  // Compare with correct results.
  mat correct("-1.53781086 -3.51358020 -0.16139887 -1.87706634  7.08985628;"
              " 1.29937798  3.45762685 -2.69910005 -3.15620704  1.09830225");

  BOOST_REQUIRE_EQUAL(data.n_rows, correct.n_rows);
  BOOST_REQUIRE_EQUAL(data.n_cols, correct.n_cols);

  // If the eigenvectors are pointed opposite directions, they will cancel
  // each other out in this summation.
  for (size_t i = 0; i < data.n_rows; i++)
  {
    if (accu(abs(correct.row(i) + data.row(i))) < 0.001 /* arbitrary */)
    {
      // Flip Armadillo coefficients for this column.
      data.row(i) *= -1;
    }
  }

  for (size_t row = 0; row < 2; row++)
    for (size_t col = 0; col < 5; col++)
      BOOST_REQUIRE_CLOSE(data(row, col), correct(row, col), 1e-3);

  // Check that the amount of variance retained is right.
  BOOST_REQUIRE_CLOSE(varRetained, 0.904876047045906, 1e-5);
}

/**
 * Test that setting the variance retained parameter to perform dimensionality
 * reduction works using the specified decomposition policy.
 */
template<typename DecompositionPolicy>
void PCAVarianceRetained()
{
    // Fake, simple dataset.
  mat data("1 0 2 3 9;"
           "5 2 8 4 8;"
           "6 7 3 1 8");

  // The normalized eigenvalues:
  //   0.616237391936100
  //   0.288638655109805
  //   0.095123952954094
  // So if we keep one dimension, the actual variance retained is
  //   0.616237391936100
  // and if we keep two, the actual variance retained is
  //   0.904876047045906
  // and if we keep three, the actual variance retained is 1.
  PCA<DecompositionPolicy> p;
  arma::mat origData = data;
  double varRetained = p.Apply(data, 0.1);

  BOOST_REQUIRE_EQUAL(data.n_rows, 1);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 0.616237391936100, 1e-5);

  data = origData;
  varRetained = p.Apply(data, 0.5);

  BOOST_REQUIRE_EQUAL(data.n_rows, 1);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 0.616237391936100, 1e-5);

  data = origData;
  varRetained = p.Apply(data, 0.7);

  BOOST_REQUIRE_EQUAL(data.n_rows, 2);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 0.904876047045906, 1e-5);

  data = origData;
  varRetained = p.Apply(data, 0.904);

  BOOST_REQUIRE_EQUAL(data.n_rows, 2);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 0.904876047045906, 1e-5);

  data = origData;
  varRetained = p.Apply(data, 0.905);

  BOOST_REQUIRE_EQUAL(data.n_rows, 3);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 1.0, 1e-5);

  data = origData;
  varRetained = p.Apply(data, 1.0);

  BOOST_REQUIRE_EQUAL(data.n_rows, 3);
  BOOST_REQUIRE_EQUAL(data.n_cols, 5);
  BOOST_REQUIRE_CLOSE(varRetained, 1.0, 1e-5);
}

/**
 * Compare the output of our exact PCA implementation with Armadillo's.
 */
BOOST_AUTO_TEST_CASE(ArmaComparisonExactPCATest)
{
  ArmaComparisonPCA<ExactSVDPolicy>();
}

/**
 * Compare the output of our randomized block krylov PCA implementation with
 * Armadillo's.
 */
BOOST_AUTO_TEST_CASE(ArmaComparisonRandomizedBlockKrylovPCATest)
{
  RandomizedBlockKrylovSVDPolicy decomposition(5);
  ArmaComparisonPCA<RandomizedBlockKrylovSVDPolicy>(false, decomposition);
}

/**
 * Compare the output of our randomized-SVD PCA implementation with Armadillo's.
 */
BOOST_AUTO_TEST_CASE(ArmaComparisonRandomizedPCATest)
{
  ArmaComparisonPCA<RandomizedSVDPolicy>();
}

/**
 * Test that dimensionality reduction with exact-svd PCA works the same way
 * MATLAB does (which should be correct!).
 */
BOOST_AUTO_TEST_CASE(ExactPCADimensionalityReductionTest)
{
  PCADimensionalityReduction<ExactSVDPolicy>();
}

/**
 * Test that dimensionality reduction with randomized block krylov PCA works the
 * same way MATLAB does (which should be correct!).
 */
BOOST_AUTO_TEST_CASE(RandomizedBlockKrylovPCADimensionalityReductionTest)
{
  RandomizedBlockKrylovSVDPolicy decomposition(5);
  PCADimensionalityReduction<RandomizedBlockKrylovSVDPolicy>(false,
      decomposition);
}

/**
 * Test that dimensionality reduction with randomized-svd PCA works the same way
 * MATLAB does (which should be correct!).
 */
BOOST_AUTO_TEST_CASE(RandomizedPCADimensionalityReductionTest)
{
  PCADimensionalityReduction<RandomizedSVDPolicy>();
}

/**
 * Test that dimensionality reduction with QUIC-SVD PCA works the same way
 * as the Exact-SVD PCA method.
 */
BOOST_AUTO_TEST_CASE(QUICPCADimensionalityReductionTest)
{
  arma::mat data, data1;
  data::Load("test_data_3_1000.csv", data);
  data1 = data;

  arma::mat backupData(data);

  // It isn't guaranteed that the QUIC-SVD will match with the exact SVD method,
  // starting with random samples. If this works 1 of 5 times, I'm fine with
  // that. All I want to know is that the QUIC-SVD method is able to solve the
  // task and is at least as good as the exact method (plus a little bit for
  // noise).
  size_t successes = 0;
  for (size_t trial = 0; trial < 5; ++trial)
  {
    if (trial > 0)
    {
      data = backupData;
      data1 = backupData;
    }

    PCA<ExactSVDPolicy> exactPCA;
    const double varRetainedExact = exactPCA.Apply(data, 1);

    PCA<QUICSVDPolicy> quicPCA;
    const double varRetainedQUIC = quicPCA.Apply(data1, 1);

    if (std::abs(varRetainedExact - varRetainedQUIC) < 0.2)
    {
      ++successes;
      break;
    }
  }

  BOOST_REQUIRE_GE(successes, 1);
  BOOST_REQUIRE_EQUAL(data.n_rows, data1.n_rows);
  BOOST_REQUIRE_EQUAL(data.n_cols, data1.n_cols);
}

/**
 * Test that setting the variance retained parameter to perform dimensionality
 * reduction works using the exact svd PCA method.
 */
BOOST_AUTO_TEST_CASE(ExactPCAVarianceRetainedTest)
{
  PCAVarianceRetained<ExactSVDPolicy>();
}

/**
 * Test that scaling PCA works.
 */
BOOST_AUTO_TEST_CASE(PCAScalingTest)
{
  // Generate an artificial dataset in 3 dimensions.
  arma::mat data(3, 5000);

  arma::vec mean("1.0 3.0 -12.0");
  arma::mat cov("1.0 0.9 0.0;"
                "0.9 1.0 0.0;"
                "0.0 0.0 12.0");
  GaussianDistribution g(mean, cov);

  for (size_t i = 0; i < 5000; ++i)
    data.col(i) = g.Random();

  // Now get the principal components when we are scaling.
  PCA<> p(true);
  arma::mat transData;
  arma::vec eigval;
  arma::mat eigvec;

  p.Apply(data, transData, eigval, eigvec);

  // The first two components of the eigenvector with largest eigenvalue should
  // be somewhere near sqrt(2) / 2.  The third component should be close to
  // zero.  There is noise, of course...
  BOOST_REQUIRE_CLOSE(std::abs(eigvec(0, 0)), sqrt(2) / 2, 0.35);
  BOOST_REQUIRE_CLOSE(std::abs(eigvec(1, 0)), sqrt(2) / 2, 0.35);
  BOOST_REQUIRE_SMALL(eigvec(2, 0), 0.1); // Large tolerance for noise.

  // The second component should be focused almost entirely in the third
  // dimension.
  BOOST_REQUIRE_SMALL(eigvec(0, 1), 0.1);
  BOOST_REQUIRE_SMALL(eigvec(1, 1), 0.1);
  BOOST_REQUIRE_CLOSE(std::abs(eigvec(2, 1)), 1.0, 0.35);

  // The third component should have the same absolute value characteristics as
  // the first (plus 20% tolerance).
  BOOST_REQUIRE_CLOSE(std::abs(eigvec(0, 0)), sqrt(2) / 2, 0.35);
  BOOST_REQUIRE_CLOSE(std::abs(eigvec(1, 0)), sqrt(2) / 2, 0.35);
  BOOST_REQUIRE_SMALL(eigvec(2, 0), 0.1); // Large tolerance for noise.

  // The eigenvalues should sum to three.
  BOOST_REQUIRE_CLOSE(accu(eigval), 3.0, 0.1); // 10% tolerance.
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/ceres.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

const double DT = 1.0 / 18;
// const Eigen::Vector3d GRAVITY{0, 0, 0};
const Eigen::Vector3d GRAVITY{0, 0, -9.8};

struct State {
  Eigen::Vector3d pos = Eigen::Vector3d::Random(); // position
  Eigen::Vector3d vel = Eigen::Vector3d::Random(); // velocity
  Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); // pose Qwr

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct Measurement{
  Eigen::Matrix3d Rwr;
  Eigen::Quaterniond qwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d acc;
  Eigen::Vector3d omega;

  Measurement(Eigen::Matrix3d Rwr,
              Eigen::Quaterniond qwr,
              Eigen::Vector3d twr,
              Eigen::Vector3d acc,
              Eigen::Vector3d omega)
    : Rwr(Rwr), qwr(qwr), twr(twr), acc(acc), omega(omega) {}

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct PositionError {
	PositionError(const Eigen::Vector3d& pos_measured)
		: pos_measured_(pos_measured) {}

	template <typename T>
	bool operator()(const T* const pos_hat_ptr,
									T* residuals_ptr) const {
		Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
		Eigen::Matrix<T, 3, 1> pos_delta = pos_hat - pos_measured_.template cast<T>();

    for (int i = 0; i < 3; i++) {
      residuals_ptr[i] = pos_delta[i];
    }
		return true;
	}

	static CostFunction* Create(const Eigen::Vector3d& pos_measured) {
		return new AutoDiffCostFunction<PositionError, 3, 3>(
			new PositionError(pos_measured)
		);
	}

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
	const Eigen::Vector3d pos_measured_;
};

struct PoseError {
  PoseError(const Eigen::Vector3d& pos_measured,
            const Eigen::Quaterniond& q_measured)
    : pos_measured_(pos_measured), q_measured_(q_measured) {}

  template <typename T>
  bool operator()(const T* const pos_hat_ptr,
                  const T* const q_hat_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 6, 1> residuals;

    Eigen::Matrix<T, 3, 1> pos_hat(pos_hat_ptr);
    Eigen::Matrix<T, 3, 1> pos_delta;
    residuals.template block<3, 1>(0, 0) = pos_hat - pos_measured_.template cast<T>();

    Eigen::Quaternion<T> q_hat(q_hat_ptr);
    Eigen::Quaternion<T> q_delta = q_measured_.conjugate().template cast<T>() * q_hat;
    residuals.template block<3, 1>(3, 0) = q_delta.vec();
    for (int i = 0; i < 6; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& pos_measured,
                              const Eigen::Quaterniond& q_measured) {
    return new AutoDiffCostFunction<PoseError, 6, 3, 4>(
      new PoseError(pos_measured, q_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d pos_measured_;
  const Eigen::Quaterniond q_measured_;
};

struct PredictionError{
  PredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const pos_b_ptr,
                  const T* const vel_b_ptr,
                  const T* const q_b_ptr,
                  const T* const pos_e_ptr,
                  const T* const vel_e_ptr,
                  const T* const q_e_ptr,
                  const T* const bias_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 9, 1> residuals;

    // quat error
    const Eigen::Quaternion<T> q_b(q_b_ptr);
    const Eigen::Quaternion<T> q_e(q_e_ptr);

    Eigen::Quaternion<T> q_new;
    Eigen::Quaternion<T> q_add;

    // // https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
    // Eigen::Quaternion<T> q_omega;
    // // q_omega.w() = 0;
    // q_omega.vec() = omega_measured_.template cast<T>() * DT * 0.5;
    // q_add = q_omega * q_b;
    // q_new.w() = q_b.w() + q_add.w();
    // q_new.vec() = q_b.vec() + q_add.vec();

    Eigen::Vector3d rotated = omega_measured_ * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    q_add = Eigen::AngleAxisd(angle, axis).template cast<T>();
    q_new = q_b * q_add;

    Eigen::Quaternion<T> q_delta = q_e.conjugate() * q_new;
    residuals.template block<3, 1>(6, 0) = q_delta.vec();

    // vel error
    const Eigen::Matrix<T, 3, 1> bias(bias_ptr);
    const Eigen::Matrix<T, 3, 1> vel_b(vel_b_ptr);
    const Eigen::Matrix<T, 3, 1> vel_e(vel_e_ptr);
    residuals.template block<3, 1>(3, 0) = vel_b + (q_b * (acc_measured_.template cast<T>() - bias) - GRAVITY) * DT - vel_e;

    // pos error
    const Eigen::Matrix<T, 3, 1> pos_b(pos_b_ptr);
    const Eigen::Matrix<T, 3, 1> pos_e(pos_e_ptr);
    residuals.template block<3, 1>(0, 0) = pos_b + vel_b * DT - pos_e;

    for (int i = 0; i < 9; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<PredictionError, 9, 3, 3, 4, 3, 3, 4, 3>(
      new PredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

struct OriPredictionError{
  OriPredictionError(const Eigen::Vector3d& acc_measured,
                  const Eigen::Vector3d& omega_measured)
    : acc_measured_(acc_measured), omega_measured_(omega_measured) {}

  template <typename T>
  bool operator()(const T* const q_b_ptr,
                  const T* const q_e_ptr,
                  T* residuals_ptr) const {
    Eigen::Matrix<T, 3, 1> residuals;

    // quat error
    const Eigen::Quaternion<T> q_b(q_b_ptr);
    const Eigen::Quaternion<T> q_e(q_e_ptr);

    Eigen::Quaternion<T> q_new;
    Eigen::Quaternion<T> q_add;

    // // https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion
    // Eigen::Quaternion<T> q_omega;
    // // q_omega.w() = 0;
    // q_omega.vec() = omega_measured_.template cast<T>() * DT * 0.5;
    // q_add = q_omega * q_b;
    // q_new.w() = q_b.w() + q_add.w();
    // q_new.vec() = q_b.vec() + q_add.vec();

    Eigen::Vector3d rotated = omega_measured_ * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    q_add = Eigen::AngleAxisd(angle, axis).template cast<T>();
    q_new = q_b * q_add;

    Eigen::Quaternion<T> q_delta = q_e.conjugate() * q_new;
    residuals.template block<3, 1>(0, 0) = T(2.0) * q_delta.vec();

    for (int i = 0; i < 3; i++) {
      residuals_ptr[i] = residuals[i];
    }
    return true;
  }

  static CostFunction* Create(const Eigen::Vector3d& acc_measured,
                              const Eigen::Vector3d& omega_measured) {
    return new AutoDiffCostFunction<OriPredictionError, 3, 4, 4>(
      new OriPredictionError(acc_measured, omega_measured));
  }

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  const Eigen::Vector3d acc_measured_;
  const Eigen::Vector3d omega_measured_;
};

std::vector<Measurement, Eigen::aligned_allocator<Measurement>> readSensorData(std::string path) {
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      // std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    Eigen::Matrix3d Rwr;
    Eigen::Quaterniond qwr;
    Eigen::Vector3d twr;
    Eigen::Vector3d acc;
    Eigen::Vector3d omega;
    Rwr << row[0], row[1], row[2],
          row[4], row[5], row[6],
          row[8], row[9], row[10];
    qwr = Rwr;
    twr << row[3], row[7], row[11];
    acc << row[16], row[17], row[18];
    omega << row[19], row[20], row[21];
    std::cout << "Rwr: " << Rwr << std::endl;
    std::cout << "qwr: " << qwr.w() << " " << qwr.vec() << std::endl;
    std::cout << "twr: " << twr << std::endl;
    std::cout << "acc: " << acc << std::endl;
    std::cout << "omega: " << omega << std::endl;

    ret.push_back(Measurement(Rwr, qwr, twr, acc, omega));
  }

  return ret;
}

void output_pose(const Eigen::Vector3d& pos,
								 const Eigen::Quaterniond& q) {
	Eigen::AngleAxisd ori(q);

	std::cout << "Location: " << pos << std::endl;
	std::cout << "Orientation: " << ori.angle() << " * " << std::endl << ori.axis() << std::endl;
}

void output_measurement(const Measurement& data) {
  std::cout << "\nData State: \n" << "R: \n" << data.Rwr << "\nt: \n" << data.twr \
            << "\nacc: \n" << data.acc << "\nomega: \n" << data.omega << std::endl;
}

void save_states(const std::string& filename,
                 std::vector<State, Eigen::aligned_allocator<State>>& states) {
  std::fstream outfile;
  outfile.open(filename.c_str(), std::istream::out);

  for (auto& state : states) {
    Eigen::Matrix3d rot = state.q.matrix();
    outfile << rot << "\n" << state.pos.transpose() << "\n" << state.vel.transpose() << "\n\n";
  }
}

double abs_pos_error(const std::vector<State, Eigen::aligned_allocator<State>>& states,
                     const std::vector<State, Eigen::aligned_allocator<State>>& gt_states) {
  double err = 0.0;
  std::cout << "abs_pos_err by step: ";
  for (int i = 0; i < states.size(); i++) {
    double step_err = (states[i].pos - gt_states[i].pos).norm();
    err += step_err;
    std::cout << step_err << " ";
  }
  std::cout << std::endl;
  return err;
}

int main(int argc, char** argv) {
  if(argc < 2) {
    std::cout << "missing arg for the csv file" << std::endl;
  }

  std::string path = argv[1];
  std::vector<Measurement, Eigen::aligned_allocator<Measurement>> data = readSensorData(path);

  if (false) {
    output_measurement(data[0]);
    output_measurement(data[1]);

    Eigen::Vector3d rotated = data[0].omega * DT;
    double angle = rotated.norm();
    Eigen::Vector3d axis = rotated.normalized();
    Eigen::Quaterniond q_add(Eigen::AngleAxisd(angle, axis));
    Eigen::Quaterniond q_new = data[0].qwr * q_add;
    Eigen::Matrix3d R_new = q_new.normalized().toRotationMatrix();

    std::cout << "R_new: \n" << R_new << std::endl;

    Eigen::Vector3d vel_add = data[0].qwr * data[0].acc * DT;
    std::cout << "vel_new: \n" << vel_add << std::endl;
    return 0;
  }

  int cnt = data.size();
  // int cnt = 3;

  // Eigen::Vector3d bias = Eigen::Vector3d::Random();
  Eigen::Vector3d bias;
  bias << 0, 0, 0;
  std::vector<State, Eigen::aligned_allocator<State>> states(cnt);
  std::vector<State, Eigen::aligned_allocator<State>> gt_states(cnt);
  std::cout << "states size: " << states.size() << std::endl;

  for (int i = 0; i < gt_states.size(); i++) {
    gt_states[i].pos = data[i].twr;
    gt_states[i].vel = Eigen::Vector3d::Zero();
    gt_states[i].q = data[i].qwr;
  }

  Problem problem;

  ceres::LossFunction* loss_function = nullptr;
  ceres::LocalParameterization* quaternion_local_parameterization =
      new ceres::EigenQuaternionParameterization;

  for (int i = 0; i < cnt; i++) {
    ceres::CostFunction* position_cost_function = PositionError::Create(data[i].twr);
    problem.AddResidualBlock(position_cost_function,
                             loss_function,
                             states[i].pos.data());
    // ceres::CostFunction* pos_cost_function = PoseError::Create(data[i].twr, data[i].qwr);
    // problem.AddResidualBlock(pos_cost_function,
    //                          loss_function,
    //                          states[i].pos.data(),
    //                          states[i].q.coeffs().data());
    // problem.SetParameterization(states[i].q.coeffs().data(),
    //                             quaternion_local_parameterization);
    if (i > 0) {
      ceres::CostFunction* pred_cost_function = PredictionError::Create(data[i].acc, data[i].omega);
      problem.AddResidualBlock(pred_cost_function,
                               loss_function,
                               states[i - 1].pos.data(),
                               states[i - 1].vel.data(),
                               states[i - 1].q.coeffs().data(),
                               states[i].pos.data(),
                               states[i].vel.data(),
                               states[i].q.coeffs().data(),
                               bias.data());
      // ceres::CostFunction* pred_cost_function = OriPredictionError::Create(data[i].acc, data[i].omega);
      // problem.AddResidualBlock(pred_cost_function,
      //                          loss_function,
      //                          states[i - 1].q.coeffs().data(),
      //                          states[i].q.coeffs().data());
      problem.SetParameterization(states[i - 1].q.coeffs().data(),
                                  quaternion_local_parameterization);
      problem.SetParameterization(states[i].q.coeffs().data(),
                                  quaternion_local_parameterization);
    }
  }

  // set bias to constant
  // problem.SetParameterBlockConstant(bias.data());
  // Eigen::Quaterniond q_noise(Eigen::AngleAxisd(0.1, Eigen::Vector3d::Random().normalized()));
  // states[0].q = data[0].qwr * q_noise;
  // states[0].q = data[0].qwr;
  // problem.SetParameterBlockConstant(states[0].q.coeffs().data());

  ceres::Solver::Options options;
	options.max_num_iterations = 200;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  options.minimizer_progress_to_stdout = true;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  if (false) {
    output_measurement(data[0]);
    output_measurement(data[1]);
    output_measurement(data[2]);

    for (int i = 0; i < states.size(); i++) {
      std::cout << "Estimated State:" << i << " \n" << "R: \n" << states[i].q.matrix()
                                              << "\nt: \n" << states[i].pos
                                              << "\nvel: \n" << states[i].vel << std::endl;
    }
  }

  std::string est_filename = "./results/est_no_ori_measure_nofixfirst_states.txt";
  // std::string est_filename = "./results/est_position_predic_ori_states.txt";
  save_states(est_filename, states);
  std::string gt_filename = "./results/gt_states.txt";
  save_states(gt_filename, gt_states);

  double err = abs_pos_error(states, gt_states);
  std::cout << "absolute position error: " << err << std::endl;
  return 0;
}


#include <iostream>

#include <Eigen/Dense>

#include <cppmath/matrix/PseudoInverseSVD.hpp>

using namespace Eigen;
using namespace std;

int main()
{
    const MatrixXd::Index rows = 42;
    const MatrixXd::Index cols = 7;

    // Generate system of linear equations
    MatrixXd A( rows, cols );
    A.setRandom();
    VectorXd x( cols );
    x.setRandom();
    VectorXd b = A * x;
    cout << "x:" << endl << x << endl;
    VectorXd diff;

    // Method call #1
    cppmath::PseudoInverseSVD< MatrixXd > pinv( A );
    MatrixXd Ainv;
    pinv.compute( &Ainv );
    VectorXd xs1 = Ainv * b;
    diff = x - xs1;
    cout << "xs1:" << endl << xs1 << endl;
    cout << "x - xs1: " << diff.squaredNorm() << endl;

    // Methof call #2
    const MatrixXd& Apinv = pinv.compute();
    VectorXd xs2 = Apinv * b;
    diff = x - xs2;
    cout << "xs2:" << endl << xs2 << endl;
    cout << "x - xs2: " << diff.squaredNorm() << endl;

    // Operator call
    VectorXd xs3 = pinv * b;
    diff = x - xs3;
    cout << "xs3:" << endl << xs3 << endl;
    cout << "x - xs3: " << diff.squaredNorm() << endl;

    return 0;
}


// Header
#include <polyvec/curve-tracer/curve_bezier.hpp>

// C++ stl
#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <limits>
#include <stdexcept>

// Eigen
#include <Eigen/Core>

// Polyvec
#include <polyvec/geom.hpp>
#include <polyvec/geometry/smooth_curve.hpp>
#include <polyvec/geometry/line.hpp>

namespace polyvec {

	// Taken from https://github.com/inkscape/lib2geom: src/2geom/bezier.h
	// Compute the value of a Bernstein-Bezier polynomial.
	// This method uses a Horner-like fast evaluation scheme.
	// param t Time value
	// param control_points control points (one per column)
	//
	template <typename Derived>
	Eigen::Matrix<double, Derived::RowsAtCompileTime,1>
		_bezier_value_at(const Eigen::MatrixBase<Derived>& control_points, const double t) {

		assert_break(t <= 1.);
		assert_break(t >= 0);

		double u = 1.0 - t;
		int degree = control_points.cols() - 1;

		Eigen::Matrix<double, Derived::RowsAtCompileTime, 1> ans;

		double bc = 1;
		double tn = 1;

		ans = control_points.col(0) * u;

		for (unsigned i = 1; i < degree; i++) {
			tn = tn * t;
			bc = bc * (degree - i + 1) / i;
			ans = (ans + tn * bc * control_points.col(i)) * u;
		}
		ans = (ans + tn * t * control_points.col(degree));

		return ans;
	}

	// From https://github.com/inkscape/lib2geom/: src/2geom/bezier.cpp
	Eigen::Matrix2Xd
		_derivative_bezier_curve(const Eigen::Matrix2Xd& control_points) {
		assert_break(control_points.cols() >= 2);
		Eigen::Matrix2Xd ans(2, control_points.cols() - 1);

		const int order = (int)control_points.cols() - 1;

		for (unsigned i = 0; i < ans.cols(); ++i) {
			ans.col(i) = order * (control_points.col(i + 1) - control_points.col(i));
		}

		return ans;
	}

	Eigen::Matrix3Xd
		_tesselate(const Eigen::Matrix2Xd& control_points, const int n_sampling) {
		Eigen::Matrix3Xd ans(3, n_sampling);

		for (int i = 0; i < n_sampling; ++i) {
			const double t = 1. / (n_sampling - 1) * i;
			ans.col(i) (0) = t;
			ans.col(i).tail<2>() = _bezier_value_at(control_points, t);
		}

		return ans;
	}

	const std::vector<double>&
		_get_gauss_quad_locs() {
		static std::vector<double> ans;
		static bool is_init = false;

		if (!is_init) {
			ans.resize(6);
			ans[0] = -9.3246951420315202781230155449399e-01L;
			ans[1] = -6.6120938646626451366139959501991e-01L;
			ans[2] = -2.3861918608319690863050172168071e-01L;
			ans[3] = -ans[2];
			ans[4] = -ans[1];
			ans[5] = -ans[0];

			for (double& a : ans) {
				a = a / 2. + 0.5;
			}

			is_init = true;
		}

		return ans;
	}

	const std::vector<double>&
		_get_gauss_quad_weights() {
		static std::vector<double> ans;
		static bool is_init = false;

		if (!is_init) {
			ans.resize(6);
			ans[0] = 1.7132449237917034504029614217273e-01L;
			ans[1] = 3.6076157304813860756983351383772e-01L;
			ans[2] = 4.6791393457269104738987034398955e-01L;
			ans[3] = ans[2];
			ans[4] = ans[1];
			ans[5] = ans[0];

			for (double& a : ans) {
				a = a / 2.;
			}

			is_init = true;
		}

		return ans;
	}

// ===================================================================
//                            Bezier Curve
// ===================================================================

    Eigen::Vector2d
    BezierCurve::pos ( const double t ) const {
        return _bezier_value_at ( _control_points_d0, t );
    }

    Eigen::Vector2d
    BezierCurve::dposdt ( const double t ) const {
        return _bezier_value_at ( _control_points_d1, t );
    }

    Eigen::Vector2d
    BezierCurve::dposdtdt ( const double t ) const {
        return _bezier_value_at ( _control_points_d2, t );
    }

	Eigen::Vector2d BezierCurve::dposdtdtdt(const double t) const
	{
		return _bezier_value_at(_control_points_d3, t);
	}

    Eigen::Matrix2Xd
    BezierCurve::dposdparams ( const double t ) const {
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
        const double b0[] = {1., 0., 0., 0.};
        const double b1[] = {0., 1., 0., 0.};
        const double b2[] = {0., 0., 1., 0.};
        const double b3[] = {0., 0., 0., 1.};

        // Value of derivatives
		Eigen::MatrixXd control(4, 4);
		control <<
			1., 0., 0., 0.,
			0., 1., 0., 0.,
			0., 0., 1., 0.,
			0., 0., 0., 1.;

		auto dxdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdp(0), 0, dxdp(1), 0, dxdp(2), 0, dxdp(3), 0;
        ans.row ( 1 ) << 0, dxdp(0), 0, dxdp(1), 0, dxdp(2), 0, dxdp(3);

        return ans;
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtdparams ( const double t ) const {
        const int order = 3;
        const double drder = order;
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
        const double b0_deriv[] = {-drder, 0., 0.};
        const double b1_deriv[] = {drder, -drder, 0.};
        const double b2_deriv[] = {0., drder, -drder};
        const double b3_deriv[] = {0., 0., drder};

		Eigen::MatrixXd control(4, 3);
		control <<
			-drder, 0., 0.,
			drder, -drder, 0.,
			0., drder, -drder,
			0., 0., drder;

        // Value of derivatives
		auto dxdtdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3), 0;
        ans.row ( 1 ) << 0, dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3);

        return ans;
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtdtdparams ( const double t ) const {
        const int order = 3;
        const double drder = order*(order-1);
        const int n_points = n_control_points();

        // Eigen::Matrix2Xd ans(2, n_points);

        // Coefficient of bernstein monomials in a bernstein polynomial
		Eigen::MatrixXd control(4, 2);
		control <<
			drder, 0.,
			-2 * drder, drder,
			drder, -2 * drder,
			0., drder;

        // Value of derivatives
		auto dxdtdtdp = _bezier_value_at(control, t);

        Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero ( 2, n_points * 2 );
        ans.row ( 0 ) << dxdtdtdp(0), 0, dxdtdtdp(1), 0, dxdtdtdp(2), 0, dxdtdtdp(3), 0;
        ans.row ( 1 ) << 0, dxdtdtdp(0), 0, dxdtdtdp(1), 0, dxdtdtdp(2), 0, dxdtdtdp(3);

        return ans;
    }

	Eigen::Matrix2Xd BezierCurve::dposdtdtdtdparams(const double t) const
	{
		const int order = 3;
		const double factor = 6;
		const int n_points = n_control_points();

		// Coefficient of bernstein monomials in a bernstein polynomial
		Eigen::MatrixXd control(4, 1);
		control <<
			-factor,
			3 * factor,
			-3 * factor,
			factor;

		// Value of derivatives
		auto dxdtdp = _bezier_value_at(control, t);

		Eigen::Matrix2Xd ans = Eigen::Matrix2Xd::Zero(2, n_points * 2);
		ans.row(0) << dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3), 0;
		ans.row(1) << 0, dxdtdp(0), 0, dxdtdp(1), 0, dxdtdp(2), 0, dxdtdp(3);

		return ans;
	}

    double
    BezierCurve::project ( const Eigen::Vector2d& point ) const {

        // int closest_idx = -1;
        double closest_t = -1;
        double closest_dist2 = std::numeric_limits<double>::max();

        // First find an initial guess for the closest point
        for ( int i = 0; i < _tesselation.cols(); ++i ) {
            double dist2 = ( point - _tesselation.col ( i ).tail<2>() ).squaredNorm();

            if ( dist2 < closest_dist2 ) {
                // closest_idx = i;
                closest_t = _tesselation.col ( i ) ( 0 );
                closest_dist2 = dist2;
            }
        }

        // Now refine your guess using newton iterations
        auto newton_iteration = [&] ( double guess ) -> double {
            Eigen::Vector2d post, dert, der2t;
            post = this->pos ( guess );
            dert = this->dposdt ( guess );
            der2t = this->dposdtdt ( guess );

            double dot = dert.dot ( point - post );
            double dotDer = der2t.dot ( point - post ) - dert.squaredNorm();

            // Make sure the iteration does not make things worse
            if ( dotDer >= -1e-30 ) {
                return guess;
            }

            // Make sure the iteration does not shoot us out of the range
            return std::max ( 0., std::min ( 1., guess - dot / dotDer ) );
        };
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );
        closest_t = newton_iteration ( closest_t );

        return closest_t;
    }

    Eigen::VectorXd
    BezierCurve::dtprojectdparams ( const double time, const Eigen::Vector2d& point ) const {
        const int n_params = n_control_points() * 2;
        const double tend = 1.;

        return SmoothCurveUtil::projection_derivatives (
                   n_params,
                   time,
                   tend,
                   point,
                   pos ( time ),
                   dposdt ( time ),
                   dposdtdt ( time ),
                   dposdparams ( time ),
                   dposdtdparams ( time ) );
    }

    Eigen::Matrix2Xd
    BezierCurve::dposprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const {
        return dposdparams ( t ) + dposdt ( t ) * dtprojectdparams.transpose();
    }

    Eigen::Matrix2Xd
    BezierCurve::dposdtprojectdparams ( const double t, const Eigen::VectorXd& dtprojectdparams ) const {
        return dposdtdparams ( t ) + dposdtdt ( t ) * dtprojectdparams.transpose();
    }

    void
    BezierCurve::set_control_points ( const Eigen::Matrix2Xd& control_points_d0_in ) {
        assert_break ( control_points_d0_in.cols() == n_control_points() );
        _control_points_d0 = control_points_d0_in;
        _control_points_d1 = _derivative_bezier_curve ( _control_points_d0 );
        _control_points_d2 = _derivative_bezier_curve ( _control_points_d1 );
		_control_points_d3 = _derivative_bezier_curve ( _control_points_d2 );
        _tesselation = _tesselate ( _control_points_d0, _n_tesselation );
    }

    const Eigen::Matrix2Xd&
    BezierCurve::get_control_points() const {
        return _control_points_d0;
    }

	void BezierCurve::set_params(const Eigen::VectorXd & params)
	{
		Eigen::Matrix2Xd controlPoints(2, 4);
		controlPoints << params[0], params[2], params[4], params[6], params[1], params[3], params[5], params[7];
		set_control_points(controlPoints);
	}

	Eigen::VectorXd BezierCurve::get_params() const
	{
		Eigen::VectorXd params(n_params());
		params <<
			_control_points_d0.coeff(0, 0), _control_points_d0.coeff(1, 0),
			_control_points_d0.coeff(0, 1), _control_points_d0.coeff(1, 1),
			_control_points_d0.coeff(0, 2), _control_points_d0.coeff(1, 2),
			_control_points_d0.coeff(0, 3), _control_points_d0.coeff(1, 3);
		return params;
	}

	//const Eigen::Matrix3Xd&
    //BezierCurve::get_tesselation3() {
    //    return _tesselation;
    //}

    Eigen::VectorXd
    BezierCurve::get_tesselationt() const {
        return _tesselation.row(0).transpose();
    }

    Eigen::Matrix2Xd
    BezierCurve::get_tesselation2() const {
        return _tesselation.bottomRows<2>();
    }

    double
    BezierCurve::length() const {
        const std::vector<double>& ww = _get_gauss_quad_weights();
        const std::vector<double>& tt = _get_gauss_quad_locs();
        double len = 0;

        for ( unsigned i = 0; i < tt.size(); ++i ) {
            len += ww[i] * dposdt ( tt[i] ).norm();
        }

        return len;
    }

    Eigen::VectorXd
    BezierCurve::dlengthdparams() {
        const int n_points = n_control_points();
        const double tol = 1e-10;
        const std::vector<double>& ww = _get_gauss_quad_weights();
        const std::vector<double>& tt = _get_gauss_quad_locs();

        Eigen::VectorXd dlendprams = Eigen::VectorXd ( n_points * 2 );
        dlendprams.setZero();

        for ( unsigned i = 0; i < tt.size(); ++i ) {
            Eigen::Vector2d tang = dposdt ( tt[i] );
            double tang_norm = std::max ( tang.norm(), tol );
            dlendprams += ww[i] * 1. / tang_norm * dposdtdparams ( tt[i] ).transpose() * tang;
        }

        return dlendprams;
    }

	geom::aabb BezierCurve::get_bounding_box() const
	{
		geom::aabb b;
		for (int i = 0; i < 4; ++i)
			b.add(_control_points_d0.col(i));
		return b;
	}

	BezierCurve::BezierCurve(const Eigen::Matrix2Xd& C) {
		set_control_points(C);
	}

	std::pair<GlobFitCurve*, GlobFitCurve*> BezierCurve::split(double t) const
	{
		Eigen::Matrix2Xd cLeft(2, 4), cRight(2, 4);

		auto& p = _control_points_d0;

		// left curve
		cLeft.col(0) = p.col(0);
		cLeft.col(1) = _bezier_value_at(p.leftCols<2>(), t);
		cLeft.col(2) = _bezier_value_at(p.leftCols<3>(), t);
		cLeft.col(3) = _bezier_value_at(p.leftCols<4>(), t);

		// right curve
		cRight.col(0) = _bezier_value_at(p.rightCols<4>(), t);
		cRight.col(1) = _bezier_value_at(p.rightCols<3>(), t);
		cRight.col(2) = _bezier_value_at(p.rightCols<2>(), t);
		cRight.col(3) = p.col(3);

		return std::make_pair(new BezierCurve(cLeft), new BezierCurve(cRight));
	}
}


// Copyright (C) 2000 Stephen Cleary
//
// 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)
//
// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_POOL_CT_GCD_LCM_HPP
#define BOOST_POOL_CT_GCD_LCM_HPP

#include <boost/static_assert.hpp>
#include <boost/type_traits/ice.hpp>

namespace boost {

namespace details {
namespace pool {

// Compile-time calculation of greatest common divisor and least common multiple

//
// ct_gcd is a compile-time algorithm that calculates the greatest common
//  divisor of two unsigned integers, using Euclid's algorithm.
//
// assumes: A != 0 && B != 0
//

#ifndef BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION

namespace details {
template <unsigned A, unsigned B, bool Bis0>
struct ct_gcd_helper;
template <unsigned A, unsigned B>
struct ct_gcd_helper<A, B, false>
{
  BOOST_STATIC_CONSTANT(unsigned, A_mod_B_ = A % B);
  BOOST_STATIC_CONSTANT(unsigned, value =
      (::boost::details::pool::details::ct_gcd_helper<
        B, static_cast<unsigned>(A_mod_B_),
        ::boost::type_traits::ice_eq<A_mod_B_, 0>::value
        >::value) );
};
template <unsigned A, unsigned B>
struct ct_gcd_helper<A, B, true>
{
  BOOST_STATIC_CONSTANT(unsigned, value = A);
};
} // namespace details

template <unsigned A, unsigned B>
struct ct_gcd
{
  BOOST_STATIC_ASSERT(A != 0 && B != 0);
  BOOST_STATIC_CONSTANT(unsigned, value =
      (::boost::details::pool::details::ct_gcd_helper<A, B, false>::value) );
};

#else

// Thanks to Peter Dimov for providing this workaround!
namespace details {
template<unsigned A> struct ct_gcd2
{
  template<unsigned B>
  struct helper
  {
    BOOST_STATIC_CONSTANT(unsigned, value = ct_gcd2<B>::helper<A % B>::value);
  };
  template<>
  struct helper<0>
  {
    BOOST_STATIC_CONSTANT(unsigned, value = A);
  };
};
} // namespace details

template<unsigned A, unsigned B> struct ct_gcd
{
  BOOST_STATIC_ASSERT(A != 0 && B != 0);
  enum { value = details::ct_gcd2<A>::helper<B>::value };
};

#endif

//
// ct_lcm is a compile-time algorithm that calculates the least common
//  multiple of two unsigned integers.
//
// assumes: A != 0 && B != 0
//
template <unsigned A, unsigned B>
struct ct_lcm
{
  BOOST_STATIC_CONSTANT(unsigned, value =
      (A / ::boost::details::pool::ct_gcd<A, B>::value * B) );
};

} // namespace pool
} // namespace details

} // namespace boost

#endif


#ifndef MLT_UTILS_ACTIVATION_FUNCTIONS_HPP
#define MLT_UTILS_ACTIVATION_FUNCTIONS_HPP

#include <Eigen/Core>

#include "../defs.hpp"

namespace mlt {
namespace utils {
namespace activation_functions {
	class SigmoidActivation {
	public:
		auto compute(MatrixXdRef x) const {
			return x.unaryExpr([](double z) { double gz = 1.0 / (1.0 + std::exp(-z)); gz = gz < 1 ? gz : 0.9999999999; return gz; }).eval();
		}

		auto gradient(MatrixXdRef x) const {
			return x.unaryExpr([](double z) { double gz = 1.0 / (1.0 + std::exp(-z)); gz = gz < 1 ? gz : 0.9999999999; return gz * (1 - gz); }).eval();
		}
	};
}
}
}
#endif


// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcConvexHull.cc
    \brief Class that calculates the "convex hull" extent of a cluster of hits
*/

#include "LACE/LpcConvexHull.hh"

#include "LACE/LpcCluster.hh"

#include <Eigen/Dense>

LpcConvexHull::LpcConvexHull()
{
}

LpcConvexHull::~LpcConvexHull()
{
}

void LpcConvexHull::findLengths(LpcCluster* theCluster)
{

    // Find the convex hull lengths along the spatial dimensions of
    // the cluster and store them in the cluster pointer. Originally,
    // this method would require the calculation of the convex hull
    // points with QHull. But, the extent of the hull is simply given
    // by the size of the rectangular box that will enclose the
    // cluster "ellipsoid", i.e. the lengths (max-min range) along
    // each of the principal axes

    if (!theCluster) {return;}

    //Retrieve the matrix of cluster hit positions
    Eigen::MatrixXd hitCoords = theCluster->getHitPositions();

    // We need to transform the point co-ordinates to lie along the principal axes.
    // Find the eigenvectors of the covariance matrix of the hit co-ordinates
    Eigen::MatrixXd covMatrix = functions_.getCovarianceMatrix(hitCoords);

    std::pair<Eigen::VectorXd, Eigen::MatrixXd> pcaEigen =
	functions_.findNormEigenVectors(covMatrix);

    Eigen::MatrixXd eVectors = pcaEigen.second;

    Eigen::MatrixXd eV = eVectors.transpose();

    // Transform the hit co-ordinates. Row = point, col = x,y,z,...
    Eigen::MatrixXd transCoords = hitCoords*eV;

    // Get the lengths along each axis
    int nDim = hitCoords.cols();
    Eigen::VectorXd lengths = Eigen::VectorXd::Zero(nDim);

    for (int i = 0; i < nDim; i++) {

	Eigen::VectorXd coordCol = transCoords.col(i);
	double maxVal = coordCol.maxCoeff();
	double minVal = coordCol.minCoeff();
	double range = fabs(maxVal - minVal);

	lengths(i) = range;

    }

    theCluster->storeConvexHull(lengths);

}


#include "utils.hpp"
#include <cmath>
#include <fstream>
#include <boost/range/numeric.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/median.hpp>

using namespace std;

namespace delphi::utils {

/**
 * Returns the square of a number.
 */
// double sqr(double x) { return x * x; }

/**
 * Returns the sum of a vector of doubles.
 */
double sum(const std::vector<double> &v) { return boost::accumulate(v, 0.0); }

/**
 * Returns the arithmetic mean of a vector of doubles.
 * Updated based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double mean(const std::vector<double> &v) {
    if (v.empty()) {
        return std::numeric_limits<double>::quiet_NaN();
    }

    return sum(v) / v.size();
}

/**
 * Returns the sample standard deviation of a vector of doubles.
 * Based on:
 * https://codereview.stackexchange.com/questions/185450/compute-mean-variance-and-standard-deviation-of-csv-number-file
 */
double standard_deviation(const double mean, const std::vector<double>& v)
{
    if (v.size() <= 1u)
        return std::numeric_limits<double>::quiet_NaN();

    auto const add_square = [mean](double sum, int i) {
        auto d = i - mean;
        return sum + d*d;
    };
    double total = std::accumulate(v.begin(), v.end(), 0.0, add_square);
    return sqrt(total / (v.size() - 1));
}

/**
 * Returns the median of a vector of doubles.
 */
double median(const std::vector<double> &xs) {
  using namespace boost::accumulators;
  accumulator_set<double, features<tag::median>> acc;
  for (auto x : xs) {
    acc(x);
  }
  return boost::accumulators::median(acc);
}

double log_normpdf(double x, double mean, double sd) {
  double var = pow(sd, 2);
  double log_denom = -0.5 * log(2 * M_PI) - log(sd);
  double log_nume = pow(x - mean, 2) / (2 * var);

  return log_denom - log_nume;
}

nlohmann::json load_json(string filename) {
  ifstream i(filename);
  nlohmann::json j = nlohmann::json::parse(i);
  return j;
}

} // namespace delphi::utils


#include <armadillo>
#include <math.h>
#include <fruitpunch/Graphics/Matrices.h>
#include <fruitpunch/Graphics/Vertex.h>
#include <fruitpunch/Graphics/Renderer.h>
#include <vector>

#define PI180 0.0174532925

using namespace arma;
using namespace std;

namespace fp_core {

/**
 * Returns
 */
fmat33 rotation_matrix(float angle) {
  float rad = angle * PI180;

  fmat33 rot;
  rot << cos(rad) << -sin(rad) << 0 << endr
      << sin(rad) << cos(rad) << 0 << endr
      << 0 << 0 << 1 << endr;
  return rot;
}

fmat33 translation_matrix(point p) {
  // generates the translation matrix
  fmat33 trans;
  trans.eye();
  trans.at(0, 2) = p.x;
  trans.at(1, 2) = p.y;

  return trans;
}

fmat33 scale_matrix(point p) {
  fmat33 trans;
  trans.eye();
  trans.at(0, 0) = p.x;
  trans.at(1, 1) = p.y;

  return trans;
}

fmat33 to_local_space_matrix(point center) {
  center.x = -center.x;
  center.y = -center.y;
  return translation_matrix(center);
}

fmat33 to_global_space_matrix(point center) {
  return translation_matrix(center);
}

void apply_matrix_transformation(point * points, int size,
    Renderer& renderer, fmat33 transformation) {

  // create point array
  for (int i = 0; i < size; i++) {
    // gets the point

    point& p = points[i];
    //printf("Added %d,%d", m_vertices[i].position().x, m_vertices[i].position().y);
    // transforms the point
    points[i] = renderer.transform_point(p, transformation);
    //printf("Transformed (%f,%f) \n", parr[i].x, parr[i].y);
  }

}

}


/**
 * @file kruskal.cpp
 * @author prakash sellathurai
 * @brief minimum spanning tree using Kruskal algorithm
 * @version 0.1
 * @date 2021-07-16
 *
 * @copyright Copyright (c) 2021
 *
 */

#include <algorithm>
#include <boost/pending/disjoint_sets.hpp>
#include <iostream>
#include <vector>

using namespace std;

/**
 * @brief Edge pairs are stored in a vector
 *
 */
class EdgePair {
public:
  int x, y, weight;
  EdgePair() {}
  EdgePair(int x, int y, int weight) : x(x), y(y), weight(weight) {}
};

/**
 * @brief Weighted Graph
 *
 */
class Graph {
private:
  int V;                                // No. of vertices
  int E;                                // No. of edges
  vector<vector<pair<int, int>>> edges; // Graph represented as adjacency list

public:
  Graph(int V) : V(V+1), E(0) { edges.resize(V+1); }
  void addEdge(int v, int w, int weight) {
    edges[v].push_back(make_pair(w, weight));
    edges[w].push_back(make_pair(v, weight));
    E++;
  }

  static bool sortbysec(const EdgePair &e1, const EdgePair &e2) {
    return e1.weight < e2.weight;
  }
  vector<EdgePair> to_edgearray() {
    vector<EdgePair> res;
    for (int i = 0; i < V; i++) {
      for (auto edge : edges[i]) {
        res.push_back(EdgePair(i, edge.first, edge.second));
      }
    }
    return res;
  }

  /**
   * @brief kruskal  algorithm, for finding minimum spanning tree weight
   *
   * @return int
   */
  int kruskal() {
    int weight = 0; /*cost of minimum spanning tree*/
    vector<EdgePair> e = to_edgearray();
    sort(e.begin(), e.end(), sortbysec);
    int vectorlist[V + 1];
    int parentlist[V + 1];
    boost::disjoint_sets<int *, int *> ds(vectorlist, parentlist);
    for (int i = 0; i < V; i++) {
      ds.make_set(i);
    }

    std::cout << std::endl;
    for (auto edge:e) {
      auto u = ds.find_set(edge.x);
      auto v = ds.find_set(edge.y);
      if (u != v) {
        std::cout << "Edge : " << edge.x << " " << edge.y << std::endl;
        weight += edge.weight;
        ds.link(edge.x, edge.y);
      }
    }
    return weight;
  }
};

int main(int argc, const char **argv) {
  Graph g(5);
  g.addEdge(0, 1, 1);
  g.addEdge(1, 2, 2);
  g.addEdge(2, 3, 3);
  std::cout << "Minimum spanning tree weight is : " << g.kruskal() << std::endl;
  return 0;
}