#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include <exception>

#include <iostream>
#include <fstream>

#define PI 3.141592653589793238462643383279502884L
#define REGULARIZATION_FACTOR 0.5

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

typedef struct {
  unsigned label;
  Eigen::VectorXd features;
} ClassificationObject;

typedef struct {
  unsigned label;
  double prior;
  Eigen::VectorXd mean;
  Eigen::MatrixXd covariance;
  double covarianceDeterminant;
  Eigen::MatrixXd covarianceInverse;
} ClassInfo;

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

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

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

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

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

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

  return data;
}

ClassInfo
ComputeClassInfo(
  const std::vector<ClassificationObject>& data,
  const unsigned classLabel,
  const unsigned featureDim) {

  ClassInfo classInfo;
  classInfo.label = classLabel;
  classInfo.mean.setZero(featureDim);
  classInfo.covariance.setZero(featureDim, featureDim);

  // compute mean and prior probability at once
  std::vector<ClassificationObject> subset;
  for (const ClassificationObject& classificationObject : data) {
    if (classificationObject.label == classLabel) {
      classInfo.mean += classificationObject.features;
      subset.push_back(classificationObject);
    }
  }
  classInfo.mean /= subset.size();
  classInfo.prior = (double) subset.size() / (double) data.size();

  // covariance matrix
  // Note: I use a nifty trick that is simpler in code (not sure if
  //       simpler computationally). Let $P_i = x_i - mu$, where $i$
  //       corresponds to a particular observation vector. Then
  //       $A$ is the concatenation of all the $P_i$ as column
  //       vectors ($A = [P_1 ... P_m]$). Then
  //       $\frac{1}{m} * A * A^t$ results in the same computations
  //       that create the covariance matrix as more traditional
  //       formulae.
  Eigen::MatrixXd A(featureDim, subset.size());
  for (unsigned j = 0; j < subset.size(); ++j) {
    A.col(j) = subset[j].features - classInfo.mean;
  }
  classInfo.covariance = A * A.transpose();
  classInfo.covariance /= (double) subset.size();
  if (classInfo.covariance.determinant() - 0.1 <= 0.0) {
    Eigen::MatrixXd regularizationMatrix;
    regularizationMatrix.setIdentity(featureDim, featureDim);
    regularizationMatrix *= REGULARIZATION_FACTOR;
    classInfo.covariance += regularizationMatrix;
  }
  classInfo.covarianceDeterminant = classInfo.covariance.determinant();
  classInfo.covarianceInverse = classInfo.covariance.inverse();

  return classInfo;
}

double
GaussianPdf(
  const Eigen::VectorXd& testFeatureVector,
  const ClassInfo& classSummary) {

  const Eigen::VectorXd& x = testFeatureVector;
  const Eigen::VectorXd& mu = classSummary.mean;
  const double& sigmaDet = classSummary.covarianceDeterminant;
  const Eigen::MatrixXd& sigmaInv = classSummary.covarianceInverse;

  double scalingFactor = 1.0 / sqrt(pow(2.0 * PI, mu.size()) * sigmaDet);
  double exponent = -0.5 * (((x - mu).transpose() * sigmaInv).dot((x - mu)));

  return scalingFactor * exp(exponent);
}

unsigned
ClassifyObject(const ClassificationObject& object, const std::vector<ClassInfo>& classSummaries) {
  unsigned mostLikelyClass = 0;
  double highestProbability = classSummaries.front().label;

  for (const ClassInfo& classSummary : classSummaries) {
    double probability = GaussianPdf(object.features, classSummary) * classSummary.prior;
    if (probability > highestProbability) {
      mostLikelyClass = classSummary.label;
      highestProbability = probability;
    }
  }

  return mostLikelyClass;
}

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

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

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

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

  return confusionMatrix;
}

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

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

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

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

	return 0;
}


/*
 *  Example code for fitting a polynomial to sample data (using Eigen 3)
 *
 *  Copyright (C) 2014  RIEGL Research ForschungsGmbH
 *  Copyright (C) 2014  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include <Eigen/QR>
#include <stdio.h>
#include <vector>

void polyfit(const std::vector<double> &xv, const std::vector<double> &yv, std::vector<double> &coeff, int order)
{
	Eigen::MatrixXd A(xv.size(), order+1);
	Eigen::VectorXd yv_mapped = Eigen::VectorXd::Map(&yv.front(), yv.size());
	Eigen::VectorXd result;

	assert(xv.size() == yv.size());
	assert(xv.size() >= order+1);

	// create matrix
	for (size_t i = 0; i < xv.size(); i++)
	for (size_t j = 0; j < order+1; j++)
		A(i, j) = pow(xv.at(i), j);

	// solve for linear least squares fit
	result = A.householderQr().solve(yv_mapped);

	coeff.resize(order+1);
	for (size_t i = 0; i < order+1; i++)
		coeff[i] = result[i];
}

int main()
{
	std::vector<double> x_values, y_values, coeff;
	double x, y;

	while (scanf("%lf %lf\n", &x, &y) == 2) {
		x_values.push_back(x);
		y_values.push_back(y);
	}

	polyfit(x_values, y_values, coeff, 3);
	printf("%f + %f*x + %f*x^2 + %f*x^3\n", coeff[0], coeff[1], coeff[2], coeff[3]);

	return 0;
}


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

#define MATRIX_SIZE 4

using namespace std;
using namespace Eigen;

int main( int argc, char** argv )
{
    /**
    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock();

    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;

    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
    */

    Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random (MATRIX_SIZE, MATRIX_SIZE);
    Eigen::Matrix<double, MATRIX_SIZE, 1>v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);
    clock_t time_stt = clock();
    Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
    cout<<"time used"<<clock()-time_stt<<endl;
    cout<<x.transpose()<<endl;
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout<<x.transpose()<<endl;
    cout<<"time used"<<clock()-time_stt<<endl;

    return 0;
}


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

using namespace Eigen;
using namespace std;

int main() {
  MatrixXf m(2, 2);
  m << 1, -2,
      -3, 4;

  cout << "1-norm(m)     = " << m.cwiseAbs().colwise().sum().maxCoeff()
       << " == " << m.colwise().lpNorm<1>().maxCoeff() << endl;

  cout << "infty-norm(m) = " << m.cwiseAbs().rowwise().sum().maxCoeff()
       << " == " << m.rowwise().lpNorm<1>().maxCoeff() << endl;
}


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

    This example demonstrates the usage of the numerical quadrature function
    integrate_function_adapt_simp().  This function takes as input a single variable
    function, the endpoints of a domain over which the function will be integrated, and a
    tolerance parameter.  It outputs an approximation of the integral of this function over
    the specified domain.  The algorithm is based on the adaptive Simpson method outlined in:

        Numerical Integration method based on the adaptive Simpson method in
        Gander, W. and W. Gautschi, "Adaptive Quadrature – Revisited,"
        BIT, Vol. 40, 2000, pp. 84-101

*/

#include <iostream>
#include <dlib/matrix.h>
#include <dlib/numeric_constants.h>
#include <dlib/numerical_integration.h>

using namespace std;
using namespace dlib;

// Here we the set of functions that we wish to integrate and comment in the domain of
// integration.

// x in [0,1]
double gg1(double x)
{
    return pow(e,x);
}

// x in [0,1]
double gg2(double x)
{
    return x*x;
}

// x in [0, pi]
double gg3(double x)
{
    return 1/(x*x + cos(x)*cos(x));
}

// x in [-pi, pi]
double gg4(double x)
{
    return sin(x);
}

// x in [0,2]
double gg5(double x)
{
    return 1/(1 + x*x);
}

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

int main(int argc, const char** argv)
{
    // We first define a tolerance parameter.  Roughly speaking, a lower tolerance will
    // result in a more accurate approximation of the true integral.  However, there are
    // instances where too small of a tolerance may yield a less accurate approximation
    // than a larger tolerance.  We recommend taking the tolerance to be in the
    // [1e-10, 1e-8] region.

    double tol = 1e-10;

    // Here we compute the integrals of the five functions defined above using the same
    // tolerance level for each.

    double m1 = integrate_function_adapt_simp(&gg1, 0.0, 1.0, tol);
    double m2 = integrate_function_adapt_simp(&gg2, 0.0, 1.0, tol);
    double m3 = integrate_function_adapt_simp(&gg3, 0.0, pi, tol);
    double m4 = integrate_function_adapt_simp(&gg4, -pi, pi, tol);
    double m5 = integrate_function_adapt_simp(&gg5, 0.0, 2.0, tol);

    // We finally print out the values of each of the approximated integrals to ten
    // significant digits.

    cout << "\nThe integral of exp(x) for x in [0,1] is "          << std::setprecision(10) <<  m1  << endl;
    cout << "The integral of x^2 for in [0,1] is "                 << std::setprecision(10) <<  m2  << endl;
    cout << "The integral of 1/(x^2 + cos(x)^2) for in [0,pi] is " << std::setprecision(10) <<  m3  << endl;
    cout << "The integral of sin(x) for in [-pi,pi] is "           << std::setprecision(10) <<  m4  << endl;
    cout << "The integral of 1/(1+x^2) for in [0,2] is "           << std::setprecision(10) <<  m5  << endl;
    cout << endl;

    return 0;
}


#include <iostream>
#include <ctime>
using namespace std;

// for Eigen
#include <Eigen/Core>
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/*
* Demo of basic use for Eigen
*/
int main(int argc, char** argv)
{
	// The basic unit in Eigen is matrix, which is a template class,
	// the first 3 parameters: data type, row, column
	Eigen::Matrix<float, 2, 3> matrix_23;

	// with typedef, Eigen provides many embedded types, but behind
	// it's still Eigen::Matrix
	// e.g. Vector3d is Eigen::Matrix<double, 3, 1>
	// e.g. Matrix3d is Eigen::Matrix<double, 3, 3>
	Eigen::Vector3d v_3d;
	Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); // 0-initialization

	// If not sure about matrix size, it can be dynamically allocated
	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
	// or simplier
	Eigen::MatrixXd matrix_x;

	matrix_23 << 1, 2, 3, 4, 5, 6;
	cout << matrix_23 << endl;

	// use () to access matrix element
	for (int i = 0; i < 1; ++i)
		for (int j = 0; j < 2; ++j)
			cout << matrix_23(i, j) << endl;

	v_3d << 3, 2, 1;

	// multiply matrix and vector - type mixing is wrong
    //Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

	// should cast
	Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
	cout << result << endl;

	// size shouldn't be wrong
    //Eigen::Matrix<double, 3, 1> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

	// some typical matrix operation
    matrix_33 = Eigen::Matrix3d::Random();
	cout << matrix_33 << endl << endl;

	cout << matrix_33.transpose() << endl;
	cout << matrix_33.sum() << endl;
	cout << matrix_33.trace() << endl;
	cout << 10*matrix_33 << endl;
	cout << matrix_33.inverse() << endl;
	cout << matrix_33.determinant() << endl;

	// Eigen
	// real symmetric matrix coudl be guaranteed being diagonalized
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(matrix_33.transpose()
		*matrix_33);
	cout << "Eigen values: " << eigen_solver.eigenvalues() << endl;
	cout << "Eigen vectors: " << eigen_solver.eigenvectors() << endl;

	// solve Ax = b
	Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
	matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
	Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
	v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);

	clock_t time_stt = clock();
	// solve directly, but inverse calculation is consuming
	Eigen::Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
	cout << "time used in normal inverse is: " << 1000*(clock() - time_stt)/
	(double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "Result with normal inverse: " << x.transpose() << endl;

	// matrix decomposition is faster
	time_stt = clock();
	x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
	cout << "time used in QR decomposition is: " << 1000*(clock() - time_stt)/
	(double)CLOCKS_PER_SEC << "ms" << endl;
    cout << "Result with Qr: " << x.transpose() << endl;

	return 0;
}


