// Copyright (c) 2017 Evan S Weinberg
// A reference piece of code which computes matrix elements
// of a reference sparse Laplace operator to fill a dense
// Eigen matrix, then computes the spectrum and prints
// the Eigenvalues.

// This code is for real, symmetric matrices.
// This code lives on github at github.com/weinbe2/eigens-with-eigen/

#include <iostream>
#include <iomanip>
#include <cmath>
#include <string>

// Borrow dense matrix eigenvalue routines.
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// This is just for convenience. By default, all matrices
// in Eigen are column major. You can replace "Dynamic"
// with a specific number to template just one size.
// You can also ust use "MatrixXd".
typedef Matrix<double, Dynamic, Dynamic, ColMajor> dMatrix;

// Reference 1-D Laplace function.
void laplace_1d(double* out, double* in, const int L, const double m2);

int main(int argc, char** argv)
{
  double *in_real;
  double *out_real;

  // Set output precision to be long.
  cout << setprecision(10);

  // Basic information about the lattice.
  const int length = 8;
  const double m_sq = 0.001;

  // Print the basic info.
  std::cout << "1D Laplace operator, length " << length << ", mass squared " << m_sq << ", zero boundary conditions.\n";
  std::cout << "Change the length and mass by modifying the source.\n";

  // Allocate.
  in_real = new double[length];
  out_real = new double[length];

  // Zero out.
  for (int i = 0; i < length; i++)
  {
    in_real[i] = out_real[i] = 0.0;
  }

  //////////////////////////
  // REAL, SYMMETRIC CASE //
  //////////////////////////

  std::cout << "Real, Symmetric case.\n\n";

  // Allocate a sufficiently gigantic matrix.
  dMatrix mat_real = dMatrix::Zero(length, length);

  // Form matrix elements. This is where it's important that
  // dMatrix is column major.
  for (int i = 0; i < length; i++)
  {
    // Set a point on the rhs for a matrix element.
    // If appropriate, zero out the previous point.
    if (i > 0)
    {
      in_real[i-1] = 0.0;
    }
    in_real[i] = 1.0;

    // Zero out the "out" vector. I defined "laplace_1d" to
    // not require this, but I put this here for generality.
    for (int j = 0; j < length; j++)
      out_real[j] = 0.0;

    // PUT YOUR MAT-VEC HERE.
    laplace_1d(out_real, in_real, length, m_sq);

    // Copy your output into the right memory location.
    // If your data layout supports it, you can also pass
    // "mptr" directly as your "output vector" when you call
    // your mat-vec.
    double* mptr = &(mat_real(i*length));

    for (int j = 0; j < length; j++)
    {
      mptr[j] = out_real[j];
    }
  }

  // We've now formed the dense matrix. We print it here
  // as a sanity check if it's small enough.
  if (length <= 16)
  {
    std::cout << mat_real << "\n";
  }

  // Get the eigenvalues and eigenvectors.
  SelfAdjointEigenSolver< dMatrix > eigsolve_real(length);
  eigsolve_real.compute(mat_real);

  // Remark: if you only want the eigenvalues, you can call
  // eigsolve_real.compute(mat_real, EigenvaluesOnly);

  // Print the eigenvalues.
  dMatrix evals = eigsolve_real.eigenvalues();
  std::cout << "The eigenvalues are:\n" << evals << "\n\n";
  // You can also index individual eigenvalues as "evals(i)", where
  // "i" zero-indexes the eigenvalues.

  // Print the eigenvectors if the matrix is small enough.
  // As a remark, this also shows you how to access the eigenvectors.
  if (length <= 16)
  {
    for (int i = 0; i < length; i++)
    {
      // You can use "VectorXd" as the type instead.
      dMatrix evec = eigsolve_real.eigenvectors().col(i);

      // Print the eigenvector.
      std::cout << "Eigenvector " << i << " equals:\n" << evec << "\n\n";

      // You can also copy the eigenvector into another array as such:
      for (int j = 0; j < length; j++)
      {
        out_real[j] = evec(j);
      }
    }
  }

  // Clean up.
  delete[] in_real;
  delete[] out_real;

  return 0;
}

// Reference 1-D Laplace function.
void laplace_1d(double* out, double* in, const int L, const double m2)
{
  // Zero boundary conditions. Set first and last element explicitly.
  out[0] = (2+m2)*in[0] - in[1];
  out[L-1] = (2+m2)*in[L-1] - in[L-2];

  // The rest.
  for (int i = 1; i < L-1; i++)
  {
    out[i] = (2+m2)*in[i] - in[i-1] - in[i+1];
  }

  // Done.
  return;
}


#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

int main(int argc, char** argv)
{
    // 机器人B在坐标系O中的坐标：
    Eigen::Vector3d B(3, 4, M_PI);

    // 坐标系B到坐标O的转换矩阵：
    Eigen::Matrix3d TOB;
    TOB << cos(B(2)), -sin(B(2)), B(0),
           sin(B(2)),  cos(B(2)), B(1),
              0,          0,        1;

    // 坐标系O到坐标B的转换矩阵:
    Eigen::Matrix3d TBO = TOB.inverse();

    // 机器人A在坐标系O中的坐标：
    Eigen::Vector3d A(1, 3, -M_PI / 2);

    // 求机器人A在机器人B中的坐标：
    Eigen::Vector3d BA;
    // TODO 参照第一课PPT
    // start your code here (5~10 lines)
    Eigen::Matrix3d TOA;

    // end your code here

    cout << "The right answer is BA: 2 1 1.5708" << endl;
    cout << "Your answer is BA: " << BA.transpose() << endl;

    return 0;
}


// A majority of this code is from Rosetta Code: https://rosettacode.org/wiki/Ackermann_function#Efficient_version
// This adaptation allows for user input
#include <iostream>
#include <sstream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/lexical_cast.hpp>

using big_int = boost::multiprecision::cpp_int;

big_int ipow(big_int base, big_int exp);
big_int ackermann(unsigned m, unsigned n);

int main(){
	std::string mStr, nStr;

	std::cout << "Ack(m,n)\nm = ";
	std::getline(std::cin, mStr);

	std::cout << "n = ";
	std::getline(std::cin, nStr);

	std::cout << "Ack(" + mStr + ',' + nStr + ") = "
		<< ackermann(boost::lexical_cast<unsigned>(mStr),boost::lexical_cast<unsigned>(nStr)) << '\n';

	return 0;
}

big_int ipow(big_int base, big_int exp){
	big_int result(1);
	while(exp){
		if(exp & 1){
			result *= base;
		}
		exp >>= 1;
		base *= base;
	}
	return result;
}

big_int ackermann(unsigned m, unsigned n){
	static big_int (*ack)(unsigned, big_int) =
		[](unsigned m, big_int n)->big_int {
			switch(m){
				case 0:
					return n+1;
				case 1:
					return n+2;
				case 2:
					return 3+2*n;
				case 3:
					return 5 + 8 * (ipow(big_int(2), n) - 1);
				default:
					return n == 0 ? ack(m - 1, big_int(1)) : ack(m - 1, ack(m, n - 1));
			}
		};
	return ack(m, big_int(n));
}


/* EuropeanOption.cpp
-Description:
	*Derived option class that uses Black-Scholes formula to give exact price.
-Variable/Object Catalogue:
	*normal_distribution<> normDist: Normal Distribution RNG object.
-Member Functions:
	// Constructors/Destructor:
	*EuropeanOption(double, double, double, double, double, bool): Set parameters to corresponding passed values.
	*EuropeanOption(const EuropeanOption&): Copy constructor.
	*~EuropeanOption(): Destructor.
	// Misc. Methods:
	*double AltPrice() const: Return price of alternate form of option using Put-Call Parity.
	*double D1() const: Return "d1", Z-score used in Black-Scholes model.
	*double D2() const: Return "d2", Z-score used in Black-Scholes model.
	*double Price() const: Return price using Black-Scholes formula.
	*double Delta() const: Return change in Price given change in s.
	*double Gamma() const: Return 2nd order change in Price given change in s.
*/

#include <boost\math\distributions.hpp>
#include <string>
#include <iostream>
#include <strstream>
#include "EuropeanOption.hpp"
#include "OptionExcept.hpp"

namespace Options
{
	////////////////////////////
	// Constructors/Destructor:
	////////////////////////////
	EuropeanOption::EuropeanOption(double t, double s, double k, double b, double r, double sigma, bool isCall) :	/* Overloaded Constructor. Set all values of */
	Option(t, s, k, b, r, sigma, isCall)
	{

	}
	EuropeanOption::EuropeanOption(const EuropeanOption &in) : Option(in)	/* Copy Constructor. */
	{

	}
	EuropeanOption::~EuropeanOption()					/* Destructor. */
	{

	}
	//////////////////////////
	// Misc. Methods:
	//////////////////////////
	double EuropeanOption::AltPrice() const			    /* Return price of alternative type using Put-Call Parity. */
	{
		if (Type()) // Return corresponding price of Put option if option is Call using Put-Call Parity.
		{
			return Price() + Param("k") * exp(-Param("r") * Param("t")) - Param("s");
		}
		else		// Return corresponding price of corresponding Call option if option is Put.
		{
			return Price() - Param("k") * exp(-Param("r") * Param("t")) + Param("s");
		}
	}
	double EuropeanOption::D1()	const					/* Calculate Z-Score used in Black-Scholes formula. */
	{
		// d1 = (ln(s/k) + (b + sigma^2/2) * t) / (sigma * sqrt(t))
		return (log(Param("s") / Param("k")) + (Param("b") + .5 * pow(Param("sigma"), 2)) * Param("t")) / (Param("sigma") * sqrt(Param("t")));
	}
	double EuropeanOption::D2() const					/* Calculate Z-Score used in Black-Scholes formula. */
	{
		// d2 = d1 - sigma * sqrt(t)
		return  D1() - Param("sigma") * sqrt(Param("t"));
	}
	double EuropeanOption::Price() const				/* Return price of option. */
	{
		try
		{
			if (Type())
			{
				return boost::math::cdf(normDist, D1()) * Param("s") * std::exp((Param("b") - Param("r")) * Param("t")) - boost::math::cdf(normDist, D2()) * Param("k") * std::exp(-Param("r") * Param("t"));
			}
			else
			{
				return boost::math::cdf(normDist, -D2()) * Param("k") * std::exp(-Param("r") * Param("t")) - boost::math::cdf(normDist, -D1()) * Param("s") * std::exp((Param("b") - Param("r")) * Param("t"));
			}
		}
		catch (...)
		{
			throw Exceptions::OptionExcept("EuropeanOption::Price() Error: Unhandled exception. ");
		}
	}
	// Greeks:
	double EuropeanOption::Delta() const				/* Change in price with respect to s (1st order). */
	{
		if (Type())
		{
			return boost::math::cdf(normDist, D1()) * std::exp((Param("b") - Param("r")) * Param("t"));  // dC/dS = N(d1) * e^((b - r)t).
		}
		else
		{
			// Using Put-Call Parity, can see that dP/dS = dC/dS - e^((b - r))t = (N(d1) - 1) * e^((b - r)t):
			return (boost::math::cdf(normDist, D1()) - 1) * std::exp((Param("b") - Param("r")) * Param("t"));
		}
	}
	double EuropeanOption::Gamma() const				/* Change in price with respect to s (2nd order). */
	{
		// Following Put-Call Parity, we see that Gamma is the same for both Puts and Calls ((d/dS)^2P = (d/dS)^2C):
		return boost::math::cdf(normDist, D1()) / (Param("s") * Param("sigma") * sqrt(Param("t"))) * exp((Param("b") - Param("r")) * Param("t"));
	}
	//////////////////////////
	// Overloaded Operators:
	//////////////////////////
	EuropeanOption& EuropeanOption::operator=(const EuropeanOption &in) /* Assignment Operator. */
	{
		if (this != &in)
		{
			Option::operator=(in);
		}
		return *this;
	}
}


#pragma once
#include "coordinate_transform.hpp"
#include "integrate.hpp"
#include "shape.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>

//----------------L2Begin----------------
//! Computes the L^2 differences between
//! u1 (considered as coefficients for Quadratic FEM) and u2.
double computeL2Difference(const Eigen::MatrixXd &vertices,
                           const Eigen::MatrixXi &dofs,
                           const Eigen::VectorXd &u1,
                           const std::function<double(double, double)> &u2) {
	const int numberOfElements = dofs.rows();

	double error = 0;
	for (int i = 0; i < numberOfElements; ++i) {
		auto &idSet = dofs.row(i);

		const auto &a = vertices.row(idSet(0));
		const auto &b = vertices.row(idSet(1));
		const auto &c = vertices.row(idSet(2));

		auto coordinateTransform = makeCoordinateTransform(b - a, c - a);
		auto volumeFactor        = std::abs(coordinateTransform.determinant());

		// (write your solution here)
		auto f = [&](double x, double y) -> double {
			Eigen::Vector2d transformedPoint = coordinateTransform * Eigen::Vector2d(x, y) + a.transpose();

			double approximateValue = 0;
			for (int j = 0; j < 6; ++j) {
				approximateValue += u1(idSet(j)) * shapefun(j, x, y);
			}

			double diff = u2(transformedPoint.x(), transformedPoint.y()) - approximateValue;

			return diff * diff * volumeFactor;
		};

		error += integrate(f);
	}

	return std::sqrt(error);
}
//----------------L2End----------------


