/**
 * \file boost/numeric/ublasx/operation/linspace.hpp
 *
 * \brief Linearly spaced vector
 *
 * Inspired by MATLAB's linspace function.
 *
 *

 *
 * Copyright (c) 2013, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP

#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief Generates a linearly spaced vector.
 *
 * Generates \a n values linearly spaced between \a a and \a b.
 * Note, in case \f$a<b\f$, generates a decreasing sequence.
 *
 * Inspired by MATLAB's linspace function.
 *
 * \param a The starting value of the linearly spaced sequence.
 * \param b The final value of the linearly spaced sequence.
 * \param n The number of values to generate
 * \return A vector of linearly spaced values in \f$[a,b]\f$; if
 *  n=1, returns \a b.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
BOOST_UBLAS_INLINE
vector<ValueT> linspace(ValueT a, ValueT b, std::size_t n = 100)
{
	// pre: n > 0
	BOOST_UBLAS_CHECK( n > 0,
					   bad_argument() );

	if (n < 2)
	{
		return scalar_vector<ValueT>(1, b);
	}

	vector<ValueT> x(n);

	--n;

	// Check for possible overflows generated by opposite signs
	const ValueT c = (b-a)*(n-1);
	if (std::isinf(c) && std::isinf(b-a))
	{
		// Overflows is happened => split computations

		const ValueT an = a/n;
		const ValueT bn = b/n;

		for (std::size_t i = 0; i <= n; ++i)
		{
			x[i] = a + bn*i - an*i;
		}
	}
	else
	{
		// No overflow

		const ValueT step = (b-a)/n;

		for (std::size_t i = 0; i <= n; ++i)
		{
			x[i] = a + step*i;
		}
	}

	// just make sure that endpoints are honored
	x[0] = a;
	x[n] = b;

	return x;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LINSPACE_HPP


/*
This program is meant to compute maximal Lyapunov expoennt of Lorenz system.
*/
#include <iostream>
#include <cmath>
#include <complex>
#include <fstream>
#include <string>
#include <ctime>
#include <cstdlib>
#include <blitz/array.h>
#include <sys/stat.h>

using namespace blitz;
// ============ Declaration of functions ===================
double Euclidean_measure(Array<double,1>);
Array<double,1> Lorenz_system(Array<double,1>);
Array<double,1> RK4_integrator(Array<double,1>, double, Array<double,1> (*F)(Array<double, 1>) );
Array<double,1> linearized_Lorenz_system(Array<double,1>, Array<double,1>);
Array<double,1> error_RK4_integrator(Array<double,1>, Array<double,1>, double, Array<double,1> (*F)(Array<double,1>, Array<double,1>));
double max_lypo_expo(Array<double,1> , double, Array<double,1> (*F)(Array<double,1>) );

// =========== I/O files ==================================
ofstream time_series_file;

int main(int argc, char const *argv[])
{
	Array<double,1> X(3);								// State vector for the reference trajectory
	X = -3.16,-5.31,13.31;							// Initial condition r = 28
	// X = 0.62225717,-0.08232857,30.60845379;				// Initial condition r = 45.92
	// X = 4.29033, -0.577049, 28.8962;
	double dt = 0.001;
	max_lypo_expo(X, dt, Lorenz_system);
	return 0;
}

double Euclidean_measure(Array<double,1> A)
{
	int N = A.numElements();
	double d = 0;
	for (int i = 0; i < N; ++i)
	{
		d += pow(A(i),2);
	}
	d = sqrt(d);
	return d;
}

Array<double,1> RK4_integrator(Array<double,1> X0, double dt, Array<double,1> (*F)(Array<double,1>) )
{
	int dim = X0.numElements();
	Array<double,1> X(dim);				// State vector
	// -------- Intermediate derivatives --------
	Array<double,1> K1(dim);
	Array<double,1> K2(dim);
	Array<double,1> K3(dim);
	Array<double,1> K4(dim);
	// ------------------------------------------
	K1 = dt*F(X0);
	X = X0 + 0.5*K1;
	K2 = dt*F(X);
	X = X0 + 0.5*K2;
	K3 = dt*F(X);
	X = X0 + K3;
	K4 = dt*F(X);
	X = X0 + (1.0/6)*(K1 + 2*K2 + 2*K3 + K4);
	return X;

}

Array<double,1> error_RK4_integrator( Array<double,1> E0, Array<double,1> X, double dt, Array<double,1> (*F)(Array<double,1>, Array<double,1>))
{
	// X is the state vector
	int dim = E0.numElements();
	Array<double,1> E(dim);				// Tangent vector
	// -------- Intermediate derivatives --------
	Array<double,1> K1(dim);
	Array<double,1> K2(dim);
	Array<double,1> K3(dim);
	Array<double,1> K4(dim);
	// ------------------------------------------
	K1 = dt*F(E0, X);
	E = E0 + 0.5*K1;
	K2 = dt*F(E0, X);
	E = E0 + 0.5*K2;
	K3 = dt*F(E0, X);
	E = E0 + K3;
	K4 = dt*F(E0, X);
	E = E0 + (1.0/6)*(K1 + 2*K2 + 2*K3 + K4);
	return E;

}

Array<double,1> Lorenz_system(Array<double,1> X)
{
	// ------ Parameters ------------------------
	double sigma = 10;//16; //10;
	double r = 28;//45.92; //28;
	double b = 8.0/3;//4; //8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-X(0) + X(1));
	F(1) = (r- X(2))*X(0) - X(1);
	F(2) = X(0)*X(1) - b*X(2);
	return F;
}

/*Array<double,1> Rotating_Lorenz_system(Array<double,1> X)
{
	// ------ Parameters ------------------------
	double sigma = 16; //10;
	double r = 45.92; //28;
	double b = 4; //8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-X(0) + X(1));
	F(1) = (r- X(2))*X(0) - X(1);
	F(2) = X(0)*X(1) - b*X(2);
	return F;
}*/

Array<double,1> linearized_Lorenz_system(Array<double,1> E, Array<double,1> X)
{
	// X is the reference trajectory while
	// E is the error along the refrence trajectory.
	// ------ Parameters ------------------------
	double sigma = 10;
	double r = 28;
	double b = 8.0/3;
	// ------------------------------------------

	Array<double,1> F(3);
	F(0) = sigma*(-E(0) + E(1));
	F(1) = (r- X(2))*E(0) - E(1)- X(0)*E(2);
	F(2) = X(1)*E(0) + X(0)*E(1)- b*E(2);
	return F;
}

double max_lypo_expo(Array<double,1> X, double dt, Array<double,1> (*F)(Array<double,1>) )
{

	// Array<double,1> X(3);				// State vector for the reference trajectory
	// X = -3.16,-5.31,13.31;				// Initial condition r = 28
	// X = 0.62225717,-0.08232857,30.60845379;				// Initial condition r = 45.92
	// X = 4.29033, -0.577049, 28.8962;

	int T_transient = 32000;
	// ======  Removing the transience to enter the attractor========
	for (int i = 0; i < T_transient; ++i)
	{
		X = RK4_integrator(X, dt, F);
	}
	// ==============================================================

	cout<<X<<endl;						// After transient, the initial condition to start with.
	// ======================== Calculation of Lyapunov exponent ===============================
	Array<double,1> Y(3);				// State vector for the second trajectory

	Array<double,1> E(3);				// Tangent vector
	E = 1e-8,0,0;						// Initial tangent vector

	double d0;
	d0 = Euclidean_measure(E); 			// Initial error
	double d = 0;						// length of tangent vector after one iteration
	double div_rate = 0;				// divergence rate of trajectories defined as log(d/d0).
	double le=0;						// Lyapunov exponent defined as the average of div_rate along the trajectories.
	int T = 100000;
	// time_series_file.open("test_data_lypo_r45.d");
	for (int i = 1; i <= T; ++i)
	{
		Y = X+E;						// Initial condition for the second trajectory

		//------Simultaneous evolution of the two trajectories by one time step ------
		X = RK4_integrator(X, dt, F);
		Y = RK4_integrator(Y, dt, F);
		//---------------------------------------------------------------------------

		E = Y-X;						// The tangent vector after one time step
		// cout<<E<<endl;
		d = Euclidean_measure(E);		// Length of the tangent vector
		// cout<<"d: "<<d<<endl;
		div_rate = log(d/d0);			// Local divergence rate of the trajectories
		le +=div_rate;					// For calculating running average of divergence rate
		E = (d0/d)*E;					// Renormalization of the tangent vector to length d0
		// cout<<E<<endl;
		// time_series_file<<div_rate<<endl;	// Writing local div rate in file
		if (i%1000==0)
		{
			cout<<i<<"\t"<<le/i<<endl;		//  Writing running average on std output
		}
	}
	// time_series_file.close();
	le = le/(T*dt);
	cout<<"Lyapunov exponent: \t"<<le<<endl;		//  Writing running average on std output

	// =========================================================================================
	return le;

}


/*
 * BezierCurve.cpp
 *
 *  Created on: Dec 5, 2008
 *      Author: eddyc
 */

#include <iostream>
#include <assert.h>
#include <vector>
#include <cmath>
#include "Point.h"
#include "Vector.h"
#include "matrix.h"
#include "BezierCurve.h"
#include <boost/foreach.hpp>

using namespace std;
using namespace math;

BezierCurve::BezierCurve(Point p1, Point p2, Point p3, Point p4) {
	this->controls.SetSize(4,4);
	controls(0,0) = p1;
	controls(1,0) = p2;
	controls(2,0) = p3;
	controls(3,0) = p4;
	curve.push_back(p1);
	curve.push_back(p2);
	curve.push_back(p3);
	curve.push_back(p4);
	initMatrices();
}

BezierCurve::BezierCurve(vector<Point> controls) : curve(controls) {
	assert(controls.size() == 4);
	this->controls(0,0) = controls[0];
	this->controls(1,0) = controls[1];
	this->controls(2,0) = controls[2];
	this->controls(3,0) = controls[3];
	initMatrices();
}

BezierCurve::BezierCurve(matrix<Point> c) : controls(c) {
	curve.push_back(c(0,0));
	curve.push_back(c(1,0));
	curve.push_back(c(2,0));
	curve.push_back(c(3,0));
	initMatrices();
}

BezierCurve::~BezierCurve() {

}

void BezierCurve::initMatrices() {
	Sk.SetSize(4,4);
	Sk(0,0) = 1.0;
	Sk(1,0) = Sk(1,1) = Sk(2,1) = 0.5;
	Sk(2,0) = Sk(2,2) = 0.25;
	Sk(3,0) = Sk(3,3) = 1.0 / 8.0;
	Sk(3,1) = Sk(3,2) = 3.0 / 8.0;

	Si.SetSize(4,4);
	Si(0,3) = Si(2,1) = 1.0;
	Si(1,2) = Si(3,0) = -1.0;
	Si(1,3) = 2.0;
	Si(2,2) = -4.0;
	Si(2,3) = 4.0;
	Si(3,1) = 6.0;
	Si(3,2) = -12.0;
	Si(3,3) = 8.0;
}