#pragma once

#include "math.h"
#include <Eigen/Dense>

namespace geodetic_trans {

// Geodetic system parameters
static double earthR = 6378137;
static double earthFlattening = 1 / 298.257223563;

/**
 * convert from radian to degrees
 * @param  radians angle in radian
 * @return         angle in degrees
 */
inline double rad2deg(const double radians) {
	return (radians / M_PI) * 180.0;
}

/**
 * convert from degree to radian
 * @param  degrees angle in degree
 * @return         angle in radian
 */
inline double deg2rad(const double degrees) {
	return (degrees / 180.0) * M_PI;
}

/**
 * convert from LLA position to ECEF position
 * @param lat latitude in [deg]
 * @param lon longitude in [deg]
 * @param alt altitude in [deg]
 * @param x   ECEF x position in [m]
 * @param y   ECEF y position in [m]
 * @param z   ECEF z position in [m]
 */
void lla2ecef(const double lat, const double lon, const float alt,
			  float* x, float* y, float* z) {

	double lat_rad = deg2rad(lat);
	double lon_rad = deg2rad(lon);

	double e2 = (2 - earthFlattening) * earthFlattening;
	double r_N = earthR / sqrt(1 - e2 * sin(lat_rad) * sin(lat_rad));
	*x = (r_N + alt) * cos(lat_rad) * cos(lon_rad);
	*y = (r_N + alt) * cos(lat_rad) * sin(lon_rad);
	*z = (r_N * (1 - e2) + alt) * sin(lat_rad);
}

void ecef2ned(const double ref_lat, const double ref_lon, const float ref_alt,
			 const float x, const float y, const float z,
			 float* north, float* east, float* down) {

	// get the reference LLA position as an ECEF coordinate
	float ref_x, ref_y, ref_z;
	lla2ecef(ref_lat, ref_lon, ref_alt, &ref_x, &ref_y, &ref_z);

	// build the rotation matrix from ECEF to NED for the given reference
	// location
	const double s_lat = sin(deg2rad(ref_lat));
    const double s_lon = sin(deg2rad(ref_lon));
    const double c_lat = cos(deg2rad(ref_lat));
    const double c_lon = cos(deg2rad(ref_lon));

    Eigen::Matrix3d rot_ecef2ned;
    rot_ecef2ned(0, 0) = -s_lat * c_lon;
    rot_ecef2ned(0, 1) = -s_lat * s_lon;
    rot_ecef2ned(0, 2) = c_lat;
    rot_ecef2ned(1, 0) = -s_lon;
    rot_ecef2ned(1, 1) = c_lon;
    rot_ecef2ned(1, 2) = 0.0;
    rot_ecef2ned(2, 0) = c_lat * c_lon;
    rot_ecef2ned(2, 1) = c_lat * s_lon;
    rot_ecef2ned(2, 2) = s_lat;

    // get the vector for the reference point to the current location and rotate
    // it
	Eigen::Vector3d ecef_vec, ned_vec;
	ecef_vec << (x - ref_x), (y - ref_y), (z - ref_z);
	ned_vec = rot_ecef2ned * ecef_vec;
	*north = ned_vec(0);
	*east = ned_vec(1);
	*down = -ned_vec(2);
}

void lla2enu(const double ref_lat, const double ref_lon, const float ref_alt,
			 const double lat, const double lon, const float alt,
			 float* east, float* north, float* up) {

	// Geodetic position to local ENU frame
	float x, y, z;
	lla2ecef(lat, lon, alt, &x, &y, &z);

	float aux_north, aux_east, aux_down;
	ecef2ned(ref_lat, ref_lon, ref_alt, x, y, z, &aux_north, &aux_east, &aux_down);

	*east = aux_east;
	*north = aux_north;
	*up = -aux_down;
}

}; // namespace geodetic_trans


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

using namespace std;
using namespace Eigen;

int main()
{
   Matrix3f A;
   Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   cout << "Here is the matrix A:\n" << A << endl;
   cout << "Here is the vector b:\n" << b << endl;
   Vector3f x = A.colPivHouseholderQr().solve(b);
   cout << "The solution is:\n" << x << endl;
}


#include <gtest/gtest.h>

#include "../Math/math.hh"
#include "helpers.hh"
#include "../geometry/LineSegment/LineSegment2/linesegment2.hh"

#include <Eigen/Core>

using namespace Eigen;
using namespace bold;

TEST (MathTests, intersectRayWithPlane)
{
  // Intersect ray pointing straight downwards
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,0)), Math::intersectRayWithPlane(Vector3d(1,2,3), Vector3d(0,0,-1), Vector4d(0,0,1,0)) );
  // As above, but flip plane (intersect from other side)
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,0)), Math::intersectRayWithPlane(Vector3d(1,2,3), Vector3d(0,0,-1), Vector4d(0,0,-1,0)) );

  // Ray's position is on plane
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,0)), Math::intersectRayWithPlane(Vector3d(1,2,0), Vector3d(0,0,1), Vector4d(0,0,1,0)) );

  // Intersect x-axis with plane perpendicular to x-axis
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,0,0)),
              Math::intersectRayWithPlane(Vector3d(0,0,0), Vector3d(1,0,0), Vector4d(-1,0,0,1)) );

  // Parallel ray doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithPlane(Vector3d(0,0,1), Vector3d(1,0,0), Vector4d(0,0,1,0)) );

  // Coplanar ray doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithPlane(Vector3d(0,0,0), Vector3d(1,0,0), Vector4d(0,0,1,0)) );

  // Ray that points away from plane doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithPlane(Vector3d(0,0,1), Vector3d(0,0,1), Vector4d(0,0,1,0)) );
}

TEST (MathTests, intersectRayWithGroundPlane)
{
  // Intersect ray pointing straight downwards
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,0)), Math::intersectRayWithGroundPlane(Vector3d(1,2,3), Vector3d(0,0,-1), 0) );

  // Ray's position is on plane
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,0)), Math::intersectRayWithGroundPlane(Vector3d(1,2,0), Vector3d(0,0,1), 0) );

  // Parallel ray doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithGroundPlane(Vector3d(0,0,1), Vector3d(1,0,0), 0) );

  // Coplanar ray doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithGroundPlane(Vector3d(0,0,0), Vector3d(1,0,0), 0) );

  // Ray that points away from plane doesn't intersect
  EXPECT_EQ ( Maybe<Vector3d>::empty(),
              Math::intersectRayWithGroundPlane(Vector3d(0,0,1), Vector3d(0,0,1), 0) );

  // Ground plane not at zero
  EXPECT_EQ ( Maybe<Vector3d>(Vector3d(1,2,1)),
              Math::intersectRayWithGroundPlane(Vector3d(1,2,3), Vector3d(0,0,-1), 1) );

  // Diagonal ray
  EXPECT_TRUE ( VectorsEqual(
		  Vector3d(1,1,0),
		  *(Math::intersectRayWithGroundPlane(Vector3d(0,0,1),
						      Vector3d(1,1,-1).normalized(),
						      0))
		  ) );
  EXPECT_TRUE ( VectorsEqual(
		  Vector3d(2,2,0),
		  *(Math::intersectRayWithGroundPlane(Vector3d(0,0,2),
						      Vector3d(1,1,-1).normalized(),
						      0))
		  ) );
}

TEST (MathTests, findPerpendicularVector)
{
  EXPECT_TRUE( VectorsEqual (Vector2d( 0,-1), Math::findPerpendicularVector(Vector2d( 1, 0))) );
  EXPECT_TRUE( VectorsEqual (Vector2d( 0, 1), Math::findPerpendicularVector(Vector2d(-1, 0))) );
  EXPECT_TRUE( VectorsEqual (Vector2d( 1, 0), Math::findPerpendicularVector(Vector2d( 0, 1))) );
  EXPECT_TRUE( VectorsEqual (Vector2d(-1, 0), Math::findPerpendicularVector(Vector2d( 0,-1))) );
}

TEST (MathTests, linePointClosestToPoint)
{
  LineSegment2d unitX(Vector2d(0,0), Vector2d::UnitX());

  EXPECT_TRUE( VectorsEqual (Vector2d(  0, 0), Math::linePointClosestToPoint(unitX, Vector2d(  0, 0))) ) << "At p1";
  EXPECT_TRUE( VectorsEqual (Vector2d(  1, 0), Math::linePointClosestToPoint(unitX, Vector2d(  1, 0))) ) << "At p2";
  EXPECT_TRUE( VectorsEqual (Vector2d(  0, 0), Math::linePointClosestToPoint(unitX, Vector2d(  0, 1))) ) << "Above p1";
  EXPECT_TRUE( VectorsEqual (Vector2d(  0, 0), Math::linePointClosestToPoint(unitX, Vector2d(  0,-1))) ) << "Below p1";
  EXPECT_TRUE( VectorsEqual (Vector2d(0.5, 0), Math::linePointClosestToPoint(unitX, Vector2d(0.5, 1))) ) << "Above midpoint";
  EXPECT_TRUE( VectorsEqual (Vector2d(0.5, 0), Math::linePointClosestToPoint(unitX, Vector2d(0.5,-1))) ) << "Below midpoint";
  EXPECT_TRUE( VectorsEqual (Vector2d(  0, 0), Math::linePointClosestToPoint(unitX, Vector2d( -1, 0))) ) << "Beyond p1";
  EXPECT_TRUE( VectorsEqual (Vector2d(  1, 0), Math::linePointClosestToPoint(unitX, Vector2d(  2, 0))) ) << "Beyond p2";
}

TEST (MathTests, createNormalRng)
{
  double mean = 1.5;
  auto rng = Math::createNormalRng(mean, 1);
  double sum = 0;
  auto loopCount = unsigned{1000};
  for (auto i = unsigned{0}; i < loopCount; i++)
  {
    sum += rng();
  }
  EXPECT_NEAR(mean, sum/loopCount, 0.2);
}

