/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_SU2_HPP
#define INTERPOL_SU2_HPP

#include <Eigen/Geometry>
#include <iomanip>

/**
 * Sinus cardinalis. Not normed version
 * @param t
 * @return sin(t)/t
 */
template<typename T>
T sinc(T theta) {
    return std::sin(theta) / theta; //si
    //return sin(M_PI*theta) / (M_PI*theta); normed //sinc
}//needed in QuaternionMapping.h

#include <hand_eye_calibration/QuaternionMapping.h>

//using namespace px; //to import logq, expq into global namespace!

//Added to namespace Eigen so that ADL works (otherwise won't compile on CLANG: https://clang.llvm.org/compatibility.html#dep_lookup)
namespace Eigen {
/**
 * Exp map applied to quaternions.
 *
 * The usual exp : so(3) -> SO(3) maps
 * from the Lie algebra (a tangent space) so(3) of skew-symmetric matrices of the from [wx,wy,wz]_x
 * to   the Lie group (a smooth manifold) SO(3) of orthonormal rotation matrices  : R e 3x3 : RR^T=I, |R|=1
 *
 * https://github.com/hengli/vmav-ros-pkg/blob/master/calibration/hand_eye_calibration/include/hand_eye_calibration/QuaternionMapping.h
 *
 * @param q' so(3) tangent space quaternion (if it was unit quaternion before, then its w is 0)
 * @return q = exp(q') SO(3) group quaternion
 */
template<typename T>
inline Eigen::Quaternion <T> expq(const Eigen::Quaternion <T> &q) {
    return px::expq(q);
    /*T a = q.vec().norm();
    T exp_w = std::exp(q.w());

    if (a == T(0)) {
        return Eigen::Quaternion<T>(exp_w, T(0), T(0), T(0));
    }

    Eigen::Quaternion <T> res;
    res.w() = exp_w * T(std::cos(a));
    res.vec() = exp_w * T(sinc(a)) * q.vec();

    return res;*/

//    assertCustom(q.w() == T(0));
//    Eigen::AngleAxis<T> angleAxis;
//    T angle = q.vec().norm();
//    angleAxis.axis() = q.vec() / angle;
//    angleAxis.angle() = angle * T(2.0);
//
//    Eigen::Quaternion<T> res(angleAxis);
//    return res;
}

/**
 * Log map applied to quaternions.
 *
 * If q was a unit quaternion, then w will be 0 in q' (since log(1)=0)
 *
 * The usual log : SO(3) -> so(3) maps
 * from the Lie group (a smooth manifold) SO(3) of orthonormal rotation matrices  : R e 3x3 : RR^T=I, |R|=1
 * to   the Lie algebra (a tangent space) so(3) of skew-symmetric matrices of the from [wx,wy,wz]_x
 *
 * https://github.com/hengli/vmav-ros-pkg/blob/master/calibration/hand_eye_calibration/include/hand_eye_calibration/QuaternionMapping.h
 *
 * @param q SO(3) group quaternion
 * @return q' = log(q) so(3) tangent space quaternion
 */
template<typename T>
inline Eigen::Quaternion <T> logq(Eigen::Quaternion <T> q) {
    return px::logq(q);
    /*

    //if(q.w()<T(0)) q.w() *=T(-1);

    T exp_w = q.norm();
    T w = std::log(exp_w);
    T a = std::acos(q.w() / exp_w);

    if (a == T(0)) {
        return Eigen::Quaternion<T>(w, T(0), T(0), T(0));
    }

    Eigen::Quaternion <T> res;
    res.w() = w;
    res.vec() = q.vec() / exp_w / (sinc(a));

    return res;//.normalized();
    */

//    assertCustom(abs(q.norm() - T(1)) < T(0.1));
//    Eigen::AngleAxis<T> angleAxis(q);
//    Eigen::Quaternion <T> res;
//    res.w() = T(0);
//    res.vec() = angleAxis.axis() * angleAxis.angle() * T(0.5);
//    return res;
}

//printing of Quaternion, Vector3 and Map<Vector3> works with this (even for Ceres Jet types)
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::QuaternionBase<T> &q) {
    return stream << std::setprecision(2) << std::fixed << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
}
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::Matrix<T,3,1> &q) {
    return stream << std::setprecision(2) << std::fixed << q.x() << ", " << q.y() << ", " << q.z();
}
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::Map<Eigen::Matrix<T, 3, 1>> &q) {
    return stream << std::setprecision(2) << std::fixed << q.x() << ", " << q.y() << ", " << q.z();
}

/**
 * Addition of quaternion coefficients.
 * \warning This operation has no direct geometric interpretation, and in the general case result in a non unit quaternion.
 * @return the addition of \c *this and \a other
 */
template<typename T>
static Eigen::Quaternion <T> operator+(const Eigen::Quaternion <T> &a, const Eigen::Quaternion <T> &b) {
    return Eigen::Quaternion<T>(a.coeffs() + b.coeffs());
}
/**
 * Substraction of quaternion coefficients.
 * \warning This operation has no direct geometric interpretation, and in the general case result in a non unit quaternion.
 * @return the substraction of \c *this and \a other
 */
template<typename T>
static Eigen::Quaternion <T> operator-(const Eigen::Quaternion <T> &a, const Eigen::Quaternion <T> &b) {
    return Eigen::Quaternion<T>(a.coeffs() - b.coeffs());
}
/**
 * Lhs multiplication of quaternion coefficients with a scalar.
 * Only useful in log space of unit quaternion (when w==0)
 */
template<typename T>
static Eigen::Quaternion <T> operator*(const T &b, const Eigen::Quaternion <T> &a) {
    return Eigen::Quaternion<T>(a.coeffs() * b);
}
/**
 * Rhs multiplication of quaternion coefficients with a scalar.
 * Only useful in log space of unit quaternion (when w==0)
 */
template<typename T>
static Eigen::Quaternion <T> operator*(const Eigen::Quaternion <T> &a, const T &b) {
    return Eigen::Quaternion<T>(a.coeffs() * b);
};

} // ns Eigen

#endif // INTERPOL_SU2_HPP


//
// This program is used to study how to calculate (SS|SS)^{m}
// for the recursive relation. Basically, it focus on the
// fmt function:
// f_{m}(t) = \int^{1}_{0} u^{2m} e^{-tu^{2}} du
// it needs the boost library to calcualte fmt
// fenglai liu
// Oct. 2013
//
// for reference, see Harris paper and the reference cited inside:
// Harris, Frank E
// Evaluation of GTO molecular integrals
// International Journal of Quantum Chemistry, 1983, Vol. 23, Page 1469--1478
//
// note:
// we found that the approximation of (2m-1)!!/R expression is
// not numerical stable. Combined with down recursive relation,
// it introduces greater error compared with other methods.
// Therefore we finally abadon it.
//
//
//

// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>

// common C++ head files may be used in the program
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<vector>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>

#include <boost/math/special_functions/binomial.hpp>  // special functions in math
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/timer.hpp>
using namespace boost::math;
using namespace boost;
using namespace std;
#define PI              3.1415926535897932384626E0

double fm(const double& a, const int& m);
double doubleFac(const int& m, const int& n);