matrix<Point> BezierCurve::apply(matrix<double> S, matrix<Point> controls) {
	matrix<Point> newControls(4,1);
	newControls(0,0) = S(0,0) * controls(0,0) + S(0,1) * controls(1,0) +
						S(0,2) * controls(2,0) + S(0,3) * controls(3,0);
	newControls(1,0) = S(1,0) * controls(0,0) + S(1,1) * controls(1,0) +
						S(1,2) * controls(2,0) + S(1,3) * controls(3,0);
	newControls(2,0) = S(2,0) * controls(0,0) + S(2,1) * controls(1,0) +
						S(2,2) * controls(2,0) + S(2,3) * controls(3,0);
	newControls(3,0) = S(3,0) * controls(0,0) + S(3,1) * controls(1,0) +
						S(3,2) * controls(2,0) + S(3,3) * controls(3,0);
	return newControls;
}

void BezierCurve::refine(int level) {
	matrix<Point> subdivision = controls;
	curve.clear();

	// get the first subdivision by applying Sk level times
	for (int k = 0; k < level; k++)
		subdivision = apply(Sk, subdivision);
	curve.push_back(subdivision(0,0));
	curve.push_back(subdivision(1,0));
	curve.push_back(subdivision(2,0));
	curve.push_back(subdivision(3,0));

	// get subsequent subdivisions by applying Si 2^level - 1 times
	for (int i = 0; i < (int)pow(2.0,level) - 1; i++) {
		subdivision = apply(Si, subdivision);
		curve.push_back(subdivision(0,0));
		curve.push_back(subdivision(1,0));
		curve.push_back(subdivision(2,0));
		curve.push_back(subdivision(3,0));
	}
}

void BezierCurve::print() {
	BOOST_FOREACH(Point p, curve) {
		cout << p << endl;
	}
}


/**
 * \file RobertBristowJohnsonFilter.cpp
 */

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

#include "RobertBristowJohnsonFilter.h"
#include "IIRFilter.h"

namespace ATK
{
  template<typename DataType>
  RobertBristowJohnsonLowPassCoefficients<DataType>::RobertBristowJohnsonLowPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonHighPassCoefficients<DataType>::RobertBristowJohnsonHighPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonBandPassCoefficients<DataType>::RobertBristowJohnsonBandPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonBandPass2Coefficients<DataType>::RobertBristowJohnsonBandPass2Coefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonBandStopCoefficients<DataType>::RobertBristowJohnsonBandStopCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonAllPassCoefficients<DataType>::RobertBristowJohnsonAllPassCoefficients(int nb_channels)
    :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

  template<typename DataType>
  RobertBristowJohnsonBandPassPeakCoefficients<DataType>::RobertBristowJohnsonBandPassPeakCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

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

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