TEST (MathTests, degreesRadiansConversion)
{
  EXPECT_EQ( 0, Math::degToRad(0) );
  EXPECT_EQ( 0, Math::radToDeg(0) );

  EXPECT_EQ( M_PI, Math::degToRad(180) );
  EXPECT_EQ( 180.0, Math::radToDeg(M_PI) );

  EXPECT_EQ( M_PI/2, Math::degToRad(90) );
  EXPECT_EQ( 90.0, Math::radToDeg(M_PI/2) );

  EXPECT_EQ( M_PI/4, Math::degToRad(45) );
  EXPECT_EQ( 45.0, Math::radToDeg(M_PI/4) );

  EXPECT_EQ( -M_PI/4, Math::degToRad(-45) );
  EXPECT_EQ( -45.0, Math::radToDeg(-M_PI/4) );
}

TEST (MathTests, smallestAngleBetween)
{
  EXPECT_NEAR( 0, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(0,1)), 0.0000001 );
  EXPECT_NEAR( 0, Math::smallestAngleBetween(Vector2d(1,1), Vector2d(1,1)), 0.0000001 );
  EXPECT_NEAR( 0, Math::smallestAngleBetween(Vector2d(-1,-1), Vector2d(-1,-1)), 0.0000001 );

  EXPECT_NEAR( M_PI, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(0,-1)), 0.0000001 );
  EXPECT_NEAR( M_PI, Math::smallestAngleBetween(Vector2d(1,1), Vector2d(-1,-1)), 0.0000001 );
  EXPECT_NEAR( M_PI, Math::smallestAngleBetween(Vector2d(1,0), Vector2d(-1,0)), 0.0000001 );

  EXPECT_NEAR( M_PI/2, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(1,0)), 0.0000001 );
  EXPECT_NEAR( M_PI/2, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(-1,0)), 0.0000001 );

  EXPECT_NEAR( M_PI/4, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(1,1)), 0.0000001 );
  EXPECT_NEAR( M_PI/4, Math::smallestAngleBetween(Vector2d(0,1), Vector2d(-1,1)), 0.0000001 );
  EXPECT_NEAR( M_PI/4, Math::smallestAngleBetween(Vector2d(-1,-1), Vector2d(-1,0)), 0.0000001 );
}