#include<iostream>

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

void Point::setCoordinates(double t1, double t2, double t3){
	///
	/// Setter Function for Point Class, to set coordinates
	///
	x=t1;
	y=t2;
	z=t3;
}

void Point::setCoordinatesAndLabel(double t1,double t2,double t3,string s){
	///
	/// Setter Function for Point Class, to set coordinates and label
	///
	x=t1;
	y=t2;
	z=t3;
	label = s;
	//cout<< "label is this "	<< label <<endl;
}

Point Point::projectPoint(double projectionPlane[]) {
    ///
    /// Function to give the projection of the current calling object on the projection plane passed in parameters as "projectionPlane"
    ///
    double a = projectionPlane[0];
	double b = projectionPlane[1];
	double c = projectionPlane[2];
	double d = projectionPlane[3];
	double t =((d-(a*x+b*y+c*z))/(a*a+b*b+c*c));
	Point projectedpoint;
	projectedpoint.x = (x+a*t);
	projectedpoint.y = (y+b*t);
	projectedpoint.z = (z+c*t);
	return projectedpoint;
}

double Point::relativePosition(double plane[]) {
    ///
    /// Function to calculate the relative position of the current calling object w.r.t the passed parameter "plane"
    ///
    double a = plane[0];
	double b = plane[1];
	double c = plane[2];
	double d = plane[3];
	double t = ((a*x+b*y+c*z-d)/(a*a+b*b+c*c));
	return t;
}

bool Point::checkcollinear(Point * a, Point * b){
	///
	/// Function to check if the current Pint object is collinear with the points passed as parameters
	///
	Vector3d v1(x, y, z);
	Vector3d v2(a->x, a->y, a->z);
	Vector3d v3(b->x, b->y, b->z);
	Vector3d v4 = (v2-v1).cross(v3-v1);
	double k = v4.dot(v4);
	return (k<0.00001);
}


#ifndef MATHTOOLBOX_RBF_INTERPOLATION_HPP
#define MATHTOOLBOX_RBF_INTERPOLATION_HPP

#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <memory>
#include <vector>

namespace mathtoolbox
{
    class AbstractRbfKernel
    {
    public:
        AbstractRbfKernel() {}
        virtual ~AbstractRbfKernel(){};

        virtual double operator()(const double r) const = 0;
    };

    class GaussianRbfKernel final : public AbstractRbfKernel
    {
    public:
        GaussianRbfKernel(const double theta = 1.0) : m_theta(theta) {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            return std::exp(-m_theta * r * r);
        }

    private:
        const double m_theta;
    };

    class ThinPlateSplineRbfKernel final : public AbstractRbfKernel
    {
    public:
        ThinPlateSplineRbfKernel() {}

        double operator()(const double r) const override
        {
            assert(r >= 0.0);
            const double value = r * r * std::log(r);
            return std::isnan(value) ? 0.0 : value;
        }
    };

    class LinearRbfKernel final : AbstractRbfKernel
    {
    public:
        LinearRbfKernel() {}

        double operator()(const double r) const override { return std::abs(r); }
    };

    class InverseQuadraticRbfKernel final : public AbstractRbfKernel
    {
    public:
        InverseQuadraticRbfKernel(const double theta = 1.0) : m_theta(theta) {}

        double operator()(const double r) const override { return 1.0 / std::sqrt(r * r + m_theta * m_theta); }

    private:
        const double m_theta;
    };

    class RbfInterpolator
    {
    public:
        RbfInterpolator(const std::function<double(const double)>& rbf_kernel = ThinPlateSplineRbfKernel());

        /// \brief Set data points and their values
        void SetData(const Eigen::MatrixXd& X, const Eigen::VectorXd& y);

        /// \brief Calculate the interpolation weights
        ///
        /// \details This method should be called after setting the data
        void CalcWeights(const bool use_regularization = false, const double lambda = 0.001);

        /// \brief Calculate the interpolatetd value at the specified data point
        ///
        /// \details This method should be called after calculating the weights
        double CalcValue(const Eigen::VectorXd& x) const;

    private:
        // RBF kernel
        const std::function<double(double)> m_rbf_kernel;

        // Data points
        Eigen::MatrixXd m_X;
        Eigen::VectorXd m_y;

        // Weights
        Eigen::VectorXd m_w;

        // Returns f(||xj - xi||)
        double CalcRbfValue(const Eigen::VectorXd& xi, const Eigen::VectorXd& xj) const;
    };
} // namespace mathtoolbox

#endif // MATHTOOLBOX_RBF_INTERPOLATION_HPP


/*
 * A very basic demo of libeigen3.
 *
 * USAGE:
 *    g++ -o predefined_matrices -I /PATH/TO/EIGEN/ predefined_matrices.cc
 * or
 *    g++ -o predefined_matrices $(pkg-config --cflags eigen3) predefined_matrices.cc
 */

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

using namespace Eigen;

int size = 3;
int low = 1;
int high = 3;

int main()
{
    /*
     * Using matrix template
     */
    {
        Matrix<double, 2, 3> m1 = Matrix<double, 2, 3>::Zero();
        Matrix<double, 2, 3> m2 = Matrix<double, 2, 3>::Ones();
        Matrix<double, 2, 3> m3 = Matrix<double, 2, 3>::Constant(3);
        Matrix<double, 2, 3> m4 = Matrix<double, 2, 3>::Random();
        Matrix<double, 1, 3> m5 = Matrix<double, 1, 3>::LinSpaced(size, low, high);
        Matrix<double, 3, 3> m6 = Matrix<double, 3, 3>::Identity();

        std::cout << std::endl << "Matrix<double, 2, 3>::Zero()" << std::endl << m1 << std::endl;
        std::cout << std::endl << "Matrix<double, 2, 3>::Ones();" << std::endl << m2 << std::endl;
        std::cout << std::endl << "Matrix<double, 2, 3>::Constant(3)" << std::endl << m3 << std::endl;
        std::cout << std::endl << "Matrix<double, 2, 3>::Random()" << std::endl << m4 << std::endl;
        std::cout << std::endl << "Matrix<double, 2, 3>::LinSpaced(size, low, high)" << std::endl << m5 << std::endl;
        std::cout << std::endl << "Matrix<double, 3, 3>::Identity()" << std::endl << m6 << std::endl;
    }

    /*
     * Using typedef matrices
     */
    {
        Matrix3d m1 = Matrix3d::Zero();
        Matrix3d m2 = Matrix3d::Ones();
        Matrix3d m3 = Matrix3d::Constant(3);
        Matrix3d m4 = Matrix3d::Random();
        Matrix3d m6 = Matrix3d::Identity();

        std::cout << std::endl << "Matrix3d::Zero()" << std::endl << m1 << std::endl;
        std::cout << std::endl << "Matrix3d::Ones();" << std::endl << m2 << std::endl;
        std::cout << std::endl << "Matrix3d::Constant(3)" << std::endl << m3 << std::endl;
        std::cout << std::endl << "Matrix3d::Random()" << std::endl << m4 << std::endl;
        std::cout << std::endl << "Matrix3d::Identity()" << std::endl << m6 << std::endl;
    }

    /*
     * Using dynamic matrices
     */
    {
        MatrixXd m1 = MatrixXd::Zero(2, 3);
        MatrixXd m2 = MatrixXd::Ones(2, 3);
        MatrixXd m3 = MatrixXd::Constant(2, 3, 9);
        MatrixXd m4 = MatrixXd::Random(2, 3);
        MatrixXd m6 = MatrixXd::Identity(3, 3);

        std::cout << std::endl << "MatrixXd::Zero(2, 3)" << std::endl << m1 << std::endl;
        std::cout << std::endl << "MatrixXd::Ones(2, 3);" << std::endl << m2 << std::endl;
        std::cout << std::endl << "MatrixXd::Constant(2, 3, 9)" << std::endl << m3 << std::endl;
        std::cout << std::endl << "MatrixXd::Random(2, 3)" << std::endl << m4 << std::endl;
        std::cout << std::endl << "MatrixXd::Identity(3, 3)" << std::endl << m6 << std::endl;
    }

    /*
     * Using dynamic vectors
     */
    {
        VectorXd m1 = VectorXd::Zero(3);
        VectorXd m2 = VectorXd::Ones(3);
        VectorXd m3 = VectorXd::Constant(3, 9);
        VectorXd m4 = VectorXd::Random(3);
        VectorXd m5 = VectorXd::LinSpaced(size, low, high);

        std::cout << std::endl << "VectorXd::Zero(3)" << std::endl << m1 << std::endl;
        std::cout << std::endl << "VectorXd::Ones(3);" << std::endl << m2 << std::endl;
        std::cout << std::endl << "VectorXd::Constant(3, 9)" << std::endl << m3 << std::endl;
        std::cout << std::endl << "VectorXd::Random(3)" << std::endl << m4 << std::endl;
        std::cout << std::endl << "VectorXd::LinSpaced(size, low, high)" << std::endl << m5 << std::endl;
    }
}


//
// Created by GXSN-Pro on 2018/10/17.
//

#include "sphere_util.hpp"
#include "math_util.hpp"
#include <cmath>
#include <boost/range/size_type.hpp>

#define earth_radius  6371008.8

#define MAX_COORDINATE_VALUE 1000000
#define MIN_COORDINATE_VALUE -1000000

using namespace OpenEarth::Geometry;

double SphereUtil::degreesToRadians(double degree) {
    return degree * M_PI / 180.0;
}

double SphereUtil::radiansToDegrees(double rad) {
    return rad * 180.0 / M_PI;
}

/**
 * 计算两点间的球面距离(单位 m)
 * @param from
 * @param to
 * */
double SphereUtil::distance(const LatLng &from,const LatLng &to) {
    //先计算两点之间的大圆夹角
    double rad = computeAngleBetween(from,to);
    return rad * earth_radius;
}

/**
 * 计算区域面积
 * google maps
 * */
double SphereUtil::area(std::vector<LatLng *> *points) {
    uint size = points->size();
    if (size < 3) { return 0; }
    double total = 0;
    LatLng *prePoint = points->at(size - 1);
    double prevTanLat = tan((M_PI / 2 - degreesToRadians(prePoint->lat)) / 2);
    double prevLng = degreesToRadians(prePoint->lon);
    for (uint i = 0; i < size; i++) {
        LatLng *point = points->at(i);
        double tanLat = tan((M_PI / 2 - degreesToRadians(point->lat)) / 2);
        double lng = degreesToRadians(point->lon);
        total += polarTriangleArea(tanLat, lng, prevTanLat, prevLng);
        prevTanLat = tanLat;
        prevLng = lng;
    }
    return fabs(total * (earth_radius * earth_radius));
}

double SphereUtil::polarTriangleArea(double tan1, double lng1, double tan2, double lng2) {
    double deltaLng = lng1 - lng2;
    double t = tan1 * tan2;
    return 2 * atan2(t * sin(deltaLng), 1 + t * cos(deltaLng));
}

double SphereUtil::bearing(const LatLng &start,const  LatLng &end) {
    double lon1 = degreesToRadians(start.lon);
    double lon2 = degreesToRadians(end.lon);
    double lat1 = degreesToRadians(start.lat);
    double lat2 = degreesToRadians(end.lat);
    double a = sin(lon2 - lon1) * cos(lat2);
    double b = cos(lat1) * sin(lat2) -
               sin(lat1) * cos(lat2) * cos(lon2 - lon1);
    return radiansToDegrees(atan2(a, b));
}

Envelope  SphereUtil::envelope(std::vector<LatLng *> *points) {
    if (points == nullptr) {
        return  Envelope(0, 0, 0, 0);
    }
    double minLon = MAX_COORDINATE_VALUE;
    double minLat = MAX_COORDINATE_VALUE;
    double maxLon = MIN_COORDINATE_VALUE;
    double maxLat = MIN_COORDINATE_VALUE;
    u_long length = points->size();
    for (u_long i = 0; i < length; i++) {
        LatLng *point = points->at(i);
        minLon = fmin(minLon, point->lon);
        minLat = fmin(minLat, point->lat);
        maxLon = fmax(maxLon, point->lon);
        maxLat = fmax(maxLat, point->lat);
    }
    return  Envelope(minLon, minLat, maxLon, maxLat);
}

LatLng SphereUtil::interpolate(const LatLng &from, const LatLng &to, double fraction) {
    double fromLat = degreesToRadians(from.lat);
    double fromLng = degreesToRadians(from.lon);
    double toLat = degreesToRadians(to.lat);
    double toLng = degreesToRadians(to.lon);
    double cosFromLat = cos(fromLat);
    double cosToLat = cos(toLat);

    // Computes Spherical interpolation coefficients.
    double angle = computeAngleBetween(from, to);
    double sinAngle = sin(angle);
    if (sinAngle < 1E-6) {
         LatLng latlng =  LatLng(
                from.lat + fraction * (to.lat - from.lat),
                from.lon + fraction * (to.lon - from.lon));;
        return latlng;
    }
    double a = sin((1 - fraction) * angle) / sinAngle;
    double b = sin(fraction * angle) / sinAngle;

    // Converts from polar to vector and interpolate.
    double x = a * cosFromLat * cos(fromLng) + b * cosToLat * cos(toLng);
    double y = a * cosFromLat * sin(fromLng) + b * cosToLat * sin(toLng);
    double z = a * sin(fromLat) + b * sin(toLat);

    // Converts interpolated vector back to polar.
    double lat = atan2(z, sqrt(x * x + y * y));
    double lng = atan2(y, x);
    return  LatLng(radiansToDegrees(lat), radiansToDegrees(lng));
}