  template<typename DataType>
  RobertBristowJohnsonLowShelvingCoefficients<DataType>::RobertBristowJohnsonLowShelvingCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1), gain(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

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

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

  template<typename DataType>
  RobertBristowJohnsonHighShelvingCoefficients<DataType>::RobertBristowJohnsonHighShelvingCoefficients(int nb_channels)
  :Parent(nb_channels), Q(1), gain(1)
  {
  }

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

    DataType w0 = 2 * boost::math::constants::pi<DataType>() * cut_frequency / input_sampling_rate;
    DataType cosw0 = std::cos(w0);
    DataType alpha = std::sin(w0) / (2 * Q);
    DataType 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(DataType_ Q)
  {
    if (Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

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

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

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

  template class RobertBristowJohnsonLowPassCoefficients<float>;
  template class RobertBristowJohnsonLowPassCoefficients<double>;
  template class RobertBristowJohnsonHighPassCoefficients<float>;
  template class RobertBristowJohnsonHighPassCoefficients<double>;
  template class RobertBristowJohnsonBandPassCoefficients<float>;
  template class RobertBristowJohnsonBandPassCoefficients<double>;
  template class RobertBristowJohnsonBandPass2Coefficients<float>;
  template class RobertBristowJohnsonBandPass2Coefficients<double>;
  template class RobertBristowJohnsonAllPassCoefficients<float>;
  template class RobertBristowJohnsonAllPassCoefficients<double>;
  template class RobertBristowJohnsonBandStopCoefficients<float>;
  template class RobertBristowJohnsonBandStopCoefficients<double>;
  template class RobertBristowJohnsonBandPassPeakCoefficients<float>;
  template class RobertBristowJohnsonBandPassPeakCoefficients<double>;
  template class RobertBristowJohnsonLowShelvingCoefficients<float>;
  template class RobertBristowJohnsonLowShelvingCoefficients<double>;
  template class RobertBristowJohnsonHighShelvingCoefficients<float>;
  template class RobertBristowJohnsonHighShelvingCoefficients<double>;

  template class IIRFilter<RobertBristowJohnsonLowPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonLowPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonHighPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonHighPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPass2Coefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPass2Coefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandStopCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandStopCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonAllPassCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonAllPassCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonBandPassPeakCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonBandPassPeakCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonLowShelvingCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonLowShelvingCoefficients<double> >;
  template class IIRFilter<RobertBristowJohnsonHighShelvingCoefficients<float> >;
  template class IIRFilter<RobertBristowJohnsonHighShelvingCoefficients<double> >;
}


#include <ipc/utils/eigen_ext.hpp>

#include <stdexcept> // std::runtime_error

#include <Eigen/Eigenvalues>

#include <ipc/utils/logger.hpp>

namespace ipc {

// Matrix Projection onto Positive Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_pd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A,
    double eps)
{
    assert(eps > 0);

    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(error("unable to project matrix onto positive definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] > 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative or zero values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] <= 0.0) {
            D.diagonal()[i] = eps;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

// Matrix Projection onto Positive Semi-Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_psd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A)
{
    // https://math.stackexchange.com/q/2776803
    Eigen::SelfAdjointEigenSolver<
        Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>
        eigensolver(A);
    if (eigensolver.info() != Eigen::Success) {
        IPC_LOG(
            error("unable to project matrix onto positive semi-definite cone"));
        throw std::runtime_error(
            "unable to project matrix onto positive definite cone");
    }
    // Check if all eigen values are zero or positive.
    // The eigenvalues are sorted in increasing order.
    if (eigensolver.eigenvalues()[0] >= 0.0) {
        return A;
    }
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> D(eigensolver.eigenvalues());
    // Save a little time and only project the negative values
    for (int i = 0; i < A.rows(); i++) {
        if (D.diagonal()[i] < 0.0) {
            D.diagonal()[i] = 0.0;
        } else {
            break;
        }
    }
    return eigensolver.eigenvectors() * D
        * eigensolver.eigenvectors().transpose();
}

template <typename DerivedA, typename DerivedB, typename Result>
Result cross(
    const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b)
{
    assert(a.size() == 3 && b.size() == 3);
    Result c(a.rows(), a.cols());
    c(0) = a(1) * b(2) - a(2) * b(1);
    c(1) = a(2) * b(0) - a(0) * b(2);
    c(2) = a(0) * b(1) - a(1) * b(0);
    return c;
}

} // namespace ipc


#include <PCP/Curvature/Curvature.h>
#include <PCP/Geometry/Geometry.h>

#include <PCP/SpacePartitioning/KdTree.h>

#include <Eigen/Eigenvalues>

namespace pcp {

// see https://perso.liris.cnrs.fr/julie.digne/articles/compute_wavejets.m
PointWiseEstimationData CurvatureComputer_WJets::compute(const Geometry& points, int i0, Scalar s) const
{
    constexpr auto order = 2;
    constexpr auto I     = std::complex<Scalar>(0,1); // complex number 0+i

    // % compute PCA normal
    Matrix3 C      = Matrix3::Zero();
    Vector3 m      = Vector3::Zero();
    int     nneigh = 0;

    for(int i : points.kdtree().range_neighbors(i0, s))
    {
        m      += points[i];
        C      += points[i] * points[i].transpose();
        nneigh += 1;
    }

    if(nneigh < NEI_COUNT_MIN) return PointWiseEstimationData::Invalid();

    m /= nneigh;
    C = C/nneigh - m * m.transpose();

    // eigenvalues are positive and ordered
    // eigenvectors are normalized
    Eigen::SelfAdjointEigenSolver<Matrix3> solver(C);
//    const Vector3 normal = solver.eigenvectors().col(0);
    const Vector3 t2 = solver.eigenvectors().col(1);
    const Vector3 t1 = solver.eigenvectors().col(2);
    const Vector3 normal = t1.cross(t2);

    Matrix3 P;
    P.col(0) = t1;
    P.col(1) = t2;
    P.col(2) = normal;

    // % compute Phi
    constexpr int ncolM = order*order/2 + 3*order/2+1; // = 6
    PCP_ASSERT(ncolM == 6);

    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> M(nneigh,ncolM);
    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1>              b(nneigh);

    M.setZero();
    b.setZero();

    int nei_count = 0;

    int j=0;
    for(int i : points.kdtree().range_neighbors(i0, s))
    {
        // % express neighbors in local coord system
        const Vector3 locneighbors = P.transpose() * (points[i] - points[i0]);

        // % switch to polar coordinates
        const Scalar r     = locneighbors.head<2>().norm();
        const Scalar theta = std::atan2(locneighbors.y(), locneighbors.x());
              Scalar z     = locneighbors.z();

        const Scalar normalized_r = r / s;
        z = z / s;

        const Scalar w = std::exp(-normalized_r * normalized_r/18.0);

        int idx = 0;
        for(int k=0; k<=order; ++k)
        {
            const Scalar rk = std::pow(normalized_r, k);
            for(int n = -k; n <= k; n += 2)
            {
                M(j,idx) = rk * std::exp(I * Scalar(n) * theta) * w;
                ++idx;
            }
        }
        b[j] = z * w;
        ++j;
        ++nei_count;
    }
    if(nei_count < NEI_COUNT_MIN) return PointWiseEstimationData::Invalid();

    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> Phi = M.colPivHouseholderQr().solve(b);

    // % correct normal
    Vector3 u = - t1 * std::imag(Phi(1) - Phi(2)) + t2 * std::real(Phi(2) + Phi(1)); // % axis
    u.normalize();
    Scalar gamma = - std::atan( 2 * std::abs(Phi(2))); // % angle

    Vector3 nc = std::cos(gamma) * normal + (1 - std::cos(gamma)) * (normal.dot(u)) * u + std::sin(gamma) * u.cross(normal);
    nc.normalize();
    Vector3 t1c = t1 - (t1.transpose() * nc) * nc;
    t1c.normalize();
    Vector3 t2c = nc.cross(t1c);
    t2c.normalize();

    // % redo everything (instead of correcting the coefficients - quick and dirty method, to be improved later)
    Matrix3 Pc;
    Pc.col(0) = t1c;
    Pc.col(1) = t2c;
    Pc.col(2) = nc;

    M.setZero();
    b.setZero();

    nei_count = 0;

    j=0;
    for(int i : points.kdtree().range_neighbors(i0, s))
    {
        // % express neighbors in local coord system
        const Vector3 locneighbors = Pc.transpose() * (points[i] - points[i0]);   // Pc not P !

        // switch to polar coordinates
        const Scalar r     = locneighbors.head<2>().norm();
        const Scalar theta = std::atan2(locneighbors.y(), locneighbors.x());
              Scalar z     = locneighbors.z();

        const Scalar normalized_r = r / s;
        z = z / s;

        const Scalar w  = std::exp(-normalized_r*normalized_r/18.0);

        int idx = 0;
        for(int k=0; k<=order; ++k)
        {
            const Scalar rk = std::pow(normalized_r, k);
            for(int n = -k; n <= k; n += 2)
            {
                M(j,idx) = rk * std::exp(I * Scalar(n) * theta) * w;
                ++idx;
            }
        }
        b[j] = z * w;
        ++j;
        ++nei_count;
    }
    if(nei_count < NEI_COUNT_MIN) return PointWiseEstimationData::Invalid();

    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> Phicorr = M.colPivHouseholderQr().solve(b);

    // % compute real(a0) and |an| n>=1
//    idx = 1;
//    an=zeros(order+1,1);
//    for k=0:order
//        for n = -k:2:k
//          if n>=0
//           an(n+1) = an(n+1) + Phicorr(idx)/(k+2);
//          end
//          idx=idx+1;
//        end
//    end
//    an(1)=real(an(1));%imaginary part is 0
//    an(2:end)=abs(an(2:end));

//    const std::complex<Scalar> phi_0_0  = Phicorr(0);
//    const std::complex<Scalar> phi_1_m1 = Phicorr(1);
//    const std::complex<Scalar> phi_1_p1 = Phicorr(2);
    const std::complex<Scalar> phi_2_m2 = Phicorr(3);
    const std::complex<Scalar> phi_2_0  = Phicorr(4);
    const std::complex<Scalar> phi_2_p2 = Phicorr(5);

    const Vector3 N = nc; // corrected normal !

    Scalar k1 = 2 * std::real(phi_2_0 + phi_2_p2 + phi_2_m2);
    Scalar k2 = 2 * std::real(phi_2_0 - phi_2_p2 - phi_2_m2);

    if(std::abs(k2) > std::abs(k1)) std::swap(k1,k2);

    // reset to original scale
    k1 /= s;
    k2 /= s;

    const Scalar H = 0.5 * (k1 + k2);

    return PointWiseEstimationData(k1, k2, H, N, Vector3::Constant(666), Vector3::Constant(666), nei_count);
}

} // namespace pcp


#include <Eigen/Core>

namespace Eigen {
namespace Vector {
template <class T>
inline void push_back(Eigen::Matrix<T,Eigen::Dynamic,1>& v, const T d) {
  Eigen::Matrix<T,Eigen::Dynamic,1> tmp = v;
  v.resize(tmp.size() + 1);
  v.head(tmp.size()) = tmp;
  v[v.size()-1] = d;
}
template <class T>
inline void wedge(Eigen::Matrix<T,3,3>& mat, const Eigen::Matrix<T,3,1>&v) {
  mat(0,0) = 0.0;
  mat(0,1) = -v[2];
  mat(0,2) = v[1];
  mat(1,0) = v[2];
  mat(1,1) = 0.0;
  mat(1,2) = -v[0];
  mat(2,0) = -v[1];
  mat(2,1) = v[0];
  mat(2,2) = 0.0;
}
}
}


#include <cstdio>
#include <iostream>
#include <vector>
#include <numeric>
#include <boost/integer/common_factor.hpp>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using boost::multiprecision::cpp_int;

const int MOD = 1000000007;

cpp_int gcd(cpp_int a, cpp_int b) {
    if (a < b) swap(a, b);
    return b == 0 ? a : gcd(b, a % b);
}

cpp_int lcm(cpp_int a, cpp_int b) {
    return a * b / gcd(a, b);
}

int main() {
    int n;
    const int SIZE = 10007;

    cpp_int A[SIZE];
    scanf("%d", &n);
    for (auto i = 0; i < n; i ++) {
        cin >> A[i];
    }
    cpp_int prod = lcm(A[0], A[1]);
    for(auto i = 2; i < n; i ++) prod = lcm(prod, cpp_int(A[i]));
    cpp_int sum = 0;
    for(auto i = 0; i < n; i++) {
        sum += (prod / A[i]);
        sum %=  MOD;
    }
    std::cout << sum << endl;
    return 0;
}


#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>

#include "FFT.h"
#include "params.h"

using namespace std;
using namespace NTL;

void FFTStep(CC_t * const f_fft, RR_t const * const f, const unsigned long N, const CC_t w0)
{
    if(N==1)
    {
        f_fft[0] = f[0];
    }
    else
    {
        if(N==2)
        {
            f_fft[0] = f[0] + ii*f[1];
            f_fft[1] = f[0] - ii*f[1];
        }
        else
        {
            assert(N%2==0);
            RR_t f0[N0/2], f1[N0/2];
            CC_t f0_fft[N0/2], f1_fft[N0/2], wk, w02;
            unsigned int k;
            for(k=0; k<(N/2); k++)
            {
                f0[k] = f[2*k];
                f1[k] = f[2*k+1];
            }

            w02 = w0*w0;
            wk = w0;
            FFTStep(f0_fft, f0, N/2, w02);
            FFTStep(f1_fft, f1, N/2, w02);
            for(k=0; k<N; k++)
            {
                f_fft[k] = f0_fft[k%(N/2)] + wk*f1_fft[k%(N/2)];
                wk *= w02;
            }
        }
    }
}

void ReverseFFTStep(CC_t * const f, CC_t const * const f_fft, const unsigned long N, const CC_t w0)
{
    if(N!=2)
    {
        assert(N%2==0);
        CC_t f0[N0/2], f1[N0/2];
        CC_t f0_fft[N0/2], f1_fft[N0/2], w02, wk;
        unsigned int k;

        w02 = w0*w0;
        wk = w0;

        for(k=0; k<N/2; k++)
        {
            f0_fft[k] = (f_fft[k] + f_fft[k+(N/2)])*0.5l;
            f1_fft[k] = wk*(f_fft[k] - f_fft[k+(N/2)])*0.5l;
            wk *= w02;
        }
        ReverseFFTStep(f0, f0_fft, (N/2), w02);
        ReverseFFTStep(f1, f1_fft, (N/2), w02);

        for(k=0; k<N/2; k++)
        {
            f[2*k] = f0[k];
            f[2*k+1] = f1[k];
        }
    }
    else
    {
        f[0] = (f_fft[0] + f_fft[1])*0.5l;
        f[1] = (f_fft[0] - f_fft[1])*(-0.5l*ii);
    }
}

void MyRealReverseFFT(double * const f, CC_t const * const f_fft)
{
    CC_t fprime[N0];
    unsigned int i;
    ReverseFFTStep(fprime, f_fft, N0, omega_1);

    for(i=0; i<N0; i++)
    {
        f[i] = (fprime[i]).real();
    }
}

void MyIntReverseFFT(long int * const f, CC_t const * const f_fft)
{
    CC_t fprime[N0];
    unsigned int i;

    ReverseFFTStep(fprime, f_fft, N0, omega_1);
    for(i=0; i<N0; i++)
    {
        f[i] = ((long int) round( fprime[i].real() ) );
    }
}

void MyIntFFT(CC_t * f_FFT, const long int * const f)
{
    RR_t f_double[N0];
    unsigned int i;
    for(i=0; i<N0; i++)
    {
        f_double[i] = ( RR_t ( f[i] ) );
    }

    FFTStep(f_FFT, f_double, N0, omega);
}

void ZZXToFFT(CC_t * f_FFT, const ZZX f)
{
    RR_t f_double[N0];
    unsigned int i;

    assert(deg(f)==N0-1);
    assert(MaxBits(f)<900);
    for(i=0; i<N0; i++)
    {
        f_double[i] = ( RR_t ( conv<double>(f[i]) ) );
    }

    FFTStep(f_FFT, f_double, N0, omega);
}

void FFTToZZX(ZZX& f, CC_t const * const f_FFT)
{
    double f_double[N0];
    unsigned int i;
    MyRealReverseFFT(f_double, f_FFT);

    f.SetLength(N0);
    for(i=0; i<N0; i++)
    {
        f[i] = conv<ZZ>(round(f_double[i]));
    }

}


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

int main(int argc, char **argv)
{
  // exploit namespaces to shorten code
  using namespace boost::numeric::ublas;
  using std::cout;
  using std::endl;

  // declare three 3x3 matrices of complex<long double> elements
  matrix<std::complex<long double> > m(3, 3), n(3, 3), o(3, 3);

  // iterate over 3x3 matrix entries
  // r : row index
  // c : column index
  for (unsigned r = 0; r < m.size1(); r++) {
    for (unsigned c = 0; c < m.size2(); c++) {
      // enumerated matrix entries
      m(r,c) = 3 * r + c;

      // complex numbers designating rows and cols
      n(r,c) = r + c * std::complex<long double>(0,1);

      // elementwise square of n
      o(r,c) = std::pow(n(r,c), 2);
    }
  }

  std::array<int,3> real = {-1, 0, 1};
  std::array<int,3> imag = {-1, 0, 1};
  int rs = real.size()-1;

  matrix<std::complex<long double> > p(3,3);

  for (unsigned r = 0; r < real.size(); r++) {
      for (unsigned c = 0; c < imag.size(); c++) {
          p(r,c) = real[rs-r] + imag[c] * std::complex<long double>(0,1);
      }
  }

  // print to screen as demonstration
  cout << "m:" << endl;
  cout << m << endl;
  cout << endl << "n:" << endl;
  cout << n << endl;
  cout << endl << "o:" << endl;
  cout << n << endl;
  cout << endl << "m + n:" << endl;
  cout << m + n << endl;
  cout << endl << "m * n:" << endl;
  cout << prod(m, n) << endl;
  cout << endl << "n * n - o:" << endl;
  cout << prod(n, n) - o << endl;

  cout << "complex plane:" << endl;
    for(unsigned i = 0;i<p.size1();i++) {
        for (unsigned j = 0; j<p.size2();++j)
        {
            cout<<p(i,j);
        }
        cout<<endl;
    }
}


/* Copyright (C) 2017 IBM Corp.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/****************************************************************************
mat_l: a module for handling matrices of type long
Used mainly in TDMatrix.cpp as trapDoorMatR is of type mat_l
*****************************************************************************/
#include <cassert>
#include <stdexcept>

#include <NTL/matrix.h>
#include "utils/tools.h"
#include "mat_l.h"
#include <cstdio>
#include <cstdlib>

NTL_CLIENT

//#define DEBUG

//X = I * n, set the dimension to n, clear and then set to value
void ident(mat_l& X, long n)
{
    X.SetDims(n,n);
    clear(X);
    for (long i = 0; i < n; i++)
        X[i][i] = 1L;
}

bool isZero(const mat_l& M)
{
  for (long i=0; i<M.NumRows(); i++) for (long j=0; j<M.NumCols(); j++) {
      if (M[i][j] != 0)
	return false;
    }
  return true;
}

/* square - compute X := A*A^t or X := A^t*A, depending on transpose flag.
   Does not reset the matrix X dimension as long as it is large enough
*/
void square(mat_l& X, const mat_l& A, bool transpose)
{
    long n = A.NumRows();
    long m = A.NumCols();

    // If X is not large enough, set its dimension. If X is large enough then
    // its dimension is not reset, and only the top-left n-by-m is affected.

    if (transpose) swap(n,m);
    if (X.NumRows() < n || X.NumCols() < m)
        X.SetDims(n,n);

    for (long i=0; i<n; i++)
    {
        for (long j=0; j<n; j++)
        {
            X[i][j] = 0;
            if (transpose)
            {
                for (long k=0; k<m; k++)
                    X[i][j] += A[k][i] * A[k][j];
            }
            else
            {
                for (long k=0; k<m; k++)
                    X[i][j] += A[i][k] * A[j][k];
            }
        }
    }
}

//mul - does not reset the matrix dimension as long as it is large enough
//multiplies a vector and a matrix

void mul(vec_l& x, const vec_l& a, const mat_l& B)
{
    long l = a.length();
    long m = B.NumCols();

    if (l != B.NumRows())
    {
        cerr << "mul(vec_l, mat_l) dimension mismatch, vec["<< l
             << "] x mat[" << B.NumRows() << "][" << B.NumCols() << "]\n";
        exit(0);
    }

    int oRow,iIndex, val;

    x.SetLength(l);
    for (oRow = 0; oRow < m; oRow++)
    {
        val=0;
        for (iIndex = 0; iIndex<l; iIndex++)
        {
            val += (a.at(iIndex) * B[iIndex][oRow]);
        }
        x[oRow] = val;
    }
}

//compute x = A*b, A is a matrix, x and b are vectors,
//function sets length of x to be equal to A.NumRows()
void mul(vec_l& x, const mat_l& A, const vec_l& b)
{
    int nRows, nCols, nVecSize;
    int i,j;
    long val;

    nCols = A.NumCols();
    nVecSize = b.length();
    nRows = A.NumRows();
    x.SetLength(nRows);

    if (nCols != nVecSize)
    {
        throw std::logic_error("Matrix dimensions do not match");
        return;
    }

    for (i = 0; i < nRows; i++)
    {
        val=0;
        for (j = 0; j < nVecSize; j++)
            val+= A[i][j] * b.at(j);
        x[i] = val;
    }
}

//matrix multiplication by a number
void mul(mat_l& X, const mat_l& A, long b)
{
    long n = A.NumRows();
    long m = A.NumCols();

    X.SetDims(n, m);
    long i, j;

    for (i = 0; i < n; i++)
        for (j = 0; j < m; j++)
            X[i][j] = A[i][j] * b;
}

// matrix multiplication, the most naive implementation
void mul(mat_l& X, const mat_l& A, const mat_l& B)
{
    assert(A.NumCols() == B.NumRows());
    mat_l tmp(INIT_SIZE, A.NumRows(), B.NumCols());
    for (long i=0; i<tmp.NumRows(); i++) for (long j=0; j<tmp.NumCols(); j++)
        {
            tmp[i][j] = 0;
            for (long k=0; k<A.NumCols(); k++)
                tmp[i][j] += A[i][k] * B[k][j];
        }
    X = tmp;
}

//Reads and sets the size of the matrix and its values from an open file. The function gets the handle of the file to read from.
long readFromFile(mat_l& X, FILE* handle)
{
    long numRows,numCols;
    long count = fread(&numRows,sizeof(long),1,handle);
    count += fread(&numCols,sizeof(long),1,handle);
    X.SetDims(numRows,numCols);
    for (long i = 0; i < numRows; i++)
        count+= fread(X[i].elts(), numCols*sizeof(long), 1, handle);
    return count;
}

//Writes the size of the matrix and its values to an open file. Gets the handle of the file to write to.
long writeToFile(mat_l& X, FILE* handle)
{
    long numRows = X.NumRows();
    long numCols = X.NumCols();
    long count = fwrite(&numRows,sizeof(long),1,handle);
    count += fwrite(&numCols,sizeof(long),1,handle);
    for (long i = 0; i < numRows; i++)
        count+= fwrite(X[i].elts(), numCols*sizeof(long), 1, handle);
    return count;
}


///////////////////////////////////////////////////////////////////////////////
// kernel::example::scalar_rp.cpp                                            //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/math/special_functions/fpclassify.hpp> //needed?
#include <boost/math/tools/precision.hpp>
#include <boost/mpl/int.hpp>

#include <boost/statistics/detail/kernel/kernels/scalar/gaussian.hpp>
#include <boost/statistics/detail/kernel/estimation/rp_visitor.hpp>
#include <boost/statistics/detail/kernel/estimation/estimator.hpp>
#include <libs/statistics/detail/kernel/example/scalar_rp.h>

void example_scalar_rp(std::ostream& out){
    out << "-> example_scalar_rp : ";
    using namespace boost;

    // This example shows how to compute a Rosenblatt-Parzen estimate of the
    // density, p(x). The type used for each data-unit, here, is double

    //Types
    typedef double                                          val_;
    typedef std::vector<val_>                               vec_;
    typedef mt19937                                         urng_;
    typedef normal_distribution<val_>                       norm_;
    typedef variate_generator<urng_&,norm_>                 gen_;
    typedef statistics::detail::kernel::scalar::gaussian_kernel<val_>      gauss_k_;
    typedef statistics::detail::kernel::rp_visitor<gauss_k_,val_>   rp_visitor_;

    // Contants
    const val_ bandwidth = 0.5;
    const val_ eps = math::tools::epsilon<val_>();
    const unsigned n = 10;

    // Initialization
    vec_ dataset(n);
    vec_ vec_rp; vec_rp.reserve(n);
    urng_ urng;
    norm_ norm;
    gen_ gen(urng,norm);
    std::generate_n(
        begin(dataset),
        n,
        gen
    );

    // Computes a density estimate for each x in dataset
    BOOST_FOREACH(val_& x,dataset){
        val_ rp = for_each(
            boost::begin(dataset),
            boost::end(dataset),
            rp_visitor_(bandwidth,x)
        ).estimate();
        vec_rp.push_back(rp);
    }

    // Same as previous but calls estimator instead of for_each
    typedef sub_range<vec_> sub_x_;
    typedef
        statistics::detail::kernel::estimator<
            sub_x_,
            statistics::detail::kernel::rp_visitor,
            gauss_k_
        > estimator_;
    estimator_ estimator(bandwidth);
    sub_x_ sub_x(dataset);
    estimator.train(sub_x);
    vec_ vec_rp2; vec_rp2.reserve(n);

    for(unsigned i = 0; i<n; i++){
        val_ x = dataset[i];
        val_ rp = vec_rp[i];
        val_ rp2 = estimator.predict(x);
        BOOST_ASSERT(fabs(rp-rp2)<eps);
    }

    out << "<-" << std::endl;

}


#include <iostream>
#include <string>
#include <iomanip>
#include <fstream>

#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Straight_skeleton_2.h>
#include <CGAL/Straight_skeleton_builder_traits_2.h>
#include <CGAL/Straight_skeleton_builder_2.h>
#include <boost/shared_ptr.hpp>
#include <CGAL/gmp.h>
#include <CGAL/Timer.h>

template<class K>
void print_point ( CGAL::Point_2<K> const& p )
{
  //CGAL::exact(p);
  std::cout <<  p.x() << " " << p.y();
}

template<class K>
void print_straight_skeleton( CGAL::Straight_skeleton_2<K> const& ss )
{
  typedef CGAL::Straight_skeleton_2<K> Ss ;

  typedef typename Ss::Vertex_const_handle     Vertex_const_handle ;
  typedef typename Ss::Halfedge_const_handle   Halfedge_const_handle ;
  typedef typename Ss::Halfedge_const_iterator Halfedge_const_iterator ;

  Halfedge_const_handle null_halfedge ;
  Vertex_const_handle   null_vertex ;

  std::cerr << "Straight skeleton with " << ss.size_of_vertices()
            << " vertices, " << ss.size_of_halfedges()
            << " halfedges and " << ss.size_of_faces()
            << " faces" << std::endl ;

  for ( Halfedge_const_iterator i = ss.halfedges_begin(); i != ss.halfedges_end(); ++i )
  {
    if ( !i->is_bisector() ) continue;
    std::cout << "2 " ;
    print_point(i->opposite()->vertex()->point()) ;
    std::cout << " 0 " ;
    print_point(i->vertex()->point());
    std::cout << " 0\n" ;
    //std::cout << " " << ( i->is_bisector() ? "bisector" : "contour" ) << std::endl;
  }
}

typedef CGAL::Exact_predicates_exact_constructions_kernel       EPEC;
typedef CGAL::Exact_predicates_inexact_constructions_kernel       EPIC;
typedef CGAL::Simple_cartesian< CGAL::Lazy_exact_nt<CGAL::Gmpq> > SCLGQ;
typedef EPEC::Exact_kernel::FT                                EPEC_exact_ft;
typedef CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt       EPEC_with_sqrt;

template< class Kernel, class FT>
void build_skeleton(const char* fname)
{
  typedef typename Kernel::Point_2                                         Point_2;
  typedef CGAL::Polygon_2<Kernel>                                 Polygon_2;
  typedef CGAL::Straight_skeleton_builder_traits_2<Kernel>        SsBuilderTraits;
  typedef CGAL::Straight_skeleton_2<Kernel>                       Ss;
  typedef CGAL::Straight_skeleton_builder_2<SsBuilderTraits,Ss>   SsBuilder;

  Polygon_2 pgn;

  std::ifstream input(fname);

  FT x,y;
  while(input)
  {
    input >> x;
    if (!input) break;
    input >> y;
    if (!input) break;
    pgn.push_back( Point_2( typename Kernel::FT(x), typename Kernel::FT(y) ) );
  }
  input.close();

  std::cout << "Polygon has " << pgn.size() <<  " points\n";

  if(!pgn.is_counterclockwise_oriented()) {
      std::cerr << "Polygon is not CCW Oriented" << std::endl;
  }
  if(!pgn.is_simple()) {
      std::cerr << "Polygon is not simple" << std::endl;
  }

  CGAL::Timer time;
  time.start();
  SsBuilder ssb;
  ssb.enter_contour(pgn.vertices_begin(), pgn.vertices_end());
  boost::shared_ptr<Ss> straight_ske = ssb.construct_skeleton();
  time.stop();

  std::cout << "Time spent to build skeleton " << time.time() << "\n";

  if(!straight_ske->is_valid()) {
      std::cerr << "Straight skeleton is not valid" << std::endl;
  }

  std::cerr.precision(60);
  print_straight_skeleton(*straight_ske);

}

int main(int argc, char** argv) {
  if (argc!=2)
    std::cerr << "Usage: " << argv[0] << " polygon_points.txt\n";
  else
  {

    std::cout <<"Running with EPIC\n";
    build_skeleton<EPIC, double>(argv[1]);

    std::cout <<"Running with SCLGQ\n";
    build_skeleton<SCLGQ, SCLGQ::FT>(argv[1]);

    std::cout <<"Running with EPEC_with_sqrt\n";
    build_skeleton<EPEC_with_sqrt, EPEC_with_sqrt::FT>(argv[1]);

    std::cout <<"Running with EPEC\n";
    build_skeleton<EPEC, EPEC_exact_ft>(argv[1]);
  }

  return 0;
}


/*!
 * @file neumann_bc.hpp
 * @brief Contains implementation of Neumann boundary conditions.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_NEUMANN_BC_HPP_
#define INCLUDE_NEUMANN_BC_HPP_

// Deal.ii
#include <deal.II/base/function.h>

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

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class NeumannBC
 * @brief Class implements scalar Neumann conditions.
 */
template <int dim>
class NeumannBC : public Function<dim>
{
public:
	NeumannBC() : Function<dim>() {}

	virtual double value(const Point<dim> &p,
						const unsigned int component = 0) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int component = 0) const override;
};

template <int dim>
double
NeumannBC<dim>::value(const Point<dim> &p,
							   const unsigned int /*component*/) const
{
	double return_value = cos(2*PI_D*p(0)) * cos(2*PI_D*p(1));

	return return_value;
}

template <int dim>
void
NeumannBC<dim>::value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = cos(2*PI_D*points[p](0)) * cos(2*PI_D*points[p](1));
	} // end ++p
}

} // end namespace Coefficients