double fm(const double& a, const int& m) {
	double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	double f12 = 1.0E0/2.0E0;
	double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

double doubleFac(const int& m, const int& n) {
	// calculate (2m-1)!!/(2m+2n+1)!!
	double x = 1.0E0;
	for(int i=0; i<=n; i++) {
		x *= 1.0E0/(2*m+2*i+1);
	}
	return x;
}

int main()
{
	///////////////////////////////////////////////////
	// global settings
	// step length and number of claculation for
	// time performance
	// global error is the error range
	// if the difference is less than the error, we
	// think the error could be omitted
	///////////////////////////////////////////////////
	int N = 10000000;
	double steplength  = 0.000001;
	double globalError = 1.0E-10;
	int M_limmit       = 10;
	int top_M_limmit   = 30;
	double T_max_limit = 36.0E0;
	double T_min_limit = 1.1E0;

	float steplength_f  = 0.00001;
	float globalError_f = 1.0E-6;

	/*
	///////////////////////////////////////////////////
	// this section testing the timing performance
	// between the different functions
	// so far, erf is 160 faster than the fmt function
	// in boost library
	///////////////////////////////////////////////////
	cout << "===========================================================" << endl;
	cout << "Time performance result: " << endl;
	cout << "Total sample number is " << N << endl;
	cout << "===========================================================" << endl;
	// testing the erf
	timer t1;
	for(int i=0; i<N; i++) {
		double a = i;
		double x = a/1000;
		double e = erf(x);
	}
	printf("erf time consuming: %-14.7f\n", t1.elapsed());

	// testing the sqrt
	timer t2;
	for(int i=0; i<N; i++) {
		double e = sqrt(i);
	}
	printf("sqrt time consuming: %-14.7f\n", t2.elapsed());

	// testing the exp
	timer t3;
	for(int i=0; i<N; i++) {
		double T = -(i+1)/N;
		double e = exp(T);
	}
	printf("exp(-T) time consuming: %-14.7f\n", t3.elapsed());

	// testing the fmt
	timer t4;
	for(int m=0; m<25; m++) {
		int n = N/25;
		for(int i=0; i<n; i++) {
			double T = 0.0001*(i+1);
			double e = fm(T,m);
		}
	}
	printf("fm(T,m) time consuming: %-14.7f\n", t4.elapsed());
	*/

	/*
	///////////////////////////////////////////////////
	// this section testing the up recursice relation
	// can we do it for m = 1 to 12?
	// 10 is perfect fine, however; 12 will have
	// error around 1.0E-10
	// we note, that up recursive relation can not
	// go with float
	///////////////////////////////////////////////////
	int numSteps = T_max_limit/steplength;
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the up recursive relation for m = 1 to " << M_limmit << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from " << T_min_limit << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	for (int j = 0; j<numSteps; j++) {
		double T = T_min_limit+steplength*(j+1);
		double sqT = sqrt(T);
		double fmt= erf(sqT)*sqrt(PI)/2.0E0;
		double et = exp(-T);
		double e0 = fmt/sqT;
		double oneO2T = 0.5E0/T;
		for(int i=1; i<=M_limmit; i++) {
			double s = fm(T,i);
			double e = oneO2T*((2*i-1)*e0-et);
			double d = fabs(s-e);
			if (d>globalError) {
				printf("fmt is %-14.12f\n", s);
				printf("T   is %-14.12f\n", T);
				printf("error/fmt is %-14.12f\n", d/fmt);
				printf("for m=%d difference is %-14.12f\n", i, d);
			}
			e0 = e;
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for double type variable" << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	int nSteps = T_max_limit/steplength;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			double T = steplength*(j+1);
			double fmt= fm(T,n);
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double s = fm(T,i);
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				double d = fabs(s-e);
				if (d>globalError) {
					printf("T is %-14.12f\n", T);
					printf("for m=%d difference is %-14.12f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for float type variable" << endl;
	cout << "error range is " << globalError_f << endl;
	cout << "step length for T is " << steplength_f << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	nSteps = T_max_limit/steplength_f;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			float T = steplength_f*(j+1);
			float fmt= fm(T,n);
			float et = exp(-T);
			float e0 = fmt;
			float t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				float s = fm(T,i);
				float e = (1.0/(2*i+1.0))*(t2*e0+et);
				float d = fabs(s-e);
				if (d>globalError_f) {
					printf("T is %-10.7f\n", T);
					printf("for m=%d difference is %-10.7f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	// this is not so accurate
	// abandoned, too
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to 10" << endl;
	cout << "the fm(t) is approximated by (2m-1)!!/(2t)^{m}*t^{1/2}" << endl;
	cout << "error range is 1.0E-10" << endl;
	cout << "step length for T is " << steplength << " from 30.0 to 50.0" << endl;
	cout << "===========================================================" << endl;
	int numSteps = (50-30)/steplength;
	double globalError = 1.0E-9;
	for (int n=1; n<=10; n++) {
		for (int j = 0; j<numSteps; j++) {

			// calculate fm(t)
			double T = 30.0E0+steplength*(j+1);
			double fmt= sqrt(PI)/(2*sqrt(T));
			for(int k=n; k>=1; k--) {
				fmt *= (2*k-1)/(2.0*T);
			}
			double s0 = fm(T,n);
			double d0 = fabs(s0-fmt);
			if (d0>globalError) {
				printf("T is %-14.12f\n", T);
				printf("error/fmt is %-14.12f\n", d0/fmt);
				printf("for m=%d difference is %-14.12f\n", n, d0);
			}

			// recursive relation
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double s = fm(T,i);
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				double d = fabs(s-e);
				if (d>globalError) {
					printf("T is %-14.12f\n", T);
					printf("error/fmt is %-14.12f\n", d/s);
					printf("for m=%d difference is %-14.12f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the power series
	// equation of 9 in Harris paper
	// We try to expand the power series up to 10 terms
	// here we test from m=1 to m=10
	// for T from 0 to 1.1
	///////////////////////////////////////////////////
	numSteps = T_min_limit/steplength;
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the power series expansion with 13 terms" << endl;
	cout << "this is for double type variable" << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 to " << T_min_limit << endl;
	cout << "===========================================================" << endl;
	for (int m = 1; m<=M_limmit; m++) {
		for (int j = 0; j<numSteps; j++) {
			double T = steplength*(j+1);
			double et = exp(-T);
			double x  = 1.0;
			double t2 = 2.0*T;
			for(int i=12; i>=1; i--) {
				x = 1.0+t2/(2.0*(m+i)+1.0)*x;
			}
			x  = (1.0/(2*m+1))*x;
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>globalError) {
				printf("fmt is %-16.14f\n", result);
				printf("for m=%d T=%f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	/*
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the power series expansion with 13 terms" << endl;
	cout << "this is for float type variable" << endl;
	cout << "error range is " << globalError_f << endl;
	cout << "step length for T is " << steplength_f << endl;
	cout << "T is ranging from 0 to " << T_min_limit << endl;
	cout << "===========================================================" << endl;
	numSteps = T_min_limit/steplength_f;
	for (int m = 1; m<=M_limmit; m++) {
		for (int j = 0; j<numSteps; j++) {
			float T  = steplength_f*(j+1);
			float et = exp(-T);
			float x  = 1.0;
			float t2 = 2.0*T;
			for(int i=12; i>=1; i--) {
				x = 1.0+t2/(2.0*(m+i)+1.0)*x;
			}
			x  = (1.0/(2*m+1))*x;
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>globalError_f) {
				printf("fmt is %-16.14f\n", result);
				printf("for m=%d T=%f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	////////////////////////////////////////////////////////
	// this is the continuous fraction implementation
	// of Harris equation, not correct yet, already abandoned
	// equation 24
	// testing the series
	// for m = 0, and T is from 0.01 to 25
	// test the convergence for the series
	////////////////////////////////////////////////////////
	/*
	int n = 20;
	for (int m=0; m<=30; m++) {
		for (int j = 0; j<2500; j++) {
			double T = 0.01*(j+1);
			double et = exp(-T);
			double minusn = 1.0E0;
			if ((n+1)%2 == 1) minusn = -1.0E0;
			double an = 2.0*(minusn*(m+n)-(m+1))/(4.0*(m+n)*(m+n)-1);
			double x  = 1+an*T;
			for(int i=n-1; i>1; i--) {
				double minusi = 1.0E0;
				if ((i+1)%2 == 1) minusi = -1.0E0;
				double ai = 2.0*(minusi*(m+i)-(m+1))/(4.0*(m+i)*(m+i)-1);
				x = (1+ai*T)/x;
			}
			double a1 = 2.0/(4.0*(m+1)*(m+1)-1);
			x = a1*T/x;
			x += 1/(2*m+1);
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>0.00000000000001) {
				printf("for %d %f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	return 0;
}


#include "EigenUtilities.h"

#include <Error.h>
#include <Eigen/Dense>

#include <iostream>

// Keep these utilities separate as using Eigen in an existing source
// file results in a 50% increase in compilation time.

// Compute the best-fitting projective transform that maps a set of 3D points
// to 2D points.
// A best-fit linear transform could be created by eliminating the denominators below.
void usgscsm::computeBestFitProjectiveTransform(std::vector<csm::ImageCoord> const& imagePts,
                                                std::vector<csm::EcefCoord>  const& groundPts,
                                                std::vector<double> & transformCoeffs) {

  if (imagePts.size() != groundPts.size())
    throw csm::Error(csm::Error::INVALID_USE,
                     "The number of inputs and outputs must agree.",
                     "computeBestFitProjectiveTransform");

  int numPts = imagePts.size();
  if (numPts < 8)
    throw csm::Error(csm::Error::INVALID_USE,
                     "At least 8 points are needed to fit a 3D-to-2D projective transform. "
                     "Ideally more are preferred.",
                     "computeBestFitProjectiveTransform");

  int numMatRows = 2 * numPts; // there exist x and y coords for each point
  int numMatCols = 14; // Number of variables in the projective transform

  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(numMatRows, numMatCols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(numMatRows);

  for (int it = 0; it < numPts; it++) {

    double x = groundPts[it].x, y = groundPts[it].y, z = groundPts[it].z;
    double r = imagePts[it].line, c = imagePts[it].samp;

    // If the solution coefficients are u0, u1, ..., must have:

    // (u0 + u1 * x + u2 * y + u3  * z) / (1 + u4  * x + u5  * y + u6  * z) = r
    // (u7 + u8 * x + u9 * y + u10 * z) / (1 + u11 * x + u12 * y + u13 * z) = c

    // Multiply by the denominators. Get a linear regression in the coefficients.

    M.row(2 * it + 0) << 1, x, y, z, -x * r, -y * r, -z * r, 0, 0, 0, 0, 0, 0, 0;
    M.row(2 * it + 1) << 0, 0, 0, 0, 0, 0, 0, 1, x, y, z, -x * c, -y * c, -z * c;

    b[2 * it + 0] = r;
    b[2 * it + 1] = c;
  }

  // Solve the over-determined system, per:
  // https://eigen.tuxfamily.org/dox/group__LeastSquares.html
  Eigen::VectorXd u = M.colPivHouseholderQr().solve(b);

  // Copy back the result to a standard vector (to avoid using Eigen too much as
  // that is slow to compile).
  transformCoeffs.resize(numMatCols);
  for (int it = 0; it < numMatCols; it++)
    transformCoeffs[it] = u[it];

  return;
}


#include <armadillo>

using namespace std;
using namespace arma;

double f1(double x)
{
    return -x * sin(sqrt(abs(x)));
}

double f1_x(double x)
{
    return -sin(sqrt(abs(x))) - (x * x * cos(sqrt(abs(x)))) / (2 * pow(abs(x), 3.0 / 2));
}

double f2(double x)
{
    return x + 5 * sin(3 * x) + 8 * cos(5 * x);
}

double f2_x(double x)
{
    return -40 * sin(5 * x) + 15 * cos(3 * x) - 1;
}

double f3(double x, double y)
{
    return -(pow(x * x + y * y, 0.25) * (pow(sin(50 * pow(x * x + y * y, 0.1)), 2)) + 1);
}

double f3_x(double x, double y)
{
    return -(0.5 * x * (pow(sin(50 * pow(y * y + x * x, 0.1)), 2) + 1))
               / pow(y * y + x * x, 0.75)
           - (20.0
              * x
              * cos(50 * pow(y * y + x * x, 0.1))
              * sin(50 * pow(y * y + x * x, 0.1)))
                 / pow(y * y + x * x, 0.65);
}

double f3_y(double x, double y)
{
    return -(0.5
             * y
             * (pow(sin(50 * pow(y * y + x * x, 0.1)), 2) + 1))
               / pow(y * y + x * x, 0.75)
           - (20.0
              * y
              * cos(50 * pow(y * y + x * x, 0.1))
              * sin(50 * pow(y * y + x * x, 0.1)))
                 / pow(y * y + x * x, 0.65);
}

double gradienteDescendente(std::function<double(double)> f_prima,
                            pair<double, double> rangoBusqueda,
                            double alpha)
{
    const double rango = rangoBusqueda.second - rangoBusqueda.first;

    double x = randu(1).eval()(0) * rango + rangoBusqueda.first;

    //    cout << "x = " << x << endl
    //         << "f'(x) =" << f_prima(x) << endl
    //         << "|f'(x)| =" << abs(f_prima(x)) << endl;

    while (abs(f_prima(x)) > 1e-5) {
        x = x - alpha * f_prima(x);

        //        cout << "x = " << x << endl
        //             << "f'(x) =" << f_prima(x) << endl
        //             << "|f'(x)| =" << abs(f_prima(x)) << endl;
    }

    return x;
}

pair<double, double> gradienteDescendente(std::function<double(double, double)> f_x,
                                          std::function<double(double, double)> f_y,
                                          array<pair<double, double>, 2> rangosBusqueda,
                                          double alpha)
{
    const double rangoX = rangosBusqueda.at(0).second - rangosBusqueda.at(0).first;
    const double rangoY = rangosBusqueda.at(1).second - rangosBusqueda.at(1).first;

    double x = randu(1).eval()(0) * rangoX + rangosBusqueda.at(0).first;
    double y = randu(1).eval()(0) * rangoY + rangosBusqueda.at(1).first;

    while (abs(f_x(x, y)) > 1e-5 && abs(f_y(x, y)) > 1e-5) {
        x = x - alpha * f_x(x, y);
        y = y - alpha * f_y(x, y);
    }

    return {x, y};
}


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

#include "stabrk3.h"

#include <Eigen/Core>
#include <vector>

#include "rkintegrator.h"

namespace StabRK3 {

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

  auto f = [](Eigen::Vector2d y) {
    Eigen::Vector2d y_dot;
    y_dot(0) = (1 - y(1)) * y(0);
    y_dot(1) = (y(0) - 1) * y(1);
    return y_dot;
  };

  for (int j = 0; j < N; ++j) {
    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;
  }

  return y;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
std::vector<Eigen::Vector2d> simulatePredPrey(
    const std::vector<unsigned int> &N_list) {
  int M = N_list.size();
  std::vector<Eigen::Vector2d> yT_list(M);

  // Initialize RK with Butcher table
  Eigen::Matrix3d A;
  A << 0, 0, 0, 1., 0, 0, 1. / 4., 1. / 4., 0;
  Eigen::Vector3d b(1.0 / 6.0, 1.0 / 6.0, 2.0 / 3.0);
  RKIntegrator<Eigen::Vector2d> integrator(A, b);

  // RHS function f for the prey/predator model
  double alpha1 = 1.0;
  double alpha2 = 1.0;
  double beta1 = 1.0;
  double beta2 = 1.0;
  auto f = [&alpha1, &alpha2, &beta1, &beta2](Eigen::Vector2d y) {
    Eigen::Vector2d temp = y;
    temp(0) *= alpha1 - beta1 * y(1);
    temp(1) *= -alpha2 + beta2 * y(0);
    return temp;
  };

  // Compute the solution(s) of the IVP(s)
  double T = 1.0;
  Eigen::Vector2d y0(100.0, 1.0);
  for (int m = 0; m < M; ++m) {
    yT_list[m] = integrator.solve(f, T, y0, N_list[m]).back();
  }

  return yT_list;
}
/* SAM_LISTING_END_1 */

}  // namespace StabRK3


#include <iostream>
#include <Eigen\Core>
#include <Eigen\SVD>

using namespace Eigen;
using namespace std;

template <typename Derived>
void print_size(const EigenBase<Derived>& b) {
	std::cout << "size (rows, cols): " << b.size() << " (" << b.rows() << ", " << b.cols() << ")" << std::endl;
}

template <typename Derived>
void print_block(const DenseBase<Derived>& b, int x, int y, int r, int c) {
	cout << "block: " << b.block(x, y, r, c) << endl;
}

template <typename Derived>
void print_max_coeff(const ArrayBase<Derived> &a) {
	cout << "max: " << a.maxCoeff() << endl;
}

template <typename Derived>
void print_inv_cond(const MatrixBase<Derived>& a) {
	const typename JacobiSVD<typename Derived::PlainObject>::SingularValuesType &sing_vals = a.jacobiSvd().singularValues();
	cout << "inv cond: " << sing_vals(sing_vals.size() - 1) / sing_vals(0) << endl;
}

template <typename DerivedA, typename DerivedB>
typename DerivedA::Scalar squaredist(const MatrixBase<DerivedA>& p1, const MatrixBase<DerivedB>& p2) {
	return (p1 - p2).squaredNorm();
}

float inv_cond(const Ref<const MatrixXf>& a) {
	const VectorXf sing_vals = a.jacobiSvd().singularValues();
	return sing_vals(sing_vals.size() - 1) / sing_vals(0);
}

void cov(const Ref<const MatrixXf> x, const Ref<const MatrixXf> y, Ref<MatrixXf> C) {
	const float num_observations = static_cast<float>(x.rows());
	const RowVectorXf x_mean = x.colwise().sum() / num_observations;
	const RowVectorXf y_mean = y.colwise().sum() / num_observations;
	C = (x.rowwise() - x_mean).transpose() * (y.rowwise() - y_mean) / num_observations;

}

void main() {
	{
		cout << "EigenBase Example" << endl;
		Vector3f v;
		print_size(v);
		print_size(v.asDiagonal());
	}

	{
		Matrix4f m = Matrix4f::Random();
		cout << "matrix m: \n" << m << endl;
		cout << "inv_cond(m): \n" << inv_cond(m) << endl;
		cout << "inv_cond(m(1:3, 1:3)): \n" << inv_cond(m.topLeftCorner(3, 3)) << endl;
		cout << "inv_cond(m+I):\n" << inv_cond(m + Matrix4f::Identity()) << endl;
	}
	{
		MatrixXf m1, m2, m3;
		m1 = MatrixXf::Random(5, 5);
		m2 = MatrixXf::Random(5, 5);
		m3 = MatrixXf::Random(5, 5);

		cov(m1, m2, m3);
		cov(m1.leftCols<3>(), m2.leftCols<3>(), m3.topLeftCorner<3, 3>());
	}
	system("pause");
}


//
// Created by xiang on 12/21/17.
//

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

using namespace Eigen;

#include <vector>
#include <fstream>
#include <iostream>
#include <iomanip>

#include "sophus/se3.h"

using namespace std;

typedef vector<Vector3d, Eigen::aligned_allocator<Vector3d>> VecVector3d;
typedef vector<Vector2d, Eigen::aligned_allocator<Vector3d>> VecVector2d;
typedef Matrix<double, 6, 1> Vector6d;

string p3d_file = "../p3d.txt";
string p2d_file = "../p2d.txt";

int main(int argc, char **argv) {

    VecVector2d p2d;
    VecVector3d p3d;
    Matrix3d K;
    double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
    K << fx, 0, cx, 0, fy, cy, 0, 0, 1;

    // load points in to p3d and p2d
    // START YOUR CODE HERE
	string p2_data, p3_data;
	ifstream if_p2(p2d_file.c_str());
	ifstream if_p3(p3d_file.c_str());
 	if( !if_p3.is_open() || !if_p2.is_open() ){
		cout << " p3d.txt or p2d.txt no found!" << endl;
		return -1;
	}
	while( getline(if_p2, p2_data) && getline(if_p3, p3_data) ){
		Vector3d  p3_coor;
		Vector2d  p2_coor;
		istringstream p2_iss(p2_data);
		istringstream p3_iss(p3_data);
		p2_iss >> p2_coor[0] >> p2_coor[1];
		p3_iss >> p3_coor[0] >> p3_coor[1] >> p3_coor[2];
		p3d.push_back(p3_coor);
		p2d.push_back(p2_coor);
	}
    // END YOUR CODE HERE
    assert(p3d.size() == p2d.size());
	cout << "p3d size:" << p3d.size() << " p2d size:" << p2d.size() << endl;

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    Sophus::SE3 T_esti(Matrix3d::Identity(), Vector3d::Zero()); // estimated pose
	for (int iter = 0; iter < iterations; iter++) {

        Matrix<double, 6, 6> H = Matrix<double, 6, 6>::Zero();
        Vector6d b = Vector6d::Zero();
		Vector2d err;
        cost = 0;
        // compute cost
        for (int i = 0; i < nPoints; i++) {
        // compute cost for p3d[I] and p2d[I]

			Vector3d TP =  T_esti * p3d[i];
			Vector3d pix_coor = K * TP;
			err[0] = p2d[i][0] - pix_coor[0]/pix_coor[2];
			err[1] = p2d[i][1] - pix_coor[1]/pix_coor[2];
			cout << err << endl;
	    // END YOUR CODE HERE

	    // compute jacobian
        	Matrix<double, 2, 6> J;
        // START YOUR CODE HERE
			double x = TP[0], y = TP[1], z = TP[2];
			double x2 = x*x, y2 = y*y, z2 = z*z;
			J << -fx/z, 0, fx*x/z2, fx*x*y/z2, -fx-fx * x2/z2, fx*y/z,
			  	 0, -fy/z, fy*y/z2, fy+fy*y2/z2, -fy*x*y/z2, -fy*x/z;
			cout << J << endl;
		// END YOUR CODE HERE

            H += J.transpose() * J;
          	b += -J.transpose() * err;

			cost += err.transpose() * err;
        }

	// solve dx
        Vector6d dx;

        // START YOUR CODE HERE
		dx = H.ldlt().solve(b);
        // END YOUR CODE HERE

        if (std::isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {
            // cost increase, update is not good
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // update your estimation
        // START YOUR CODE HERE
		T_esti = Sophus::SE3::exp(dx) * T_esti;
        // END YOUR CODE HERE

        lastCost = cost;

        cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
    }

    cout << "estimated pose: \n" << T_esti.matrix() << endl;
    return 0;
}


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

#ifndef BOOST_MATH_STATISTICS_LINEAR_REGRESSION_HPP
#define BOOST_MATH_STATISTICS_LINEAR_REGRESSION_HPP

#include <cmath>
#include <algorithm>
#include <utility>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>

namespace boost::math::statistics {

template<class RandomAccessContainer>
auto simple_ordinary_least_squares(RandomAccessContainer const & x,
                                   RandomAccessContainer const & y)
{
    using Real = typename RandomAccessContainer::value_type;
    if (x.size() <= 1)
    {
        throw std::domain_error("At least 2 samples are required to perform a linear regression.");
    }

    if (x.size() != y.size())
    {
        throw std::domain_error("The same number of samples must be in the independent and dependent variable.");
    }
    auto [mu_x, mu_y, cov_xy] = boost::math::statistics::means_and_covariance(x, y);

    auto var_x = boost::math::statistics::variance(x);

    if (var_x <= 0) {
        throw std::domain_error("Independent variable has no variance; this breaks linear regression.");
    }

    Real c1 = cov_xy/var_x;
    Real c0 = mu_y - c1*mu_x;

    return std::make_pair(c0, c1);
}

template<class RandomAccessContainer>
auto simple_ordinary_least_squares_with_R_squared(RandomAccessContainer const & x,
                                   RandomAccessContainer const & y)
{
    using Real = typename RandomAccessContainer::value_type;
    if (x.size() <= 1)
    {
        throw std::domain_error("At least 2 samples are required to perform a linear regression.");
    }

    if (x.size() != y.size())
    {
        throw std::domain_error("The same number of samples must be in the independent and dependent variable.");
    }
    auto [mu_x, mu_y, cov_xy] = boost::math::statistics::means_and_covariance(x, y);

    auto var_x = boost::math::statistics::variance(x);

    if (var_x <= 0) {
        throw std::domain_error("Independent variable has no variance; this breaks linear regression.");
    }

    Real c1 = cov_xy/var_x;
    Real c0 = mu_y - c1*mu_x;

    Real squared_residuals = 0;
    Real squared_mean_deviation = 0;
    for(decltype(y.size()) i = 0; i < y.size(); ++i) {
        squared_mean_deviation += (y[i] - mu_y)*(y[i]-mu_y);
        Real ei = (c0 + c1*x[i]) - y[i];
        squared_residuals += ei*ei;
    }

    Real Rsquared;
    if (squared_mean_deviation == 0) {
        // Then y = constant, so the linear regression is perfect.
        Rsquared = 1;
    } else {
        Rsquared = 1 - squared_residuals/squared_mean_deviation;
    }

    return std::make_tuple(c0, c1, Rsquared);
}

}
#endif


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

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

#include "sophus/so3.hpp"
#include "sophus/se3.hpp"

int main( int argc, char** argv )
{
    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    Sophus::SO3d SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    // Sophus::SO3d SO3_v( 0., 0., M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数
    Sophus::SO3d SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout << "SO(3) from matrix: " << endl << SO3_R.matrix() << endl;
    // cout << "SO(3) from vector: " << SO3_v.matrix() << endl;
    cout << "SO(3) from quaternion :" << endl << SO3_q.matrix() << endl;

    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so3 = " << endl << so3.transpose()<<endl;
    // hat 为向量到反对称矩阵
    cout << "so3 hat=\n" << endl << Sophus::SO3d::hat(so3)<<endl;
    // 相对的，vee为反对称到向量
    cout << "so3 hat vee= " << endl << Sophus::SO3d::vee( Sophus::SO3d::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些

    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
    cout << "SO3 updated = " << endl << SO3_updated.matrix() << endl;

    /********************萌萌的分割线*****************************/
    cout << "************我是分割线*************" << endl;
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3d SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt.matrix()<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt.matrix()<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<endl << se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3d::vee( Sophus::SE3d::hat(se3) ).transpose()<<endl;

    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

    return 0;
}


#include <cmath>  // sin, cos
#include <limits>  // limits<double>

#include <Eigen/Dense>

#include "Geometry.hpp"

using namespace cppmath;

geometry::Matrix3T geometry::getRotationXYZMatrix( double x, double y, double z )
{
    Matrix3T rot;

    rot( 0, 0 ) = cos( y ) * cos( z );
    rot( 0, 1 ) = -cos( y ) * sin( z );
    rot( 0, 2 ) = sin( y );

    rot( 1, 0 ) = cos( x ) * sin( z ) + cos( z ) * sin( x ) * sin( y );
    rot( 1, 1 ) = cos( x ) * cos( z ) - sin( x ) * sin( y ) * sin( z );
    rot( 1, 2 ) = -cos( y ) * sin( x );

    rot( 2, 0 ) = sin( x ) * sin( z ) - cos( x ) * cos( z ) * sin( y );
    rot( 2, 1 ) = cos( z ) * sin( x ) + cos( x ) * sin( y ) * sin( z );
    rot( 2, 2 ) = cos( x ) * cos( y );

    return rot;
}

bool geometry::findOrthogonalVector( Vector3T* const o, const Vector3T& v )
{
    // 0 = Vx Ox + Vy Oy + Vz Oz
    if( v.isZero( 1e3 * std::numeric_limits< double >::min() ) )
    {
        return false;
    }

    // avoid division by zero, pick max(abs) as divisor
    Vector3T::Index i;
    v.cwiseAbs().maxCoeff( &i );
    if( i == 2 ) // z
    {
        // - Vz Oz = Vx Ox + Vy Oy
        //      Ox = Vy
        //      Oy = Vx
        //      Oz = -2(Vx * Vy) / Vz
        o->x() = v.y();
        o->y() = v.x();

        o->z() = ( -2.0 * v.x() * v.y() ) / v.z();
        return true;
    }
    if( i == 1 ) // y
    {
        // - Vy Oy = Vx Ox + Vz Oz
        //      Ox = Vz
        //      Oz = Vx
        //      Oy = -2(Vx * Vz) / Vy
        o->x() = v.z();
        o->z() = v.x();
        o->y() = ( -2.0 * v.x() * v.z() ) / v.y();
        return true;
    }
    if( i == 0 ) // x
    {
        // - Vx Ox = Vy Oy + Vz Oz
        //      Oy = Vz
        //      Oz = Vy
        //      Ox = -2(Vy * Vz) / Vx
        o->y() = v.z();
        o->z() = v.y();
        o->x() = ( -2.0 * v.y() * v.z() ) / v.x();
        return true;
    }
    return false;
}

bool geometry::findTangentPlane( Vector3T* const u, Vector3T* const v, const Vector3T& n )
{
    if( !findOrthogonalVector( u, n ) )
    {
        return false;
    }
    *v = n.cross( *u );
    return true;
}


/*MCIRGAME - Point Connection Game in a Circle
#number-theory #probability-theory #big-numbers #dynamic-programming #bitmasks #disjoint-set-2

English 	Vietnamese

This is a small but ancient game. You are supposed to write down the
numbers 1, 2, 3, . . . , 2n − 1, 2n consecutively in clockwise order
on the ground to form a circle, and then, to draw some straight line
segments to connect them into number pairs. Every number must be connected
to exactly one another.
And, no two segments are allowed to intersect. It’s still a simple game, isn’t it?
But after you’ve written down the 2n numbers, can you tell me in how
many diﬀerent ways can you connect the numbers into pairs? Life is harder, right!

Input

Each line of the input ﬁle will be a single positive number n, except
the last line, which is a number −1.
You may assume that 1 ≤ n ≤ 150.

Sample Input
2
-1

Output

For each n, print in a single line the number of ways to connect the 2n
numbers into pairs.

Sample output
2

Note : Big num! */

#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

int512_t binom(unsigned int n, unsigned int k)
{
    int512_t res = 1;

    // Since C(n, k) = C(n, n-k)
    if (k > n - k)
        k = n - k;

    // Calculate value of [n*(n-1)*---*(n-k+1)] / [k*(k-1)*---*1]
    for (int i = 0; i < k; ++i)
    {
        res *= (n - i);
        res /= (i + 1);
    }

    return res;
}

int512_t nth_catalan(unsigned n)
{
    return binom(2*n, n) / (n+1);
}

int main()
{
    int n;

    while (std::cin >> n && n != -1)
    {
        std::cout << nth_catalan(n) << std::endl;
    }

    return 0;
}

SPOJ/MCIRGAME - Point Connection Game in a Circle.cpp


#include <iostream>
#include "../include/iteration.h"
#include "../include/ESolve.h"
#include <Eigen/Dense>

namespace SimpleSolve{
    void ESolve()
    {
        /// F
        auto F = [](const Eigen::VectorXd &m){
            Eigen::VectorXd res(3);
            res << m(0)-m(1)-2,
                    m(1)+m(2)-3,
                    K1 * (m(0) * std::abs(m(0)) + m(1) * std::abs(m(1))) - K3 * m(2) * std::abs(m(2));
            return res;
        };
        /// jacobian of F
        auto DF= [] (const Eigen::VectorXd &m){
            Eigen::MatrixXd J(3,3);
            J << 1, -1, 0,
                    0, 1, 1,
                    2 * K1 * std::abs(m(0)) ,  2 * K1 * std::abs(m(1)),  -2 * K3 * std::abs(m(2));
            return J;
        };

        Eigen::VectorXd m(3);
        m << 1.0, 1.0, 1.0; // initial value
        const double tolerance = 1e-14; //  Creates a Newton object which is pointed to by newton
        std::unique_ptr<SimpleSolve::Iteration> newton = std::make_unique<SimpleSolve::Newton>(m, tolerance, F, DF);
        VectorXd newtonResults = newton->solve(m);  // Dynamic dispatch works as expect
        std::cout << std::setprecision(17) << "solution = " << newtonResults << "\n";
        std::cout << std::setprecision(17) << "error norm = " << F(m).norm() << "\n";

    }

}

/* Dynamic dispatch: The function that is executed is chosen at runtime based on the dynamic type of the object.
 * As opposed to static dispatch, where the exact function that is going to be called is known at compile time */


/**
Implementation of common loss function.

@file cnet_loss.hpp
@author Bastian Schoettle EN RC PREC
*/

#ifndef CNET_LOSS_HPP
#define CNET_LOSS_HPP

#include <memory>
#include <math.h>
#include <Eigen/Dense>
#include "cnet_common.hpp"

#define fraction float(1e-10)

namespace Cnet
{

	class Loss
	{

	public:
		virtual float calculate(MatrixRm* data, MatrixRm* targets) = 0;
		virtual MatrixRm derivative(MatrixRm* data, MatrixRm* targets) = 0;

		virtual std::unique_ptr<Loss> clone() = 0;
	};

	class MseLoss : public Loss
	{
	public:

		float calculate(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == targets->cols() && data->rows() == targets->rows());
            return ((*data - *targets).squaredNorm()) / (targets->cols()*2);
		}

		MatrixRm derivative(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == targets->cols() && data->rows() == targets->rows());
            return (*data - *targets);
		}

        std::unique_ptr<Loss> clone() override
        {
            return std::unique_ptr<MseLoss>(new MseLoss());
        }

	};

	class BinaryCrossEntropyLoss : public Loss
	{
	public:

		float calculate(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == 1 && data->rows() == 1 && targets->rows() == 1 && targets->cols() == 1);
            float stable_target = (*targets)(0, 0) - fraction;
			float stable_output = (*data)(0, 0) - fraction;
			return -(stable_target*log(stable_output) + (1 - stable_target)*log(1 - stable_output));
		}

		MatrixRm derivative(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == 1 && data->rows() == 1 && targets->rows() == 1 && targets->cols() == 1);
            MatrixRm deltas(1, 1);
			deltas(0, 0) = ((*data)(0, 0) - (*targets)(0, 0));
			return deltas;
		}

        std::unique_ptr<Loss> clone() override
        {
            return std::unique_ptr<BinaryCrossEntropyLoss>(new BinaryCrossEntropyLoss());
        }

	};

	class CrossEntropyLoss : public Loss
	{
	private:

		static inline float multiclass_cross_entropy(MatrixRm* outputs, MatrixRm* targets)
		{
			float sum = 0.f;
			for (int i = 0; i < outputs->cols(); i++)
			{
				sum += (*targets)(0, i) * log((*outputs)(0, i));
			}
			return -1 * sum;
		}

	public:

		float calculate(MatrixRm* data, MatrixRm* targets) override
		{
            return multiclass_cross_entropy(data, targets);
		}

		MatrixRm derivative(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == targets->cols() && data->rows() == targets->rows());
            return (*data - *targets);
		}

        std::unique_ptr<Loss> clone() override
        {
            return std::unique_ptr<CrossEntropyLoss>(new CrossEntropyLoss());
        }
	};

	class SparseSoftmaxCrossEntropyLoss : public Loss
	{

	public:

		float calculate(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == targets->cols() && targets->rows() == 1);
            MatrixRm softmaxed = MatrixRm::Zero(data->rows(), data->cols());
			for (int i = 0; i < data->cols(); i++)
			{
				MatrixRm chunk = data->block(0, i, data->rows(), 1);
				MatrixRm normalized_data = chunk.array() - chunk.maxCoeff();
				float exp_sum = 0.f;
				for (int row = 0; row < chunk.rows(); ++row)
				{
					exp_sum += exp(normalized_data(row, 0));
				}

				for (int row = 0; row < chunk.rows(); ++row)
				{
					softmaxed(row, i) = exp(normalized_data(row, 0)) / exp_sum;
				}
			}
			float sum = 0.f;
			for (unsigned int i = 0; i < targets->cols(); i++)
			{
				sum += log(std::max(softmaxed(round((*targets)(0, i)), i), (float) 1e-15));
			}
			return - ((1/ (float)targets->cols()) * sum);
		}

		MatrixRm derivative(MatrixRm* data, MatrixRm* targets) override
		{
			assert(data->cols() == targets->cols() && targets->rows() == 1);
            MatrixRm softmaxed = MatrixRm::Zero(data->rows(), data->cols());
			for (int i = 0; i < data->cols(); i++)
			{
				MatrixRm chunk = data->block(0, i, data->rows(), 1);
				MatrixRm normalized_data = chunk.array() - chunk.maxCoeff();
				float exp_sum = 0.f;
				for (int row = 0; row < chunk.rows(); ++row)
				{
					exp_sum += exp(normalized_data(row, 0));
				}

				for (int row = 0; row < chunk.rows(); ++row)
				{
					softmaxed(row, i) = exp(normalized_data(row, 0)) / exp_sum;
				}
			}
			for (unsigned int i = 0; i < targets->cols(); i++)
			{
				softmaxed(round((*targets)(0, i)), i) -= 1;
			}
			return softmaxed;
		}

        std::unique_ptr<Loss> clone() override
        {
            return std::unique_ptr<SparseSoftmaxCrossEntropyLoss>(new SparseSoftmaxCrossEntropyLoss());
        }

	};

}