double SphereUtil::computeAngleBetween(const LatLng &from, const LatLng &to) {
    return distanceRadians(degreesToRadians(from.lat), degreesToRadians(from.lon),
                           degreesToRadians(to.lat), degreesToRadians(to.lon));
}

double SphereUtil::distanceRadians(double lat1, double lng1, double lat2, double lng2) {
    return MathUtil::arcHav(MathUtil::havDistance(lat1, lat2, lng1 - lng2));
}


#include "projection.hpp"

#include <Eigen/Geometry>

namespace neon::geometry
{
matrix2x project_to_plane(matrix3x const& nodal_coordinates)
{
    auto const lnodes = nodal_coordinates.cols();

    matrix2x plane_coordinates = matrix::Zero(2, lnodes);

    auto const normal = unit_outward_normal(nodal_coordinates);

    // Orthogonal basis vectors
    vector3 e1, e2;

    // Algorithm implemented from Jeppe Revall Frisvad titled
    // Building an Orthonormal Basis from a 3D Unit Vector Without Normalization
    if (normal(2) <= -0.9999999)
    {
        // Handle the singularity
        e1 = -vector3::UnitY();
        e2 = -vector3::UnitX();
    }
    else
    {
        auto const a = 1.0 / (1.0 + normal(2));
        auto const b = -normal(0) * normal(1) * a;
        e1 << 1.0 - normal(0) * normal(0) * a, b, -normal(0);
        e2 << b, 1.0 - normal(1) * normal(1) * a, -normal(1);
    }
    // Project the points onto the plane using e1 and e2
    for (int lnode = 0; lnode < lnodes; ++lnode)
    {
        plane_coordinates(0, lnode) = e1.dot(nodal_coordinates.col(lnode));
        plane_coordinates(1, lnode) = e2.dot(nodal_coordinates.col(lnode));
    }
    return plane_coordinates;
}

vector3 unit_outward_normal(matrix3x const& nodal_coordinates)
{
    using namespace Eigen;
    Hyperplane<double, 3> hp = Hyperplane<double, 3>::Through(nodal_coordinates.col(0),
                                                              nodal_coordinates.col(1),
                                                              nodal_coordinates.col(2));
    return -hp.normal();
}
}


/*
imgalt - Image Alignment Tool
Author: GreatAttractor

version 0.5
2014/05/22

This code can be freely distributed and used for any purpose.

File description:
    Fast Fourier Transform functions implementation.

*/
#include "fft.h"
#include <cmath>
#include <stdlib.h>
#include <boost/cstdint.hpp>
using namespace std;
using namespace boost;

const float PI = 3.1415926536f;
const complex<float> I = complex<float>(0, 1);

/// Returns floor(log2(N))
inline int quickLog2(unsigned N)
{
    int result = 0;
    while (N > 0)
    {
        N >>= 1;
        result++;
    }
    return result - 1;
}

/*
/// Quick raising to integer power
template<typename T>
inline T binpow(T x, int n)
{
    T pow = 1;
    while (n != 0)
    {
        if (n & 1)
            pow *= x;
        n /= 2;
        x *= x;
    }
    return pow;
}

uint32_t ReverseBits(uint32_t n, int width)
{
    uint32_t result = 0;
    result |= (n & 1);
    for (int i = 1; i <= width-1; i++)
    {
        result <<= 1;
        n >>= 1;
        result |= (n & 1);
    }

    return result;
}*/

// Not using fft1d_Pease at the moment, because even with precalculated twiddle factors
// and without bit reversal it's not faster than the recursive approach.

/// Calculates 1-dimensional discrete Fourier transform
/** Uses in-place Pease algorithm (iterative). */
/*template<typename InputT>
void fft1d_Pease(
    InputT input[],  ///< Input vector of length 'N'
    unsigned N,      ///< Number of elements in 'input', has to be a power of two
    std::complex<float> output[] ///< Output vector of length 'N'
)
{
    int t = quickLog2(N);

    for (unsigned i = 0; i < N; i++)
        output[ReverseBits(i, t)] = input[i];

    complex<float> w = exp(-2.0f * PI * I * (1.0f/N));

    for (int c = t-1; c >= 0; c--)
        for (int r = 0; r < 1<<(t-1); r++)
        {
            unsigned r0 = r & ((1<<c) - 1);
            unsigned r1 = r >> c;
            unsigned a0 = (r0 << (t-c)) + r1;
            unsigned a1 = a0 + (1 << (t-c-1));
            complex<float> y0 = output[a0 + 1];
            complex<float> y1 = binpow<complex<float> >(w, r1 << c) * output[a1 + 1];
            output[a0 + 1] = y0 + y1;
            output[a1 + 1] = y0 - y1;
        }
}*/

/// Calculates 1-dimensional discrete Fourier transform or its inverse (not normalized by N, caller must do this)
template<typename InputT>
void fft1d(
    InputT input[],  ///< Input vector of length 'N'
    unsigned N,      ///< Number of elements in 'input', has to be a power of two
    std::complex<float> output[], ///< Output vector of length 'N'
    int stride,       ///< Stride of the input vector
    int outputStride, ///< Stride of the output vector

    /// Pointer to the initial twiddle factor for N, i.e. exp(-2*pi*i/N) (or exp(2*pi*i/N) for inverse transform).
    /// NOTE: (twiddlePtr-1) has to point to the lower twiddle factor, i.e. exp(+-2*pi*i/(N/2))
    complex<float> *twiddlePtr
)
{
    if (N == 1)
        output[0] = input[0];
    else
    {
        fft1d(input, N/2, output, 2*stride, outputStride, twiddlePtr - 1);
        fft1d(input + stride, N/2, output + N/2*outputStride, 2*stride, outputStride, twiddlePtr - 1);

        // Initial twiddle factor
        complex<float> tfactor0 = *twiddlePtr;

        complex<float> tfactor = 1.0f;
        for (unsigned k = 0; k <= N/2 - 1; k++)
        {
            complex<float> t = output[k*outputStride];
            complex<float> h = tfactor * output[(k + (N>>1))*outputStride];
            output[k*outputStride] = t + h;
            output[(k + (N>>1))*outputStride] = t - h;
            tfactor *= tfactor0; // in effect, tfactor = exp(-2*PI*I * k/N)
        }
    }
}

void CalcTwiddleFactors(unsigned N, complex<float> table[], bool inverse)
{
    for (int n = quickLog2(N); n >= 0; n--)
    {
        table[n] = (inverse ?
            exp(2.0f * PI * I * (1.0f/N)) :
            exp(-2.0f * PI * I * (1.0f/N)));

        N >>= 1;
    }
}

/// Calculates 1-dimensional discrete Fourier transform
void CalcFFT1D(
    uint8_t input[],
    unsigned N,
    std::complex<float> output[])
{
    complex<float> *twiddleFactors = new complex<float>[(quickLog2(N) + 1)];
    CalcTwiddleFactors(N, twiddleFactors, false);

    fft1d<uint8_t>(input, N, output, 1, 1, twiddleFactors + quickLog2(N));

    free(twiddleFactors);
}

/// Calculates 1-dimensional inverse discrete Fourier transform
void CalcFFTinv1D(
    complex<float> input[],
    unsigned N,
    std::complex<float> output[])
{
    complex<float> *twiddleFactors = new complex<float>[(quickLog2(N) + 1)];
    CalcTwiddleFactors(N, twiddleFactors, true);

    fft1d(input, N, output, 1, 1, twiddleFactors + quickLog2(N));

    // Normalize to obtain the inverse transform
    float Ninv = 1.0f/N;
    for (unsigned k = 0; k < N; k++)
        output[k] *= Ninv;

    free(twiddleFactors);
}

/// Calculates 2-dimensional discrete Fourier transform using row-column algorithm
void CalcFFT2D(
    float input[], ///< Input array containing N*N elements
    unsigned N, ///< Number of rows and columns; has to be a power of two
    std::complex<float> output[] ///< Output array containing N*N elements
        )
{
    int k;

    complex<float> *twiddleFactors = new complex<float>[(quickLog2(N) + 1)];
    CalcTwiddleFactors(N, twiddleFactors, false);

    // Calculate 1-dimensional transforms of all the rows
    std::complex<float> *fftrows = new std::complex<float>[N*N*sizeof(std::complex<float>)];
    #pragma omp parallel for
    for (k = 0; k < N; k++)
        fft1d<float>(input + k*N, N, fftrows + k*N, 1, 1, twiddleFactors + quickLog2(N));

    // Calculate 1-dimensional transforms of all columns in 'fftrows' to get the final result
    #pragma omp parallel for
    for (k = 0; k < N; k++)
        fft1d<std::complex<float> >(fftrows + k, N, output + k, N, N, twiddleFactors + quickLog2(N));

    free(twiddleFactors);
    free(fftrows);
}

/// Calculates 2-dimensional inverse discrete Fourier transform using row-column algorithm
void CalcFFTinv2D(
    std::complex<float> input[], ///< Input array containing N*N elements
    unsigned N, ///< Number of rows and columns; has to be a power of two
    std::complex<float> output[] ///< Output array containing N*N elements
        )
{
    int k;
    float Ninv = 1.0f/N;

    complex<float> *twiddleFactors = new complex<float>[(quickLog2(N) + 1)];
    CalcTwiddleFactors(N, twiddleFactors, true);

    // Calculate 1-dimensional inverse transforms of all the rows
    std::complex<float> *fftrows = new std::complex<float>[N*N];
    #pragma omp parallel for
    for (k = 0; k < N; k++)
        fft1d<std::complex<float> >(input + k*N, N, fftrows + k*N, 1, 1, twiddleFactors + quickLog2(N));

    for (k = 0; k < N*N; k++)
        fftrows[k] *= Ninv;

    // Calculate 1-dimensional inverse transforms of all columns in 'fftrows' to get the final result
    #pragma omp parallel for
    for (k = 0; k < N; k++)
        fft1d<std::complex<float> >(fftrows + k, N, output + k, N, N, twiddleFactors + quickLog2(N));

    free(twiddleFactors);
    free(fftrows);

    for (k = 0; k < N*N; k++)
        output[k] *= Ninv;
}

/// Calculates cross-power spectrum of two 2D discrete Fourier transforms
void CalcCrossPowerSpectrum2D(
    std::complex<float> F1[], ///< First discrete Fourier transform (N*N elements)
    std::complex<float> F2[], ///< Second discrete Fourier transform (N*N elements)
    std::complex<float> output[], ///< Cross-correlation of F1 and F2 (N*N elements)
    unsigned N ///< Number of rows and columns, has to be a power of 2
    )
{
    #pragma omp parallel for
    for (int i = 0; i < N*N; i++)
    {
        output[i] = std::conj(F1[i]) * F2[i];
        float magn = std::abs(output[i]);
        if (magn > 1.0e-8f)
            output[i] /= magn;
    }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Custom triangle template Example

#include <iostream>

#include <boost/array.hpp>
#include <boost/tuple/tuple.hpp>

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/register/ring.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/io/dsv/write.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
struct triangle : public boost::array<P, 3>
{
};

// Register triangle<P> as a ring
BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(triangle)

namespace boost { namespace geometry { namespace dispatch {

// Specializations of area dispatch structure, implement algorithm
template<typename Point>
struct area<triangle<Point>, ring_tag>
{
    template <typename Strategy>
    static inline double apply(triangle<Point> const& t, Strategy const&)
    {
        return 0.5  * ((get<0>(t[1]) - get<0>(t[0])) * (get<1>(t[2]) - get<1>(t[0]))
                     - (get<0>(t[2]) - get<0>(t[0])) * (get<1>(t[1]) - get<1>(t[0])));
    }
};

}}} // namespace boost::geometry::dispatch

int main()
{
    //triangle<boost::geometry::point_xy<double> > t;
    triangle<boost::tuple<double, double> > t;
    t[0] = boost::make_tuple(0, 0);
    t[1] = boost::make_tuple(5, 0);
    t[2] = boost::make_tuple(2.5, 2.5);

    std::cout << "Triangle: " << boost::geometry::dsv(t) << std::endl;
    std::cout << "Area: " << boost::geometry::area(t) << std::endl;

    //boost::geometry::point_xy<double> c;
    boost::tuple<double, double> c;
    boost::geometry::centroid(t, c);
    std::cout << "Centroid: " << boost::geometry::dsv(c) << std::endl;

    return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

class Stats
{
public:
  using ArrayXd = Eigen::ArrayXd;
  ArrayXd process(Eigen::Ref<ArrayXd> input, double low, double mid,
                  double high)
  {
    using namespace std;
    index   length = input.size();
    ArrayXd out = ArrayXd::Zero(7);
    double  mean = input.mean();
    double  stdev = sqrt((input - mean).square().mean());
    double skewness = ((input - mean) / (stdev == 0 ? 1 : stdev)).cube().mean();
    double kurtosis = ((input - mean) / (stdev == 0 ? 1 : stdev)).pow(4).mean();
    ArrayXd sorted = input;
    sort(sorted.data(), sorted.data() + length);
    double lowVal = sorted(lrint(low * (length - 1)));
    double midVal = sorted(lrint(mid * (length - 1)));
    double highVal = sorted(lrint(high * (length - 1)));
    out << mean, stdev, skewness, kurtosis, lowVal, midVal, highVal;
    return out;
  }
};
} // namespace algorithm
} // namespace fluid