#endif /* INCLUDE_NEUMANN_BC_HPP_ */


#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <cmath>
using Eigen::MatrixXd;
using Eigen::VectorXd;
using namespace std;

int n_aug = 7;
double glambda;
double weight1;
double weightn;
double dt;
VectorXd x = VectorXd(5);//state vector
VectorXd x_aug(7);//augmented vector
MatrixXd P_aug(7,7);//augmented covariance matrix
MatrixXd P = MatrixXd(5, 5);//covariance matrix
MatrixXd sigma_aug(7,15);//augmented sigma point matrix
MatrixXd predict_sigma_aug(5,15);//predicted augmented sigma point matrix
MatrixXd measured_predict_sigma_aug(3,15);// predicted sigma points in measurement space
VectorXd stateMean(5);
MatrixXd stateCovariance(5,5);
VectorXd measurementMean(3);
MatrixXd measurementCovariance(3,3);
MatrixXd crossCorrelationMatrix(5,3);
MatrixXd kalmanGain;
VectorXd z(3);
int ccount = 0;
float m_rho,m_pi,m_rdot;
 //Process noise standard deviation longitudinal acceleration in m/s^2
  double std_a = 0.2;

  //Process noise standard deviation yaw acceleration in rad/s^2
  double std_yawdd = 0.2;