#endif


#include <iostream>
#include <cmath>
#include <boost/math/special_functions/lambert_w.hpp>

// Implementation details in this paper: https://artemlos.net/docs/2014/01/MathExploration.pdf

using namespace std;

// Artem’s method - calculated the number of digits in interval [1,a]
constexpr long double artemG(long double a)
{
  return (a - 1LL) * pow(10LL, a) + ((pow(10LL, a - 1LL) - 1LL) / 9.0) * 8.0 * 10.0 + 9LL;
}

// Not defined for n = 1
long double inverseG(int n)
{
  const auto log5p2 = log(5.0L) + log(2.0L);
  const auto lambertFx = boost::math::lambert_w0((9.0L * n - 1) * log5p2 / (9 * pow(10.0L, 1.0L / 9.0L)));
  return (9.0L * lambertFx + log5p2) / (9.0L * log5p2);
}

void printDigit(int index)
{
  if (index == 1)
  {
    cout << "index: 1 output: 1" << endl;
    return;
  }

  auto a{inverseG(index)};
  int aCeil{static_cast<int>(ceil(a))};
  auto gOfACeil{artemG(aCeil)};
  auto gOfA{artemG(a)};

  // 4.3 from paper
  int p = pow(10.0L, aCeil) - 1.0L - floor((gOfACeil - gOfA) / aCeil);
  int r = fmod(gOfACeil - gOfA, aCeil);

  auto output{to_string(p)};
  cout << "index: " << index << " output: " << output[output.size() - 1 - r] << endl;
}