TEST (MathTests, alignUp)
{
  auto transform = Affine3d::Identity();

  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(transform).matrix() ) );

  auto rotated = AngleAxisd(.5 * M_PI, Vector3d(0, 0, 1)) * transform;
  EXPECT_TRUE( MatricesEqual( rotated.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(M_PI, Vector3d(0, 0, 1)) * transform;
  EXPECT_TRUE( MatricesEqual( rotated.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(1.5 * M_PI, Vector3d(0, 0, 1)) * transform;
  EXPECT_TRUE( MatricesEqual( rotated.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(2 * M_PI, Vector3d(0, 0, 1)) * transform;
  EXPECT_TRUE( MatricesEqual( rotated.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.64637 * M_PI, Vector3d(0, 0, 1)) * transform;
  EXPECT_TRUE( MatricesEqual( rotated.matrix(), Math::alignUp(rotated).matrix() ) );

  rotated = AngleAxisd(0.1 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.25 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.49 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );

  rotated = AngleAxisd(-0.1 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(-0.25 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(-0.49 * M_PI, Vector3d(1, 0, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );

  rotated = AngleAxisd(0.1 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.25 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.49 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );

  rotated = AngleAxisd(-0.1 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(-0.25 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(-0.49 * M_PI, Vector3d(0, 1, 0)) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );

  rotated = AngleAxisd(0.1 * M_PI, Vector3d(1, 1, 0).normalized()) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.25 * M_PI, Vector3d(1, 1, 0).normalized()) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );
  rotated = AngleAxisd(0.49 * M_PI, Vector3d(1, 1, 0).normalized()) * transform;
  EXPECT_TRUE( MatricesEqual( transform.matrix(), Math::alignUp(rotated).matrix() ) );

}

TEST(MathTests, lerp)
{
  // Ratio based
  EXPECT_NEAR( 0, Math::lerp(0, 0, 1), 0.00001 );
  EXPECT_NEAR( 0, Math::lerp(0, 0, 0), 0.00001 );
  EXPECT_NEAR( 0, Math::lerp(0, 0, 100), 0.00001 );
  EXPECT_NEAR( 5, Math::lerp(0, 5, 100), 0.00001 );

  EXPECT_NEAR( 5, Math::lerp(0.5, 0, 10), 0.00001 );
  EXPECT_NEAR( 5, Math::lerp(0.5, 10, 0), 0.00001 );

  EXPECT_NEAR( 5, Math::lerp(1, 4, 5), 0.00001 );
  EXPECT_NEAR( 20, Math::lerp(2, 0, 10), 0.00001 );
  EXPECT_NEAR( -10, Math::lerp(-1, 0, 10), 0.00001 );

  // Map based
  EXPECT_NEAR( 10, Math::lerp(0, 0, 1, 10, 20), 0.00001 );
  EXPECT_NEAR( 15, Math::lerp(0.5, 0, 1, 10, 20), 0.00001 );
  EXPECT_NEAR( 20, Math::lerp(1, 0, 1, 10, 20), 0.00001 );
  // outside range
  // TODO this is inconsistent with the other lerp function which extends beyond the given domain
  EXPECT_NEAR( 20, Math::lerp(2, 0, 1, 10, 20), 0.00001 ); // other lerp gives 30
  EXPECT_NEAR( 10, Math::lerp(0, 1, 2, 10, 20), 0.00001 ); // other lerp gives 0

  EXPECT_TRUE ( VectorsEqual(Vector2d(0,0), Math::lerp(0.0, Vector2d(0,0), Vector2d(10,10))) );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5,5), Math::lerp(0.5, Vector2d(0,0), Vector2d(10,10))) );
  EXPECT_TRUE ( VectorsEqual(Vector2d(9,9), Math::lerp(0.9, Vector2d(0,0), Vector2d(10,10))) );
}

Quaterniond create(double pitch, double roll, double yaw)
{
  AngleAxisd pitchAngle(pitch, Vector3d::UnitX());
  AngleAxisd rollAngle(roll, Vector3d::UnitY());
  AngleAxisd yawAngle(yaw, Vector3d::UnitZ());

  Quaternion<double> q = rollAngle * pitchAngle * yawAngle;
  return q;
}

TEST(MathTests, normaliseRads)
{
  // Ensures return value is in range [-PI,PI)
  EXPECT_NEAR ( 0.0, Math::normaliseRads(0), 0.0001 );
  EXPECT_NEAR ( -M_PI, Math::normaliseRads(M_PI), 0.0001 );
  EXPECT_NEAR ( -M_PI, Math::normaliseRads(-M_PI), 0.0001 );
  EXPECT_NEAR ( M_PI/2, Math::normaliseRads(M_PI/2), 0.0001 );
  EXPECT_NEAR ( -M_PI/2, Math::normaliseRads(-M_PI/2), 0.0001 );
  EXPECT_NEAR ( 0, Math::normaliseRads(6*M_PI), 0.0001 );
}

TEST(MathTests, angleDiffRads)
{
  EXPECT_NEAR ( 0.0, Math::angleDiffRads(M_PI, M_PI), 0.0001 );
  EXPECT_NEAR ( 0.25 * M_PI, Math::angleDiffRads(0, 0.25 * M_PI), 0.0001 );
  EXPECT_NEAR ( 0.25 * M_PI, Math::angleDiffRads(1.75 * M_PI, 0), 0.0001 );
  EXPECT_NEAR ( M_PI, Math::angleDiffRads(.25 * M_PI, 1.25 * M_PI), 0.0001 );
  EXPECT_NEAR ( M_PI, Math::angleDiffRads(1.25 * M_PI, 0.25 * M_PI), 0.0001 );
  EXPECT_NEAR ( 1.5 * M_PI, Math::angleDiffRads(0, 1.5 * M_PI), 0.0001 );
  EXPECT_NEAR ( 1.5 * M_PI, Math::angleDiffRads(0.5 * M_PI, 0), 0.0001 );
}

TEST(MathTests, shortestAngleDiffRads)
{
  // Assumes input angles are both in range [-PI,PI)
  EXPECT_NEAR ( 0.0, Math::shortestAngleDiffRads(0, 0), 0.0001 );
  EXPECT_NEAR ( 0.1, Math::shortestAngleDiffRads(0, 0.1), 0.0001 );
  EXPECT_NEAR ( -0.1, Math::shortestAngleDiffRads(0.1, 0), 0.0001 );
  EXPECT_NEAR ( 0.0, Math::shortestAngleDiffRads(0, 2*M_PI), 0.0001 );
  EXPECT_NEAR ( 0.0, Math::shortestAngleDiffRads(-M_PI, M_PI), 0.0001 );
  EXPECT_NEAR ( M_PI, Math::shortestAngleDiffRads(-M_PI/2, M_PI/2), 0.0001 );
  EXPECT_NEAR ( -M_PI/2, Math::shortestAngleDiffRads(0, 3*M_PI/2), 0.0001 );
  EXPECT_NEAR ( M_PI/2, Math::shortestAngleDiffRads(0, -3*M_PI/2), 0.0001 );
  EXPECT_NEAR ( 2*M_PI/3, Math::shortestAngleDiffRads(-M_PI/3, M_PI/3), 0.0001 );
  EXPECT_NEAR ( -2*M_PI/3, Math::shortestAngleDiffRads(-2*M_PI/3, 2*M_PI/3), 0.0001 );
  EXPECT_NEAR ( 2*M_PI/3, Math::shortestAngleDiffRads(2*M_PI/3, -2*M_PI/3), 0.0001 );
  EXPECT_NEAR ( -M_PI/3, Math::shortestAngleDiffRads(0, 11*M_PI/3), 0.0001 );
  EXPECT_NEAR ( 2.00605, Math::shortestAngleDiffRads(-2.44379, -0.43774), 0.0001 );
}

TEST(MathTests, angleToPoint)
{
  EXPECT_EQ ( 0, Math::angleToPoint(Vector2d(0, 2)) );
  EXPECT_EQ ( 0, Math::angleToPoint(Vector2d(0, 3)) );
  EXPECT_NEAR (  M_PI/4, Math::angleToPoint(Vector2d(-1, 1)), 0.0001 );
  EXPECT_NEAR ( -M_PI/4, Math::angleToPoint(Vector2d(1, 1)), 0.0001 );
  EXPECT_NEAR (  M_PI/2, Math::angleToPoint(Vector2d(-1, 0)), 0.0001 );
  EXPECT_NEAR ( -M_PI/2, Math::angleToPoint(Vector2d(1, 0)), 0.0001 );
  EXPECT_NEAR ( -M_PI,   Math::angleToPoint(Vector2d(0, -1)), 0.0001 );
}


#ifndef BRUTE_FORCE_HPP
#define BRUTE_FORCE_HPP

#include <boost/multiprecision/cpp_int.hpp>

#include "big_int_type.hpp"
#include "euclidean.hpp"
#include "mod_exponentiation.hpp"

big_int sub_abs(const big_int &x, const big_int &y);

big_int naive_factorization(const big_int &n);

big_int pollard_algorithm(const big_int &n);

big_int brute_force_key(const big_int &e, const big_int &n, bool naive = false);

#endif


#ifndef MATH_FUNCTIONS
#define MATH_FUNCTIONS

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

namespace Math {
class MathFunc {
  typedef Eigen::Matrix<double, 3, 3> RotationMatrix;
  typedef Eigen::Matrix<double, 3, 1> EulerVector;
  typedef Eigen::Matrix<double, 3, 1> Axis;
  typedef Eigen::Matrix<double, 4, 1> Quaternion;

 public:
  template<typename T>
  static inline int getSign(T val) {
    return (T(0) < val) - (val < T(0));
  }

  static inline RotationMatrix skewM(Eigen::Vector3d &vec) {
    RotationMatrix mat;
    mat << 0.0, -vec(2), vec(1),
        vec(2), 0.0, -vec(0),
        -vec(1), vec(0), 0.0;
    return mat;
  }

  static inline RotationMatrix expM(EulerVector &vec) {
    RotationMatrix rot;
    double angle = vec.norm();
    if (angle < 1e-10) return RotationMatrix::Identity();
    Axis axis = vec / angle;
    Eigen::Matrix3d vecSkew = skewM(axis);
    rot = RotationMatrix::Identity() + sin(angle) * vecSkew + (1.0 - cos(angle)) * vecSkew * vecSkew;
    return rot;
  }

  static inline RotationMatrix expM(double angle, Axis &axis) {
    RotationMatrix rot;
    Eigen::Matrix3d vecSkew = skewM(axis);
    rot = RotationMatrix::Identity() + sin(angle) * vecSkew + (1.0 - cos(angle)) * vecSkew * vecSkew;
    return rot;
  }

  template<typename Derived, typename Dtype>
  static inline void limitAbsoluteValue(Eigen::MatrixBase<Derived> &matrix, Dtype threshold) {
    int numberOfElements = matrix.rows() * matrix.cols();
    for (int rowID = 0; rowID < matrix.rows(); rowID++)
      for (int colID = 0; colID < matrix.cols(); colID++)
        if (abs(matrix(rowID, colID)) > threshold)
          matrix(rowID, colID) = getSign(Dtype(1.0)) * threshold;
  }
  static inline Quaternion EulertoQuat(double roll, double pitch,  double yaw)
  {
    Quaternion q;
    double t0 = std::cos(yaw * 0.5);
    double t1 = std::sin(yaw * 0.5);
    double t2 = std::cos(roll * 0.5);
    double t3 = std::sin(roll * 0.5);
    double t4 = std::cos(pitch * 0.5);
    double t5 = std::sin(pitch * 0.5);

    q[0] = t0 * t2 * t4 + t1 * t3 * t5;
    q[1] = t0 * t3 * t4 - t1 * t2 * t5;
    q[2] = t0 * t2 * t5 + t1 * t3 * t4;
    q[3] = t1 * t2 * t4 - t0 * t3 * t5;
    return q;
  }

  static inline void QuattoEuler(const Quaternion& q, double& roll, double& pitch, double& yaw)
  {
    double ysqr = q[2] * q[2];

    // roll (x-axis rotation)
    double t0 = +2.0 * (q[0] * q[1] + q[2] * q[3]);
    double t1 = +1.0 - 2.0 * (q[1] * q[1] + ysqr);
    roll = std::atan2(t0, t1);

    // pitch (y-axis rotation)
    double t2 = +2.0 * (q[0] * q[2] - q[3] * q[1]);
    t2 = t2 > 1.0 ? 1.0 : t2;
    t2 = t2 < -1.0 ? -1.0 : t2;
    pitch = std::asin(t2);

    // yaw (z-axis rotation)
    double t3 = +2.0 * (q[0] * q[3] + q[1] * q[2]);
    double t4 = +1.0 - 2.0 * (ysqr + q[3] * q[3]);
    yaw = std::atan2(t3, t4);
  }

  static inline RotationMatrix quatToRotMat(Quaternion &q) {
    RotationMatrix R;
    R << q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3),
        2 * q(1) * q(2) - 2 * q(0) * q(3),
        2 * q(0) * q(2) + 2 * q(1) * q(3),

        2 * q(0) * q(3) + 2 * q(1) * q(2),
        q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3),
        2 * q(2) * q(3) - 2 * q(0) * q(1),

        2 * q(1) * q(3) - 2 * q(0) * q(2),
        2 * q(0) * q(1) + 2 * q(2) * q(3),
        q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);
    return R;
  }

  static inline Quaternion rotMatToQuat(RotationMatrix &R) {
    Quaternion quat;
    double tr = R.trace();
    if (tr > 0.0) {
      double S = sqrt(tr + 1.0) * 2.0; // S=4*qw
      quat(0) = 0.25 * S;
      quat(1) = (R(2, 1) - R(1, 2)) / S;
      quat(2) = (R(0, 2) - R(2, 0)) / S;
      quat(3) = (R(1, 0) - R(0, 1)) / S;
    } else if ((R(0, 0) > R(1, 1)) & (R(0, 0) > R(2, 2))) {
      double S = sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2.0; // S=4*qx
      quat(0) = (R(2, 1) - R(1, 2)) / S;
      quat(1) = 0.25 * S;
      quat(2) = (R(0, 1) + R(1, 0)) / S;
      quat(3) = (R(0, 2) + R(2, 0)) / S;
    } else if (R(1, 1) > R(2, 2)) {
      double S = sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)) * 2.0; // S=4*qy
      quat(0) = (R(0, 2) - R(2, 0)) / S;
      quat(1) = (R(0, 1) + R(1, 0)) / S;
      quat(2) = 0.25 * S;
      quat(3) = (R(1, 2) + R(2, 1)) / S;
    } else {
      double S = sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)) * 2.0; // S=4*qz
      quat(0) = (R(1, 0) - R(0, 1)) / S;
      quat(1) = (R(0, 2) + R(2, 0)) / S;
      quat(2) = (R(1, 2) + R(2, 1)) / S;
      quat(3) = 0.25 * S;
    }
    return quat;
  }

  static inline Quaternion quatMultiplication(Quaternion &q, Quaternion &p) {
    Quaternion quat;
    quat << p(0) * q(0) - p(1) * q(1) - p(2) * q(2) - p(3) * q(3),
        p(0) * q(1) + p(1) * q(0) - p(2) * q(3) + p(3) * q(2),
        p(0) * q(2) + p(1) * q(3) + p(2) * q(0) - p(3) * q(1),
        p(0) * q(3) - p(1) * q(2) + p(2) * q(1) + p(3) * q(0);
    return quat;
  }

  static inline Quaternion boxplusB_Frame(Quaternion &quat, EulerVector &rotation) {
    Quaternion quat2;
    double norm = rotation.norm();
    double halfNorm = 0.5 * norm;
    double sinHalfNorm = sin(halfNorm);
    quat2 << cos(halfNorm), sinHalfNorm * rotation(0) / norm, sinHalfNorm * rotation(1) / norm, sinHalfNorm
        * rotation(2) / norm;
    return quatMultiplication(quat, quat2);
  }

  static inline Quaternion boxplusI_Frame(Quaternion &quat, EulerVector &rotation) {
    RotationMatrix rotmat = expM(rotation);
    Quaternion quat2 = rotMatToQuat(rotmat);
    return quatMultiplication(quat2, quat);
  }

  static inline void normalizeQuat(Quaternion &q) {
    double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    q[0] = q[0] / norm;
    q[1] = q[1] / norm;
    q[2] = q[2] / norm;
    q[3] = q[3] / norm;
  }

  static inline Quaternion angleAxisToQuat(double angle, Axis &axis) {
    Quaternion quat;
    quat << cos(angle / 2.0), axis * sin(angle / 2.0);
    return quat;
  }

  static inline Quaternion rotateQuatByAngleAxis(Quaternion &q, double angle, Axis &axis) {

//  Quaternion quat;
//  quat = quatMultiplication(angleAxisToQuat(angle, axis), q);
//  return quat;

    EulerVector vector = angle * axis;
    return boxplusI_Frame(q, vector);
  }

  template<typename Dtype>
  static inline Dtype standardDev(Eigen::Matrix<Dtype, 1, -1> &samples) {
    Eigen::Matrix<Dtype, 1, -1> centered = samples.array() - samples.mean();
    return std::sqrt(centered.array().square().sum() / (samples.cols() - 1));
  }

  template<typename Dtype>
  static inline void normalize(Eigen::Matrix<Dtype, -1, 1> &samples) {
    Eigen::Matrix<Dtype, -1, 1> centered = samples.array() - samples.mean();
    Dtype std = std::sqrt(centered.array().square().sum() / (samples.cols() - 1));
    samples = centered / std;
  }

  template<typename Dtype>
  static inline void normalize(Eigen::Matrix<Dtype, 1, -1> &samples) {
    Eigen::Matrix<Dtype, 1, -1> centered = samples.array() - samples.mean();
    Dtype std = std::sqrt(centered.array().square().sum() / (samples.cols() - 1));
    samples = centered / std;
  }

  template<typename Dtype>
  static inline void normalize(std::vector<Dtype> &samples) {
    Eigen::Matrix<Dtype, 1, -1> sampleEigen(1, samples.size());
    memcpy(sampleEigen.data(), &samples[0], sizeof(Dtype) * samples.size());
    normalize(sampleEigen);
    memcpy(&samples[0], sampleEigen.data(), sizeof(Dtype) * samples.size());
  }

  /// gives you the smaller angle difference
  static inline double angleDiff(double a, double b) {
    return M_PI - std::fabs(std::fmod(std::fabs(a - b), 2.0 * M_PI) - M_PI);
  }

  /// gives you the smaller angle difference, positive if a is more clockwise
  static inline double angleDiffSigned(double a, double b) {
    double diff = std::fmod(a - b, 2.0 * M_PI);
    if (diff > 0) {
      return (diff > M_PI) ? diff - 2.0 * M_PI : diff;
    } else {
      return (-diff > M_PI) ? diff + 2.0 * M_PI : diff;
    }
  }

  /// keeping track of the indices. if you do not care about index, use std::sort
  /// assending order
  template<typename Dtype, typename IndType>
  static inline void sort ( std::vector<Dtype>& value, std::vector<IndType>& indx){
    unsigned i, j,  flag = 1;    // set flag to 1 to start first pass
    Dtype temp; // holding variable
    IndType temp2;
    unsigned numLength = value.size( );
    for(i = 1; (i <= numLength) && flag; i++)
    {
      flag = 0;
      for (j=0; j < (numLength -1); j++)
      {
        if (value[j+1] < value[j])      // descending order simply changes to >
        {
          temp = value[j];             // swap elements
          temp2 = indx[j];
          value[j] = value[j+1];
          indx[j] = indx[j+1];
          value[j+1] = temp;
          indx[j+1] = temp2;
          flag = 1;               // indicates that a swap occurred.
        }
      }
    }
  }

  /// keeping track of the indices. if you do not care about index, use std::sort
  /// assending order
  template<typename Dtype, typename IndType>
  static inline void sort ( Eigen::Matrix<Dtype, 1, -1>& value, Eigen::Matrix<IndType, 1, -1>& indx){
    unsigned i, j,  flag = 1;    // set flag to 1 to start first pass
    Dtype temp; // holding variable
    IndType temp2;
    unsigned numLength = value.size( );
    for(i = 1; (i <= numLength) && flag; i++)
    {
      flag = 0;
      for (j=0; j < (numLength -1); j++)
      {
        if (value[j+1] < value[j])      // descending order simply changes to >
        {
          temp = value[j];             // swap elements
          temp2 = indx[j];
          value[j] = value[j+1];
          indx[j] = indx[j+1];
          value[j+1] = temp;
          indx[j+1] = temp2;
          flag = 1;               // indicates that a swap occurred.
        }
      }
    }
  }

};

}