MatrixXd  R_radar_(3,3);

 // Radar measurement noise standard deviation radius in m
 double std_radr_ = 0.3;

  // Radar measurement noise standard deviation angle in rad
 double std_radphi_ = 0.03;

  // Radar measurement noise standard deviation radius change in m/s
 double  std_radrd_ = 0.3;

void calculateError(VectorXd state,VectorXd groundTruth){
    double error;
    for(int i=0;i<3;i++){
        error += abs(state(i)) - abs(groundTruth(i));
    }
   //print result
  cout<<"-------------------------------------------------------------"<<endl;
  cout << "Updated state x: " << endl << x << endl;
  cout << "Updated state covariance P: " << endl << P << endl;
  cout << "Error : " << endl << error << endl;
  cout << "Ground Truth: " << endl << groundTruth << endl;
   cout<<"-------------------------------------------------------------"<<endl;
}

void updateState(){
  //set state dimension
  int n_x = 5;

  //set augmented dimension
  int n_aug = 7;

  //set measurement dimension, radar can measure r, phi, and r_dot
  int n_z = 3;

  //define spreading parameter
  double lambda = 3 - n_aug;

  //set vector for weights
  VectorXd weights = VectorXd(2*n_aug+1);
   double weight_0 = lambda/(lambda+n_aug);
  weights(0) = weight_0;
  for (int i=1; i<2*n_aug+1; i++) {  //2n+1 weights
    double weight = 0.5/(n_aug+lambda);
    weights(i) = weight;
  }

  MatrixXd Xsig_pred = predict_sigma_aug;
  MatrixXd Zsig = measured_predict_sigma_aug;
  VectorXd z_pred = measurementMean;
  MatrixXd S = MatrixXd(n_z,n_z);
 S.fill(0.0);

  for (int i = 0; i < 2*n_aug +1; i++) {  //2n+1 simga points
    //residual
    VectorXd z_diff = Zsig.col(i) - z_pred;
    //angle normalization
   while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
  while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
    S = S + weights(i) * z_diff * z_diff.transpose();
  }

  //add measurement noise covariance matrix
  S = S + R_radar_;
  //create example vector for incoming radar measurement

  //create matrix for cross correlation Tc
  MatrixXd Tc = MatrixXd(n_x, n_z);

  //calculate cross correlation matrix
  Tc.fill(0.0);
  for (int i = 0; i < 2 * n_aug + 1; i++) {  //2n+1 simga points

    //residual
    VectorXd z_diff = Zsig.col(i) - z_pred;
    //angle normalization
    while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
    while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;

    // state difference
    VectorXd x_diff = Xsig_pred.col(i) - x;
    //angle normalization
    while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
    while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;

    Tc = Tc + weights(i) * x_diff * z_diff.transpose();
  }

  //Kalman gain K;
  MatrixXd K = Tc * S.inverse();

  //residual
  VectorXd z_diff = z - z_pred;

  //angle normalization
  while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
  while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;

  //update state mean and covariance matrix
  x = stateMean + K * z_diff;
  P = stateCovariance - K*S*K.transpose();

}

VectorXd radar2State(VectorXd m_radar){
   double r_m_rho = m_radar(0);
   double r_m_pi = m_radar(1);
   double r_m_rdot = m_radar(2);
   double r_m_px = r_m_rho*cos(r_m_pi);
        if ( r_m_px < 0.0001 ) {
        r_m_px = 0.0001;
      }
        double r_m_py = r_m_rho*sin(r_m_pi);
        if(r_m_py< 0.0001){
          r_m_py = 0.0001;
        }
        double r_m_vx = r_m_rdot*cos(r_m_pi);
        double r_m_vy = r_m_rdot*sin(r_m_pi);
        double r_m_v  = sqrt(r_m_vx*r_m_vx + r_m_vy*r_m_vy);
  VectorXd r_x(5);
  r_x << r_m_px,r_m_py,r_m_v,0,0;
  return r_x;
}

void NormalizeAngleOnComponent(VectorXd vector, int index) {
  while (vector(index)> M_PI) vector(index)-=2.*M_PI;
  while (vector(index)<-M_PI) vector(index)+=2.*M_PI;
}

void convertIntoMeasurementSpace(){
    double rho,theta,r_rho;
    for(int i=0;i<predict_sigma_aug.cols();i++){
        rho = sqrt((predict_sigma_aug(0,i)*predict_sigma_aug(0,i))+(predict_sigma_aug(1,i)*predict_sigma_aug(1,i)));
        theta = atan2(predict_sigma_aug(1,i),predict_sigma_aug(0,i));
         while (theta> M_PI) theta-=2.*M_PI;
        while (theta<-M_PI) theta+=2.*M_PI;
        r_rho = (predict_sigma_aug(0,i)*cos(predict_sigma_aug(3,i))*predict_sigma_aug(2,i) + predict_sigma_aug(1,i)*sin(predict_sigma_aug(3,i))*predict_sigma_aug(2,i))/rho;
        measured_predict_sigma_aug.col(i) << rho,theta,r_rho;
    }

}

void predictMeanAndCovariance(MatrixXd x,int distinction){
    VectorXd extractedMean(x.rows());
    extractedMean.fill(0.0);
    MatrixXd extractedCovariance(x.rows(),x.rows());
    extractedCovariance.fill(0.0);
    weight1 = glambda/(glambda+n_aug);
    weightn = 0.5/(glambda+n_aug);
    VectorXd x_diff(x.rows());
    for(int i = 0;i<x.cols();i++){
        if(i==0){
            extractedMean += weight1*x.col(i);
        }
        else{
             extractedMean += weightn*x.col(i);
        }
    }

    for(int i=0;i<x.cols();i++){
        int select;
        if(x.rows()>3){
            select = 3;
        }
        else{
            select = 1;
        }
        if(i==0){
             while (x_diff(select)> M_PI) x_diff(select)-=2.*M_PI;
             while (x_diff(select)<-M_PI) x_diff(select)+=2.*M_PI;
            x_diff = x.col(i)-extractedMean;
            extractedCovariance += weight1*x_diff*(x_diff.transpose());
        }
        else{
             while (x_diff(select)> M_PI) x_diff(select)-=2.*M_PI;
             while (x_diff(select)<-M_PI) x_diff(select)+=2.*M_PI;
             x_diff = x.col(i)-extractedMean;
             extractedCovariance += weightn*x_diff*(x_diff.transpose());
        }
    }
    if(distinction==0){
        stateMean = extractedMean;
        stateCovariance = extractedCovariance;
    }
    else{
        measurementMean = extractedMean;
        measurementCovariance = extractedCovariance;
    }

}

void predictSigmaPoints(double dt){
    for(int i=0;i<sigma_aug.cols();i++){
        VectorXd state(5);
        state = sigma_aug.col(i).head(5);
        VectorXd diffrential(5);
        VectorXd noise(5);
        noise(0) = 0.5*dt*dt*cos(state(3))*sigma_aug(5,i);
        noise(1) = 0.5*dt*dt*sin(state(3))*sigma_aug(5,i);
        noise(2) = dt*sigma_aug(5,i);
        noise(3) = 0.5*dt*dt*sigma_aug(6,i);
        noise(4) = dt*sigma_aug(6,i);
        if(state(4)!=0 || state(4)!=0.0){
            diffrential(0) = (state(2)/state(4))*(sin(state(3) + (state(4)*dt)) - sin(state(3)));
            diffrential(1) = (state(2)/state(4))*(cos(state(3)) - cos(state(3) + (state(4)*dt)) );
            diffrential(2) = 0;
            diffrential(3) = dt*state(4);
            diffrential(4) = 0;
            state  = state + diffrential + noise;

        }
        else{
            diffrential(0) = state(2)*cos(state(3))*dt;
            diffrential(1) = state(2)*sin(state(3))*dt;
            diffrential(2) = 0;
            diffrential(3) = dt*state(4);
            diffrential(4) = 0;
            state  = state + diffrential + noise;
        }
        predict_sigma_aug.col(i) = state;
    }
}