#include <iostream>
#include <Eigen/Geometry>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

/**
Euler angle defination: zyx
Rotation matrix: C_body2ned
**/
Quaterniond euler2quaternion(Vector3d euler)
{
  double cr = cos(euler(0)/2);
  double sr = sin(euler(0)/2);
  double cp = cos(euler(1)/2);
  double sp = sin(euler(1)/2);
  double cy = cos(euler(2)/2);
  double sy = sin(euler(2)/2);
  Quaterniond q;
  q.w() = cr*cp*cy + sr*sp*sy;
  q.x() = sr*cp*cy - cr*sp*sy;
  q.y() = cr*sp*cy + sr*cp*sy;
  q.z() = cr*cp*sy - sr*sp*cy;
  return q;
}

Matrix3d quaternion2mat(Quaterniond q)
{
  Matrix3d m;
  double a = q.w(), b = q.x(), c = q.y(), d = q.z();
  m << a*a + b*b - c*c - d*d, 2*(b*c - a*d), 2*(b*d+a*c),
       2*(b*c+a*d), a*a - b*b + c*c - d*d, 2*(c*d - a*b),
       2*(b*d - a*c), 2*(c*d+a*b), a*a-b*b - c*c + d*d;
  return m;
}

Vector3d mat2euler(Matrix3d m)
{
  double r = atan2(m(2, 1), m(2, 2));
  double p = asin(-m(2, 0));
  double y = atan2(m(1, 0), m(0, 0));
  Vector3d rpy(r, p, y);
  return rpy;
}

Quaterniond mat2quaternion(Matrix3d m)
{
  //return euler2quaternion(mat2euler(m));
  Quaterniond q;
  double a, b, c, d;
  a = sqrt(1 + m(0, 0) + m(1, 1) + m(2, 2))/2;
  b = (m(2, 1) - m(1, 2))/(4*a);
  c = (m(0, 2) - m(2, 0))/(4*a);
  d = (m(1, 0) - m(0, 1))/(4*a);
  q.w() = a; q.x() = b; q.y() = c; q.z() = d;
  return q;
}

//ZYX
Matrix3d euler2mat(Vector3d euler)
{
  double cr = cos(euler(0));
  double sr = sin(euler(0));
  double cp = cos(euler(1));
  double sp = sin(euler(1));
  double cy = cos(euler(2));
  double sy = sin(euler(2));
  Matrix3d m;
  m << cp*cy,  -cr*sy + sr*sp*cy, sr*sy + cr*sp*cy,
       cp*sy,  cr*cy + sr*sp*sy,  -sr*cy + cr*sp*sy,
       -sp,    sr*cp,             cr*cp;
  return m;
}

Vector3d quaternion2euler(Quaterniond q)
{
  return mat2euler(quaternion2mat(q));
}

double quaternion2yaw(Quaterniond q)
{
  double a = q.w(), b = q.x(), c = q.y(), d = q.z();
  double y = atan2(2*(b*c+a*d), (a*a + b*b - c*c - d*d));
  return y;
}

//Euler motion equation
//ZYX Euler angles velocity to body frame wx wy wz  w_phi,  w_theta,  w_psi     q: roll pitch yaw  (phi theta psi)
Matrix3d w_Euler2Body(Vector3d q)
{
    double cr = cos( q(0));
    double sr = sin( q(0));
    double cp = cos( q(1));
    double sp = sin( q(1));
    double cy = cos( q(2));
    double sy = sin( q(2));

    Matrix3d G;
    G << 1.,    0.,       -sp,
         0.,    cr,   cp * sr,
         0.,   -sr,   cp * cr;

    // Vector3d w(0,0,0);
    // w = G * qdot;
    return G;
}
Matrix3d w_Body2Euler(Vector3d q)
{
    double cr = cos( q(0));
    double sr = sin( q(0));
    double cp = cos( q(1)) + 0.00000001;
    double sp = sin( q(1));
    double cy = cos( q(2));
    double sy = sin( q(2));

    Matrix3d G_inv;
    G_inv << 1., sp*sr/cp, cr*sp/cp,
             0.,       cr,      -sr,
             0.,    sr/cp,    cr/cp;

    // Vector3d qdot(0,0,0);
    // qdot = G_inv * w;
    return G_inv;
}
//ZXY Euler angles velocity to body frame wx wy wz  w_phi,  w_theta,  w_psi
// Vector3d Euler2Body(Vector3d q, Vector3d qdot)
// {
//     Vector3d w(0,0,0);
//     Matrix3d G;
//     G << cos(q.y),   0.,  -cos(q.x)*sin(q.y),
//          0.,         1.,   sin(q.x),
//          sin(q.y),   0.,   cos(q.x)*cos(q.y);
//     w = G * qdot;
//     return w;
// }
// Vector3d Body2Euler(Vector3d q, Vector3d w)
// {
//     Vector3d qdot(0,0,0);
//     Matrix3d G_inv;
//     G_inv << cos(q.y),   0.,    sin(q.y),
//              (sin(q.x)*sin(q.y))/(cos(q.x)+0.00000001), 1., -(cos(q.y)*sin(q.x))/(cos(q.x)+0.00000001),
//              -sin(q.y)/(cos(q.x)+0.00000001), 0., cos(q.y)/(cos(q.x)+0.00000001);
//     qdot = G_inv * w;
//     return qdot;
// }


#include <iostream>
#include <fstream>
#include <cstring>

#include "Common.hpp"
#include "BUSData.h"

#include "bustools_predict.h"
#include <iostream>
#include <fstream>
#include <cmath>

#include <Eigen/Core>
#include <unsupported/Eigen/SpecialFunctions>
#include <LBFGSB.h>
#include <cppoptlib/meta.h>
#include <cppoptlib/boundedproblem.h>
#include <cppoptlib/solver/lbfgsbsolver.h>
#include <thread>
#include <memory>
#include <mutex>

//based on the boost implementation
//r is the size parameter
inline double DensityNegBin(double k, double r, double mu) {
	//so, the standard parameterization is r and p, but we use mean (mu) and size
	//mean = mu = r(1-p)/p <=> p = r/(mu+r), size (i.e. r) in this parameterization and r in the standard one is the same.
	double p = r / (mu + r);
	return exp(lgamma(r + k) - lgamma(r) - lgamma(k + 1)) * pow(p, r) * pow((1 - p), k);
}

//So, this one is different from the negative binomial below in that the zero is not included.
//We here want to look at the log likelihood for observed molecules (i.e. not including the zeros) and how they would fit the negative
//binomial, which means that the density function needs to be modified. The probability for zero needs to be set to zero, and the other
//values scaled up to get to a sum of 1.
inline double ZTNBLogLikelihood(const double* hist, size_t histLen, double size, double mu) {
	//first, get the probability of zero, probZero - all other values should be divided by 1-probZero to normalize the sum of non-zeros to 1
	double probZero = DensityNegBin(0, size, mu);
	double scale = 1 - probZero;

	//now calculate the log likelihood
	double ll = 0;
	for (size_t i = 0; i < histLen; ++i) {
		ll += log(DensityNegBin(double(i + 1), size, mu) / scale) * (*(hist + i));
	}

	return ll;
}

inline double NegBinLogLikelihood(const double* hist, size_t histLen, double size, double mu, double zeroCount) {
	//loglikelihood for nonzero counts
	double ll = 0;
	for (size_t i = 0; i < histLen; ++i) {
		ll += log(DensityNegBin(double(i + 1), size, mu)) * (*(hist + i));
	}
	//...and for the zero counts
	ll += log(DensityNegBin(0, size, mu)) * zeroCount;

	return ll;
}

//using LBFGSpp
class Optimizer
{
public:
	//make the variables public to be able to update them without a lot of extra code
	double mean = 0;
	double zeroCounts = 0;
	double totMol = 0;
	const double* hist = nullptr;
	size_t histLen = 0;
	Optimizer() {}

	//returns objective function value. The grad vector should be modified (i.e. set gradients)
	//the vector x is the in parameters, in this case the size (i.e. r) parameter in the distribution
	//The math here is the same as in preseqR
	double operator()(const Eigen::VectorXd& x, Eigen::VectorXd& grad) {
		double firstTerm = Eigen::numext::digamma(x[0]) * zeroCounts;
		for (size_t i = 0; i < histLen; ++i) {
			firstTerm += *(hist+i) * Eigen::numext::digamma(double(i + 1 + x[0]));
		}
		firstTerm /= totMol;

		double secondTerm = Eigen::numext::digamma(x[0]);

		double thirdTerm = log(x[0]) - log(x[0] + mean);

		grad[0] = -firstTerm + secondTerm - thirdTerm;

		return -NegBinLogLikelihood(hist, histLen, x[0], mean, zeroCounts) / totMol;
	}
};

//using CppOptimizationLibrary
class Optimizer2 : public cppoptlib::BoundedProblem<double, 1> {
public:
	//make the variables public to be able to update them without a lot of extra code
	double mean = 0;
	double zeroCounts = 0;
	double totMol = 0;
	const double* hist = nullptr;
	size_t histLen = 0;

	//The math here is the same as in preseqR

	double value(const TVector& x) {
		return -NegBinLogLikelihood(hist, histLen, x[0], mean, zeroCounts) / totMol;
	}

	void gradient(const TVector& x, TVector& grad)
	{
		double firstTerm = Eigen::numext::digamma(x[0]) * zeroCounts;
		for (size_t i = 0; i < histLen; ++i) {
			firstTerm += *(hist + i) * Eigen::numext::digamma(double(i + 1 + x[0]));
		}
		firstTerm /= totMol;

		double secondTerm = Eigen::numext::digamma(x[0]);

		double thirdTerm = log(x[0]) - log(x[0] + mean);

		grad[0] = -firstTerm + secondTerm - thirdTerm;
	}
};

//size and mu are both in and out parameters - the in parameters are the inital values in the expectation maximization algorithm
//the hist is just a pointer into the large hist vector to avoid unnecessary copying
//returns the log likelihood
//This function uses LBFGSpp
double PredictZTNBEmAlg1(const double* hist, size_t histLen, double& size, double& mu) {
	//estimate start values
	double histSum = 0; //S in the R code
	double histWeights = 0;
	for (size_t i = 0; i < histLen; ++i) {
		histSum += *(hist + i);
		histWeights += (*(hist + i)) * (i+1);
	}
	mu = (histWeights - histSum)/histSum;
	size = 1;

	double zeroProbability = DensityNegBin(0, size, mu);

	//estimate the total number of molecules
	double totMol = histSum / (1 - zeroProbability); //L in the R code

	//the estimated number of zero counts
	double zeroCounts = totMol * zeroProbability;

	//estimate the mean and variance
	double mean = 0, variance = 0;
	double precalcMeanSum = 0; //reuse this sum in the EM loop below since it is constant throughout the algorithm
	for (size_t i = 0; i < histLen; ++i) {
		precalcMeanSum += *(hist + i) * double(i + 1);//remember that the hist index is off by one, i.e. that the hist[0] corresponds to molecules with 1 copy and so forth
	}
	mean = precalcMeanSum / totMol;
	//..and the variance
	for (size_t i = 0; i < histLen; ++i) {
		variance += *(hist + i) * pow(double(i + 1) - mean, 2);
	}
	variance = (variance + mean * mean * zeroCounts) / (totMol - 1);

	Optimizer op;

	// set all params
	op.mean = mean;
	op.zeroCounts = zeroCounts;
	op.totMol = totMol;
	op.hist = hist;
	op.histLen = histLen;

	//using LBFGSpp
	LBFGSpp::LBFGSBParam<double> param;
	//So, the epsilon and max iterations here are for the LBFGS algorithm, not the EM, that comes later!
	param.epsilon = 1e-8; //It seems that the optim function in R uses about 1e-8
	param.max_iterations = 100; //defaults to 100 in the optim function in R
	param.max_linesearch = 500; //don't want it to fail...

	// Create solver and function object
	LBFGSpp::LBFGSBSolver<double> solver(param);

	// Initial guess
	Eigen::VectorXd x = Eigen::VectorXd::Zero(1);

	if (variance > mean) {
		x[0] = mean * mean / (variance - mean);
	}
	else {
		x[0] = size;
	}

	// Bounds
	Eigen::VectorXd lb = Eigen::VectorXd::Constant(1, 0.0001);
	Eigen::VectorXd ub = Eigen::VectorXd::Constant(1, 10000);

	// x will be overwritten to be the best point found
	double fx = 0;
	int numIter = solver.minimize(op, x, fx, lb, ub);

	size_t iter = 0;
	double lastNegLL = 10000000000000;
	double currNegLL = -ZTNBLogLikelihood(hist, histLen, x[0], mean);

	//termination criteria for the EM algorithm
	const double MAX_ERROR = 1e-8;
	const double MAX_ERROR_FAST = 1e-5;
	const size_t MAX_ITER = 100000;
	const size_t ITER_FAST_LIMIT = 200; //if the number of iterations goes over this number, start using the lower error threshold, MAX_ERROR_FAST, to quicken things up

	//The EM algorithm starts here
	while (fabs(lastNegLL - currNegLL) / histSum > MAX_ERROR&&
		iter < MAX_ITER &&
		!(iter >= ITER_FAST_LIMIT && fabs(lastNegLL - currNegLL) / histSum <= MAX_ERROR_FAST))
	{
		lastNegLL = currNegLL;

		//update distribution params
		size = x[0];
		mu = mean;

		// E step: estimate the number of unobserved species

		zeroProbability = DensityNegBin(0, size, mu);
		totMol = histSum / (1 - zeroProbability);
		zeroCounts = totMol * zeroProbability;

		//mean and variance
		mean = precalcMeanSum / totMol;
		//...and variance
		for (size_t i = 0; i < histLen; ++i) {
			variance += *(hist + i) * pow(double(i + 1) - mean, 2);
		}
		variance = (variance + mean * mean * zeroCounts) / (totMol - 1);

		// M step: estimate the parameters size and mu
		//rerun the LBFGSB

		// set params
		op.mean = mean;
		op.zeroCounts = zeroCounts;
		op.totMol = totMol;

		if (variance > mean) {
			x[0] = mean * mean / (variance - mean);
		}
		else {
			x[0] = size;
		}

		//avoid printing of warning text in console
		if (x[0] > 10000.0) {
			x[0] = 10000.0;
		}
		if (x[0] < 0.0001) {
			x[0] = 0.0001;
		}

		int numIter = solver.minimize(op, x, fx, lb, ub);

		currNegLL = -ZTNBLogLikelihood(hist, histLen, x[0], mean);

		//std::cout << "error: " << (lastNegLL - currNegLL) / histSum << "\n";

		++iter;

		//if (iter > 300) break;//tmp
	}

	//std::cout << "iterations: " << iter << " error: " << (lastNegLL - currNegLL) / histSum << "\n";

	return -currNegLL;
}