#endif //math functions


#include "goto_solver.h"
#include <Eigen/Dense>
#include <iostream>

static Eigen::MatrixXd A_goto(6,6);

static double t_goto = 0.5;

inline Eigen::MatrixXd A_from_t(double t)
{
    Eigen::MatrixXd A(6,6);
    A << 1.0,0.0,0.0,0.0,0.0,0.0,
    0.0,1.0,0.0,0.0,0.0,0.0,
    0.0,0.0,2.0,0.0,0.0,0.0,
    1,t,pow(t,2),pow(t,3),pow(t,4),pow(t,5),
    0,1,2*t,3*pow(t,2),4*pow(t,3),5*pow(t,4),
    0.0,0.0,2.0,6.0*t,12.0*pow(t,2),20.0*pow(t,3);
    return A;
}

static TrajectoryInfo cal_goto_with_A(double x_init, double y_init, double z_init,
                                    double x_dest, double y_dest, double z_dest,
                                    Eigen::MatrixXd &A, double t)
{
    TrajectoryInfo cur_traj;
    Eigen::VectorXd bx(6),by(6), bz(6), solx(6), soly(6), solz(6);
    bx<< x_init,0.0,0.0,x_dest,0.0,0.0;
    solx = A.colPivHouseholderQr().solve(bx);
    by<< y_init,0.0,0.0,y_dest,0.0,0.0;
    soly = A.colPivHouseholderQr().solve(by);
    bz<< z_init,0.0,0.0,z_dest,0.0,0.0;
    solz = A.colPivHouseholderQr().solve(bz);
    cur_traj.duration = t;
    cur_traj.xcoef[0] = 0;
    cur_traj.xcoef[1] = 0;
    cur_traj.ycoef[0] = 0;
    cur_traj.ycoef[1] = 0;
    cur_traj.zcoef[0] = 0;
    cur_traj.zcoef[1] = 0;
    for(int i = 0; i < 6; i++){
      cur_traj.xcoef[7-i] = solx(i);
      cur_traj.ycoef[7-i] = soly(i);
      cur_traj.zcoef[7-i] = solz(i);
    }
    return cur_traj;
}

TrajectoryInfo cal_goto(double x_init, double y_init, double z_init, double x_dest, double y_dest, double z_dest)
{
    return cal_goto_with_A(x_init, y_init, z_init, x_dest, y_dest, z_dest, A_goto, t_goto);
}

TrajectoryInfo cal_goto_with_t(double x_init, double y_init, double z_init,
                               double x_dest, double y_dest, double z_dest,
                               double t)
{
    Eigen::MatrixXd A = A_from_t(t);
    return cal_goto_with_A(x_init, y_init, z_init, x_dest, y_dest, z_dest, A, t);
}

void set_t_goto(double _t_goto)
{
    t_goto = _t_goto;
    A_goto = A_from_t(t_goto);
}


#include <math.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Geometry>
#include <eigen3/unsupported/Eigen/NumericalDiff>

typedef Eigen::VectorXd vector_t;
typedef Eigen::MatrixXd matrix_t;
typedef Eigen::Transform<double,2,Eigen::Affine> trafo2d_t;

trafo2d_t forward_kinematics(vector_t const & q ) ;

template<typename T>
T pseudoInverse(const T &a, double epsilon = std::numeric_limits<double>::epsilon());

template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>

struct Functor
{
    // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
};

typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

int m_inputs, m_values;

Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }

};

struct numericalDifferentiationFKFunctor : Functor<double>
{
    // Simple constructor
    numericalDifferentiationFKFunctor(): Functor<double>(3,3) {}

    // Implementation of the objective function
    int operator()(const Eigen::VectorXd &q, Eigen::VectorXd &fvec) const
    {
        trafo2d_t t= forward_kinematics(q);
        double theta=atan2( t.rotation()(1,0),t.rotation()(0,0));
        double x = t.translation()(0);
        double y = t.translation()(1);

        fvec(0) = x;
        fvec(1) = y;
        fvec(2) = theta;

        return 0;
    }
};

Eigen::MatrixXd numericalDifferentiationFK(const Eigen::VectorXd &q);

Eigen::VectorXd transformationMatrixToPose(trafo2d_t const &m);

Eigen::VectorXd distanceError(trafo2d_t const &golesStart, trafo2d_t const &poseStart);

template <typename T> inline constexpr
int signum(T x, std::false_type is_signed);

template <typename T> inline constexpr
int signum(T x, std::true_type is_signed);

template <typename  T>
void normaliseAngle(T &q);

template <typename  T>
void normaliseAngle2(T &q);

void normaliseAngle(Eigen::VectorXd &q);

void normaliseAngle2(Eigen::VectorXd &q);

vector_t inverse_kinematics(vector_t const & q_start, trafo2d_t const & goal );

vector_t inverse_kinematics(trafo2d_t const & goal );


/*
 For more information, please see: http://software.sci.utah.edu
 The MIT License
 Copyright (c) 2015 Scientific Computing and Imaging Institute,
 University of Utah.

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

//ComputePCA Algorithm: Computes Principal Component Analysis.

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/Math/ComputePCA.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Eigen/SVD>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>

using namespace SCIRun;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;

//Let's do some math.
//Algorithm:
void ComputePCAAlgo::run(MatrixHandle input, DenseMatrixHandle& LeftPrinMat, DenseMatrixHandle& PrinVals, DenseMatrixHandle& RightPrinMat) const{

    //Throws an error if one or both of the input matrix dimensions is zero.
    if (input->nrows() == 0 || input->ncols() == 0){

        THROW_ALGORITHM_INPUT_ERROR("Input has a zero dimension.");
    }

    //Input matrix: nxm
    if (matrixIs::dense(input))
    {
        //First, we have to center the data.
        auto denseInputCentered = centerData(input);

        //After the data is centered, then we compute SVD on the centered matrix.
        //Centered Matrix = U*S*Vt, Vt = V transpose
        Eigen::JacobiSVD<DenseMatrix::EigenBase> svd_mat(denseInputCentered, Eigen::ComputeFullU | Eigen::ComputeFullV);

        //U: Left principal matrix, nxn, orthogonal
        LeftPrinMat = boost::make_shared<DenseMatrix>(svd_mat.matrixU());

        //S: Principal values nxm, diagonal
        PrinVals = boost::make_shared<DenseMatrix>(svd_mat.singularValues());

        //V: Right singular mxm, orthognol
        RightPrinMat = boost::make_shared<DenseMatrix>(svd_mat.matrixV());
    }
    else
    {
        //Throw an error if the matrix is not dense.
        //Sparse matrices not supported at this time.
        THROW_ALGORITHM_INPUT_ERROR("ComputePCA works for dense matrix input only.");
    }
}

//Centers input matrix.
DenseMatrix ComputePCAAlgo::centerData(MatrixHandle input_matrix)
{
    //Casts the matrix as dense.
    auto denseInput = castMatrix::toDense(input_matrix);

    //Counts the number of rows in the input matrix.
    auto rows = denseInput->rows();

    //Calulates the centering matrix (C).
    // C = Identity(nxn) - 1/n * matrix of ones(nxn)
    auto centerMatrix = Eigen::MatrixXd::Identity(rows,rows) - (1.0/rows)*Eigen::MatrixXd::Constant(rows,rows,1);

    //Multiplying the input matrix by the centering matrix.
    auto denseInputCentered = centerMatrix * *denseInput;

    return denseInputCentered;
}

//Run the algorithm.
AlgorithmOutput ComputePCAAlgo::run(const AlgorithmInput& input) const
{
    auto input_matrix = input.get<Matrix>(Variables::InputMatrix);

    DenseMatrixHandle LeftPrinMat;
    DenseMatrixHandle PrinVals;
    DenseMatrixHandle RightPrinMat;

    run(input_matrix, LeftPrinMat, PrinVals, RightPrinMat);

    AlgorithmOutput output;

    output[LeftPrincipalMatrix] = LeftPrinMat;
    output[PrincipalValues] = PrinVals;
    output[RightPrincipalMatrix] = RightPrinMat;

    return output;
}

//Outputs:
AlgorithmOutputName ComputePCAAlgo::LeftPrincipalMatrix("LeftPrincipalMatrix");
AlgorithmOutputName ComputePCAAlgo::PrincipalValues("PrincipalValues");
AlgorithmOutputName ComputePCAAlgo::RightPrincipalMatrix("RightPrincipalMatrix");


/*
 * p61, exercise 07 for chap 03
 * rbt01, rbt02 poses are known in unnormalized Quaternion form
 * of transformation from world frame to camera frame
 * the goal is find a point position expressed in rbt02 frame with
 * known coordinate in rbt01 frame
*/
#include <iostream>

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

int main(int argc, char** argv)
{
    Eigen::Vector4d q1(0.35, 0.2, 0.3, 0.1);
    q1.normalize();
    // quaternion from 4d vector with order (w, x, y, z)
    Eigen::Quaterniond Q1 = Eigen::Quaterniond(q1);
    // Tij represents the transformation from frame j to frame i
    // 0 usually represents the world frame
    Eigen::Isometry3d T10 = Eigen::Isometry3d::Identity();
    T10.rotate(Q1);
    T10.pretranslate(Eigen::Vector3d(0.3, 0.1, 0.1));

    Eigen::Vector4d q2(-0.5, 0.4, -0.1, 0.2);
    q2.normalize();
    // quaternion from 4d vector with order (w, x, y, z)
    Eigen::Quaterniond Q2 = Eigen::Quaterniond(q2);

    Eigen::Isometry3d T20 = Eigen::Isometry3d::Identity();
    T20.rotate(Q2);
    T20.pretranslate(Eigen::Vector3d(-0.1, 0.5, 0.3));

    Eigen::Vector3d p_in1(0.5, 0, 0.2);
    Eigen::Vector3d p_in2 = T20*T10.inverse()*p_in1;

    std::cout << "the point observed in robo 2 frame is: " << p_in2.transpose() << std::endl;
    return 0;
}