void generateAugmentedMatrix(VectorXd x,MatrixXd P){
  x_aug.head(5) = x;
  x_aug(5) = 0;
  x_aug(6) = 0;

  //create augmented covariance matrix
  P_aug.fill(0.0);
  P_aug.topLeftCorner(5,5) = P;
  P_aug(5,5) = std_a*std_a;
  P_aug(6,6) = std_yawdd*std_yawdd;

}

MatrixXd genarateSigmaPoints(VectorXd state,MatrixXd P){
    int nsigma = 2*state.rows() + 1;
    int lambda = 3 - n_aug;
    glambda = lambda;
    MatrixXd sigmaPoints(state.rows(),nsigma);
    double coeff = lambda + state.rows();
    MatrixXd stage1 = P*coeff;
    MatrixXd stage2 = stage1.llt().matrixL();
    MatrixXd stage3_1(stage2.rows(),stage2.cols());
    MatrixXd stage3_2(stage2.rows(),stage2.cols());
    for(int i=0;i<stage2.cols();i++){
        stage3_1.col(i) = state + stage2.col(i);
        stage3_2.col(i) = state - stage2.col(i);
    }
    MatrixXd stage3(stage3_1.rows(),state.cols()+stage3_1.cols()+stage3_2.cols());
    stage3 << state,stage3_1,stage3_2;
    return stage3;

}

int main(){
predict_sigma_aug.fill(0.0);
measured_predict_sigma_aug.fill(0.0);
stateMean.fill(0.0);
stateCovariance.fill(0.0);
measurementMean.fill(0.0);
measurementCovariance.fill(0.0);
kalmanGain.fill(0.0);

 R_radar_ << std_radr_*std_radr_, 0, 0,
              0, std_radphi_*std_radphi_, 0,
              0, 0,std_radrd_*std_radrd_;

ifstream reader("C:\\DEV\\website\\data\\obj_pose-laser-radar-synthetic-input.txt");
   string line;
     long long c_timestamp;
  long long p_timestamp;
  char type;
  int count = 0;//for initializing
  double gt_x;//ground truth
  double gt_y;//ground truth
  double gt_vy;//ground truth
  double gt_vx;//ground truth
   double gt_v;

   while(getline(reader,line)){
    istringstream my_stream(line);
    my_stream>>type;
    if(type=='R'){
         my_stream>>m_rho;
      my_stream>>m_pi;
      my_stream>>m_rdot;
      z<< m_rho,m_pi,m_rdot;
       my_stream>>c_timestamp;
        if(count==0){
        x  = radar2State(z);
         P <<  1, 0, 0, 0, 0,
        0, 1, 0, 0, 0,
        0, 0, 1, 0, 0,
        0, 0, 0, 1, 0,
        0, 0, 0, 0, 1;

        p_timestamp = c_timestamp;
        count++;
        continue;
      }
       dt = (c_timestamp - p_timestamp)/1000000.0;
      p_timestamp = c_timestamp;
       my_stream>>gt_x;
      my_stream>>gt_y;
      my_stream>>gt_vx;
      my_stream>>gt_vy;
      gt_v = sqrt(gt_vx*gt_vx + gt_vy*gt_vy);
       VectorXd gt(3);
       gt << gt_x,gt_y,gt_v;
        generateAugmentedMatrix(x,P);//converts vector of size 5 to 7 and matrix of 5x5 to 7x7
    sigma_aug = genarateSigmaPoints(x_aug,P_aug);//outputs 7x15 matrix
    predictSigmaPoints(0.1);//outputs 5x15 matrix
    predictMeanAndCovariance(predict_sigma_aug,0);//outputs a vector of size 5 and matrix of size 5x5
    convertIntoMeasurementSpace();//outputs 3x15 matrix from 5x15 matrix
    predictMeanAndCovariance(measured_predict_sigma_aug,1);//mean and covariance in measurement space
    updateState();
    calculateError(x,gt);
    }
   }
    return 0;
}


/**
 * @file lqr_controller.h
 * @brief Infinite-horizon Linear Quadratic Regulator.
 *
 */

#ifndef LQR_CONTROLLER_H
#define LQR_CONTROLLER_H

#include <Eigen/Dense>

namespace controller
{

class LQR
{
    public:
        LQR(const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R);
        ~LQR() {};

        /**
         * @brief An infinite-horizon LQR controller with penalties on the state feedback and input signal (Q and R, respectively).
         * @brief LQR gain is solved by searching for stable poles of the system through the Hamiltonian matrix.
         *
         * @param A System dynamics matrix.
         * @param B Input matrix.
         * @param E State error.
         * @return Eigen::MatrixXd LQR gain, K.
         */
        Eigen::MatrixXd computeHamiltonian(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& E);

    private:
        // Declare generic MAT for LQR computations.
        Eigen::MatrixXd K;
        Eigen::MatrixXd Q;
        Eigen::MatrixXd R;
        Eigen::MatrixXd I;
        Eigen::MatrixXd P;

        // Declare Hamilton matrix.
        Eigen::MatrixXd Ham;

        // Output.
        Eigen::MatrixXd cmd_vel;
        Eigen::MatrixXd controlUpdate;
};

} // namespace controller

#endif /* LQR_CONTROLLER_H */


#ifndef _VGUGV_COMMON_HELPER_FUNCTIONS__
#define _VGUGV_COMMON_HELPER_FUNCTIONS_

#include <chrono>
#include <cmath>
#include "commDefs.h"
#include <Eigen/Dense>
#include "cameraBase.h"
#include "cudaDefs.h"

namespace VGUGV
{
  namespace Common
  {
    typedef long long T_int64;
    typedef std::chrono::high_resolution_clock T_clock;
    typedef std::chrono::time_point<T_clock> T_time;

    /*-------------------------------------------------------------------------------------------------*/
    /*------------------------------------ FUNCTION PROTOTYPES ----------------------------------------*/
    /*-------------------------------------------------------------------------------------------------*/

    template<typename T> T INPI(T angle);
    template<typename T> T deg2rad(T deg);
    template<typename T> T rad2deg(T rad);
    template<typename T> void rotMatrix2Euler(T r00, T r01, T r02,
					   T r10, T r11, T r12,
					   T r20, T r21, T r22,
					   T& phi, T& theta, T& psi);
    template<typename T> void euler2rotMatrix(T phi, T theta, T psi,
					      T& r00, T& r01, T& r02,
					      T& r10, T& r11, T& r12,
					      T& r20, T& r21, T& r22);

    T_time   currentTime();
    T_int64  elapsedTime(T_time start, TIME_TYPE type);

    Eigen::Matrix3f so3Exp(Eigen::Vector3f omega);
    Eigen::Matrix3f fov2K(float fovDeg, int pixelWidth, int pixelHeight);
    Eigen::Vector2f distort_radialNtangential(CameraBase::Ptr cameraModel, Eigen::Vector2f pixel0);
    Eigen::Matrix3f computePlanarHomography(Eigen::Matrix4f T_l2r, float planeDepth_ref, Eigen::Vector3f planeNormal_ref);

    float  bilinearInterpolation(const unsigned char* pData, int nRows, int nCols, float r, float c);
    __m128 bilinearInterpolation(const unsigned char* pData, int nRows, int nCols, const SSE_m128_v2& pixels);
    bool getImagePatch(unsigned char* pImageData, int nRows, int nCols, int r, int c, int nPatchSize, unsigned char* pPatch);
    bool getImagePatch(CameraBase::Ptr pRefCamera,
		       CameraBase::Ptr pCurCamera,
		       unsigned char* pCurImgData,
		       Eigen::Matrix3f Hl2r,
		       int r,
		       int c,
		       int nPatchSize,
		       unsigned char* pPatch);

    float znccScore(unsigned char* pRefPatch, unsigned char* pCurPatch, int nSize);

    //! Method to cluster 1D data into different clusters, each cluster is approximated by a Gaussian distribution
      /*!
      \param inputData (data, weight), data is already sorted ascendingly.
      \param dataSize total number of data pairs
      \param Th threshold value used to split two clusters
      \param models fitted Gaussian models (mean, variance, weight)
      \return (nModels, nBestModelIndex)
      */
    Eigen::Vector2i cluster_1Ddata(Eigen::Vector2f* inputData, int dataSize, float Th, Eigen::Vector3f* models);

    //! Method to fit 1D data into a Gaussian distribution
      /*!
      \param inputData (data, weight), data is already sorted ascendingly.
      \param dataSize total number of data pairs
      \return (mean, variance, weight)
      */
    Eigen::Vector3f gaussian_fit(Eigen::Vector2f* inputData, int dataSize);

    float depthFromSubpixelInterpolation(const std::array<float, 3>& depths, const std::array<float, 3>& similarityScores);

    int intergerDivUp(int a, int b);

    // SSE related functions
    void printSSE_m128(const __m128& a);
    __m128 _mm_floor_ps2(const __m128& a);
  }
}
#endif


///////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//[mpfr_variable

/*`
This example illustrates the use of variable-precision arithmetic with
the `mpfr_float` number type.  We'll calculate the median of the
beta distribution to an absurdly high precision and compare the
accuracy and times taken for various methods.  That is, we want
to calculate the value of `x` for which ['I[sub x](a, b) = 0.5].

Ultimately we'll use Newtons method and set the precision of
mpfr_float to have just enough digits at each iteration.

The full source of the this program is in [@../../example/mpfr_precision.cpp]

We'll skip over the #includes and using declations, and go straight to
some support code, first off a simple stopwatch for performance measurement:

*/

//=template <class clock_type>
//=struct stopwatch { /*details \*/ };

/*`
We'll use `stopwatch<std::chono::high_resolution_clock>` as our performance measuring device.

We also have a small utility class for controlling the current precision of mpfr_float:

   struct scoped_precision
   {
      unsigned p;
      scoped_precision(unsigned new_p) : p(mpfr_float::default_precision())
      {
         mpfr_float::default_precision(new_p);
      }
      ~scoped_precision()
      {
         mpfr_float::default_precision(p);
      }
   };

*/
//<-
#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/relative_difference.hpp>
#include <iostream>
#include <chrono>

using boost::multiprecision::mpfr_float;
using boost::math::ibeta_inv;
using namespace boost::math::policies;

template <class clock_type>
struct stopwatch
{
public:
   typedef typename clock_type::duration duration_type;

   stopwatch() : m_start(clock_type::now()) { }

   stopwatch(const stopwatch& other) : m_start(other.m_start) { }

   stopwatch& operator=(const stopwatch& other)
   {
      m_start = other.m_start;
      return *this;
   }

   ~stopwatch() { }

   float elapsed() const
   {
      return float(std::chrono::nanoseconds((clock_type::now() - m_start)).count()) / 1e9f;
   }

   void reset()
   {
      m_start = clock_type::now();
   }

private:
   typename clock_type::time_point m_start;
};