//Similar to above, but uses CppOptimizationLibrary
double PredictZTNBEmAlg2(const double* hist, size_t histLen, double& size, double& mu) {

	//estimate start values
	double histSum = 0; //S in the R code
	double histWeights = 0;
	for (size_t i = 0; i < histLen; ++i) {
		histSum += *(hist + i);
		histWeights += (*(hist + i)) * (i+1);
	}
	mu = (histWeights - histSum)/histSum;
	size = 1;

	double zeroProbability = DensityNegBin(0, size, mu);

	//estimate the total number of molecules
	double totMol = histSum / (1 - zeroProbability); //L in the R code

	//the estimated number of zero counts
	double zeroCounts = totMol * zeroProbability;

	//estimate the mean and variance
	double mean = 0, variance = 0;
	double precalcMeanSum = 0; //reuse this sum in the EM loop below since it is constant throughout the algorithm
	for (size_t i = 0; i < histLen; ++i) {
		precalcMeanSum += *(hist + i) * double(i + 1);//remember that the hist index is off by one, i.e. that the hist[0] corresponds to molecules with 1 copy and so forth
	}
	mean = precalcMeanSum / totMol;
	//..and the variance
	for (size_t i = 0; i < histLen; ++i) {
		variance += *(hist + i) * pow(double(i + 1) - mean, 2);
	}
	variance = (variance + mean * mean * zeroCounts) / (totMol - 1);

	Optimizer2 op;
	// set all params
	op.mean = mean;
	op.zeroCounts = zeroCounts;
	op.totMol = totMol;
	op.hist = hist;
	op.histLen = histLen;

	//using CppOptimizationLibrary
	op.setLowerBound(Optimizer2::TVector::Ones(1) * 0.0001);
	op.setUpperBound(Optimizer2::TVector::Ones(1) * 10000);
	cppoptlib::LbfgsbSolver<Optimizer2> solver;
	//change the stop criteria to speed things up
	cppoptlib::LbfgsbSolver<Optimizer2>::TCriteria crit = cppoptlib::LbfgsbSolver<Optimizer2>::TCriteria::defaults();
	crit.iterations = 1000; //default is 10000
	solver.setStopCriteria(crit);
	Optimizer2::TVector x = Optimizer2::TVector::Zero(1);

	if (variance > mean) {
		x[0] = mean * mean / (variance - mean);
	}
	else {
		x[0] = size;
	}

//	solver.setDebug(cppoptlib::DebugLevel::Low);
	// x will be overwritten to be the best point found
	solver.minimize(op, x);

	// Bounds
	Eigen::VectorXd lb = Eigen::VectorXd::Constant(1, 0.0001);
	Eigen::VectorXd ub = Eigen::VectorXd::Constant(1, 10000);

	size_t iter = 0;
	double lastNegLL = 10000000000000;
	double currNegLL = -ZTNBLogLikelihood(hist, histLen, x[0], mean);

	//termination criteria for the EM algorithm
	const double MAX_ERROR = 1e-8;
	const double MAX_ERROR_FAST = 1e-5;
	const size_t MAX_ITER = 400; //Since this algorithm is pretty slow, don't iterate too many times. The algorithm may get stuck here, on a few values only, don't let them dictate the total execution time too much.
	const size_t ITER_FAST_LIMIT = 200; //if the number of iterations goes over this number, start using the lower error threshold, MAX_ERROR_FAST, to quicken things up
	const double LARGE_NEG_LL = 100000000000000000.0;

	double bestNegLL = LARGE_NEG_LL;
	double bestMu = -1;
	double bestSize = -1;

	//The EM algorithm starts here
	while (fabs(lastNegLL - currNegLL) / histSum > MAX_ERROR&&
		iter < MAX_ITER &&
		!(iter >= ITER_FAST_LIMIT && fabs(lastNegLL - currNegLL) / histSum <= MAX_ERROR_FAST))
	{
		//temp, remove
		//double err = fabs(lastNegLL - currNegLL);

		lastNegLL = currNegLL;

		//update distribution params
		size = x[0];
		mu = mean;

		// E step: estimate the number of unobserved species

		zeroProbability = DensityNegBin(0, size, mu);
		totMol = histSum / (1 - zeroProbability);
		zeroCounts = totMol * zeroProbability;

		//mean and variance
		mean = precalcMeanSum / totMol;
		//...and variance
		for (size_t i = 0; i < histLen; ++i) {
			variance += *(hist + i) * pow(double(i + 1) - mean, 2);
		}
		variance = (variance + mean * mean * zeroCounts) / (totMol - 1);

		// M step: estimate the parameters size and mu
		//rerun the LBFGSB

		// set params
		op.mean = mean;
		op.zeroCounts = zeroCounts;
		op.totMol = totMol;

		if (variance > mean) {
			x[0] = mean * mean / (variance - mean);
		}
		else {
			x[0] = size;
		}

		//avoid printing of warning text in console
		if (x[0] > 10000.0) {
			x[0] = 10000.0;
		}
		if (x[0] < 0.0001) {
			x[0] = 0.0001;
		}

		solver.minimize(op, x);
		//std::cout << "val: " << x[0] << "\n";

		currNegLL = -ZTNBLogLikelihood(hist, histLen, x[0], mean);

		//std::cout << "Iteration: " << iter << " x val: " << x[0] << " ll " << currNegLL << " lldiff: " << err << "\n";

		//keep track of the best values we had in case it doesn't converge - better to return those
		if (currNegLL < bestNegLL) {
			bestNegLL = currNegLL;
			bestMu = mu;
			bestSize = size;
		}

		//std::cout << "error: " << (lastNegLL - currNegLL) / histSum << "\n";

		++iter;

		//if (iter > 300) break;//tmp
	}

	//std::cout << "iterations: " << iter << " error: " << (lastNegLL - currNegLL) / histSum << "\n";

	if (bestNegLL < LARGE_NEG_LL) {
		currNegLL = bestNegLL;
		mu = bestMu;
		size = bestSize;
	}

	//std::cout << "Iteration: " << iter << " mu: " << mu << " size: " << size << " ll: " << currNegLL << "\n";

	return -currNegLL;
}

//size and mu are out parameters describing the negative binomial
double PredictZTNBForGene(const double* hist, size_t histLen, double t, double& size, double& mu, int index) {
	double histSum = 0; //S in the R code
	for (size_t i = 0; i < histLen; ++i) {
		histSum += *(hist + i);
	}
	if (histSum == 0.0) {
		return 0.0;//nothing to do...
	}
	//initial values of negative binomial, taken from R code
	size = 1.0;
	mu = 0.5;
	//fit the ZTNB (will update size and mu)
	//So, the trick here is to first use Alg1 - it is faster, but fails sometimes. If it fails,
	//use Alg2
	try {
		//std::cout << "Alg1: " << index << "\n";
		PredictZTNBEmAlg1(hist, histLen, size, mu);
		//std::cout << "Alg1 done: " << index << "\n";
	}
	catch (std::exception&)
	{
		//std::cout << "Alg2: " << index << "\n";
		PredictZTNBEmAlg2(hist, histLen, size, mu);
		//std::cout << "Alg2 done: " << index << "\n";
	}

	//std::cout << "Mu: " << mu << " Size: " << size << "\n";

	//estimate the total number of molecules
	double zeroProbability = DensityNegBin(0, size, mu);
	double totMol = histSum / (1 - zeroProbability); //L in the R code

	//The prediction is based on the assumption that the size parameter remains the same, while
	//the mean is scaled up with t. We then look at how many non-zero molecules we would get
	double zeroProbScaled = DensityNegBin(0, size, mu*t);
	double result = totMol * (1 - zeroProbScaled);
	if (result < histSum) { //safety check, we should never get fewer molecules than what we started with!
		result = histSum;
	}

	return result;
}

//This is a thread scheduler
class PredictionExecuter
{
public:
	PredictionExecuter(const std::vector<double>& hists, const std::vector<size_t>& histLengths, double t, std::vector<double>& predVals, std::vector<double>& sizeVals, std::vector<double>& muVals, const uint32_t histmax)
		: m_hists(hists)
		, m_histLengths(histLengths)
		, m_t(t)
		, m_predVals(predVals)
		, m_sizeVals(sizeVals)
		, m_muVals(muVals)
		, m_histmax(histmax)
	{}
	void Execute(int numThreads) {
		//Some test code:
		//std::cout << "Predicting 1345 \n";
		//double size = 0;
		//double mu = 0;
		//PredictZTNBForGene(&m_hists[1345 * m_histmax], m_histLengths[1345], m_t, size, mu, 1345);
		//std::cout << "End Predicting 1345 \n";

		//create the threads
		for (int i = 0; i < numThreads; ++i) {
			m_threads.push_back(std::shared_ptr<std::thread> (new std::thread(&PredictionExecuter::ThreadFunc, this)));
		}
		//wait for them to finish
		for (int i = 0; i < numThreads; ++i) {
			m_threads[i]->join();
		}
		std::cerr << "\n";
	}
private:
	void ThreadFunc() {
		size_t i = 0;
		while (GetNextIndex(i)) {
			double size = 0;
			double mu = 0;
			m_predVals[i] = PredictZTNBForGene(&m_hists[i * m_histmax], m_histLengths[i], m_t, size, mu, i);
			m_sizeVals[i] = size;
			m_muVals[i] = mu;
		}
	}
	bool GetNextIndex(size_t& index) {
		std::lock_guard<std::mutex> lg(m_mutex);
		if (m_nextIndex == m_histLengths.size()) {
			return false;
		} else {
			std::cerr << "\rProcessing gene: " << m_nextIndex + 1 << " of " << m_histLengths.size();//so, we use indexes starting at 1 in the printout
			index = m_nextIndex++;
			return true;
		}
	}
	std::mutex m_mutex;
	size_t m_nextIndex = 0;
	const std::vector<double>& m_hists;
	const std::vector<size_t>& m_histLengths;
	double m_t;
	std::vector<double>& m_predVals;
	std::vector<double>& m_sizeVals;
	std::vector<double>& m_muVals;
	const uint32_t m_histmax;
	std::vector<std::shared_ptr<std::thread>> m_threads;
};