int main()
{
  for (int i = 1; i < 100; ++i)
    printDigit(i);

  // Example in paper
  printDigit(206788);
}


// [[Rcpp::depends(BH)]]
#include <Rcpp.h>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/erf.hpp>

using namespace Rcpp;

//' Outputs physicist version of Hermite Polynomials
//'
//'
//' The method calculates the physicist version of Hermite polynomials,
//' \eqn{H_k(x)} from \eqn{k=0,\dots,N} for the vector of values, x.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @param x A numeric vector.
//' @return A numeric matrix with N+1 rows and length(x) columns.
//' @export
// [[Rcpp::export]]
NumericMatrix hermite_polynomial(int N, NumericVector x) {
  int x_size = x.size();
  NumericMatrix hermite(N + 1, x_size);
  for(int i = 0; i < x_size; ++i) {
    hermite(0,i) = 1;
  }
  if (N==0){
    return hermite;
  }
  for(int i = 0; i < x_size; ++i) {
    hermite(1,i) = 2 * x[i];
  }
  if (N==1){
    return hermite;
  }
  for(int j = 0; j < x_size; ++j) {
    for(int i = 2; i <= N; ++i) {
      hermite(i,j) = 2 * x[j] * hermite(i - 1,j) - 2 *
       (((double)i) - 1) * hermite(i - 2,j);
    }
  }
  return hermite;
}

//' Outputs Hermite normalization factors
//'
//' The method returns numeric normalization factors that, when multiplied by
//' the physicist Hermite polynomials \eqn{H_k(x)}, yield orthonormal
//' Hermite functions \eqn{h_k(x)} for \eqn{k=0,\dots,N}.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @return A numeric vector of length N+1
//' @export
// [[Rcpp::export]]
NumericVector hermite_normalization(int N) {
  NumericVector out(N + 1);
  double sqrt_pi = sqrt(M_PI);
  for(int i = 0; i <= N; ++i) {
    out(i) = 1/sqrt(pow((double)2,(double)i) *
      boost::math::factorial<double>((double)i) * sqrt_pi);
  }
  return out;
}

//' Outputs orthonormal Hermite functions
//'
//'
//' The method calculates the orthonormal Hermite functions, \eqn{h_k(x)}
//' from \eqn{k=0,\dots,N} for the vector of values, x.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @param x A numeric vector.
//' @param normalization A numeric vector of normalization factors generated by
//' the hermite_normalization function.
//' @return A numeric matrix with N+1 rows and length(x) columns.
//' @export
// [[Rcpp::export]]
NumericMatrix hermite_function(int N, NumericVector x,
                               NumericVector normalization) {
  int x_size = x.size();
  NumericMatrix hermite(N + 1, x_size);
  NumericMatrix out(N + 1, x_size);
  NumericVector expFac(x_size);
  for(int i = 0; i < x_size; ++i) {
    hermite(0,i) = 1;
    expFac(i) = exp(-1 * x[i] * x[i] / 2);
    out(0,i) = hermite(0,i) * normalization[0] * expFac(i);
  }
  if (N==0){
    return out;
  }
  for(int i = 0; i < x_size; ++i) {
    hermite(1,i) = 2 * x[i];
    out(1,i) = hermite(1,i) * normalization[1] * expFac(i);
  }
  if (N==1){
    return out;
  }
  for(int j = 0; j < x_size; ++j) {
    for(int i = 2; i <= N; ++i) {
      hermite(i,j) = 2 * x[j] * hermite(i - 1,j) - 2 *
       (((double)i) - 1) * hermite(i - 2,j);
      out(i,j) = hermite(i,j) * normalization[i] * expFac(j);
    }
  }
  return out;
}