struct scoped_precision
{
   unsigned p;
   scoped_precision(unsigned new_p) : p(mpfr_float::default_precision())
   {
      mpfr_float::default_precision(new_p);
   }
   ~scoped_precision()
   {
      mpfr_float::default_precision(p);
   }
};
//->

/*`
We'll begin with a reference method that simply calls the Boost.Math function `ibeta_inv` and uses the
full working precision of the arguments throughout.  Our reference function takes 3 arguments:

* The 2 parameters `a` and `b` of the beta distribution, and
* The number of decimal digits precision to achieve in the result.

We begin by setting the default working precision to that requested, and then, since we don't know where
our arguments `a` and `b` have been or what precision they have, we make a copy of them - note that since
copying also copies the precision as well as the value, we have to set the precision expicitly with a
second argument to the copy.  Then we can simply return the result of `ibeta_inv`:
*/
mpfr_float beta_distribution_median_method_1(mpfr_float const& a_, mpfr_float const& b_, unsigned digits10)
{
   scoped_precision sp(digits10);
   mpfr_float half(0.5), a(a_, digits10), b(b_, digits10);
   return ibeta_inv(a, b, half);
}
/*`
You be wondering why we needed to change the precision of our variables `a` and `b` as well as setting the default -
there are in fact two ways in which this can go wrong if we don't do that:

* The variables have too much precision - this will cause all arithmetic operations involving those types to be
promoted to the higher precision wasting precious calculation time.
* The variables have too little precision - this will cause expressions involving only those variables to be
calculated at the lower precision - for example if we calculate `exp(a)` internally, this will be evaluated at
the precision of `a`, and not the current default.

Since our reference method carries out all calculations at the full precision requested, an obvious refinement
would be to calculate a first approximation to `double` precision and then to use Newton steps to refine it further.

Our function begins the same as before: set the new default precision and then make copies of our arguments
at the correct precision.  We then call `ibeta_inv` with all double precision arguments, promote the result
to an `mpfr_float` and perform Newton steps to obtain the result.  Note that our termination condition is somewhat
cude: we simply assume that we have approximately 14 digits correct from the double-precision approximation and
that the precision doubles with each step.  We also cheat, and use an internal Boost.Math function that calculates
['I[sub x](a, b)] and it's derivative in one go:

*/
mpfr_float beta_distribution_median_method_2(mpfr_float const& a_, mpfr_float const& b_, unsigned digits10)
{
   scoped_precision sp(digits10);
   mpfr_float half(0.5), a(a_, digits10), b(b_, digits10);
   mpfr_float guess = ibeta_inv((double)a, (double)b, 0.5);
   unsigned current_digits = 14;
   mpfr_float f, f1;
   while (current_digits < digits10)
   {
      f = boost::math::detail::ibeta_imp(a, b, guess, boost::math::policies::policy<>(), false, true, &f1) - half;
      guess -= f / f1;
      current_digits *= 2;
   }
   return guess;
}
/*`
Before we refine the method further, it might be wise to take stock and see how method's 1 and 2 compare.
We'll ask them both for 1500 digit precision, and compare against the value produced by `ibeta_inv` at 1700 digits.
Here's what the results look like:

[pre
Method 1 time = 0.611647
Relative error: 2.99991e-1501
Method 2 time = 0.646746
Relative error: 7.55843e-1501
]

Clearly they are both equally accurate, but Method 1 is actually faster and our plan for improved performance
hasn't actually worked.  It turns out that we're not actually comparing like with like, because `ibeta_inv` uses
Halley iteration internally which churns out more digits of precision rather more rapidly than Newton iteration.
So the time we save by refining an initial `double` approximation, then loose it again by taking more iterations
to get to the result.

Time for a more refined approach.  It follows the same form as Method 2, but now we set the working precision
within the Newton iteration loop, to just enough digits to cover the expected precision at each step.  That means
we also create new copies of our arguments at the correct precision within the loop, and likewise change the precision
of the current `guess` each time through:

*/

mpfr_float beta_distribution_median_method_3(mpfr_float const& a_, mpfr_float const& b_, unsigned digits10)
{
   mpfr_float guess = ibeta_inv((double)a_, (double)b_, 0.5);
   unsigned current_digits = 14;
   mpfr_float f(0, current_digits), f1(0, current_digits), delta(1);
   while (current_digits < digits10)
   {
      current_digits *= 2;
      scoped_precision sp((std::min)(current_digits, digits10));
      mpfr_float a(a_, mpfr_float::default_precision()), b(b_, mpfr_float::default_precision());
      guess.precision(mpfr_float::default_precision());
      f = boost::math::detail::ibeta_imp(a, b, guess, boost::math::policies::policy<>(), false, true, &f1) - 0.5f;
      guess -= f / f1;
   }
   return guess;
}

/*`
The new performance results look much more promising:

[pre
Method 1 time = 0.591244
Relative error: 2.99991e-1501
Method 2 time = 0.622679
Relative error: 7.55843e-1501
Method 3 time = 0.143393
Relative error: 4.03898e-1501
]

This time we're 4x faster than `ibeta_inv`, and no doubt that could be improved a little more by carefully
optimising the number of iterations and the method (Halley vs Newton) taken.

Finally, here's the driver code for the above methods:

*/

int main()
{
   try {
      mpfr_float a(10), b(20);

      mpfr_float true_value = beta_distribution_median_method_1(a, b, 1700);

      stopwatch<std::chrono::high_resolution_clock> my_stopwatch;

      mpfr_float v1 = beta_distribution_median_method_1(a, b, 1500);
      float hp_time = my_stopwatch.elapsed();
      std::cout << "Method 1 time = " << hp_time << std::endl;
      std::cout << "Relative error: " << boost::math::relative_difference(v1, true_value) << std::endl;

      my_stopwatch.reset();
      mpfr_float v2 = beta_distribution_median_method_2(a, b, 1500);
      hp_time = my_stopwatch.elapsed();
      std::cout << "Method 2 time = " << hp_time << std::endl;
      std::cout << "Relative error: " << boost::math::relative_difference(v2, true_value) << std::endl;

      my_stopwatch.reset();
      mpfr_float v3 = beta_distribution_median_method_3(a, b, 1500);
      hp_time = my_stopwatch.elapsed();
      std::cout << "Method 3 time = " << hp_time << std::endl;
      std::cout << "Relative error: " << boost::math::relative_difference(v3, true_value) << std::endl;
   }
   catch (const std::exception& e)
   {
      std::cout << "Found exception with message: " << e.what() << std::endl;
   }
   return 0;
}
//]