// Eigen
#include <Eigen/Dense>

// OpenCV
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

// Local
#include "estimators/homography_pose_estimator.h"

namespace estimators
{

HomographyPoseEstimator::HomographyPoseEstimator(const Eigen::Matrix3d& K)
    : K_{K}
{ }

PoseEstimate HomographyPoseEstimator::estimate(const std::vector<cv::Point2f>& image_points,
                                               const std::vector<cv::Point3f>& world_points,
                                               const std::vector<float>& matched_distances)
{
  // Set a minimum required number of points,
  // here 3 times the theoretic minimum.
  constexpr size_t min_number_points = 12;

  // Check that we have enough points.
  if (image_points.size() < min_number_points)
  {
    return {};
  }

  // Compute the homography and extract the inliers.
  std::vector<char> inliers;
  cv::Mat H_cv = cv::findHomography(world_points, image_points, cv::RANSAC, 3, inliers);

  std::vector<cv::Point2f> inlier_image_points;
  std::vector<cv::Point3f> inlier_world_points;
  for (size_t i=0; i<inliers.size(); ++i)
  {
    if (inliers[i] > 0)
    {
      inlier_image_points.push_back(image_points[i]);
      inlier_world_points.push_back(world_points[i]);
    }
  }

  // Check that we have enough inliers.
  if (inlier_image_points.size() < min_number_points)
  {
    return {};
  }

  // Convert homography to Eigen matrix.
  Eigen::Matrix3d H;
  cv::cv2eigen(H_cv, H);

  // Compute the matrix M
  Eigen::Matrix3d M;
  M = K_.lu().solve(H);

  // Extract M_bar (the two first columns of M).
  Eigen::MatrixXd M_bar = M.leftCols<2>();

  // Perform SVD on M_bar.
  auto svd = M_bar.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);

  // TODO 3: Compute R_bar.
  // Compute R_bar (the two first columns of R)
  // from the result of the SVD.
  Eigen::Matrix<double, 3, 2> R_bar;
  R_bar = svd.matrixU() * svd.matrixV().transpose();

  // Construct R by inserting R_bar and
  // computing the third column of R from the two first.
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  R.col(0) = R_bar.col(0);
  R.col(1) = R_bar.col(1);
  R.col(2) = R_bar.col(0).cross(R_bar.col(1));

  if (R.determinant() < 0.0)
    R.col(2) *= -1;

  // Compute the scale factor lambda.
  double lambda =
    (R_bar.transpose() * M).trace() /
    (M.transpose() * M).trace();

  // Extract the translation t.
  // Check that this is the correct solution
  // by testing the last element of t.
  Eigen::Vector3d t = lambda * M.col(2);
  if (t.z() < 0)
  {
    t *= -1;
    R.col(0) *= -1;
    R.col(1) *= -1;
  }

  // Return camera pose in the world.
  Sophus::SE3d pose_C_W(R, t);
  return {pose_C_W.inverse(), Eigen::MatrixXd::Zero(6, 6), inlier_image_points, inlier_world_points, std::make_pair(0, 0)};
}

} // namespace estimators


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

    This is an example illustrating the use the general purpose non-linear
    optimization routines from the dlib C++ Library.

    The library provides implementations of the conjugate gradient,  BFGS,
    L-BFGS, and BOBYQA optimization algorithms.  These algorithms allow you to
    find the minimum of a function of many input variables.  This example walks
    though a few of the ways you might put these routines to use.

*/

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

using namespace std;
using namespace dlib;

// ----------------------------------------------------------------------------------------

// In dlib, the general purpose solvers optimize functions that take a column
// vector as input and return a double.  So here we make a typedef for a
// variable length column vector of doubles.  This is the type we will use to
// represent the input to our objective functions which we will be minimizing.
typedef matrix<double,0,1> column_vector;

// ----------------------------------------------------------------------------------------
// Below we create a few functions.  When you get down into main() you will see that
// we can use the optimization algorithms to find the minimums of these functions.
// ----------------------------------------------------------------------------------------

double rosen (const column_vector& m)
/*
    This function computes what is known as Rosenbrock's function.  It is
    a function of two input variables and has a global minimum at (1,1).
    So when we use this function to test out the optimization algorithms
    we will see that the minimum found is indeed at the point (1,1).
*/
{
    const double x = m(0);
    const double y = m(1);

    // compute Rosenbrock's function and return the result
    return 100.0*pow(y - x*x,2) + pow(1 - x,2);
}

// This is a helper function used while optimizing the rosen() function.
const column_vector rosen_derivative (const column_vector& m)
/*!
    ensures
        - returns the gradient vector for the rosen function
!*/
{
    const double x = m(0);
    const double y = m(1);

    // make us a column vector of length 2
    column_vector res(2);

    // now compute the gradient vector
    res(0) = -400*x*(y-x*x) - 2*(1-x); // derivative of rosen() with respect to x
    res(1) = 200*(y-x*x);              // derivative of rosen() with respect to y
    return res;
}

// This function computes the Hessian matrix for the rosen() fuction.  This is
// the matrix of second derivatives.
matrix<double> rosen_hessian (const column_vector& m)
{
    const double x = m(0);
    const double y = m(1);

    matrix<double> res(2,2);

    // now compute the second derivatives
    res(0,0) = 1200*x*x - 400*y + 2; // second derivative with respect to x
    res(1,0) = res(0,1) = -400*x;   // derivative with respect to x and y
    res(1,1) = 200;                 // second derivative with respect to y
    return res;
}

// ----------------------------------------------------------------------------------------

class test_function
{
    /*
        This object is an example of what is known as a "function object" in C++.
        It is simply an object with an overloaded operator().  This means it can
        be used in a way that is similar to a normal C function.  The interesting
        thing about this sort of function is that it can have state.

        In this example, our test_function object contains a column_vector
        as its state and it computes the mean squared error between this
        stored column_vector and the arguments to its operator() function.

        This is a very simple function, however, in general you could compute
        any function you wanted here.  An example of a typical use would be
        to find the parameters of some regression function that minimized
        the mean squared error on a set of data.  In this case the arguments
        to the operator() function would be the parameters of your regression
        function.  You would loop over all your data samples and compute the output
        of the regression function for each data sample given the parameters and
        return a measure of the total error.   The dlib optimization functions
        could then be used to find the parameters that minimized the error.
    */
public:

    test_function (
        const column_vector& input
    )
    {
        target = input;
    }

    double operator() ( const column_vector& arg) const
    {
        // return the mean squared error between the target vector and the input vector
        return mean(squared(target-arg));
    }

private:
    column_vector target;
};

// ----------------------------------------------------------------------------------------

class rosen_model
{
    /*!
        This object is a "function model" which can be used with the
        find_min_trust_region() routine.
    !*/

public:
    typedef ::column_vector column_vector;
    typedef matrix<double> general_matrix;

    double operator() (
        const column_vector& x
    ) const { return rosen(x); }

    void get_derivative_and_hessian (
        const column_vector& x,
        column_vector& der,
        general_matrix& hess
    ) const
    {
        der = rosen_derivative(x);
        hess = rosen_hessian(x);
    }
};

// ----------------------------------------------------------------------------------------

int main()
{
    try
    {
        // make a column vector of length 2
        column_vector starting_point(2);

        // Set the starting point to (4,8).  This is the point the optimization algorithm
        // will start out from and it will move it closer and closer to the function's
        // minimum point.   So generally you want to try and compute a good guess that is
        // somewhat near the actual optimum value.
        starting_point = 4, 8;

        // The first example below finds the minimum of the rosen() function and uses the
        // analytical derivative computed by rosen_derivative().  Since it is very easy to
        // make a mistake while coding a function like rosen_derivative() it is a good idea
        // to compare your derivative function against a numerical approximation and see if
        // the results are similar.  If they are very different then you probably made a
        // mistake.  So the first thing we do is compare the results at a test point:
        cout << "Difference between analytic derivative and numerical approximation of derivative: "
              << length(derivative(rosen)(starting_point) - rosen_derivative(starting_point)) << endl;

        cout << "Find the minimum of the rosen function()" << endl;
        // Now we use the find_min() function to find the minimum point.  The first argument
        // to this routine is the search strategy we want to use.  The second argument is the
        // stopping strategy.  Below I'm using the objective_delta_stop_strategy which just
        // says that the search should stop when the change in the function being optimized
        // is small enough.

        // The other arguments to find_min() are the function to be minimized, its derivative,
        // then the starting point, and the last is an acceptable minimum value of the rosen()
        // function.  That is, if the algorithm finds any inputs to rosen() that gives an output
        // value <= -1 then it will stop immediately.  Usually you supply a number smaller than
        // the actual global minimum.  So since the smallest output of the rosen function is 0
        // we just put -1 here which effectively causes this last argument to be disregarded.

        find_min(bfgs_search_strategy(),  // Use BFGS search algorithm
                 objective_delta_stop_strategy(1e-7), // Stop when the change in rosen() is less than 1e-7
                 rosen, rosen_derivative, starting_point, -1);
        // Once the function ends the starting_point vector will contain the optimum point
        // of (1,1).
        cout << "rosen solution:\n" << starting_point << endl;

        // Now let's try doing it again with a different starting point and the version
        // of find_min() that doesn't require you to supply a derivative function.
        // This version will compute a numerical approximation of the derivative since
        // we didn't supply one to it.
        starting_point = -94, 5.2;
        find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                               objective_delta_stop_strategy(1e-7),
                                               rosen, starting_point, -1);
        // Again the correct minimum point is found and stored in starting_point
        cout << "rosen solution:\n" << starting_point << endl;

        // Here we repeat the same thing as above but this time using the L-BFGS
        // algorithm.  L-BFGS is very similar to the BFGS algorithm, however, BFGS
        // uses O(N^2) memory where N is the size of the starting_point vector.
        // The L-BFGS algorithm however uses only O(N) memory.  So if you have a
        // function of a huge number of variables the L-BFGS algorithm is probably
        // a better choice.
        starting_point = 0.8, 1.3;
        find_min(lbfgs_search_strategy(10),  // The 10 here is basically a measure of how much memory L-BFGS will use.
                 objective_delta_stop_strategy(1e-7).be_verbose(),  // Adding be_verbose() causes a message to be
                                                                    // printed for each iteration of optimization.
                 rosen, rosen_derivative, starting_point, -1);

        cout << endl << "rosen solution: \n" << starting_point << endl;

        starting_point = -94, 5.2;
        find_min_using_approximate_derivatives(lbfgs_search_strategy(10),
                                               objective_delta_stop_strategy(1e-7),
                                               rosen, starting_point, -1);
        cout << "rosen solution: \n"<< starting_point << endl;

        // dlib also supports solving functions subject to bounds constraints on
        // the variables.  So for example, if you wanted to find the minimizer
        // of the rosen function where both input variables were in the range
        // 0.1 to 0.8 you would do it like this:
        starting_point = 0.1, 0.1; // Start with a valid point inside the constraint box.
        find_min_box_constrained(lbfgs_search_strategy(10),
                                 objective_delta_stop_strategy(1e-9),
                                 rosen, rosen_derivative, starting_point, 0.1, 0.8);
        // Here we put the same [0.1 0.8] range constraint on each variable, however, you
        // can put different bounds on each variable by passing in column vectors of
        // constraints for the last two arguments rather than scalars.

        cout << endl << "constrained rosen solution: \n" << starting_point << endl;

        // You can also use an approximate derivative like so:
        starting_point = 0.1, 0.1;
        find_min_box_constrained(bfgs_search_strategy(),
                                 objective_delta_stop_strategy(1e-9),
                                 rosen, derivative(rosen), starting_point, 0.1, 0.8);
        cout << endl << "constrained rosen solution: \n" << starting_point << endl;

        // In many cases, it is useful if we also provide second derivative information
        // to the optimizers.  Two examples of how we can do that are shown below.
        starting_point = 0.8, 1.3;
        find_min(newton_search_strategy(rosen_hessian),
                 objective_delta_stop_strategy(1e-7),
                 rosen,
                 rosen_derivative,
                 starting_point,
                 -1);
        cout << "rosen solution: \n"<< starting_point << endl;

        // We can also use find_min_trust_region(), which is also a method which uses
        // second derivatives.  For some kinds of non-convex function it may be more
        // reliable than using a newton_search_strategy with find_min().
        starting_point = 0.8, 1.3;
        find_min_trust_region(objective_delta_stop_strategy(1e-7),
            rosen_model(),
            starting_point,
            10 // initial trust region radius
        );
        cout << "rosen solution: \n"<< starting_point << endl;

        // Now let's look at using the test_function object with the optimization
        // functions.
        cout << "\nFind the minimum of the test_function" << endl;

        column_vector target(4);
        starting_point.set_size(4);

        // This variable will be used as the target of the test_function.   So,
        // our simple test_function object will have a global minimum at the
        // point given by the target.  We will then use the optimization
        // routines to find this minimum value.
        target = 3, 5, 1, 7;

        // set the starting point far from the global minimum
        starting_point = 1,2,3,4;
        find_min_using_approximate_derivatives(bfgs_search_strategy(),
                                               objective_delta_stop_strategy(1e-7),
                                               test_function(target), starting_point, -1);
        // At this point the correct value of (3,5,1,7) should be found and stored in starting_point
        cout << "test_function solution:\n" << starting_point << endl;

        // Now let's try it again with the conjugate gradient algorithm.
        starting_point = -4,5,99,3;
        find_min_using_approximate_derivatives(cg_search_strategy(),
                                               objective_delta_stop_strategy(1e-7),
                                               test_function(target), starting_point, -1);
        cout << "test_function solution:\n" << starting_point << endl;

        // Finally, let's try the BOBYQA algorithm.  This is a technique specially
        // designed to minimize a function in the absence of derivative information.
        // Generally speaking, it is the method of choice if derivatives are not available.
        starting_point = -4,5,99,3;
        find_min_bobyqa(test_function(target),
                        starting_point,
                        9,    // number of interpolation points
                        uniform_matrix<double>(4,1, -1e100),  // lower bound constraint
                        uniform_matrix<double>(4,1, 1e100),   // upper bound constraint
                        10,    // initial trust region radius
                        1e-6,  // stopping trust region radius
                        100    // max number of objective function evaluations
        );
        cout << "test_function solution:\n" << starting_point << endl;

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}