//' Outputs the sum of orthonormal Hermite functions
//'
//'
//' The method calculates the sum of orthonormal Hermite functions,
//' \eqn{\sum_{i} h_k(x_{i})} from \eqn{k=0,\dots,N} for the vector of values,
//' x.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @param x A numeric vector.
//' @param normalization A numeric vector of normalization factors generated by
//' the hermite_normalization function.
//' @return A numeric vector of length N+1.
//' @export
// [[Rcpp::export]]
NumericVector hermite_function_sum(int N, NumericVector x,
                               NumericVector normalization) {
  int x_size = x.size();
  NumericMatrix hermite(N + 1, x_size);
  NumericVector out(N + 1);
  NumericVector expFac(x_size);
  int i = 0;
  int j = 0;
  for(i = 0; i <= x_size-4; i += 4) {
    hermite(0,i) = 1;
    hermite(0,i+1) = 1;
    hermite(0,i+2) = 1;
    hermite(0,i+3) = 1;
    expFac(i) = exp(-1 * x[i] * x[i] / 2);
    expFac(i+1) = exp(-1 * x[i+1] * x[i+1] / 2);
    expFac(i+2) = exp(-1 * x[i+2] * x[i+2] / 2);
    expFac(i+3) = exp(-1 * x[i+3] * x[i+3] / 2);
    out(0) += (hermite(0,i) * expFac(i) +
      hermite(0,i+1) * expFac(i+1) +
      hermite(0,i+2) * expFac(i+2) +
      hermite(0,i+3) * expFac(i+3)) * normalization[0];
  }
  for(; i < x_size; ++i) {
    hermite(0,i) = 1;
    expFac(i) = exp(-1 * x[i] * x[i] / 2);
    out(0) += hermite(0,i) * normalization[0] * expFac(i);
  }
  if (N==0){
    return out;
  }
  for(i = 0; i <= x_size - 4; i += 4) {
    hermite(1,i) = 2 * x[i];
    hermite(1,i+1) = 2 * x[i+1];
    hermite(1,i+2) = 2 * x[i+2];
    hermite(1,i+3) = 2 * x[i+3];
    out(1) += (hermite(1,i) * expFac(i) +
      hermite(1,i+1) * expFac(i+1) +
      hermite(1,i+2) * expFac(i+2) +
      hermite(1,i+3) * expFac(i+3)) * normalization[1];
  }
  for(; i < x_size; ++i) {
    hermite(1,i) = 2 * x[i];
    out(1) += hermite(1,i) * normalization[1] * expFac(i);
  }
  if (N==1){
    return out;
  }
  for(j = 0; j <= x_size - 4; j += 4) {
    for(int k = 2; k <= N; ++k) {
      hermite(k,j) = 2 * x[j] * hermite(k - 1,j) - 2 *
        (((double)k) - 1) * hermite(k - 2,j);
      hermite(k,j+1) = 2 * x[j+1] * hermite(k - 1,j+1) - 2 *
        (((double)k) - 1) * hermite(k - 2,j+1);
      hermite(k,j+2) =  2 * x[j+2] * hermite(k - 1,j+2) - 2 *
        (((double)k) - 1) * hermite(k - 2,j+2);
      hermite(k,j+3) =  2 * x[j+3] * hermite(k - 1,j+3) - 2 *
        (((double)k) - 1) * hermite(k - 2,j+3);
      out(k) += (hermite(k,j) * expFac(j) +
        hermite(k,j+1) * expFac(j+1) +
        hermite(k,j+2) * expFac(j+2) +
        hermite(k,j+3) * expFac(j+3)) * normalization[k];

    }
  }
  for(; j < x_size; ++j) {
    for(int k = 2; k <= N; ++k) {
      hermite(k,j) = 2 * x[j] * hermite(k - 1,j) - 2 *
        (((double)k) - 1) * hermite(k - 2,j);
      out(k) += hermite(k,j) * normalization[k] * expFac(j);
    }
  }
  return out;
}

//' Outputs lower integral of the orthonormal Hermite functions
//'
//' The method calculates \eqn{\int_{-\infty}^{x} h_k(t) dt}
//' for \eqn{k=0,\dots,N} and the vector of values x.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @param x A numeric vector.
//' @param hermite_function_mat A numeric matrix of Hermite function values
//' generated by the function hermite_function.
//' @return A numeric matrix with N+1 rows and length(x) columns.
//' @export
// [[Rcpp::export]]
NumericMatrix hermite_integral_val(int N, NumericVector x,
                                   NumericMatrix hermite_function_mat) {
  int x_size = x.size();
  NumericMatrix out(N + 1, x_size);
  for(int i = 0; i < x_size; ++i) {
    out(0,i) = pow(M_PI,0.25)/sqrt((double) 2) *
     boost::math::erfc<double>((double) -1 * (1 / sqrt((double)2)) * x[i]);
  }
  if (N == 0){
    return out;
  }
  for(int i = 0; i < x_size; ++i) {
    out(1,i) = -1 * sqrt((double) 2)/pow(M_PI,0.25) *exp(-1 * x[i] * x[i] / 2);
  }
  if (N == 1){
    return out;
  }
  for(int i = 2; i <= N; ++i) {
    for(int j = 0; j < x_size; ++j) {
      out(i,j) = -1 * sqrt(2/((double)i)) * hermite_function_mat(i - 1,j) +
       sqrt((((double)i) - 1)/((double)i)) * out(i - 2,j);
    }
  }
  return out;
}

//' Outputs upper integral of the orthonormal Hermite functions
//'
//' The method calculates \eqn{\int_{x}^{\infty} h_k(t) dt}
//' for \eqn{k=0,\dots,N} and the vector of values x.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @param x A numeric vector.
//' @param hermite_function_mat A numeric matrix of Hermite function values
//' generated by the function hermite_function.
//' @return A numeric matrix with N+1 rows and length(x) columns.
//' @export
// [[Rcpp::export]]
NumericMatrix hermite_integral_val_upper(int N,
                         NumericVector x, NumericMatrix hermite_function_mat) {
  int x_size = x.size();
  NumericMatrix out(N + 1, x_size);
  for(int i = 0; i < x_size; ++i) {
    out(0,i) = pow(M_PI,0.25)/sqrt((double) 2) *
     boost::math::erfc<double>((double) (1 / sqrt((double) 2)) * x[i]);
  }
  if (N == 0){
    return out;
  }
  for(int i = 0; i < x_size; ++i) {
    out(1,i) = sqrt((double) 2)/pow(M_PI,0.25) *exp(-1 * x[i] * x[i] / 2);
  }
  if (N == 1){
    return out;
  }
  for(int i = 2; i <= N; ++i) {
    for(int j = 0; j < x_size; ++j) {
      out(i,j) = sqrt(((double)2) / ((double)i)) * hermite_function_mat(i-1,j)+
       sqrt((((double)i)-1)/((double)i)) * out(i-2,j);
    }
  }
  return out;
}

//' Outputs integral of the orthonormal Hermite functions on the full domain
//'
//' The method calculates \eqn{\int_{-\infty}^{\infty} h_k(t) dt}
//' for \eqn{k=0,\dots,N}.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param N An integer number.
//' @return A numeric matrix with N+1 rows and 1 columns.
//' @export
// [[Rcpp::export]]
NumericMatrix hermite_int_full_domain(int N) {
  NumericMatrix out(N + 1, 1);
  out(0,0) = pow(M_PI,0.25)*sqrt((double) 2);
  if (N == 0){
    return out;
  }
  out(1,0) = 0;
  if (N == 1){
    return out;
  }
  for(int i = 2; i <= N; ++i) {
      out(i,0) = sqrt((((double)i)-1)/((double)i)) * out(i-2,0);
  }
  return out;
}

//' Standardizes the observation x and updates the online moment inputs
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param x A numeric value.
//' @param n_obs A numeric value. The number of observations.
//' @param current_mean A numeric value.
//' @param current_var A numeric value.
//' @return A numeric vector. The first element is the updated mean. The
//' second element is the updated variance times n_obs. The third element is the
//' updated, standardized value of x.
//' @export
// [[Rcpp::export]]
NumericVector standardizeInputs(double x, double n_obs, double current_mean,
                                double current_var) {
  NumericVector outputVec(3);
  double prev_running_mean = current_mean;
  outputVec[0] =  (current_mean * (n_obs - 1) + x) / n_obs;
  if (n_obs < 2){
    return outputVec;
  }
  outputVec[1] = current_var + (x - prev_running_mean) * (x - outputVec[0]);
  double running_std = sqrt(outputVec[1] / (n_obs - 1));
  outputVec[2] = (x - outputVec[0]) / running_std;
  return outputVec;
}

//' Standardizes the observation x and updates the online moment inputs
//'
//' The online moments are updated via exponential weighting.
//'
//' @author Michael Stephanou <michael.stephanou@gmail.com>
//'
//' @param x A numeric value.
//' @param n_obs A numeric value. The number of observations.
//' @param lambda A numeric value.
//' @param current_mean A numeric value.
//' @param current_var A numeric value.
//' @return A numeric vector. The first element is the updated mean. The
//' second element is the updated variance times n_obs. The third element is the
//' updated, standardized value of x.
//' @export
// [[Rcpp::export]]
NumericVector standardizeInputsEW(double x, double n_obs,double lambda,
                                  double current_mean, double current_var) {
  NumericVector outputVec(3);
  if (n_obs < 2){
    current_mean = x;
    current_var = 1;
    outputVec[0] =  current_mean;
    outputVec[1] = current_var;
    return outputVec;
  }
  outputVec[0] =  (1 - lambda) * current_mean + lambda * x;
  current_var = (1 - lambda) * current_var + lambda * pow((x - outputVec[0]),2);
  outputVec[1] = current_var;
  double running_std = sqrt(current_var);
  outputVec[2] = (x - outputVec[0]) / running_std;
  return outputVec;
}


#ifndef FUNCTIONS_H
#define FUNCTIONS_H

#include "types.hpp"
#include <Eigen/Core>
#include <cmath>
#include <numeric>

namespace Optima {

class TestFunctions {

public:

    /**
     * N-Dimensional generalization of the Ackley function
     *
     * This function has a minimum when all parameter values are zero.
     *
     * The hyper parameters for this function are hard coded to the following:
     * a = 20
     * b = 0.2
     * c = 2 * pi
     *
     * @param params :: n dimensional array of x values
     * @return value of the function evaluated with the given parameters
     */
    static double ackley(const Parameters &params) {
        const double a = 20;
        const double b = 0.2;
        const double c = 2 * M_PI;

        double powerSum = std::accumulate(params.begin(), params.end(), 0,
                [](const double sum, const std::pair<std::string, double>& item) {
                    return sum + std::pow(item.second, 2.0);
                });

        double cosSum = std::accumulate(params.begin(), params.end(), 0,
                [&c](const double sum, const std::pair<std::string, double>& item) {
                    return sum + std::cos(c*item.second);
                });

        cosSum *= 1. / params.size();
        powerSum *= 1. / params.size();
        powerSum = std::sqrt(powerSum);

        return -a * std::exp( -b * powerSum ) - std::exp(cosSum) + a + std::exp(1);
    }

    /**
     * N-Dimensional generalization of the Rosenbrock function
     *
     * The minimum of this function will be at zero
     *
     * @param vec :: n dimensional array of x values
     * @return value of the function evaluated at the point defined by vec
     */
    static double rosen(const Parameters &params) {
        using namespace std;
        double y = 0;
        for (size_t i = 0; i < params.size() - 1; ++i) {
            const double x1 = params.at("x" + std::to_string(i));
            const double x2 = params.at("x" + std::to_string(i + 1));
            y += 100 * pow(x2 - pow(x1, 2.0), 2.0) + pow(1 - x1, 2.0);
        }

        return y;
    }