void bustools_predict(Bustools_opt &opt) {

	//Prepare and load histograms
	//////////////////

	//read the hist file
	std::string hist_ifn = opt.predict_input + ".hist.txt";
	std::string counts_ifn = opt.predict_input + ".mtx";
	std::string gene_ifn = opt.predict_input + ".genes.txt";
	std::string barcode_ifn = opt.predict_input + ".barcodes.txt";

	std::string corr_counts_ofn = opt.output + ".mtx";
	std::string nb_params_ofn = opt.output + ".nb_params.txt";
	std::string corr_gene_ofn = opt.output + ".genes.txt";
	std::string corr_barcode_ofn = opt.output + ".barcodes.txt";

	//just copy the genes and barcodes files, they don't change
	copy_file(gene_ifn, corr_gene_ofn);
	copy_file(barcode_ifn, corr_barcode_ofn);

	//Get gene list associated with count matrix and histograms
	std::vector<std::string> genes;
	parseGenesList(gene_ifn, genes);

	//Allocate histograms
	//Indexed as gene*histmax + histIndex
	size_t n_genes = genes.size();
	const uint32_t histmax = 100;//set molecules with more than histmax copies to histmax
	std::vector<double> histograms = std::vector<double>(n_genes * histmax, 0);
	std::vector<size_t> histogramLengths = std::vector<size_t>(n_genes, 0);
	std::string line;

	//load histograms file
	{
		std::ifstream inf(hist_ifn);
		size_t geneIndex = 0;
		while (std::getline(inf, line)) {
			std::stringstream ss(line);
			double num = 0;
			size_t histIndex = 0;
			while (ss >> num) {
				histograms[geneIndex * histmax + histIndex++] = num;
			}
			histogramLengths[geneIndex] = histIndex;
			++geneIndex;
		}
	}

	//fix the histograms if they have only a single non-zero value (i.e. for example looks like this: 1 0 0 0)
	//however, do not fix completely empty histograms
	//the strategy is to always add a one after the last item in the histogram
	//(this will be next to the only non-zero number, which will be last)
	//the strategy may lead to that a gene gets more counts. This, however, doesn't matter since we only calculate
	//a scaling factor for the gene, so that will be normalized out.
	for (size_t i = 0; i < histogramLengths.size(); ++i) {
		size_t nonZeros = 0;
		for (size_t j = 0; j < histogramLengths[i]; ++j) {
			if (histograms[i * histmax + j] != 0) {
				++nonZeros;
				if (nonZeros > 1) {
					break;
				}
			}
		}
		if (nonZeros == 1) {
			if (histogramLengths[i] == histmax) { //rare case, add a count before instead of after
				histograms[i * histmax + histmax - 2] = 1;
			} else {
				histograms[i * histmax + histogramLengths[i]] = 1;
				histogramLengths[i]++;//also extend the histogram one step;
			}
			//so, if we can, remove one count from the current value
			if (histograms[i * histmax + histogramLengths[i] - 1] > 1) {
				histograms[i * histmax + histogramLengths[i] - 1] -= 1.0;
			}
		}
	}

	//Predict
	//////////////////

	std::vector<double> predVals(n_genes, 0);
	std::vector<double> sizeVals(n_genes, 0);
	std::vector<double> muVals(n_genes, 0);
	int numThreads = std::thread::hardware_concurrency();
	std::cerr << "Using " << numThreads << " threads\n";
	PredictionExecuter pe(histograms, histogramLengths, opt.predict_t, predVals, sizeVals, muVals, histmax);
	pe.Execute(numThreads);

	//calculate the sum of all histograms and all predvals:
	double histSum = 0;
	std::vector<double> umisPerGene(n_genes, 0);
	std::vector<double> countsPerGene(n_genes, 0);
	double predSum = 0;

	for (size_t i = 0; i < predVals.size(); ++i) {
		for (size_t j = 0; j < histogramLengths[i]; ++j) {
			umisPerGene[i] += histograms[i * histmax + j];
			countsPerGene[i] += histograms[i * histmax + j]*(j+1);
		}
		histSum += umisPerGene[i];
		predSum += predVals[i];
	}

	//so the genes should be scaled according to the following:
	//geneScaling = histSum/predSum * predVal/umisPerGene;
	std::vector<double> geneScaling(n_genes, 1.0);//1.0 for all empty genes
	double globScale = histSum / predSum;
	for (size_t i = 0; i < predVals.size(); ++i) {
		if (umisPerGene[i] > 0) {
			geneScaling[i] = globScale * predVals[i] / umisPerGene[i];
		}
	}

	//Modify counts
	//////////////////

	std::cerr << "Creating corrected counts matrix...\n";

	//read and write the counts matrix
	{
		std::ifstream inf(counts_ifn);
		std::string test;
		std::ofstream of(corr_counts_ofn);
		//first number is cells, second genes
		//first handle header, we just leave that untouched
		while (std::getline(inf, line)) {
			of << line << '\n';
			if ((!line.empty()) && line[0] != '%') {
				break;
			}
		}
		//now all data with scaling
		size_t cell = 0, gene = 0;
		double count = 0;
		while (std::getline(inf, line)) {
			if ((!line.empty()) && line[0] == '%') {
				of << line << '\n';
				continue;
			}
			std::stringstream ss(line);
			if (ss >> cell >> gene >> count) {
				of << cell << " " << gene << " " << count * geneScaling[gene-1] << '\n';
			}
		}
	}

	//write negative binomial params (file with header)
	std::cerr << "Writing negative binomial params...\n";
	{
		std::ofstream of(nb_params_ofn);

		//header
		of << "gene\tmu\tsize\tUMIs\tcounts\n";

		for (size_t i = 0; i < genes.size(); ++i) {
			of << genes[i] << '\t' << muVals[i] << '\t' << sizeVals[i] << '\t' << umisPerGene[i] << '\t' << countsPerGene[i] << '\n';
		}
	}

}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// MetricTools.hpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Conctact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

#include "MetricTools.hpp"

#include <Eigen/Eigenvalues>
#include <cmath>
#include <iostream>

bool isMatrixDiagonal(const Eigen::MatrixXd &A, const double epsilon){

    for(unsigned int i=0; i<(unsigned int)A.rows(); ++i)
        for(unsigned int j=0; j<(unsigned int)A.cols(); ++j) {
            if(i==j) continue;
            if(fabs(A(i,j)) > epsilon)
                return false;
        }
    return true;
}

bool isMatrixIdentity(const Eigen::MatrixXd &A, const double epsilon){

    if(A.rows() != A.cols())
        return false;

    for(unsigned int i=0; i<(unsigned int)A.rows(); ++i)
        for(unsigned int j=0; j<(unsigned int)A.cols(); ++j) {
            if( (i==j) && (fabs(A(i,j)-1) > epsilon) )
                return false;

            if( (i!=j) && (fabs(A(i,j)) > epsilon) )
                return false;
        }

    return true;
}

bool isMatrixPermutationOfDiagonal(const Eigen::MatrixXd &metric, const double epsilon){

 return isMatrixDiagonal(metric.transpose()*metric,epsilon);
}

unsigned int getRank(const Eigen::MatrixXd &metric){

    Eigen::FullPivLU<Eigen::MatrixXd> lu(metric);
    return lu.rank();
}

void eigenDecomposition(const Eigen::MatrixXd &M, Eigen::MatrixXd &P, Eigen::MatrixXd &A){

    Eigen::EigenSolver<Eigen::MatrixXd> eigensolv;
    eigensolv.compute(M,true);
    P = eigensolv.eigenvectors().real();
    A = Eigen::MatrixXd::Zero(M.rows(), M.cols());
    A.diagonal() = eigensolv.eigenvalues().real();
}

double minAbsNonZeroValue(Eigen::VectorXd x){

    // deal only with positive values
    x = x.cwiseAbs();

    // for first min value
    bool firstValue = true;
    double minVal = 0.0; //std::numeric_limits<double>::infinity();
    for(unsigned int i=0; i<(unsigned int)x.size(); ++i){

        // zero values should be ignored
        if(x(i) < std::numeric_limits<double>::epsilon())
            continue;

        // if it is the first non zero value
        if(firstValue){
            minVal = x(i);
            firstValue = false;
        }else{
            // looking for the lowest value
            if(x(i) < minVal)
                minVal = x(i);
        }
    }

    return minVal; // negative if only zero values in the vector
}

// put the per column smallest element of P equal to 1 and update A consequently
// if P initially contains many sqrt(0.5), they will (probably) be transform to 1
// at the end, P is not an othrogonal matrix anymore.
Eigen::MatrixXd eigenRefinement(Eigen::MatrixXd &P, Eigen::MatrixXd &D, Eigen::MatrixXd &Pinv){

    // initialize the scale matrix to Identity
    Eigen::MatrixXd scaleMatrix = Eigen::MatrixXd::Identity(D.rows(),D.cols());

    // when possible, put integers in P, update Pinv consequently
    for(int i=0; i<P.rows(); ++i){

        // convert the per line smallest non-zero value to 1
        double minVal = minAbsNonZeroValue(P.col(i));
        P.col(i)    /= minVal;
        Pinv.row(i) *= minVal;
        //D(i,i) = 1.0 / (minVal*minVal);
        scaleMatrix(i,i) = 1.0/minVal;
    }
    return scaleMatrix;
}

// check if the clean up of the matrices still leads to a set of matrices whose product is equal to the initial metric.
bool checkNumericalCleanUp(const Eigen::MatrixXd &M, const Eigen::MatrixXd &P, const Eigen::MatrixXd & A, const double epsilon){

    Eigen::MatrixXd M2 = P * A * P.transpose();
    Eigen::MatrixXd nullMatrix = (M - M2).cwiseAbs();

    for(unsigned int i=0; i<(unsigned int)M.rows(); ++i)
        for(unsigned int j=0; j<(unsigned int)M.cols(); ++j)
            if(nullMatrix(i,j) > epsilon)
                return false;

    return true;
}

// check if the clean up of the matrices still leads to a set of matrices whose product is equal to the initial metric.
bool checkNumericalCleanUp(const Eigen::MatrixXd &M,
                           const Eigen::MatrixXd &P,
                           const Eigen::MatrixXd &A,
                           const Eigen::MatrixXd &Pinv,
                           const double epsilon){

    Eigen::MatrixXd M2 = P * A * Pinv;
    Eigen::MatrixXd identity = (M - M2).cwiseAbs();

    for(unsigned int i=0; i<(unsigned int)M.rows(); ++i)
        for(unsigned int j=0; j<(unsigned int)M.cols(); ++j)
            if(identity(i,j) > epsilon)
                return false;

    return true;
}

// when pertinent, replace a supposed integer value by the nearby int, etc.
Eigen::SparseMatrix<double> numericalCleanUpSparse(const Eigen::MatrixXd &M, const double epsilon){

    Eigen::SparseMatrix<double> N(M.rows(),M.cols());

    for(unsigned int i=0; i<(unsigned int)M.rows(); ++i)
        for(unsigned int j=0; j<(unsigned int)M.cols(); ++j){

            // ignore the zero
            if(fabs(M(i,j)) < epsilon)
                continue;

            // round near integers to integers
            int val = std::lround(M(i,j));
            if( fabs(val - M(i,j)) < epsilon ){
                N.insert(i,j) = val;
                continue;
            }

            // if failed to round with integer
            // round with decimal
            for(double d=0; d<=10; ++d){
                double decimal = -0.5 + d/10.0;
                if( fabs(val + decimal - M(i,j)) < epsilon ){
                    N.insert(i,j) = val + decimal;
                    continue;
                }
            }
        }

    return N;
}

// for vectors: when pertinent, replace a supposed integer value by the nearby int, etc.
Eigen::VectorXd vectorNumericalCleanUp(const Eigen::VectorXd& original, const double epsilon){
    Eigen::VectorXd outputVector(original);
    for(unsigned int j=0; j<(unsigned int)outputVector.size(); ++j) {

        // ignore the zero
        if (fabs(original(j)) < epsilon) {
            outputVector(j) = 0.0;
            continue;
        }

        // round near integers to integers
        int val = std::lround(original(j));
        if (fabs(val - original(j)) < epsilon) {
            outputVector(j) = val;
            continue;
        }

        // some negative power of 2
        const int maxNegPower = 7; // 2^{-6} = 0.015625 or the algebra dimension
        const double step = pow(2,-maxNegPower);
        for(double x=val-0.5; x<=val+0.5; x+=step)
            if(fabs(x - original(j)) < epsilon) {
                outputVector(j) = x;
                continue;
            }

        // if failed to round with integer
        // round with decimal
        for(int d=0; d<=10; ++d) {
            double decimal = -0.5 + d / 10.0;
            if (fabs(val + decimal - original(j)) < epsilon) {
                outputVector(j) = val + decimal;
                continue;
            }
        }
    }
    return outputVector;
}

// for matrices: when pertinent, replace a supposed integer value by the nearby int, etc.
Eigen::MatrixXd numericalCleanUp(const Eigen::MatrixXd &M, const double epsilon){

    Eigen::MatrixXd N = M; //Eigen::MatrixXd::Zero(M.rows(),M.cols());

    for(unsigned int i=0; i<(unsigned int)M.rows(); ++i)
        N.row(i) = vectorNumericalCleanUp(N.row(i), epsilon);

    return N;
}