#include <Eigen/KroneckerProduct>
#include <Eigen/Sparse>
#include <vector>
#include <iostream>
#include <functional>
#include <math.h>
#include <fstream>
#include "colormap.h"
#include <Eigen/IterativeLinearSolvers>

#define COL(a, b, c) colors.push_back(Color(a, b, c));

typedef Eigen::SparseMatrix<double> mat;
typedef Eigen::MatrixXd mat2;
typedef std::vector<std::function<double(double)>> function_array;

double source(double x, double y){
    return 20*sin(M_PI * y)*sin(1.5*M_PI*x+M_PI);
}

mat source_flattened(int Nx, int Ny, double h, std::function<double(double, double)> f){
    auto index = [](int nx, int ny, int Nx){return nx+(Nx-1)*ny;};
    int size = (Nx-1)*(Ny-1);
    mat vec(size, 1);
    std::vector<Eigen::Triplet<double>> vals((Nx-1)*(Nx-1));
    int count = 0;
    for(int j = 1; j < Ny; j++){
        for(int i = 1; i < Nx; i++){
            vals[count] = Eigen::Triplet<double>(count, 0, f(i*h, j*h));
            count++;
        }
    }
    vals.shrink_to_fit();
    vec.setFromTriplets(vals.begin(), vals.end());
    return vec;
}

mat make_eigen(int N, int index,double h){
    mat vec(N, 1);
    vec.insert(index, 0) = 1/h/h;
    return vec;
}

mat fill_from_function(int N, double h, std::function<double(double)> f){
    mat vec(N,1);
    for(int i = 1; i <= N; i++){
        vec.insert(i-1,0) = f(i*h);
    }
    return vec;
}

mat make_boundary_vec(int Nx, int Ny, double h, function_array boundaries){
    mat Bx_lower(Nx-1, 1);
    mat Bx_upper(Nx-1, 1);
    mat By_left(Ny-1, 1);
    mat By_right(Ny-1, 1);

    Bx_lower = fill_from_function(Nx-1, h, boundaries[0]);
    By_right = fill_from_function(Ny-1, h, boundaries[1]);
    Bx_upper = fill_from_function(Nx-1, h, boundaries[2]);
    By_left = fill_from_function(Ny-1, h, boundaries[3]);

    return Eigen::kroneckerProduct(make_eigen(Ny-1, 0, h), Bx_lower).eval()+\
           Eigen::kroneckerProduct(make_eigen(Ny-1, Ny-2, h), Bx_upper).eval()+\
           Eigen::kroneckerProduct(By_left, make_eigen(Nx-1, 0, h)).eval()+\
           Eigen::kroneckerProduct(By_right, make_eigen(Nx-1, Nx-2, h)).eval();
}

std::vector<Eigen::Triplet<double>> make_1D_Laplacian(int N, double h){
    std::vector<Eigen::Triplet<double>> vals(N*3);
    int counter = 0;

    for(int i = 0; i < N; i++){
        vals[counter] = Eigen::Triplet<double>(i, i, 2.f/h/h);
        counter++;
        if(i != 0){
            vals[counter] = Eigen::Triplet<double>(i-1, i, -1.f/h/h);
            counter++;
            vals[counter] = Eigen::Triplet<double>(i, i-1, -1.f/h/h);
            counter++;
        }
    }
    vals.shrink_to_fit();
    return vals;
}

mat build_Matrix(int Nx, int Ny, double h){
    mat Dx(Nx-1, Nx-1);
    mat Dy(Ny-1, Ny-1);

    auto Dx_vals = make_1D_Laplacian(Nx-1, h);
    Dx.setFromTriplets(Dx_vals.begin(), Dx_vals.end());

    auto Dy_vals = make_1D_Laplacian(Ny-1, h);
    Dy.setFromTriplets(Dy_vals.begin(), Dy_vals.end());

    mat Iy(Ny-1,Ny-1);
    Iy.setIdentity();

    mat Ix(Nx-1,Nx-1);
    Ix.setIdentity();

    auto L1 = Eigen::kroneckerProduct(Iy, Dx).eval();
    auto L2 = Eigen::kroneckerProduct(Dy, Ix).eval();

    int size = (Nx-1)*(Ny-1);
    return(L1+L2);
}

double normalize(double val, double max, double min){
    double x = ((val-min)/(max-min));
    return x;
}

int main(int argc, char* argv[]){
    double xmax = 2;
    double ymax = 1;
    double h = atof(argv[1]);
    int Nx = (int)(xmax/h);
    int Ny = (int)(ymax/h);
    std::cout << Nx << ", " << Ny << std::endl;
    std::cout << "Assembling Matrix" << std::endl;
    auto L = build_Matrix(Nx, Ny, h);
    auto source_vec = source_flattened(Nx, Ny, h, std::bind<double>(source, std::placeholders::_1, std::placeholders::_2));

    function_array fns(4);
    fns[0] = std::bind<double>([](double x){return sin(0.5*M_PI*x);}, std::placeholders::_1);
    fns[1] = std::bind<double>([](double x){return sin(2*M_PI*x);}, std::placeholders::_1);
    fns[2] = std::bind<double>([](double x){return 0;}, std::placeholders::_1);
    fns[3] = std::bind<double>([](double x){return sin(2*M_PI*x);}, std::placeholders::_1);

    auto boundary_conds = make_boundary_vec(Nx, Ny, h, fns);

    Eigen::ConjugateGradient<mat, Eigen::Upper|Eigen::Lower> solver;
    std::cout << "Matrix assembled \n Compression Start" << std::endl;
    L.makeCompressed();
    std::cout << "Matrix compressed \nStart Solving Start" << std::endl;
    solver.analyzePattern(L);
    solver.factorize(L);
    std::cout << source_vec.innerSize() << std::endl;
    std::cout << boundary_conds.innerSize() << std::endl;
    std::cout << L.outerSize() << std::endl;
    Eigen::VectorXd un = solver.solve(source_vec+boundary_conds);
    std::cout << "Solve\nMaking Solution" << std::endl;

    double max_val = un.maxCoeff();
    double min_val = un.minCoeff();
    std::cout << "System solved \n writing to file" << std::endl;

    auto norm = std::bind<double>(normalize, std::placeholders::_1, max_val, min_val);

    std::vector<Color> colors;
    COL(47, 0, 135);
    COL(98, 0, 164);
    COL(146, 0, 166);
    COL(186, 47, 138);
    COL(216, 91, 105);
    COL(238, 137, 73);
    COL(246, 189, 39);
    COL(28, 250, 21);

    Colormap cmap(colors);

    std::fstream fs;
    fs.open("default.ppm", std::fstream::out | std::fstream::trunc | std::fstream::in);
    std::cout << fs.is_open() << std::endl;
    //print header to file
    fs << "P3\n"<<(Nx-1)<<" "<<(Ny-1)<<"\n255\n";
    for(int j = (Ny-2); j >= 0; j--){
        for(int i = 0; i < (Nx-1); i++){
            auto val = norm(un.coeff(i+j*(Nx-1),0));
            Color c = cmap.get_val(val);
            fs << c.R << " " << c.G << " " << c.B << " ";
        }
        fs << "\n";

    }
    fs.close();
    std::cout << "File written, Exiting" << std::endl;
}


#include "writer.hpp"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <tuple>
#include <utility>