    /**
     * N-Dimensional parabolic function centered on the origin.
     *
     * The function has the form x1**2 + x2**2 + ... + xn**2 = y
     *
     * The minimum of this function will be at zero
     *
     * @param vec :: n dimensional arry of x values
     * @return value of the function evaludated at the point defined by vec
     */
    static double parabola(const Parameters &params) {
        using namespace std;
        double y = 0;
        for (const auto &param : params)
            y += std::pow(param.second, 2.0);

        return y;
    }

};
}
#endif // FUNCTIONS_H


// ALGOLAB BGL Tutorial 3
// Code snippets demonstrating
// - MinCostMaxFlow with negative edge costs using cycle_canceling
// - MinCostMaxFlow with negative edge costs using successive_shortest_path_nonnegative_weights

// Compile and run with one of the following:
// g++ -std=c++11 -O2 bgl_mincostmaxflow.cpp -o bgl_mincostmaxflow; ./bgl_mincostmaxflow
// g++ -std=c++11 -O2 -I path/to/boost_1_58_0 bgl_mincostmaxflow.cpp -o bgl_mincostmaxflow; ./bgl_mincostmaxflow

// Includes
// ========
// STL includes
#include <iostream>
#include <cstdlib>
// BGL includes
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>
// Namespaces
using namespace boost;
using namespace std;

// BGL Graph definitions
// =====================
// Graph Type with nested interior edge properties for Cost Flow Algorithms
typedef adjacency_list_traits<vecS, vecS, directedS> Traits;
typedef adjacency_list<vecS, vecS, directedS, no_property,
    property<edge_capacity_t, long,
        property<edge_residual_capacity_t, long,
            property<edge_reverse_t, Traits::edge_descriptor,
                property <edge_weight_t, long> > > > > Graph;
// Interior Property Maps
typedef property_map<Graph, edge_capacity_t>::type      EdgeCapacityMap;
typedef property_map<Graph, edge_weight_t >::type       EdgeWeightMap;
typedef property_map<Graph, edge_residual_capacity_t>::type ResidualCapacityMap;
typedef property_map<Graph, edge_reverse_t>::type       ReverseEdgeMap;
typedef graph_traits<Graph>::vertex_descriptor          Vertex;
typedef graph_traits<Graph>::edge_descriptor            Edge;
typedef graph_traits<Graph>::out_edge_iterator  OutEdgeIt; // Iterator

// Custom Edge Adder Class, that holds the references
// to the graph, capacity map, weight map and reverse edge map
// ===============================================================
class EdgeAdder {
    Graph &G;
    EdgeCapacityMap &capacitymap;
    EdgeWeightMap &weightmap;
    ReverseEdgeMap  &revedgemap;

public:
    EdgeAdder(Graph & G, EdgeCapacityMap &capacitymap, EdgeWeightMap &weightmap, ReverseEdgeMap &revedgemap)
        : G(G), capacitymap(capacitymap), weightmap(weightmap), revedgemap(revedgemap) {}

    void addEdge(int u, int v, long c, long w) {
        Edge e, reverseE;
        tie(e, tuples::ignore) = add_edge(u, v, G);
        tie(reverseE, tuples::ignore) = add_edge(v, u, G);
        capacitymap[e] = c;
        weightmap[e] = w;
        capacitymap[reverseE] = 0;
        weightmap[reverseE] = -w;
        revedgemap[e] = reverseE;
        revedgemap[reverseE] = e;
    }
};

int main() {
    const int N=7;
    const int v_source = 0;
    const int v_farm1 = 1;
    const int v_farm2 = 2;
    const int v_farm3 = 3;
    const int v_shop1 = 4;
    const int v_shop2 = 5;
    const int v_target = 6;

    // Create Graph and Maps
    Graph G(N);
    EdgeCapacityMap capacitymap = get(edge_capacity, G);
    EdgeWeightMap weightmap = get(edge_weight, G);
    ReverseEdgeMap revedgemap = get(edge_reverse, G);
    ResidualCapacityMap rescapacitymap = get(edge_residual_capacity, G);
    EdgeAdder eaG(G, capacitymap, weightmap, revedgemap);

    // Add the edges
    eaG.addEdge(v_source,v_farm1,3,0);
    eaG.addEdge(v_source,v_farm2,1,0);
    eaG.addEdge(v_source,v_farm3,2,0);

    eaG.addEdge(v_farm1,v_shop1,1,1);
    eaG.addEdge(v_farm1,v_shop2,1,5);
    eaG.addEdge(v_farm2,v_shop1,1,2);
    eaG.addEdge(v_farm2,v_shop2,1,2);
    eaG.addEdge(v_farm3,v_shop1,1,3);
    eaG.addEdge(v_farm3,v_shop2,2,2);

    eaG.addEdge(v_shop1,v_target,3,0);
    eaG.addEdge(v_shop2,v_target,3,0);

    // Run the algorithm

    // Option 1: Min Cost Max Flow with cycle_canceling
    int flow1 = push_relabel_max_flow(G, v_source, v_target);
    cycle_canceling(G);
    int cost1 = find_flow_cost(G);
    cout << "-----------------------" << endl;
    cout << "Minimum Cost Maximum Flow with cycle_canceling()" << endl;
    cout << "flow " << flow1 << endl; // 5
    cout << "cost " << cost1 << endl; // 12

    // Option 2: Min Cost Max Flow with successive_shortest_path_nonnegative_weights
    successive_shortest_path_nonnegative_weights(G, v_source, v_target);
    int cost2 = find_flow_cost(G);
    int flow2 = 0;
    // Iterate over all edges leaving the source to sum up the flow values.
    OutEdgeIt e, eend;
    for(tie(e, eend) = out_edges(vertex(v_source,G), G); e != eend; ++e) {
        flow2 += capacitymap[*e] - rescapacitymap[*e];
    }
    cout << "-----------------------" << endl;
    cout << "Minimum Cost Maximum Flow with successive_shortest_path_nonnegative_weights()" << endl;
    cout << "flow " << flow2 << endl; // 5
    cout << "cost " << cost2 << endl; // 12
    cout << "-----------------------" << endl;

    return 0;
}


#include <iostream>

#include <Eigen/Dense>

Eigen::MatrixXd Jacobian(const Eigen::MatrixXd &A, Eigen::VectorXd &z) {
	int n = z.size() - 1;
	int lambda = z(n);

	Eigen::MatrixXd J(n + 1, n + 1);
	J.topLeftCorner(n , n) = (A - z(n) * Eigen::MatrixXd::Identity(n, n));
	J.row(n) = -z.transpose();
	J.col(n) = -z;

	// overwrite value back to 0
	J(n, n) = 0;

	return J;
}

void eigNewton(const Eigen::MatrixXd &A, double tol, int maxItr, Eigen::VectorXd &z) {

	int n = z.size() - 1;
	Eigen::VectorXd F(n + 1);

	for(int i = 0; i < maxItr; i++) {
		Eigen::VectorXd x = z.head(n);
		F.head(n) = A * x - z(n) * x;
		F(n) = 1 - 0.5 * x.squaredNorm();

		if(F.squaredNorm() < tol) {
			return;
		}

		z += Jacobian(A, z).fullPivLu().solve(-F);
	}
}

int main() {
	int n = 2;
	int m = n + 1;

	Eigen::VectorXd x = Eigen::VectorXd::Ones(n);
	x << 1.0, 0.0;
	double lambda = 0.0;

	Eigen::VectorXd z(m);
	z.head(n) = x;
	z(n) = lambda;

	Eigen::MatrixXd A = Eigen::MatrixXd::Ones(n, n);
	A << 2, 1, 1, 2;

	eigNewton(A, .0001, 10, z);

	std::cout << "eigenvector:" << std::endl;
	std::cout << z.head(n) << std::endl;
	std::cout << "eigenvalue: " << z(n) << std::endl;

	return 0;
}


#ifndef matrix_hpp
#define matrix_hpp

#include <three-dim-util/camera.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace threedimutil
{
    inline Eigen::Matrix4d make_perspective(double fov,
                                            double aspect,
                                            double near,
                                            double far)
    {
        const double val = std::tan(0.5 * fov);

        Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
        mat(0, 0) = 1.0 / (aspect * val);
        mat(1, 1) = 1.0 / (val);
        mat(2, 2) = far / (near - far);
        mat(3, 2) = - 1.0;
        mat(2, 3) = - 2.0 * (far * near) / (far - near);

        return mat;
    }

    inline Eigen::Matrix4d make_perspective(double   fov,
                                            unsigned frame_width,
                                            unsigned frame_height,
                                            double   near,
                                            double   far)
    {
        const double aspect = static_cast<double>(frame_width) / static_cast<double>(frame_height);

        return make_perspective(fov, aspect, near, far);
    }

    inline Eigen::Matrix4d make_look_at(const Eigen::Vector3d& camera_position,
                                        const Eigen::Vector3d& target_position,
                                        const Eigen::Vector3d& up_direction)
    {
        Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();

        const Eigen::Vector3d forward = (target_position - camera_position).normalized();
        const Eigen::Vector3d side    = forward.cross(up_direction).normalized();
        const Eigen::Vector3d up      = side.cross(forward);

        mat.block<1, 3>(0, 0) = side.transpose();
        mat.block<1, 3>(1, 0) = up.transpose();
        mat.block<1, 3>(2, 0) = - forward.transpose();
        mat(0, 3) = - side.dot(camera_position);
        mat(1, 3) = - up.dot(camera_position);
        mat(2, 3) = forward.dot(camera_position);

        return mat;
    }

    inline Eigen::Matrix4d make_look_at(const Camera& camera)
    {
        return make_look_at(camera.position(), camera.target(), camera.up());
    }

    inline Eigen::Matrix4d make_ortho(double left,
                                      double right,
                                      double bottom,
                                      double top,
                                      double near,
                                      double far)
    {
        const double dx = right - left;
        const double dy = top - bottom;
        const double dz = far - near;
        const double tx = - (right + left) / dx;
        const double ty = - (top + bottom) / dy;
        const double tz = - (far + near) / dz;
        return (Eigen::Matrix4d() <<
                2.0 / dx,      0.0,        0.0,  tx,
                     0.0, 2.0 / dy,        0.0,  ty,
                     0.0,      0.0, - 2.0 / dz,  tz,
                     0.0,      0.0,        0.0, 1.0
                ).finished();
    }

    inline Eigen::Matrix4d make_ortho_2d(double left,
                                         double right,
                                         double bottom,
                                         double top)
    {
        const double dx = right - left;
        const double dy = top - bottom;
        const double tx = - (right + left) / dx;
        const double ty = - (top + bottom) / dy;
        return (Eigen::Matrix4d() <<
                2.0 / dx,      0.0,   0.0,  tx,
                     0.0, 2.0 / dy,   0.0,  ty,
                     0.0,      0.0, - 1.0, 0.0,
                     0.0,      0.0,   0.0, 1.0
                ).finished();
    }
}

#endif /* matrix_hpp */


/*
       fft.cpp

  nabla^2 u = exp(-x^2-y^2)  on (0,1)^2 w/ u=0 on BC

g++ -I/usr/local/include/eigen3

*/

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <complex>
#include <Eigen/Dense>
#include <unsupported/Eigen/FFT>

using namespace std;

void sineTransform2D(Eigen::MatrixXd & Mk, const Eigen::MatrixXd & Mx) {
  // Mk_{nm} = sum_{ij=1}^{N-1} Mx_{ij} sin(in pi/N) sin (jm pi/N)
  int N = Mx.rows();
  int M = Mx.cols();
  Eigen::FFT<double> fft;
  Eigen::MatrixXd Mrow(N,M);
  std::vector<double> f(2*N,0.0);
  std::vector<std::complex<double> > fkk(2*N,0.0);
  // sine transform rows
  for (int j=0;j<M;j++) {
    for (int i=1;i<N;i++) {
      f[i] = Mx(i,j);
      f[2*N-i] = -Mx(i,j);
    }
    f[0] = 0.0;
    f[N] = 0.0;
    fft.fwd(fkk,f);
    for (int m=0;m<N;m++)  Mrow(m,j) = -1.0*imag(fkk[m])/2.0;
  }
  // sine transform columns
  f.resize(2*M);
  for (int m=0;m<N;m++) {
    for (int j=1;j<M;j++) {
      f[j] = Mrow(m,j);
      f[2*M-j] = -Mrow(m,j);
    }
    f[0] = 0.0;
    f[M] = 0.0;
    fft.fwd(fkk,f);
    for (int n=0;n<M;n++) Mk(m,n) = -1.0*imag(fkk[n])/2.0;
  }
  return;
}