Eigen::SparseMatrix<double, Eigen::ColMajor> computePerGradeTransformationMatrix(const Eigen::MatrixXd &vectorTransformationMatrix,
                                                                                const unsigned int dimension,
                                                                                const unsigned int grade,
                                                                                const double epsilon){
    // Generate grade-vectors
    // i.e. dimension=3
    // generate: (e1+e2+e3) (e1+e2+e3) (e1+e2+e3)
    std::vector<std::vector<unsigned int> > listOfVectors;
    for(unsigned int j=0;j<dimension;++j){
        std::vector<unsigned int> oneVector;
        for(unsigned int i=0; i<dimension; ++i)
            oneVector.push_back((unsigned int) 1 << i);
        listOfVectors.push_back(oneVector);
    }

    // Compute the set of combination whose length is grade, in dimension space (i.e. for grade 2, dim 3: (1,2) (1,3) (2,3) )
    std::vector<std::vector<unsigned int> > sequence = generateCombinations(dimension, grade);

    // init the resulting transformation matrix
    Eigen::SparseMatrix<double, Eigen::ColMajor> resultSparse(sequence.size(), sequence.size());
//    Eigen::MatrixXd kVectorMetric = Eigen::MatrixXd::Zero(sequence.size(), sequence.size()); // to remove

    // link the indices used for the XOR wedge with the index in the sequence of combinations using an array
    // i.e. for grade 2, dim 3 :                (1,2) (1,3) (2,3)
    // order induced by the xor:                scal, 1, 2, 12, 3, 13, 23, 123
    // return their position in the sequence:      -, -, -,  0, -,  1,  2, -
    std::vector<unsigned int> xorIndex2Combination = getSetOfCombinationsFromXorIndexation(dimension, sequence);

    // generate the transformation matrix
    for(unsigned int l=0; l<sequence.size(); ++l) { // for each element of grade "grade'

        std::vector<unsigned int> currentVector = listOfVectors[0];
        std::vector<double>       currentCoeffs; // first line of the transformation matrix

        // fill the currentCoeffs using the transformation
        for(int idxTMat=0; idxTMat<vectorTransformationMatrix.cols(); ++idxTMat)
            currentCoeffs.push_back(vectorTransformationMatrix(sequence[l][0],idxTMat));

        std::vector<unsigned int> nextVector;
        std::vector<unsigned int> result;

        for(unsigned int j=0;j<sequence[l].size()-1;++j) {
            // Contains the number of wedge to be computed,
            // (v1^v2)^v3 for example

            nextVector = listOfVectors[j+1];
            std::vector<unsigned int> resultIdxTmp; // result = mv1 ^ mvss2
            std::vector<double> resultCoeffTmp;     // coefficient of the result

            // wedge between the last k-vector and a vector to make a (k+1)-vector up to 'grade'
            for(unsigned int i=0; i<currentVector.size(); ++i) {
                for(unsigned int k=0; k<nextVector.size(); ++k) {

                    // index initialisation
                    unsigned int mvC = 0;
                    unsigned int mvA = currentVector[i];
                    unsigned int mvB = nextVector[k];

                    // coefficient computation
                    double coeffA = currentCoeffs[i];
                    double coeffB = vectorTransformationMatrix(sequence[l][j+1],k);
                    double coeffC = 0.0; // To be changed with the transformation matrix
                    if((coeffA != 0.0) && (coeffB != 0.0)){
                        getSignAndBladeOuterProduct(mvC, mvA, mvB, coeffC, coeffA, coeffB);
                        // update the results
                        if(coeffC !=0.0) {
                            resultIdxTmp.push_back(mvC);
                            resultCoeffTmp.push_back(coeffC);
                        }
                    }
                }
            }

            currentVector = resultIdxTmp;
            currentCoeffs = resultCoeffTmp;
            resultIdxTmp.clear();
            resultCoeffTmp.clear();
        }

        for(unsigned int idxRes=0; idxRes<currentVector.size(); ++idxRes){
            if(fabs(currentCoeffs[idxRes])>epsilon)
                resultSparse.coeffRef(l,xorIndex2Combination[currentVector[idxRes]]) += currentCoeffs[idxRes];
        }

    }
    return resultSparse;
}

// convert the grade transformation matrix (inverse or not) to a vector of values (row, colums,value). no interpretation are done
std::vector<double> transformationMatricesToVectorOfComponents(
        const Eigen::SparseMatrix<double, Eigen::ColMajor> &transformationMatrix, const int grade, const bool isInverse) {
    std::vector<double> outputVector;
    for (int k = 0; k < transformationMatrix.outerSize(); ++k)
        for (Eigen::SparseMatrix<double, Eigen::ColMajor>::InnerIterator it(transformationMatrix,k); it; ++it) {
            outputVector.push_back((double)it.row());
            outputVector.push_back((double)it.col());
            outputVector.push_back((double)it.value());
        }
    return outputVector;
}

// Compute the inverse of a transformation matrix given by transformationMatrix
Eigen::SparseMatrix<double, Eigen::ColMajor>  computeInverseTransformationMatrix(const Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrix, const double epsilon){
    Eigen::SparseMatrix<double, Eigen::ColMajor> inverseTransformationMatrix(transformationMatrix.rows(),transformationMatrix.rows());
    Eigen::SparseLU<Eigen::SparseMatrix<double, Eigen::ColMajor>> solver;
    solver.analyzePattern(transformationMatrix);
    solver.factorize(transformationMatrix);
    // compute the inverse M.M-1 = Id
    // i.e. solve all system where the unknown vector is a column of M-1
    for(unsigned int j=0; j<(unsigned int)inverseTransformationMatrix.cols(); ++j){
        Eigen::SparseVector<double> vecId(inverseTransformationMatrix.rows());
        vecId.insert(j) = 1.0;
        Eigen::VectorXd x = solver.solve(vecId);
        // recopy into sparse matrix
        for(unsigned i=0; i<(unsigned int)x.size(); ++i)
            if(fabs(x(i)) > epsilon)
                inverseTransformationMatrix.insert(i,j) = x(i);
    }

    return inverseTransformationMatrix;
}

// Compute the transformation matrices for grades ranging from 1 to d
// For each per-grade transformation matrix, we construct a string containing the list of its non-zero elements.
// We also pick up the number of non zero elements of each sparse matrices, the result is put into transformationMatricesSize
std::pair<std::vector<double>,std::vector<double>> computeTransformationMatricesToVector(const Eigen::MatrixXd &P, const double epsilon, std::vector<unsigned int>& transformationMatricesSizes,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& allTransformationMatrices,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& allInverseTransformationMatrices){
    std::pair<std::vector<double>,std::vector<double>> transformationMatrices; // contains non-inverse and inverse transformation matrices

    // push the scalar transformation matrix
    Eigen::SparseMatrix<double, Eigen::ColMajor> spscalarTransformationMatrix(1, 1);
    spscalarTransformationMatrix.insert(0,0)=1.0;
    allTransformationMatrices.push_back(spscalarTransformationMatrix);

    // The number of non-zeros elements of the grade 0 transformation matrix is simply 1
    transformationMatricesSizes.push_back(1);

    // SCALAR transformation matrix to a list of triplets (here only 1) which will be generated afterwards
    std::vector<double> currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spscalarTransformationMatrix, 0, false);
    transformationMatrices.first.insert(std::end(transformationMatrices.first), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));
    currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spscalarTransformationMatrix, 0, true); // inverse transformation matrices
    transformationMatrices.second.insert(std::end(transformationMatrices.second), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));
    allInverseTransformationMatrices.push_back(spscalarTransformationMatrix);

    // push the transformation matrix to the vector of transformation matrices
    Eigen::SparseMatrix<double, Eigen::ColMajor> spVectorTransformationMatrix(P.rows(), P.cols());
    for(unsigned int j=0; j<(unsigned int)P.cols(); ++j)
        for(unsigned int i=0; i<(unsigned int)P.rows(); ++i)
            if(fabs(P(i,j)) >epsilon) // if non-zero value, insert it
                spVectorTransformationMatrix.insert(i,j)=P(i,j);
    allTransformationMatrices.push_back(spVectorTransformationMatrix);

    // extract the number of non-zeros elements of the grade 1transformation matrix
    transformationMatricesSizes.push_back((unsigned int)spVectorTransformationMatrix.nonZeros());

    // convert the grade 1 transformation matrix to a list of triplets which will be generated afterwards
    currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spVectorTransformationMatrix, 0, false);
    transformationMatrices.first.insert(std::end(transformationMatrices.first), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));

    // compute also the inverse transformation matrix
    Eigen::SparseMatrix<double, Eigen::ColMajor> spVectorInverseTransformation = computeInverseTransformationMatrix(spVectorTransformationMatrix, epsilon);

    // and convert it to a list of triplets
    currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spVectorInverseTransformation, 0, true);
    transformationMatrices.second.insert(std::end(transformationMatrices.second), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));
    allInverseTransformationMatrices.push_back(spVectorInverseTransformation);

    // remaining transformation matrices
    for(unsigned int i=2;i<=(unsigned int)P.cols();++i){

        Eigen::SparseMatrix<double, Eigen::ColMajor> spPerGradeTransformation = computePerGradeTransformationMatrix(P, (unsigned int) P.rows(), i, epsilon); // resultMatrix, inputMatrix, dimension, nb vectors to wedge

        // extract the number of non-zeros elements of the current transformation matrix
        transformationMatricesSizes.push_back((unsigned int)spPerGradeTransformation.nonZeros());

        // convert the current transformation matrix to a list of triplets which will be generated afterwards
        currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spPerGradeTransformation, 0, false);
        transformationMatrices.first.insert(std::end(transformationMatrices.first), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));
        allTransformationMatrices.push_back(spPerGradeTransformation);

        // compute also the inverse of the current transformation matrix
        Eigen::SparseMatrix<double, Eigen::ColMajor> spPerGradeInverseTransformation = computeInverseTransformationMatrix(spPerGradeTransformation, epsilon);

        // convert the current transformation matrix to a list of triplets which will be generated afterwards
        currentBasisTransformComponents = transformationMatricesToVectorOfComponents(spPerGradeInverseTransformation, 0, true);
        transformationMatrices.second.insert(std::end(transformationMatrices.second), std::begin(currentBasisTransformComponents), std::end(currentBasisTransformComponents));
        allInverseTransformationMatrices.push_back(spPerGradeInverseTransformation);
    }
    return transformationMatrices;
}


#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <string>
#include <armadillo>
// use namespace for output and input
using namespace std;
using namespace arma;
// object for output files
ofstream ofile;
// Functions used
inline double f(double x) {return 100.0*exp(-10.0*x);}
inline double exact(double x) {return 1.0-(1-exp(-10))*x-exp(-10*x);}

// Begin main program
int main(int argc, char *argv[]){
  int exponent;
    string filename;
    // We read also the basic name for the output file and the highest power of 10^n we want
    if( argc <= 1 ){
          cout << "Bad Usage: " << argv[0] <<
              " read also file name on same line and max power 10^n" << endl;
          exit(1);
    }
        else{
        filename = argv[1]; // first command line argument after name of program
        exponent = atoi(argv[2]);
    }
    // Loop over powers of 10
    for (int i = 1; i <= exponent; i++){
      int  n = (int) pow(10.0,i);
      // Declare new file name
      string fileout = filename;
      // Convert the power 10^i to a string
      string argument = to_string(i);
      // Final filename as filename-i-
      fileout.append(argument);
      double h = 1.0/(n);
      double hh = h*h;
      // Set up arrays for the simple case
      vec d(n+1);  vec solution(n+1);  vec b(n+1);  vec x(n+1);
      // Quick setup of updated diagonal elements and enpoint values of x and b
      x(0) = 0.0; x(n) = 1.0;  b(0) = hh*f(x(0)); b(n) = hh*f(x(n)); d(0) = 2.0;  d(n) = 2.0;
      for (int i = 1; i < n; i++){
	d(i) = (i+1.0)/( (double) i);
        x(i) = i*h;
	b(i) = hh*f(x(i));
      }
     // Forward substitution
      for (int i = 2; i < n; i++) b(i) = b(i) + b(i-1)/d(i-1);
      // Backward substitution
      solution(n-1) = b(n-1)/d(n-1);
      for (int i = n-2; i > 0; i--) solution(i) = (b(i)+solution(i+1))/d(i);
      // Now open file and write out results
      ofile.open(fileout);
      ofile << setiosflags(ios::showpoint | ios::uppercase);
      ofile << "       x:             approx:          exact:       relative error" << endl;
      for (int i = 1; i < n;i++) {
	double RelativeError = fabs((exact(x(i))-solution(i))/exact(x(i)));
	ofile << setw(15) << setprecision(8) << x(i);
	ofile << setw(15) << setprecision(8) << solution(i);
	ofile << setw(15) << setprecision(8) << exact(x(i));
         ofile << setw(15) << setprecision(8) << log10(RelativeError) << endl;
      }
      ofile.close();
    }
    return 0;
}


#include "writer.hpp"
#include <Eigen/Core>
#include <cmath>
#include <functional>
#include <iostream>
#include <stdexcept>

void apply_boundary_conditions(Eigen::ArrayXXd &u, int k) {
    auto N = u.rows();
    u(0, k) = u(1, k);
    u(N - 1, k) = u(N - 2, k);
}

//----------------upwindFDBegin----------------
/// Uses forward Euler and upwind finite differences to compute u from time 0 to
/// time T
///
/// @param[in] u0 the initial conditions in the physical domain, excluding
/// ghost-points.
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
upwindFD(const Eigen::VectorXd &u0,
         double dt,
         double T,
         const std::function<double(double)> &a,
         const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));

    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
// (write your solution here)

    /* Main loop */
// (write your solution here)

    return {std::move(u), std::move(time)};
}
//----------------upwindFDEnd----------------