//----------------stepBegin----------------
//! Does one Forward-Euler timestep of the heat equation
//!
//! @param[out] u at the end, u will contain the values at time t^{n+1}
//! @param[in] uPrevous should contain the values at time t^n
//! @param[in] dr the cell length in r direction
//! @param[in] dt the timestep size
void stepHeatEquation(Eigen::VectorXd &      u,
                      const Eigen::VectorXd &uPrevious,
                      const double           dr,
                      const double           dt) {
	// (write your solution here)
	#pragma omp parallel for if(u.size() > 10000)
	for (int i = 1; i < u.size() - 1; ++i) {
		u(i) = uPrevious(i) + dt / (dr * dr) * ((i + 0.5) * dr * uPrevious(i + 1) - 2 * i * dr * uPrevious(i) + (i - 0.5) * dr * uPrevious(i - 1)) / (i * dr);
	}
	u(0)            = u(1);
	u(u.size() - 1) = 0;
}
//----------------stepEnd----------------

//----------------solveBegin----------------
//! Gives an approximation to the heat equation with the given initial data
//!
//! @param initialData represents the function \tilde{u}_0.
//!
//! @param shouldStop is a function taking as first
//!        parameter the current value of u, and
//!        as second value the current time t.
//!        The simulation should run until shouldStop(u,t) == true. That is
//!
//!        \code{.cpp}
//!        while(!shouldStop(u,t)) { /* Do one more timestep */ }
//!        \endcode
//!
//! @param N the number of inner points
//! @param cfl the constant C with which we choose the timestep size. We set
//!        \code{.cpp}
//!        dt = cfl*dr*dr
//!        \endcode
Eigen::VectorXd solveHeatEquation(const std::function<double(double)> &initialData,
                                  const std::function<bool(const Eigen::VectorXd &, double)> &shouldStop,
                                  const int N,
                                  double    cfl = 0.5) {
	Eigen::VectorXd u1(N + 2), u2(N + 2);
	u1.setZero();

	Eigen::VectorXd &u         = u1;
	Eigen::VectorXd &uPrevious = u2;

	// Set the initial value
	// (write your solution here)

	double t = 0;

	double dr = 1.0 / (N + 1);
	double dt = cfl * dr * dr;

	for (int i = 0; i < u.size() - 1; ++i) {
		u(i) = initialData(i * dr);
	}

	while (!shouldStop(u, t)) {
		// make one step forward.
		// Make sure you swap u and uPrevious
		// accordingly!
		// And update the current time
		// (write your solution here)
		std::swap(u, uPrevious);
		t += dt;
		stepHeatEquation(u, uPrevious, dr, dt);
	}
	// Return the final solution
	return u;
}
//----------------solveEnd----------------

bool stopAtTimeOne(const Eigen::VectorXd &u, double t) {
	std::ignore = u;
	return t > 1;
}

//----------------convergenceBegin----------------
void convergenceStudy() {
	const size_t NReference = (1 << 10) - 2;
	auto initialData        = [](double r) {
		return 1 - r * r * cos(r);
	};

	auto stopAtTime0025 = [](const Eigen::VectorXd &, double t) {
		return t > 0.025;
	};

	std::cout << "Computing reference solution" << std::endl;
	auto uReference = solveHeatEquation(initialData, stopAtTime0025, NReference, 0.5);
	std::cout << "Done computing reference solution" << std::endl;
	std::vector<size_t> resolutions;
	std::vector<double> errors;

	//// NPDE_TEMPLATE_START
	for (int k = 3; k < 10; ++k) {
		const size_t N = (1 << k) - 2;
		auto         u = solveHeatEquation(initialData, stopAtTime0025, N, 0.5);

		double maxError = 0;

		size_t ratioReference = (NReference + 2) / (N + 2);
		for (int i = 0; i < u.rows(); ++i) {
			maxError = std::max(maxError, std::abs(u[i] - uReference[i * ratioReference]));
		}

		resolutions.push_back(N);
		errors.push_back(maxError);
	}
	//// NPDE_TEMPLATE_END
	writeToFile("resolutions.txt", resolutions);
	writeToFile("errors.txt", errors);
}
//----------------convergenceEnd----------------

int main(int, char **) {
	auto initialData = [](double) {
		return 20;
	};

	auto u05 = solveHeatEquation(initialData, stopAtTimeOne, 20, 0.5);
	writeToFile("u_05.txt", u05);

	auto u051 = solveHeatEquation(initialData, stopAtTimeOne, 20, 0.51);
	writeToFile("u_051.txt", u051);

	//----------------maxstopBegin----------------
	// (write your solution here)
	auto stopMax4 = [](const Eigen::VectorXd &u, double t) -> bool {
		if (u.maxCoeff() <= 4.0) {
			std::cout << "Max 4 reached after time t = " << t << std::endl;
			return true;
		}

		return false;
	};

	solveHeatEquation(initialData, stopMax4, 500, 0.5);
	//----------------maxstopEnd----------------

	convergenceStudy();
	return 0;
}


/**
 * @file stabrk3.cc
 * @brief NPDE homework StabRK3 code
 * @author Unknown, Oliver Rietmann, Philippe Peter
 * @date 13.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "stabrk3.h"

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

namespace StabRK3 {

/* SAM_LISTING_BEGIN_0 */
Eigen::Vector2d PredPrey(Eigen::Vector2d y0, double T, unsigned int M) {
  double h = T / M;
  Eigen::Vector2d y = y0;

#if SOLUTION
  // Define right-hand-saide function for Lotka-Volterra ODE
  auto f = [](Eigen::Vector2d y) -> Eigen::Vector2d {
    return {(1 - y(1)) * y(0), (y(0) - 1) * y(1)};
  };
  // Main timstepping loop: uniform stepsize
  for (int j = 0; j < M; ++j) {
    // Compute increments and updates according to \lref{def:rk} for the method
    // described by the Butcher scheme \prbeqref{eq:rkesv}
    Eigen::Vector2d k1 = f(y);
    Eigen::Vector2d k2 = f(y + h * k1);
    Eigen::Vector2d k3 = f(y + (h / 4.) * k1 + (h / 4.) * k2);
    y = y + (h / 6.) * k1 + (h / 6.) * k2 + (2. * h / 3.) * k3;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif

  return y;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
void SimulatePredPrey() {
#if SOLUTION
  // Parameters
  double T = 1.0;
  Eigen::Vector2d y0(100.0, 1.0);

  // (Approximate) reference solution
  Eigen::Vector2d y_ref = PredPrey(y0, T, std::pow(2, 14));

  Eigen::ArrayXd error(12);
  Eigen::ArrayXd M(12);
  // Studying the error for geometrically increasing numbers of equidistant
  // timesteps is the most appropriate approach to empirically exploring
  // algebraic convergence.
  M << 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192;

  // Compute errors
  for (int i = 0; i < M.size(); ++i) {
    Eigen::Vector2d y = PredPrey(y0, T, M(i));
    error(i) = (y - y_ref).norm();
  }
  // Print error table
  PrintErrorTable(M, error);
#else
  //====================
  // Your code goes here
  //====================
#endif
}

void PrintErrorTable(const Eigen::ArrayXd& M, const Eigen::ArrayXd& error) {
  std::cout << std::setw(15) << "N" << std::setw(15) << "error" << std::setw(15)
            << "rate" << std::endl;
  // Formatted output in C++
  for (unsigned int i = 0; i < M.size(); ++i) {
    std::cout << std::setw(15) << M(i) << std::setw(15) << error(i);
    if (i > 0) {
      std::cout << std::setw(15) << std::log2(error(i - 1) / error(i));
    }
    std::cout << std::endl;
  }
}
/* SAM_LISTING_END_1 */

}  // namespace StabRK3


#include <empiricalcpp/src/quadrature.hpp>
#include <cassert>
#include <boost/multi_array.hpp>
#include <Eigen/Dense>

using empirical::Scalar;
using empirical::epsScalar;
using empirical::PI;
using namespace empirical::quadrature;

namespace {

    Quadrature::points_weights_type trapezoid_gen(const Quadrature::size_type N1, const Scalar min, const Scalar max) {
        Quadrature::vector_type points(N1), weights(N1);
        const Quadrature::size_type N = N1 - 1;
        const Scalar step = (max - min) / N;
        const Scalar weight = (max - min) / N;
        for (Quadrature::size_type i = 0; i < N1; i++) {
            points[i] = min + i * step;
            weights[i] = weight;
        }
        weights[0] = weight / 2.0;
        weights[N - 1] = weight / 2.0;
        return Quadrature::points_weights_type({ points, weights });
    }

    Quadrature::points_weights_type periodic_trapezoid_gen(const Quadrature::size_type N1, const Scalar min, const Scalar max) {
        Quadrature::vector_type points(N1), weights(N1);
        const Scalar step = (max - min) / N1;
        const Scalar weight = (max - min) / N1;
        for (Quadrature::size_type i = 0; i < N1; i++) {
            points[i] = (2 * min + step * (2 * i + 1)) / 2;
            weights[i] = weight;
        }
        return Quadrature::points_weights_type({ points, weights });
    }

    Quadrature::points_weights_type lgl_gen(const Quadrature::size_type N1, const Scalar min, const Scalar max) {
        using namespace Eigen;
        typedef Array<Scalar, Dynamic, 1> Vector;
        const int64_t N = N1 - 1;

        Vector x = Vector::LinSpaced(N1, 0, PI).cos();
        Vector xold = Vector::Constant(N1, 1, Scalar(2));
        Array<Scalar, Dynamic, Dynamic> P = Array<Scalar, Dynamic, Dynamic>::Zero(N1, N1);

        while ((x - xold).abs().maxCoeff() > epsScalar) {
            xold = x;
            P.col(0).setOnes();
            P.col(1) = x;

            for (Quadrature::size_type k = 2; k < N1; k++) {
                P.col(k) = (Scalar(2 * k - 1) * x * P.col(k - 1) - Scalar(k - 1) * P.col(k - 2)) / Scalar(k);
            }
            x = xold - (x * P.col(N) - P.col(N - 1)) / (Scalar(N1) * P.col(N));
        }

        // Populate x2 with the weights.
        xold = Scalar(max - min) / (Scalar(N) * Scalar(N1) * P.col(N).square());

        // Reverse the arrays and store them.
        Quadrature::vector_type points(N1);
        Quadrature::vector_type weights(N1);

        const Scalar scale = (max - min) / 2.0;
        const Scalar mid = min + scale;
        for (Quadrature::size_type i = 0; i < N1; i++) {
            points[i] = mid + (x(N - i, 0) * scale);
            weights[i] = xold(N - i, 0);
        }
        return Quadrature::points_weights_type({ points, weights });
    }

}

namespace empirical {
    namespace quadrature {

        Quadrature trapezoid(const Quadrature::size_type N, const Scalar min, const Scalar max) {
            return Quadrature(trapezoid_gen, N, min, max);
        }

        Quadrature periodicTrapezoid(const Quadrature::size_type N, const Scalar min, const Scalar max) {
            return Quadrature(periodic_trapezoid_gen, N, min, max);
        }

        Quadrature legendreGaussLobatto(const Quadrature::size_type N, const Scalar min, const Scalar max) {
            return Quadrature(lgl_gen, N, min, max);
        }
    }
}