int main (void) {
  ofstream ofs;
  ofs.open("fft.dat");
  int N;
  cout << "enter N " << endl;
  cin >> N ;
  double del = 1.0/(N+1);

  Eigen::MatrixXd rho(N+1,N+1);
  Eigen::MatrixXd rhoHat(N+1,N+1);
  Eigen::MatrixXd uHat(N+1,N+1);
  Eigen::MatrixXd u(N+1,N+1);

  for (int i=1;i<N+1;i++) {
  for (int j=1;j<N+1;j++) {
    double x = del*i;
    double y = del*j;
    rho(i,j) = exp(-(x*x+y*y)/0.1);
    // if (j == N) rho(i,j) = rh - vz/(dely*dely);
  }}
  sineTransform2D(rhoHat,rho);
  for (int n=1;n<N+1;n++) {
  for (int m=1;m<N+1;m++) {
    uHat(n,m) = rhoHat(n,m)/(2.0*(cos(n*M_PI/(N+1))-1.0)/(del*del) +
                         2.0*(cos(m*M_PI/(N+1))-1.0)/(del*del)) ;
  }}
  // invert to obtain u(n,m)
  sineTransform2D(u,uHat);
  for (int i=1;i<N+1;i++) {
  for (int j=1;j<N+1;j++) {
    ofs << i*del << " " << j*del << " " << u(i,j)*4/((N+1)*(N+1)) << endl;
  }
  ofs << "   " << endl;
  }

  cout << " x y u in fft.dat " << endl;
  ofs.close();
  return 0;
}


/** @file
 * @brief NPDE WaveABC2D
 * @author Erick Schulz
 * @date 11/12/2019
 * @copyright Developed at ETH Zurich
 */

#include "waveabc2d.h"

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

namespace WaveABC2D {

/** @brief The evolution is performed by successively solving a linear sytem
 *
 *                        A * x(k) = b(k-1)
 *
 * at each time step. The matrix A is independent of time and completely
 * determined by the equation's parameter epsilon and the fixed uniform step
 * size of the method. Here, x will be a (M + 1) x 2 matrix storing the discrete
 * solution in its first column and the discrete time derivative in the second
 * column. The discrete time derivative is needed, because it is introduced as
 * an unknown in the implicit method to reduce the order of the ODE.
 *
 * @param epsilon Equation parameter in [0,1].
 * @param M Number of timesteps.*/
/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd scalarImplicitTimestepping(double epsilon, unsigned int M) {
  /* PROBLEM SETUP */
  double step_size = 1.0 / M;  // time step "tau"
  // Rows contain sequence of solutions states
  Eigen::MatrixXd x(M + 1, 2);
  // INITIAL DATA
  x(0, 0) = 1.0;
  x(0, 1) = 1.0;

  Eigen::MatrixXd A(2, 2);
  Eigen::VectorXd b(2);
  // clang-format off
    A << epsilon + 0.5*step_size*(1-epsilon), 0.5*step_size,
         -0.5*step_size,                      1.0;
  // clang-format on
  // TIMESTEPPING
  for (int k = 1; k < M + 1; k++) {
    // Update right hand side
    b(0) = (epsilon - 0.5 * step_size * (1 - epsilon)) * x(k - 1, 0) -
           0.5 * step_size * x(k - 1, 1);
    b(1) = x(k - 1, 1) + 0.5 * step_size * x(k - 1, 0);
    // Solve the linear system
    x.row(k) = A.fullPivLu().solve(b);
  }
  return x.col(1);
}  // scalarImplicitTimestepping
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
void testConvergenceScalarImplicitTimestepping() {
  std::cout << "Testing convergence of the implicit method." << std::endl;
  int nIter = 13;    // total number of iterations
  unsigned int M;    // number of equidistant steps
  double step_size;  // time step "tau"
  double epsilon = 0.2;

  // Error between the approx solutions as given by the implicit method
  // and the exact solution vector computed from the anlytic formula vector
  // computed from the anlytic formula
  std::cout << "Computing approximate solutions..." << std::endl;
  double diff;  // temporary variable used to compute error at various nodes
  double max_norm_errors[nIter];  // errors vector for all approx. sols
  Eigen::VectorXd approx_sol_vec;
  Eigen::VectorXd exact_sol_vec;
  unsigned int M_stored[13] = {10,  20,  30,  40,  50,  60, 80,
                               100, 160, 200, 320, 500, 640};
  for (int k = 0; k < nIter; k++) {
    // M = 10 * std::pow(2, k);
    M = M_stored[k];
    exact_sol_vec.resize(M + 1);
    step_size = 1.0 / M;
    // Creating exact solution vector for epsilon = 1/5. This vector is created
    // by evaluating the exact solution using the analytic formula
    //                      x(t) = exp(-2*t)*cos(t)
    // at the equidistant nodes of the time steps.
    for (int i = 0; i < M + 1; i++) {
      exact_sol_vec(i) =
          std::exp(-2 * i * step_size) *
          (3 * std::sin(i * step_size) + std::cos(i * step_size));
    }
    // Computing approximate solution
    approx_sol_vec = scalarImplicitTimestepping(epsilon, M);
    // Computing the error in the maximum norm
    for (int i = 0; i < M + 1; i++) {
      max_norm_errors[k] = 0;
      diff = std::abs(approx_sol_vec(i) - exact_sol_vec(i));
      max_norm_errors[k] =
          (diff > max_norm_errors[k]) ? diff : max_norm_errors[k];
    }
  }
  // Computing rates of convergence
  /*std::cout << "Computing convergence rates..." << std::endl;
    double rates[nIter - 1];
    double avg_rate = 0.0;
    for (int k = 0; k < nIter - 1; k++) {
      rates[k] = log2(max_norm_errors[k] / max_norm_errors[k + 1]);
      avg_rate += rates[k];
    }
    avg_rate = avg_rate / (nIter - 1);
  */
  /* SAM_LISTING_END_2 */

  // Printing results
  std::cout << "\n" << std::endl;
  std::cout << "*********************************************************"
            << std::endl;
  std::cout << "         Convergence of the Implicit Method              "
            << "\n                  (epsilon = " << epsilon << ")" << std::endl;
  std::cout << "*********************************************************"
            << std::endl;
  std::cout << "--------------------- RESULTS ---------------------------"
            << std::endl;
  std::cout << "Iteration"
            << "\t| Nsteps"
            << "\t| error" << std::endl;
  //            << "\t\t| rates" << std::endl;
  std::cout << "---------------------------------------------------------"
            << std::endl;
  for (int k = 0; k < nIter; k++) {
    std::cout << k << "\t"
              << "\t|" << M_stored[k] /*10 * std::pow(2, k)*/ << "\t\t|"
              << max_norm_errors[k];
    /*  if (k > 0) {
          std::cout << "\t|" << rates[k - 1];
        }
    */
    std::cout << "\n";
  }
  std::cout << "---------------------------------------------------------"
            << std::endl;
  /*    std::cout << "Average rate of convergence: " << avg_rate << "\n" <<
   * std::endl;
   */

}  // testConvergenceScalarImplicitTimestepping
/* SAM_LISTING_END_2 */

/* Implementing member functions of class progress_bar */
void progress_bar::write(double fraction) {
  // clamp fraction to valid range [0,1]
  if (fraction < 0)
    fraction = 0;
  else if (fraction > 1)
    fraction = 1;

  auto width = bar_width - message.size();
  auto offset = bar_width - static_cast<unsigned>(width * fraction);

  os << '\r' << message;
  os.write(full_bar.data() + offset, width);
  os << " [" << std::setw(3) << static_cast<int>(100 * fraction) << "%] "
     << std::flush;
}

}  // namespace WaveABC2D


#pragma once

#include <Eigen/Dense>

namespace math
{

// Returns true if the matrices are element-wise equal within tolerance.
// Throws an exception if the two matrices are of different sizes.
bool is_equal(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2, const double tol=1e-12);

// Returns true if a matrix is equal to its transpose. Returns false otherwise.
// Throws an exception if the matrix is not square.
bool is_symmetric(const Eigen::MatrixXd &mat, const double tol=1e-12);

// Computes the Gradient of func() using finite differencing around pt. The gradient has the same dimensions as the
// input for func(), which should be the same as the dimension of pt.
Eigen::VectorXd gradient(
        const std::function<double(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt,
        const double delta = 1e-2);

// Computes the Jacobian of func() using finite differencing around pt. Jacobian has dimension [output_dim x input_dim]
// where output_dim is the dimension of the output of func() and input_dim is the input dimension of
// func(), which should be same as the dimension of pt.
Eigen::MatrixXd jacobian(
        const std::function<Eigen::VectorXd(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt,
        const double delta = 1e-2);

// Computes the Hessian of func() using finite differencing around pt. The Hessian has dimension [input_dim x input_dim]
// where input_dim is the dimension of the input for func(), which should be the same as the
// dimension of pt.
Eigen::MatrixXd hessian(
        const std::function<double(const Eigen::VectorXd&)> &func,
        const Eigen::VectorXd &pt,
        const double delta = 1e-2);

// Projects a symmetric matrix onto the PSD (positive semi-definite) cone by bumping up any
// eigenvalues <= 0 to min_eigval.  If min_eigval is 0 this projects onto the boundary of the PSD cone.
// For numerical stability, a value higher than 0 is recommended.
Eigen::MatrixXd project_to_psd(const Eigen::MatrixXd &symmetric_mat, const double min_eigval = 0);

// Throws exception if the matrix mat is not positive-semi definite with all eigenvalues
// greater than the specified min_eigval.
void check_psd(const Eigen::MatrixXd &square_mat, const double min_eigval = 0);

} // namespace math


#include <iostream>
#include <chrono>

#include <Eigen/Core>  // Eigen 核心部分
#include <Eigen/Dense>  // 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Geometry>  // Eigen/Geometry 模块提供了各种旋转和平移的表示

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>

#include <ceres/ceres.h>

struct CURVE_FITTING_COST{

    CURVE_FITTING_COST(double t, double y): _t(t), _y(y) {}

    template<typename T>
    bool operator() (const T *const x, T *residual) const {
        residual[0] = T(_y) - ceres::exp(x[0]*T(_t)*T(_t) + x[1]*T(_t) + x[2]);
        return true;
    }

    const double _t, _y;
};

int main(int argc, char **argv){

    double a_e = 2.0, b_e = -1.0, c_e = 5.0;  // 待拟合参数 及估计初值
    double a = 1.0, b = 2.0, c = 1.0;  // 待拟合参数 实际值
    cv::RNG rng;  // OpenCV 随机数产生器
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;

    // 生成数据
    int num_data = 100;
    std::vector<double> t_data, y_data;

    for(int i = 0; i < num_data; i++){
        double t = i / 100.0;
        double y = exp(a*t*t + b*t + c) + rng.gaussian(w_sigma * w_sigma);
        t_data.push_back(t);
        y_data.push_back(y);
    }

    double x[3] = {a_e, b_e, c_e};

    // 构建最小二乘问题
    ceres::Problem problem;
    for(int i = 0; i < num_data; i++){
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new
                CURVE_FITTING_COST(t_data[i], y_data[i])),
            nullptr,
            x
        );
    }

    // 配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);

    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast <std::chrono::duration<double>> (t2 - t1);
    std::cout << "time_cost = " << time_used.count() << "second" << std::endl;

    std::cout << summary.BriefReport() << std::endl;
    std::cout << "estimated_parameters = " << x[0] << " " << x[1] << " " << x[2] << std::endl;
    return 0;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  typedef Matrix<float,Dynamic,2> DataMatrix;
// let's generate some samples on the 3D plane of equation z = 2x+3y (with some noise)
DataMatrix samples = DataMatrix::Random(12,2);
VectorXf elevations = 2*samples.col(0) + 3*samples.col(1) + VectorXf::Random(12)*0.1;
// and let's solve samples * [x y]^T = elevations in least square sense:
Matrix<float,2,1> xy
 = (samples.adjoint() * samples).llt().solve((samples.adjoint()*elevations));
cout << xy << endl;

  return 0;
}