// stochastic_regression.cpp
/*
    This program performs a stochastic regression imputation
    for a target variable y with missing values using completely
    observed data X. The calculations are run in serial and then
    in parallel. The parallel implementation is based on C++'s
    std::thread. For matrix computations the C++ header only
    library Eigen is used. You need Eigen to compile this code.
    Eigen is free software and available at
    https://eigen.tuxfamily.org/

    To comopile this program use one of the following:
    $ g++ -I eigen-3.4.0 stochastic_regression.cpp -o stochastic_regression.out -std=c++2a -lpthread -O3
    $ clang++ -I eigen-3.4.0 stochastic_regression.cpp -o stochastic_regression.out -std=c++2a -lpthread -O3

    To run it, type
    $ ./stochastic_regression.out
*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <random>
#include <string>
#include <vector>
#include <thread>
#include <future>
#include <chrono>

#include <Eigen/Dense>
#include <Eigen/QR>

// Linear model object that can fit data in the form of a target y
// and features X using multiple linear regression.
class LinearModel
{
public:
    double sigma;
    Eigen::VectorXd beta;
    Eigen::VectorXd residuals;
    Eigen::MatrixXd design;
    Eigen::MatrixXd inv_matrix_prod;

    LinearModel();
    ~LinearModel();
    void fit(Eigen::MatrixXd X, Eigen::VectorXd y);
    Eigen::VectorXd predict(Eigen::MatrixXd X);
};

LinearModel::LinearModel()
{
}

LinearModel::~LinearModel()
{
}

// Predicts y for a given Matrix X using the Matrix-Vector-Product
// of X and beta.
Eigen::VectorXd LinearModel::predict(Eigen::MatrixXd X)
{
    return(X * beta);
}

// Fit a linear model of the form y = X * beta + epsilon
// using least squares.
void LinearModel::fit(Eigen::MatrixXd X, Eigen::VectorXd y)
{
    // Add ones to the model for the intercept parameter.
    // Therefore resize and reshape the matrix X.
    X.conservativeResize(X.rows(), X.cols() + 1);
    for (int i = X.cols() - 1; i > 0 ; i--)
    {
        X.col(i) = X.col(i - 1);
    }
    X.col(0) = Eigen::VectorXd::Ones(X.rows());
    design = X;

    // Calculate OLS estimates beta.
    Eigen::MatrixXd XX = X.transpose() * X;
    inv_matrix_prod = XX.inverse();
    beta = X.transpose() * y;
    beta = inv_matrix_prod * beta;

    // Calculate the residuals.
    Eigen::VectorXd predictions = predict(X);
    residuals = y - predictions;
    Eigen::VectorXd residuals_sq = residuals.array().pow(2);

    // Calculate sigma, that is the variance of the error terms.
    sigma = sqrt(1.0f / (X.rows() - beta.size()) * residuals_sq.sum());
}

// Generates draws from a multivariate normal distribution.
// For reference see:
// https://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c
struct normal_random_variable
{
    normal_random_variable(Eigen::MatrixXd const& covar)
        : normal_random_variable(Eigen::VectorXd::Zero(covar.rows()), covar)
    {}

    normal_random_variable(Eigen::VectorXd const& mean, Eigen::MatrixXd const& covar)
        : mean(mean)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
        transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Eigen::VectorXd mean;
    Eigen::MatrixXd transform;

    Eigen::VectorXd operator()() const
    {
        static std::mt19937 gen{ std::random_device{}() };
        static std::normal_distribution<> dist;

        return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](auto x) { return dist(gen); });
    }
};

// Reads a CSV file and pushes its content into an Eigen::MatrixXd.
// For reference see:
// https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix/39146048
// https://www.py4u.net/discuss/80056
Eigen::MatrixXd read_matrix_csv(const std::string & filepath)
{
    // Read values form file and write it to buffer vector.
    std::ifstream indata;
    indata.open(filepath);
    std::string line;
    std::vector<double> values;
    uint rows = 0;
    while (std::getline(indata, line))
    {
        std::stringstream lineStream(line);
        std::string cell;
        while (std::getline(lineStream, cell, ','))
        {
            values.push_back(std::stod(cell));
        }
        ++rows;
    }

    // Detect or set number of rows/columns
    size_t num_rows = rows;
    size_t num_cols = values.size() / rows;

    // Map buffer to Eigen::Matrix and return it.
    Eigen::MatrixXd csv_matrix = Eigen::Map<Eigen::Matrix <double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> (&values.data()[0], num_rows, num_cols);
    return csv_matrix;
}

// Performs stochastic regression imputation on a data vector y
// and observed variables X.
Eigen::VectorXd stochastic_regression_imputation(Eigen::MatrixXd X_miss, Eigen::MatrixXd X_obs, Eigen::VectorXd y_obs)
{
    int n_obs = X_obs.rows();
    int n_miss = X_miss.rows();

    std::default_random_engine generator;
    std::chi_squared_distribution<double> chi(n_obs - 3);

    // Fit linear model only on the observed values.
    LinearModel regmod;
    regmod.fit(X_obs, y_obs);
    Eigen::VectorXd beta_hat = regmod.beta;

    // Random draws for sigma^2 from the scaled inverse Chi^2-distribution.
    double sigma_sq_tilde = (n_obs - 3) * pow(regmod.sigma, 2) / chi(generator);

    // Random draws for beta from a mv. normal distribution.
    Eigen::MatrixXd variance = regmod.inv_matrix_prod * sigma_sq_tilde;
    normal_random_variable sample {beta_hat, variance};
    Eigen::VectorXd beta_tilde = sample();
    Eigen::VectorXd means = regmod.design * beta_tilde;

    // Draw the imputations for y from a normal distribution.
    Eigen::VectorXd y_imputation(n_miss);
    for (int i = 0; i < n_miss; i++)
    {
        std::normal_distribution<double> normal(means[i], sqrt(sigma_sq_tilde));
        y_imputation[i] = normal(generator);
    }

    return y_imputation;
}

// Performs M multiple imputations using stochastic_regression
// and a Bayesian linear model.
Eigen::MatrixXd multiple_imputation(int num_imp, Eigen::MatrixXd X_miss, Eigen::MatrixXd X_obs, Eigen::VectorXd y_obs)
{
    Eigen::MatrixXd imputations(X_obs.rows() + X_miss.rows(), num_imp);

    for (int m = 0; m < num_imp; m++)
    {
        // Calculate imputations for y and concantenate to one vector.
        Eigen::VectorXd y_imp = stochastic_regression_imputation(X_miss, X_obs, y_obs);
        Eigen::VectorXd y_complete(y_obs.size() + y_imp.size());
        y_complete << y_obs, y_imp;
        imputations.col(m) = y_complete;
    }

    return imputations;
}

// Performs M multiple imputations using stochastic_regression
// and a Bayesian linear model. This function wraps the calculation
// output for the use in a parallel setting.
void parallel_multiple_imputation(int num_imp, Eigen::MatrixXd X_miss, Eigen::MatrixXd X_obs, Eigen::VectorXd y_obs, std::promise<Eigen::MatrixXd> && p)
{
    Eigen::MatrixXd imputations(X_obs.rows() + X_miss.rows(), num_imp);
    imputations = multiple_imputation(num_imp, X_miss, X_obs, y_obs);
    p.set_value(imputations);
}

// Calculates the time difference between "end" and "begin" and prints
// the result to the terminal.
void print_runtime(std::chrono::steady_clock::time_point begin, std::chrono::steady_clock::time_point end, const std::string & msg)
{
    std::cout << msg << " Runtime = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() << " [µs]" << std::endl;
    std::cout << msg << " Runtime = " << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() / 1e6 << " [s]" << std::endl;
}

// Prints the global mean of an matrix to the terminal.
// If col_means is ture, the mean for each column of
// the data matrix is printed.
void print_means(Eigen::MatrixXd data, const std::string & msg, bool col_means = false)
{
    std::cout << msg << " Mean = " << data.mean() << std::endl;
    std::cout << std::endl;

    if (col_means)
    {
        for (int i = 0; i < data.cols(); i++)
        {
            std::cout << "Mean of imputation " << i << " is " << data.col(i).mean() << std::endl;
        }
    }
}

int main()
{
    // Imputation parameters.
    int num_imp = 10000;

    // Load the data.
    Eigen::MatrixXd X_miss = read_matrix_csv("data/X_miss.csv");
    Eigen::VectorXd y_miss = read_matrix_csv("data/y_miss.csv");
    Eigen::MatrixXd X_obs = read_matrix_csv("data/X_obs.csv");
    Eigen::VectorXd y_obs = read_matrix_csv("data/y_obs.csv");
    Eigen::MatrixXd imputations(X_obs.rows() + X_miss.rows(), num_imp);

    std::chrono::steady_clock::time_point begin, end;

    // Calculate one imputation.
    //Eigen::VectorXd y_imp = stochastic_regression_imputation(X_miss, X_obs, y_obs);
    //std::cout << y_imp << std::endl;

    // Calculate multiple imputations in serial execution.
    begin = std::chrono::steady_clock::now();
    imputations = multiple_imputation(num_imp, X_miss, X_obs, y_obs);
    end = std::chrono::steady_clock::now();
    print_runtime(begin, end, "(Serial)");
    print_means(imputations, "(Serial)");
    //std::cout << imputations << std::endl;

    // Parallel version using std::thread. Here the number of
    // multiple imputations is distributed to different threads.
    int num_cores = std::thread::hardware_concurrency();
    begin = std::chrono::steady_clock::now();

    std::vector<std::thread> threads;
    std::vector<std::future<Eigen::MatrixXd>> futures;

    for (int i = 0; i < num_cores; i++)
    {
        std::promise<Eigen::MatrixXd> p;
        futures.push_back(p.get_future());
        threads.push_back(std::thread(parallel_multiple_imputation, num_imp / num_cores, X_miss, X_obs, y_obs, std::move(p)));
    }

    for (auto &th : threads)
    {
        th.join();
    }

    // Get the results from the parallel execution and
    // assemble them in a matrix.
    int col_counter = 0;
    for (auto &f : futures)
    {
        imputations.block(0, (num_imp / num_cores) * col_counter, imputations.rows(), num_imp / num_cores) = f.get();
        col_counter++;
    }

    end = std::chrono::steady_clock::now();
    std::cout << "Running in parallel on " << num_cores << " core(s)." << std::endl;
    print_runtime(begin, end, "(Parallel)");
    print_means(imputations, "(Parallel)");

    return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <cmath>
#include <fstream>
#include <numeric>

// Input: m Vector, k Vector, l Vector (even though it is not needed as we later realized), dimension N
Eigen::ArrayXcd springchain(Eigen::VectorXd& m, Eigen::VectorXd& k, Eigen::VectorXd& l, int N){
    // Initialisation of relative coordinates x (in protocol eta)
    Eigen::VectorXd x(N);
    x(0) = 0.;
    std::partial_sum(l.begin(), l.end(), &x(1), std::plus<double>());

    // Definition of M^-1 and K matrix
    Eigen::MatrixXd M_inv = m.asDiagonal().inverse();
    Eigen::MatrixXd K(N, N);
    for (int i= 0; i < N; i++){
        for(int j= 0; j <= i; j++){
            K(i,j) = 0;
            if(i==j){
                if(i == 0){
                    K(i,i) = k(i);
                    K(i, i+1) = -k(i);
                }
                else if(i== N-1){
                    K(i,i) = k(i-1);
                    K(i, i-1) = -k(i-1);
                }
                else{
                    K(i,i) = k(i-1)+k(i);
                    K(i, i-1) = -k(i-1);
                    K(i, i+1) = -k(i);
                }
            }
        }
    }
    Eigen::MatrixXd A = M_inv*K;
    // Calculate Eigenvalues
    return A.eigenvalues();
}

// Lanczos algorithm, sadly not finished. Returns only the tridiag matrix
Eigen::MatrixXd lanczos(Eigen::MatrixXd& A){

    // Implementation as defined in the lecture

    // q matrix with the new basis q
    Eigen::MatrixXd q(A.rows(), A.cols()+2);
    q(0,0) = 0;
    q(0,1) = 1;
    // gamma vector with all the normalisations
    Eigen::VectorXd gamma(A.cols()+2);
    gamma(1) = 1;
    // delta vector with diagonal elements of the new tridiag matrix
    Eigen::VectorXd delta(A.cols()+1);

    // the new basis build with a for loop
    for (int i=1; i<=A.rows(); i++){
        gamma(i+1) = std::sqrt(q.col(i).transpose()* q.col(i));
        delta(i) = q.col(i).transpose()*A*q.col(i);
        q.col(i+1) = 1/gamma(i+1) * (A- delta(1)*Eigen::MatrixXd::Identity(A.rows(), A.cols()))*q.col(i) - gamma(i)*q.col(i-1);
    }

    // make new tridiag matrix based on gamma-, and delta-vector, with some error
    Eigen::MatrixXd T(A.rows(), A.cols());
    for (int i= 0; i < A.rows()-1; i++){
        T(i,i) = delta(i+1);
        T(i, i+1) = gamma(i+3);
        T(i+1, i) = gamma(i+3);
    }
    T(A.rows()-1, A.cols()-1) = delta(A.cols());

    return T;

    // Begin of Jacobi-Rotation Implementation, sadly not finished
    Eigen::MatrixXd Z = Eigen::MatrixXd::Identity(T.rows(), T.cols());

    for (int i=0; i < T.rows()-1; i++){
        Eigen::JacobiRotation<double> J;
        Eigen::MatrixXd J_1 = Eigen::MatrixXd::Identity(T.rows(), T.cols());
        J.makeJacobi(T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)), 0, 1);
        J_1(Eigen::seq(i,i+1), Eigen::seq(i,i+1)) << J.c(), J.s(), -J.s(), J.c();
        T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)).applyOnTheLeft(0, 1, J.adjoint());
        T(Eigen::seq(i,i+1), Eigen::seq(i,i+1)).applyOnTheRight(0, 1, J);
        Z = Z * J_1;
    }

}

int main()
{
	// Ex. 1
    // N is the dimension, here 10 for part b)
    int N = 10;
    Eigen::VectorXd m(N), k(N-1), l(N-1);
    // Initialisation of m, k and l
    for (int i=0; i<N; i++){
        m(i) = i+1;
    }
    for (int j=0; j<N-1; j++){
        k(j) = N-j-1;
        l(j) = std::abs(5.-j-1) + 1;
    }
    // Calculate omega_squared with springchain function
    Eigen::ArrayXcd omega_squared = springchain(m, k, l, N);
    // Calculate omega with taking the square root of all omega_squared values
    Eigen::ArrayXd omega = omega_squared.real().abs().sqrt();

    // Output in file ./output/omega.txt
    std::ofstream file;
    file.open("output/omega.txt", std::ofstream::trunc);
    file << "\nDas Ergebnis:\n" << omega << std::endl;
    file.close();

    // Ex. 2
    int dim = 5;
    Eigen::MatrixXd A= Eigen::MatrixXd::Random(dim,dim);
    Eigen::MatrixXd T = lanczos(A);

    std::ofstream file2;
    file2.open("output/tridiag.txt", std::ofstream::trunc);
    file2 << "Die Zufallsmatrix A:\n" << A << std::endl;
    file2 << "\nwird zur Tridiagonalmatrix:\n" << T << std::endl;
    file2.close();

    return 0;
}