//----------------centeredFDBegin----------------
/// Uses forward Euler and centered finite differences to compute u from time 0
/// to time T
///
/// @param[in] u0 the initial conditions, as column vector
/// @param[in] dt the time step size
/// @param[in] T the solution is computed for the interval [0, T]. T is assumed
/// to be a multiple of of dt.
/// @param[in] a the advection velocity
/// @param[in] domain left & right limit of the domain
///
/// @return returns the solution 'u' at every time-step and the corresponding
/// time-steps. The solution `u` includes the ghost-points.
std::pair<Eigen::ArrayXXd, Eigen::VectorXd>
centeredFD(const Eigen::VectorXd &u0,
           double dt,
           double T,
           const std::function<double(double)> &a,
           const std::pair<double, double> &domain) {

    auto N = u0.size();
    auto nsteps = int(round(T / dt));
    auto u = Eigen::ArrayXXd(N + 2, nsteps + 1);
    auto time = Eigen::VectorXd(nsteps + 1);

    auto [xL, xR] = domain;
    double dx = (xR - xL) / (N - 1.0);

    /* Initialize u */
// (write your solution here)

    /* Main loop */
// (write your solution here)

    return {std::move(u), std::move(time)};
}
//----------------centeredFDEnd----------------

/* Initial condition: rectangle */
double ic(double x) {
    if (x < 0.25 || x > 0.75)
        return 0.0;
    else
        return 2.0;
}

int main() {
    double T = 2.0;
    double dt = 0.002; // Change this for timestep comparison
    int N = 101;

    double xL = 0.0;
    double xR = 5.0;
    auto domain = std::pair<double, double>{xL, xR};

    auto a = [](double x) { return std::sin(2.0 * M_PI * x); };

    Eigen::VectorXd u0(N);
    double h = (xR - xL) / (N - 1.0);
    /* Initialize u0 */
    for (int i = 0; i < u0.size(); i++) {
        u0[i] = ic(xL + h * i);
    }

    const auto &[u_upwind, time_upwind] = upwindFD(u0, dt, T, a, domain);
    writeToFile("time_upwind.txt", time_upwind);
    writeMatrixToFile("u_upwind.txt", u_upwind);

    const auto &[u_centered, time_centered] = centeredFD(u0, dt, T, a, domain);
    writeToFile("time_centered.txt", time_centered);
    writeMatrixToFile("u_centered.txt", u_centered);
}


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file tylorintegrator.hpp Solution for Problem 3, implementing TaylorIntegrator class

//! \brief Implements an autonomous ODE integrator based on Taylor expansion
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class TaylorIntegrator {
public:
    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a Taylor expansion method
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function (and its derivatives).
    //! \param[in] odefun function handle for rhs f and its derivatives
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &odefun, double T, const State & y0, unsigned int N) const {
        // Iniz step size
        double h = T / N;

        // Will contain all steps, reserve memory for efficiency
        std::vector<State> res;
        res.reserve(N);

        // Store initial data
        res.push_back(y0);

        // Initialize some memory to store temporary values
        State ytemp1 = y0;
        State ytemp2 = y0;
        // Pointers to swap previous value
        State * yold = &ytemp1;
        State * ynew = &ytemp2;

        // Loop over all fixed steps
        for(unsigned int k = 0; k < N; ++k) {
            // Compute, save and swap next step
            step(odefun, h, *yold, *ynew);
            res.push_back(*ynew);
            std::swap(yold, ynew);
        }

        return res;
    }

private:

    //! \brief Perform a single step of the Taylor expansion for the solution of the autonomous ODE
    //! Compute a single explicit step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs and its derivatives.
    //! \param[in] odefun function handle for rhs f and the derivatives
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &odefun, double h, const State & y0, State & y1) const {
        // Compute values for Taylor expansion, including Jacobian and Hessian matrix
        auto fy0 =  odefun.f(y0);
        auto dfy0fy0 =  odefun.df(y0, fy0);
        auto df2y0fy0 =  odefun.df(y0, dfy0fy0);
        auto d2fy0fy0 = odefun.d2f(y0, fy0);

        // Plug values into Taylor expansion for next step
        y1 = y0 + fy0*h + dfy0fy0*h*h/2. + (df2y0fy0 + d2fy0fy0)*h*h*h/6.;
    }

};

Nummerical Methods for CSE/PS12/solutions_ps12/taylorintegrator.hpp


#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>
#include <sm/assert_macros.hpp>
#include <sm/kinematics/rotations.hpp>

namespace sm {
namespace kinematics {

// Euler angle rotations.
Eigen::Matrix3d Rx(double radians) {
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0, 0) = 1;
    C(0, 1) = 0.0;
    C(0, 2) = 0;
    C(1, 0) = 0.0;
    C(1, 1) = c;
    C(1, 2) = -s;
    C(2, 0) = 0.0;
    C(2, 1) = s;
    C(2, 2) = c;

    return C;
}

Eigen::Matrix3d Ry(double radians) {
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0, 0) = c;
    C(0, 1) = 0.0;
    C(0, 2) = s;
    C(1, 0) = 0.0;
    C(1, 1) = 1;
    C(1, 2) = 0.0;
    C(2, 0) = -s;
    C(2, 1) = 0.0;
    C(2, 2) = c;

    return C;
}
Eigen::Matrix3d Rz(double radians) {
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0, 0) = c;
    C(0, 1) = -s;
    C(0, 2) = 0.0;
    C(1, 0) = s;
    C(1, 1) = c;
    C(1, 2) = 0.0;
    C(2, 0) = 0.0;
    C(2, 1) = 0.0;
    C(2, 2) = 1;

    return C;
}

Eigen::Matrix3d rph2R(double x, double y, double z) {
    Eigen::Matrix3d C;
    double cx = cos(x);
    double sx = sin(x);
    double cy = cos(y);
    double sy = sin(y);
    double cz = cos(z);
    double sz = sin(z);
    //[cos(z)*cos(y), -sin(z)*cos(x)+cos(z)*sin(y)*sin(x),  sin(z)*sin(x)+cos(z)*sin(y)*cos(x)]
    //[sin(z)*cos(y),  cos(z)*cos(x)+sin(z)*sin(y)*sin(x), -cos(z)*sin(x)+sin(z)*sin(y)*cos(x)]
    //[      -sin(y),                       cos(y)*sin(x),                       cos(y)*cos(x)]
    C(0, 0) = cz * cy;
    C(0, 1) = -sz * cx + cz * sy * sx;
    C(0, 2) = sz * sx + cz * sy * cx;
    C(1, 0) = sz * cy;
    C(1, 1) = cz * cx + sz * sy * sx;
    C(1, 2) = -cz * sx + sz * sy * cx;
    C(2, 0) = -sy;
    C(2, 1) = cy * sx;
    C(2, 2) = cy * cx;

    return C;
}
Eigen::Matrix3d rph2R(Eigen::Vector3d const& x) { return rph2R(x[0], x[1], x[2]); }

Eigen::Vector3d R2rph(Eigen::Matrix3d const& C) {
    double phi = asin(C(2, 0));
    double theta = atan2(C(2, 1), C(2, 2));
    double psi = atan2(C(1, 0), C(0, 0));

    Eigen::Vector3d ret;
    ret[0] = theta;
    ret[1] = -phi;
    ret[2] = psi;

    return ret;
}

//// Small angle approximation.
template <typename Scalar_>
Eigen::Matrix<Scalar_, 3, 3> crossMx(Scalar_ x, Scalar_ y, Scalar_ z) {
    Eigen::Matrix<Scalar_, 3, 3> C;
    C(0, 0) = 0.0;
    C(0, 1) = -z;
    C(0, 2) = y;
    C(1, 0) = z;
    C(1, 1) = 0.0;
    C(1, 2) = -x;
    C(2, 0) = -y;
    C(2, 1) = x;
    C(2, 2) = 0.0;
    return C;
}
template Eigen::Matrix<double, 3, 3> crossMx(double x, double y, double z);
template Eigen::Matrix<float, 3, 3> crossMx(float x, float y, float z);

template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, 3, 1> > const&);
template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, 3, 1> > const&);

template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, Eigen::Dynamic, 1> > const&);
template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, Eigen::Dynamic, 1> > const&);

// Axis Angle rotation.
Eigen::Matrix3d axisAngle2R(double a, double ax, double ay, double az) {
    SM_ASSERT_LT_DBG(std::runtime_error, fabs(sqrt(ax * ax + ay * ay + az * az) - 1.0), 1e-4,
                     "The axis is not a unit vector. ||a|| = " << (sqrt(ax * ax + ay * ay + az * az)));

    if (a < 1e-12) return Eigen::Matrix3d::Identity();
    // e = [ax ay az]
    // e*(e') + (eye(3) - e*(e'))*cos(a) - crossMx(e) * sin(a) =
    //[         ax^2+ca*(1-ax^2), ax*ay-ca*ax*ay+sa*az, ax*az-ca*ax*az-sa*ay]
    //[ ax*ay-ca*ax*ay-sa*az,         ay^2+ca*(1-ay^2), ay*az-ca*ay*az+sa*ax]
    //[ ax*az-ca*ax*az+sa*ay, ay*az-ca*ay*az-sa*ax,         az^2+ca*(1-az^2)]
    double sa = sin(a);
    double ca = cos(a);
    double ax2 = ax * ax;
    double ay2 = ay * ay;
    double az2 = az * az;
    double const one = double(1);

    Eigen::Matrix3d C;
    C(0, 0) = ax2 + ca * (one - ax2);
    C(0, 1) = ax * ay - ca * ax * ay + sa * az;
    C(0, 2) = ax * az - ca * ax * az - sa * ay;
    C(1, 0) = ax * ay - ca * ax * ay - sa * az;
    C(1, 1) = ay2 + ca * (one - ay2);
    C(1, 2) = ay * az - ca * ay * az + sa * ax;
    C(2, 0) = ax * az - ca * ax * az + sa * ay;
    C(2, 1) = ay * az - ca * ay * az - sa * ax;
    C(2, 2) = az2 + ca * (one - az2);

    return C;
}
Eigen::Matrix3d axisAngle2R(double x, double y, double z) {
    double a = sqrt(x * x + y * y + z * z);
    if (a < 1e-12) return Eigen::Matrix3d::Identity();

    double d = 1 / a;
    return axisAngle2R(a, x * d, y * d, z * d);
}

Eigen::Matrix3d axisAngle2R(Eigen::Vector3d const& x) { return axisAngle2R(x[0], x[1], x[2]); }

Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const& C) {
    // Sometimes, because of roundoff error, the value of tr ends up outside
    // the valid range of arccos. Truncate to the valid range.
    double tr = std::max(-1.0, std::min((C(0, 0) + C(1, 1) + C(2, 2) - 1.0) * 0.5, 1.0));
    double a = acos(tr);

    Eigen::Vector3d axis;

    if (fabs(a) < 1e-10) {
        return Eigen::Vector3d::Zero();
    }

    axis[0] = (C(2, 1) - C(1, 2));
    axis[1] = (C(0, 2) - C(2, 0));
    axis[2] = (C(1, 0) - C(0, 1));
    double n2 = axis.norm();
    if (fabs(n2) < 1e-10) return Eigen::Vector3d::Zero();

    double scale = -a / n2;
    axis = scale * axis;

    return axis;
}

// Utility functions
double angleMod(double radians) { return (double)(radians - (SM_2PI * boost::math::round(radians / SM_2PI))); }
double deg2rad(double degrees) { return (double)(degrees * SM_DEG2RAD); }
double rad2deg(double radians) { return (double)(radians * SM_RAD2DEG); }

Eigen::Matrix3d Cx(double radians) { return Rx(-radians); }
Eigen::Matrix3d Cy(double radians) { return Ry(-radians); }
Eigen::Matrix3d Cz(double radians) { return Rz(-radians); }

Eigen::Matrix3d rph2C(double x, double y, double z) { return rph2R(-x, -y, -z); }

Eigen::Matrix3d rph2C(Eigen::Vector3d const& x) { return rph2C(x[0], x[1], x[2]); }
Eigen::Matrix3d rph2C(Eigen::VectorXd const& x) {
    SM_ASSERT_EQ_DBG(std::runtime_error, x.size(), 3, "The input vector must have 3 components");
    return rph2C(x[0], x[1], x[2]);
}

Eigen::Matrix3d rph2C(Eigen::MatrixXd const& A, unsigned column) {
    SM_ASSERT_EQ_DBG(std::runtime_error, A.rows(), 3, "The input matrix must have 3 rows");
    SM_ASSERT_LT_DBG(std::runtime_error, column, A.cols(), "The requested column is out of bounds");
    return rph2C(A(0, column), A(1, column), A(2, column));
}

Eigen::Vector3d C2rph(Eigen::MatrixXd const& C) {
    SM_ASSERT_EQ_DBG(std::runtime_error, C.rows(), 3, "The input matrix must be 3x3");
    SM_ASSERT_EQ_DBG(std::runtime_error, C.cols(), 3, "The input matrix must be 3x3");

    Eigen::Vector3d rph;

    rph[1] = asin(C(2, 0));
    rph[2] = atan2(-C(1, 0), C(0, 0));
    rph[0] = atan2(-C(2, 1), C(2, 2));

    return rph;
}

Eigen::Vector3d C2rph(Eigen::Matrix3d const& C) {
    Eigen::Vector3d rph;

    rph[1] = asin(C(2, 0));
    rph[2] = atan2(-C(1, 0), C(0, 0));
    rph[0] = atan2(-C(2, 1), C(2, 2));

    return rph;
}

}  // namespace kinematics
}  // namespace sm