/*  C++ code for generating the Campbell-Robson CSF image.

This algorhtim generates one image in linear scale with the C(cols,rows)
function, and another image in logarithmic scale with the Cl(cols,rowas)
function.

*/
#include "grapher.cpp"
#include <iostream>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>       /* sqrt, sin, log, exp */

using namespace cv;
using namespace std;
namespace po = boost::program_options;

// precalculated constant = sqrt(2)*pi
#define sqrt2pi 4.442882938

//computes a sinusoidal function with variable frecuency
//given by: f = 1 / sqrt(2x)
double s( double x, int cols);

//computes a linear function with m = 127/(rows-1) and b = 0
double a( double y, int rows);

//creates the linear scale image given by:
//C(x) = s(x)*a(y)+128
Mat C( int cols, int rows);

//changes the linear varible x to a logarithmic scale
// f(x) = k*exp(alfa*x)
// f(0)	= fmax = k
// f(R-1) =	R-1
double f( int x, int R);

//Shows a row of a channel
std::vector<int> channelRow(Mat *pChannel, int pRow);

//creates the logarithmic scale image given by:
//C(x) = s(f(x))*a(f(y))+128
Mat Cl( int cols, int rows);

int main (int argc, const char* argv[])
{
	int cols = 512;
	int rows = 512;
  int row;
  po::options_description description("ImageShow Usage");
  description.add_options()
      ("help,h", "Display this help message")
      ("rows,r", po::value<int>(), "Rows size.")
      ("cols,c", po::value<int>(), "Cols size.")
      ("showrow,x", po::value<int>(), "Show row.")
      ("lineal,l", "Lineal scale.");

  po::positional_options_description p;
  p.add("file", -1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(description).positional(p).run(), vm);
  po::notify(vm);

  if(vm.count("help")){
    cout << description;
    return 0;
  }
  if(vm.count("rows")){
    rows = vm["rows"].as<int>();
  }
  if(vm.count("cols")){
    cols = vm["cols"].as<int>();
  }
  if(vm.count("lineal")){
    namedWindow( "lin", WINDOW_AUTOSIZE );
    imshow( "lin", C(cols,rows) );
    waitKey(0);
    return 0;
  }
  if(vm.count("showrow")){
    Mat tmp= Cl(cols,rows);
    row = vm["showrow"].as<int>();
    grapher<int> graph((int)cols/2+10,5,(int)cols/2+10, 128);
    std::vector<std::vector <int> > posiciones(1, std::vector<int>(cols));
    for(int i = 0; i< cols; i++){
        posiciones[0][i] = i;
    }
    std::vector<std::vector <int> > frecuencias;
    std::vector<std::string> titulo;
    frecuencias.push_back(channelRow(&tmp,row));
    titulo.push_back("Fila");
    std::cout << "todo bien" << std::endl;
    graph(&posiciones, &frecuencias, &titulo);
    waitKey(0);
    return 0;
  }

//linear scale image
  //logarithmic scale image
  namedWindow( "log", WINDOW_AUTOSIZE );
  imshow( "log", Cl(cols,rows) );
  waitKey(0);
  return 0;
}

// x : input value
// cols : maximum value / number of columns
double s( double x, int cols){
  double correct = 0.125;
	return sin (sqrt2pi * sqrt((cols-1)+correct-x));
}

// y : input value
// rows : maximum value / number of rows
double a( double y, int rows){
	return 127.0/(rows-1)*y;
}

// cols : number of desired columns on the output image
// rows : number of desired rows on the output image
Mat C( int cols, int rows){
	Mat image(rows, cols, CV_8UC1, Scalar(0));
  	for(int x=0;x<cols;x++){
  		for(int y=0;y<rows;y++){
  			image.at<uint8_t>(y,x) = (uint8_t) (s(x,cols)*a(y,rows)+128);
  		}
  	}
  	return image;
}

// x : input value
// R : maximum value
// k : maximum frequency
double f( int x, int R){
	double k = 10;
	double alfa = log(pow((double)(R-1)/k,(double)1/(R-1)));
	return k*exp(alfa*x);
}

// cols : number of desired colums on the output image
// rows : number of desired rows on the output image
Mat Cl( int cols, int rows){
	Mat image(rows, cols, CV_8UC1, Scalar(0));
  	for(int x=0;x<cols;x++){
  		for(int y=0;y<rows;y++){
  			image.at<uint8_t>(y,x) = (uint8_t) (s(f(x,cols),cols)*a(f(y,rows),rows)+128);
  		}
  	}
  	return image;
}

/**
*
* @param pChannel mat with the image
* @param pRow
*/
std::vector<int> channelRow(Mat *pChannel, int pRow){
  std::vector<int> fondo (pChannel->cols, 0);
  for(unsigned i = 0; i< pChannel->cols; i++){
    fondo.at(i)=(int)pChannel->at<uchar>(pRow, i);
  }
  return fondo;
}


/**
  *
  * @file MatrixOperation.hpp
  * @brief matlab like functions
  * @author Naoki Takahashi
  *
  **/

#pragma once

#include "Matrix.hpp"

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace Tools {
	namespace Math {
		template <typename T>
		T norm(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &matrix);

		template <typename T>
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> diagonal(const Eigen::Matrix<T, Eigen::Dynamic, 1> &vector);
		template <typename T>
		MatrixX<T> diagonal(const VectorX<T> &vector);

		template <typename T>
		T dot(const Eigen::Matrix<T, Eigen::Dynamic, 1> &vector1, const Eigen::Matrix<T, Eigen::Dynamic, 1> &vector2);

		template <typename T>
		Eigen::Matrix<T, 3, 1> cross(const Eigen::Matrix<T, 3, 1> &vector1, const Eigen::Matrix<T, 3, 1> &vector2);
	}
}


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "Vector.h"

BOOST_AUTO_TEST_CASE( CAN_ADD_TWO_VECTORS )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    Vector3<float> v2{0.2f, 0.2f, 0.2f};

    float expectedValue = 0.1f + 0.2f;

    Vector3<float> actualResult = v1 + v2;
    Vector3<float> expectedResult {expectedValue, expectedValue, expectedValue};

    BOOST_ASSERT(actualResult == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_SUBTRACT_TWO_VECTORS )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    Vector3<float> v2{0.2f, 0.2f, 0.2f};

    float expectedValue = 0.1f - 0.2f;

    Vector3<float> actualResult = v1 - v2;
    Vector3<float> expectedResult {expectedValue, expectedValue, expectedValue};

    BOOST_ASSERT(actualResult == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_MULTIPLY_TWO_VECTORS )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    Vector3<float> v2{0.2f, 0.2f, 0.2f};

    float expectedValue = 0.1f * 0.2f;

    Vector3<float> actualResult = v1 * v2;
    Vector3<float> expectedResult {expectedValue, expectedValue, expectedValue};

    BOOST_ASSERT(actualResult == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_SCALE_VECTORS )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    float scalar = 5.0f;

    float expectedValue = 0.1f * 5.0f;

    Vector3<float> actualResult = v1 * scalar;
    Vector3<float> actualResult2 = scalar * v1;
    Vector3<float> expectedResult {expectedValue, expectedValue, expectedValue};

    BOOST_ASSERT(actualResult == expectedResult);
    BOOST_ASSERT(actualResult2 == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_SCALE_VECTORS_MIXED )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    double scalar = 5.0;

    float expectedValue = 0.1f * 5.0f;

    Vector3<float> actualResult = v1 * scalar;
    Vector3<float> actualResult2 = scalar * v1;
    Vector3<float> expectedResult {expectedValue, expectedValue, expectedValue};

    BOOST_ASSERT(actualResult == expectedResult);
    BOOST_ASSERT(actualResult2 == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_CALCULATE_DOT )
{
    Vector3<float> v1{0.1f, 0.0f, 0.0f};
    Vector3<float> v2{0.1f, 0.0f, 0.0f};

    float actualResult = dot(v1, v2);
    float actualResult2 = dot(v2, v1);
    float expectedResult = 0.1f * 0.1f;

    BOOST_ASSERT(actualResult == expectedResult);
    BOOST_ASSERT(actualResult2 == expectedResult);
}

BOOST_AUTO_TEST_CASE( CAN_CALCULATE_CROSS )
{
    Vector3<float> v1{0.1f, 0.1f, 0.1f};
    Vector3<float> v2{0.25f, 0.15f, 0.35f};

    Vector3<float> actualResult = cross(v1, v2);
    Vector3<float> expectedResult {0.02f, -0.01f, -0.01f};

    BOOST_ASSERT(actualResult == expectedResult);
}


//
//  PolyfitBoost.hpp
//  Polyfit
//
//  Created by Patrick Löber on 23.11.18.
//  Copyright © 2018 Patrick Loeber. All rights reserved.
//
//  C++ implementation if polyfit using boost library. See also:
//     https://www.numpy.org/devdocs/reference/generated/numpy.polynomial.polynomial.polyfit.html
//     https://docs.scipy.org/doc/numpy-1.10.0/reference/generated/numpy.polyfit.html

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

/*
 Finds the coefficients of a polynomial p(x) of degree n that fits the data,
 p(x(i)) to y(i), in a least squares sense. The result p is a row vector of
 length n+1 containing the polynomial coefficients in incremental powers.

 param:
 xValues        x axis values
 yValues        y axis values
 degree         polynomial degree including the constant
 weights        optional, weights to apply to the y-coordinates of the sample points

 return:
 coefficients of a polynomial starting at the constant coefficient and
 ending with the coefficient of power to degree.
*/
template <typename T>
std::vector<T> polyfit_boost(const std::vector<T> &xValues, const std::vector<T> &yValues, const int degree, const std::vector<T>& weights = std::vector<T>())
{
    using namespace boost::numeric::ublas;

    if (xValues.size() != yValues.size())
        throw std::invalid_argument("X and Y vector sizes do not match");

    bool useWeights = weights.size() > 0 && weights.size() == xValues.size();

    // one more because of c0 coefficient
    int numCoefficients = degree + 1;

    size_t nCount = xValues.size();
    matrix<T> X(nCount, numCoefficients);
    matrix<T> Y(nCount, 1);

    // fill Y matrix
    for (size_t i = 0; i < nCount; i++)
    {
        if (useWeights)
            Y(i, 0) = yValues[i] * weights[i];
        else
            Y(i, 0) = yValues[i];
    }

    // fill X matrix (Vandermonde matrix)
    for (size_t nRow = 0; nRow < nCount; nRow++)
    {
        T nVal = 1.0f;
        for (int nCol = 0; nCol < numCoefficients; nCol++)
        {
            if (useWeights)
                X(nRow, nCol) = nVal * weights[nRow];
            else
                X(nRow, nCol) = nVal;
            nVal *= xValues[nRow];
        }
    }

    // transpose X matrix
    matrix<T> Xt(trans(X));
    // multiply transposed X matrix with X matrix
    matrix<T> XtX(prec_prod(Xt, X));
    // multiply transposed X matrix with Y matrix
    matrix<T> XtY(prec_prod(Xt, Y));

    // lu decomposition
    permutation_matrix<int> pert(XtX.size1());
    const std::size_t singular = lu_factorize(XtX, pert);
    // must be singular
    assert(singular == 0);

    // backsubstitution
    lu_substitute(XtX, pert, XtY);

    // copy the result to coeff
    return std::vector<T>(XtY.data().begin(), XtY.data().end());
}

/*
 Calculates the value of a polynomial of degree n evaluated at x. The input
 argument coefficients is a vector of length n+1 whose elements are the coefficients
 in incremental powers of the polynomial to be evaluated.

 param:
 coefficients       polynomial coefficients generated by polyfit() function
 xValues            x axis values

 return:
 Fitted Y values.
*/
template<typename T>
std::vector<T> polyval( const std::vector<T>& coefficients, const std::vector<T>& xValues )
{
    size_t nCount = xValues.size();
    size_t nDegree = coefficients.size();
    std::vector<T> yValues( nCount );

    for (size_t i = 0; i < nCount; i++ )
    {
        T yVal = 0;
        T xPowered = 1;
        T xVal = xValues[i];
        for (size_t j = 0; j < nDegree; j++ )
        {
            // multiply current x by coefficient
            yVal += coefficients[j] * xPowered;
            // power up the X
            xPowered *= xVal;
        }
        yValues[i] = yVal;
    }

    return yValues;
}