#ifndef BURKARDT_NON_HPP
#define BURKARDT_NON_HPP

/*
 * nonlinear equation example set from
 *  http://people.sc.fsu.edu/~jburkardt/f_src/test_nonlin/test_nonlin.html
 *  http://people.sc.fsu.edu/~jburkardt/f_src/test_nonlin/test_nonlin.f90
 */

#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

namespace ub = boost::numeric::ublas;

struct GenRosen {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i;

		y(0) = 1. - x(0);
		for (i=1; i<s; i++) {
			y(i) = 10. * (x(i) - x(i-1) * x(i-1));
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Powell {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);

		y(0) = x(0) + 10. * x(1);
		y(1) = sqrt(5.) * (x(2) - x(3));
		y(2) = (x(1) - 2. * x(2)) * (x(1) - 2. * x(2));
		y(3) = sqrt(10.) * (x(0) - x(3)) * (x(0) - x(3));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Wood {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);
		T tmp1, tmp2;

		tmp1 = x(1) - x(0) * x(0);
		tmp2 = x(3) - x(2) * x(2);

		y(0) = -200. * x(0) * tmp1 - (1. - x(0));
		y(1) = 200. * tmp1 + 20.2 * (x(1) - 1.) + 19.8 * (x(3) - 1.);
		y(2) = -180. * x(2) * tmp2 - (1. - x(2));
		y(3) = 180. * tmp2 + 20.2 * (x(3) - 1.) + 19.8 * (x(1) - 1.);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Watson {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j, k;
		T sum1, sum2, tmp, ti;

		for (i=0; i<s; i++) {
			y(i) = 0;
		}

		for (i=1; i<=29; i++) {
			ti = i / 29.;
			sum1 = 0.;
			tmp = 1.;
			for (j=1; j<s; j++) {
				sum1 += (T)j * tmp * x(j);
				tmp *= ti;
			}
			sum2 = 0;
			tmp = 1.;
			for (j=0; j<s; j++) {
				sum2 += tmp * x(j);
				tmp *= ti;
			}
			tmp = (sum1 - sum2 * sum2 - 1.) / ti;
			for (k=0; k<s; k++) {
				y(k) += tmp * ((T)k - 2. * ti * sum2);
				tmp *= ti;
			}
		}

		y(0) += 3. * x(0) - 2. * x(0) + x(1) + 2. * x(0)*x(0)*x(0);
		y(1) += x(1) - x(0) * x(0) - 1.;

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Chebyquad {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j;
		T t1, t2, t3;

		for (i=0; i<s; i++) {
			y(i) = 0.;
		}

		for (j=0; j<s; j++) {
			t1 = 1.;
			t2 = x(j);
			for (i=0; i<s; i++) {
				y(i) += t2;
				t3 = 2. * x(j) * t2 - t1;
				t1 = t2;
				t2 = t3;
			}
		}

		for (i=0; i<s; i++) {
			y(i) /= (T)s;
		}

		for (i=0; i<s; i++) {
			if ( (i+1) % 2 == 0) {
				y(i) += 1. / (T)((i+1)*(i+1)-1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Brown {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int i, j;
		T sum, prod;

		sum = 0.;
		for (i=0; i<s; i++) {
			sum += x(i);
		}

		for (i=0; i<s-1; i++) {
			y(i) = x(i) + sum - (T)(s+1);
		}

		prod = 1.;
		for (i=0; i<s; i++) {
			prod *= x(i);
		}

		y(s-1) = prod - 1.;

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct DBVP {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k;
		T h, tmp;

		h = 1. / (s + 1);

		for (k=0; k<s; k++) {
			tmp = x(k) + (T)(k+1) * h + 1.;
			y(k) = 2. * x(k) + 0.5 * h * h * tmp*tmp*tmp;
			if (k > 0) {
				y(k) -= x(k-1);
			}
			if (k < s-1) {
				y(k) -= x(k+1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct DIntEq {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int j, k;
		T h, tk, tj, sum1, sum2, tmp;

		h = 1. / (s + 1);

		for (k=0; k<s; k++) {
			tk = (k+1.) / (s+1.);
			sum1 = 0.;
			for (j=0; j<k+1; j++) {
				tj = (j+1.) * h;
				tmp = x(j) + tj + 1.;
				sum1 += tj * tmp*tmp*tmp;
			}
			sum2 = 0.;
			for (j=k+1; j<s; j++) {
				tj = (j+1.) * h;
				tmp = x(j) + tj + 1.;
				sum2 += (1. - tj) * tmp*tmp*tmp;
			}
			y(k) = x(k) + h * ( (1. - tk) * sum1 + tk * sum2) / 2.;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct VDim {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int j;
		T sum1, tmp;

		sum1 = 0.;

		for (j=0; j<s; j++) {
			sum1 += (j+1.) * (x(j) - 1.);
		}

		tmp = sum1 * (1. + 2. * sum1 * sum1);

		for (j=0; j<s; j++) {
			y(j) = x(j) - 1. + (j+1.) * tmp;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Broyden {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k;

		for (k=0; k<s; k++) {
			y(k) = (3. - 2. * x(k)) * x(k) + 1.;
			if (k > 0) {
				y(k) -= x(k-1);
			}
			if (k < s-1) {
				y(k) -= 2. * x(k+1);
			}
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct BroydenBand {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);
		int k, k1, k2, j;
		T tmp;

		for (k=0; k<s; k++) {
			k1 = k - 5;
			if (k1 < 0) k1 = 0;
			k2 = k + 1;
			if (k2 > s-1) k2 = s-1;

			tmp = 0.;
			for (j=k1; j<=k2; j++) {
				if (j != k) {
					tmp += x(j) * (1. + x(j));
				}
			}

			y(k) = x(k) + (2. + 5. * x(k)*x(k)) + 1. - tmp;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Hammarling2x2 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(4);

		y(0) = (x(0) * x(0) + x(1) * x(2)) - 0.0001;
		y(1) = (x(0) * x(1) + x(1) * x(3)) - 1.;
		y(2) = (x(2) * x(0) + x(3) * x(2)) - 0.;
		y(3) = (x(2) * x(1) + x(3) * x(3)) - 0.0001;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(4);
		for (i=0; i<4; i++) {
			x(i).assign(-100., 100.);
		}
	}
};

struct Hammarling3x3 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(9);

		y(0) = (x(0) * x(0) + x(1) * x(3) + x(2) * x(6)) - 0.0001;
		y(1) = (x(0) * x(1) + x(1) * x(4) + x(2) * x(7)) - 1.;
		y(2) = (x(0) * x(2) + x(1) * x(5) + x(2) * x(8));

		y(3) = (x(3) * x(0) + x(4) * x(3) + x(5) * x(6));
		y(4) = (x(3) * x(1) + x(4) * x(4) + x(5) * x(7)) - 0.0001;
		y(5) = (x(3) * x(2) + x(4) * x(5) + x(5) * x(8));

		y(6) = (x(6) * x(0) + x(7) * x(3) + x(8) * x(6));
		y(7) = (x(6) * x(1) + x(7) * x(4) + x(8) * x(7));
		y(8) = (x(6) * x(2) + x(7) * x(5) + x(8) * x(8)) - 0.0001;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(9);
		for (i=0; i<9; i++) {
			x(i).assign(-100., 100.);
		}
	}
};

struct P17 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) + x(1) - 3.;
		y(1) = x(0) * x(0) + x(1) * x(1) - 9.;;

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct P19 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * (x(0) * x(0) + x(1) * x(1));
		y(1) = x(1) * (x(0) * x(0) + x(1) * x(1));

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(2);
		for (i=0; i<2; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct P20 {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(1);

		y(0) = x(0) * (x(0) - 5.) * (x(0) - 5.);

		return y;
	}

	template<class T>
	void range(ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(1);
		for (i=0; i<1; i++) {
			x(i).assign(-10., 10.);
		}
	}
};

struct Chandrasekhar {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int s = x.size();
		ub::vector<T> y(s);

		ub::vector<T> mu(s);
		T sum, term;
		int i, j;

		for (i=0; i<s; i++) {
			y(i) = x(i);
		}
		for (i=0; i<s; i++) {
			mu(i) = (2.*(i+1.)-1.) / (2. * s);
		}

		for (i=0; i<s; i++) {
			sum = 0.;
			for (j=0; j<s; j++) {
				sum += mu(i) * x(j) / (mu(i) + mu(j));
			}
			term = 1. - 0.9 * sum / (2. * s);
			y(i) -= 1. / term;
		}

		return y;
	}

	template<class T>
	void range(int n, ub::vector< kv::interval<T> >& x) {
		int i;

		x.resize(n);
		for (i=0; i<n; i++) {
			x(i).assign(-3., 3.);
		}
	}
};

#endif // BURKARDT_NON_HPP


#include "soar.hpp"

#include <Eigen/Dense>
#include <fmt/format.h>
#include <vector>

using namespace Eigen;

Soar::Soar(const Ref<const MatrixXd> &matA, const Ref<const MatrixXd> &matB)
    : ndim_(matA.rows()), matA_(matA), matB_(matB),
      u_(VectorXd::Random(ndim_)) {}

MatrixXd Soar::compute(int n) {
  VectorXd q = u_ / u_.norm();
  VectorXd f = VectorXd::Zero(ndim_);

  // initialize
  MatrixXcd matQ = MatrixXcd::Zero(ndim_, n);
  MatrixXcd matP = MatrixXcd::Zero(ndim_, n);
  MatrixXcd matT = MatrixXcd::Zero(n, n);
  std::vector<int> deflation;

  matQ.col(0) = q;

  for (int i = 0; i < n - 1; ++i) {
    // Recurrence role
    VectorXcd r = matA_ * matQ.col(i) + matB_ * matP.col(i);
    std::complex<double> norm_init = r.norm();
    MatrixXcd basis = matQ.leftCols(i + 1);

    // Modified Gram Schmidt procedure
    // First orthogonalization
    VectorXcd coef = VectorXd::Zero(i + 1);
    for (int j = 0; j < i + 1; ++j) {
      // Projection coeficients and projection subtraction
      VectorXcd v = basis.col(j);
      coef(j) = v.dot(r);
      r -= coef(j) * v;
    }
    // Saving coeficients
    matT.col(i).head(i + 1) = coef;

    // Reorthogonalization, if needed.
    if (r.norm() < 0.7 * norm_init) {
      // Second Gram Schmidt orthogonalization
      for (int j = 0; j < i + 1; ++j) {
        VectorXd v = basis.col(j);
        coef(j) = v.dot(r);
        r -= coef(j) * v;
      }
      matT.col(i).head(i + 1) += coef;
    }

    double r_norm = r.norm();
    matT(i + 1, i) = r_norm;

    // check for breakdown
    if (r_norm > tol_) {
      matQ.col(i + 1) = r / r_norm;
      VectorXd e_i = VectorXd::Zero(i + 1);
      e_i(i) = 1.0;
      // VectorXd v_aux = matT.block(1, 0, i + 1, i + 1).ldlt().solve(e_i);
      VectorXd v_aux =
          matT.block(1, 0, i + 1, i + 1).colPivHouseholderQr().solve(e_i);
      f = matQ.leftCols(i + 1) * v_aux;
    } else {
      // Deflation reset
      matT(i + 1, i) = 1.0;
      matQ.col(i + 1) = VectorXd::Zero(ndim_);
      VectorXd e_i = VectorXd::Zero(i + 1);
      e_i(i) = 1.0;
      // VectorXd v_aux = matT.block(1, 0, i + 1, i + 1).ldlt().solve(e_i);
      VectorXd v_aux =
          matT.block(1, 0, i + 1, i + 1).colPivHouseholderQr().solve(e_i);
      f = matQ.leftCols(i + 1) * v_aux;

      // Deflation verification
      VectorXd f_proj;
      for (int k : deflation) {
        VectorXd p = matP.col(k);
        double coef_f = p.dot(f) / p.dot(p);
        f_proj = f - coef_f * p;
      }

      if (f_proj.norm() > tol_) {
        deflation.push_back(i);
      } else {
        fmt::print("SOAR lucky breakdown.\n");
        break;
      }
    }
    matP.col(i + 1) = f;
  }

  fmt::print("zero1: {:9.3f}\n",
             (matQ.transpose() * matQ - MatrixXd::Identity(n, n)).norm());

  VectorXd e_n = VectorXd::Zero(n - 1);
  e_n(n - 2) = 1.0;
  VectorXd r = matA_ * matQ.col(n - 2) + matB_ * matP.col(n - 2);
  for (int i = 0; i < n - 1; ++i) {
    double coef = matQ.col(i).dot(r);
    r -= coef * matQ.col(i);
  }
  double nm = (matA_ * matQ.leftCols(n - 1) + matB_ * matP.leftCols(n - 1) -
               matQ.leftCols(n - 1) * matT.topLeftCorner(n - 1, n - 1) -
               r * e_n.transpose())
                  .norm();
  fmt::print("zero2: {:9.3f}\n", nm);
  return matQ;
}


//test_ptf_var.cpp

#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include<memory>

#include <Eigen/Dense>

#include "compute_returns_eigen.h"
#include "portfolio.h"
#include "instrument.h"
#include "ptf_var.h"
#include "var_model.h"
#include "compute_var.h"

using namespace Eigen;
using namespace std;

void readCSV(std::istream &input, std::vector< std::vector<std::string> > &output)
//https://www.gamedev.net/topic/444193-c-how-to-load-in-a-csv-file/
{
	std::string csvLine;
	// read every line from the stream
	while( std::getline(input, csvLine) )
	{
		std::istringstream csvStream(csvLine);
		std::vector<std::string> csvColumn;
		std::string csvElement;
		// read every element from the line that is seperated by commas
		// and put it into the vector or strings
		while( std::getline(csvStream, csvElement, ',') )
		{
			csvColumn.push_back(csvElement);
		}
		output.push_back(csvColumn);
	}
}

int main()
{

    try{

    // Read mid FX fix for currency pairs majors and exotics
    // daily series obtained for Bank of England through
	//https://www.quandl.com

	std::fstream file("/home/mrnoname/Documents/VaR/data/StockIndexData.csv", ios::in);
	if(!file.is_open())
	{
		std::cout << "File not found!\n";
		return 1;
	}
	// typedef to save typing for the following object
	typedef std::vector< std::vector<std::string> > csvVector;
	csvVector csvData;

	readCSV(file, csvData);

    //test
    for(size_t i = 0;i < 5; ++i){
        for(size_t j = 0;j < csvData[i].size();++j){
            cout << csvData[i][j] << '\t';

        }

        cout << endl;
    }
    cout << endl;

    // Remove lines with missing values

    size_t n(csvData.size() - 1);
    size_t m(csvData[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n-1062));

    for(size_t i = 1062;i < n;++i){
        for(size_t j = 1;j < csvData[i].size();++j){
            std::string tmp = csvData[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1062] = 99999.;
            }
            else{
                _prices[j-1][i-1062] = std::stod(tmp);
            }
        }
    }

    std::vector<std::string> indexNames(csvData[0].size() - 1);

    for(size_t i = 1;i < csvData[0].size();++i){
        indexNames[i-1] = csvData[0][i];
    }

	//Remove missing values to compute trailling returns
    //Asynchornous time series. Shift to the next value
    Mat prices;
    prices.resize(m,Vec(0));

	for(size_t i = 0;i < _prices.size();++i){
        for(size_t j = 0;j < _prices[i].size();++j){
            if(!((_prices[i][j] == 99999) || (_prices[i][j] == 0)))
                prices[i].push_back(_prices[i][j]);
        }
	}

    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,252,true));
	// 252 / 4 = 63 - 3 months
    // 4 * 252 = 1008 use 4 years of data to compute mean, and std dev

    Mat _rtns = cr->getReturns();

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

    // Case of full replication of index - DJIA,GSPC,NDX,GDAXI,FCHI,SSEC,SENSEX : 7 indices

	double a = double(1./7.); //cout << a << endl;

	std::vector<double> weights{a,a,a,a,a,a,a}; //initialization. Equi-weighted asset for mere convenience

	Ptf _ptf;

	for(unsigned int i = 0;i < 7;++i){
        shared_ptr<Instrument> instrument(new DeltaOne());
        auto p = std::make_pair(i,instrument);
		_ptf.push_back(p);
	}

	shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, false, 1.e+07));

	cout << "ptf's avg rtn: " << ptf->getMeanPtfRn() << endl;
	cout << "ptf's vol: " << ptf->getPtfSdev() << endl<< endl;

	double alpha = .05;

	VaRPtfCompute model(ptf, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - equi index " << alpha << " : " << model.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	RiskMetricsVaR var1; //(.05,.94,false);

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics(ptf, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics.computeVaR() << endl;

	// 2. GARCH

	GarchVaR var2; //(.05, 0., .25, .75, false);

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch(ptf, var2);

	cout << "GARCH VaR: " << VaRGarch.computeVaR() << endl;

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

	// 3. Historical method

	HistoricalVaR var3;

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical(ptf, var3);

	cout << "Historical VaR: " << VaRHistorical.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	HistoricalVaR var4(.05, .98, hybrid);

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical1(ptf, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical1.computeVaR() << endl;

	// 5. Historical method - HW method

	HistoricalVaR var5(.05, .94, hw);

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical2(ptf, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical2.computeVaR() << endl;

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

	// Compute conponent VaR

	Vec compVaR = model.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << compVaR[i] << endl;

    double sumCompVaR(0.);
    for(auto& i : compVaR) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	Vec marVaR = model.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << marVaR[i] << endl;

	// Compute incremental VaR

	double amount = 1.e+06;

	cout << endl << "incremental VaR - add " << amount << " : " << model.computeIncrementalVaR(amount) << endl;

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

    weights = {.1,.15,.2,.1,.1,.2,.15};

	shared_ptr<Portfolio> ptf1(new Portfolio(_ptf, weights, cr, false, 1.e+07));

	cout << "ptf's avg rtn: " << ptf1->getMeanPtfRn() << endl;
	cout << "ptf's vol: " << ptf1->getPtfSdev() << endl<< endl;

	VaRPtfCompute model1(ptf1, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - active index " << alpha << " : " << model1.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics1(ptf1, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics1.computeVaR() << endl;

	// 2. GARCH

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch1(ptf1, var2);

	cout << "GARCH VaR: " << VaRGarch1.computeVaR() << endl;

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

	// 3. Historical method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical11(ptf1, var3);

	cout << "Historical VaR: " << VaRHistorical11.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical12(ptf1, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical12.computeVaR() << endl;

	// 5. Historical method - HW method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical13(ptf1, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical13.computeVaR() << endl;

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

	// Compute conponent VaR

	compVaR = model1.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << compVaR[i] << endl;

    sumCompVaR = 0.;
    for(auto& i : compVaR) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	marVaR = model1.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 7;++i)
		cout << indexNames[i] << ": " << marVaR[i] << endl;

	// Compute incremental VaR

	cout << endl << "incremental VaR - add " << amount << " : " << model1.computeIncrementalVaR(amount) << endl;

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

	// Buy 1 call and 1 put on S&P 500. Sell .5 unit of index

	// Case of derivatives, full replication of index

	Vec weights1 =  {1.,1.,-.5};

	Ptf _ptf1;

	shared_ptr<Instrument> instrument(new Derivatives(0.46118, 0.01013));
    _ptf1.push_back(std::make_pair(1,instrument));
    shared_ptr<Instrument> instrument1(new Derivatives(-0.52983, 0.00658));
    _ptf1.push_back(std::make_pair(1,instrument1));
    shared_ptr<Instrument> instrument2(new DeltaOne());
	_ptf1.push_back(std::make_pair(1,instrument2));

	/*
    Call @SPX 161216C02200000 Delta0.46118 Gamma0.01013 Rho0.38430 Theta-0.79242 Vega1.69184 Impvol0.11648

    Put @SPX 161216P02200000 Delta-0.52983 Gamma0.00658 Rho-0.34403 Theta-0.93300 Vega1.67799 Impvol0.11835

	*/

	shared_ptr<Portfolio> ptf2(new Portfolio(_ptf1, weights1, cr, false, 1.e+07));

    VaRPtfCompute model2(ptf2, alpha);

    // Compute ptf VaR of index

	cout << endl << "Portfolio VaR - equity derivatives " << alpha << " : " << model2.getPtfVaR() << endl;

	//-----------------------------------------------------------------
	cout << endl <<  "Compute daily VaR using different methods - alpha .05" << endl;

	// 1. Riskmetrics

	VaRParamCompute<Portfolio, RiskMetricsVaR> VaRRiskMetrics21(ptf2, var1);

	cout << "Riskmetrics VaR: " << VaRRiskMetrics21.computeVaR() << endl;

	// 2. GARCH

    VaRParamCompute<Portfolio, GarchVaR> VaRGarch22(ptf1, var2);

	cout << "GARCH VaR: " << VaRGarch22.computeVaR() << endl;

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

	// 3. Historical method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical21(ptf1, var3);

	cout << "Historical VaR: " << VaRHistorical21.computeVaR() << endl;

	// 4. Historical method - weighting scheme

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical22(ptf1, var4);

	cout << "Historical VaR w/ weighting scheme: " << VaRHistorical22.computeVaR() << endl;

	// 5. Historical method - HW method

	VaRnoneParamCompute<Portfolio, HistoricalVaR> VaRHistorical23(ptf1, var5);

	cout << "Historical VaR w/ HW weighting scheme: " << VaRHistorical23.computeVaR() << endl;

	// Compute conponent VaR

	Vec compVaR2 = model2.computeComponentVaR();

	cout << endl << "component VaR: " << endl;

	for(size_t i = 0;i < 3;++i)
		cout << i << ": " << compVaR2[i] << endl;

    sumCompVaR = 0.;
    for(auto& i : compVaR2) sumCompVaR += i;
    cout << "sum component VaR: " << sumCompVaR << endl;

	// Compute marginal VaR

	Vec marVaR2 = model2.computeMarginalVaR();

	cout << endl << "marginal VaR: " << endl;

	for(size_t i = 0;i < 3;++i)
		cout << i << ": " << marVaR2[i] << endl;

	// Compute incremental VaR

	cout << endl << "incremental VaR - add " << amount << " : " << model2.computeIncrementalVaR(amount) << endl;

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

    return 0;

    } catch (const std::exception& e) { // caught by reference to base
        std::cout << " a standard exception was caught, with message '"
                  << e.what() << "'\n";
    }

}


#include "RBGL.hpp"

#include <stdlib.h>

#include <boost/graph/simple_point.hpp>

extern "C"
{

#include <Rdefines.h>
#include <R_ext/Random.h>
#include <Rmath.h>

    using namespace std;
    using namespace boost;

    static void delta_and_tau
     (const Graph_ud& g, vector<int>& v_delta, vector<int>& v_tau)
    {
        Graph_ud::vertex_iterator vi, v_end;
        Graph_ud::adjacency_iterator ui, u_end, wi, w_end;

        int dv = 0, tv = 0;

        v_delta.clear();
        v_tau.clear();

        for ( tie(vi, v_end) = vertices(g); vi != v_end; ++vi )
        {
            // delta(v)
            dv = 0;
            for ( tie(ui, u_end) = adjacent_vertices(*vi, g);
                    ui != u_end; ++ui )
            {
                wi = ui;
                for ( ++wi; wi != u_end; ++wi )
                    if ( edge(*ui, *wi, g).second ) dv++;
            }
            v_delta.push_back(dv);

            // tau(v)
            dv = degree(*vi, g);
            tv = dv * ( dv - 1 ) / 2;
            v_tau.push_back(tv);
        }
    }

    SEXP clusteringCoef(
        SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in,
        SEXP weighted, SEXP R_v_weights_in)
    {
        int i;

        int NV = INTEGER(num_verts_in)[0];
        vector<double> v_weight(NV, 1);

        if ( INTEGER(weighted)[0] )
        {
            double* weights = REAL(R_v_weights_in);
            for ( i = 0; i < NV; i++ ) v_weight[i] = weights[i];
        }

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);
        vector<int> v_delta, v_tau;
        delta_and_tau(g, v_delta, v_tau);

        double nn = 0;		// count nodes w/ deg(v) >= 2
        double cG = 0;
	Graph_ud::vertex_descriptor v;
        for ( i = 0; i < NV; i++ )
        {
	    v = vertex(i, g);
            if ( out_degree(v, g) >= 2 && v_tau[i] > 0 )
            {
                cG += v_weight[i] * v_delta[i] / v_tau[i];
                nn += v_weight[i];
            }
        }

        if ( nn ) cG /= nn;

        SEXP ccoef;
        PROTECT(ccoef = NEW_NUMERIC(1));
        REAL(ccoef)[0] = cG;
        UNPROTECT(1);
        return(ccoef);
    }

    SEXP transitivity( SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in)
    {
        int NV = INTEGER(num_verts_in)[0];

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);
        vector<int> v_delta, v_tau;
        delta_and_tau(g, v_delta, v_tau);

	double tG = 0;
        double sum_dv = 0, sum_tv = 0;
        for ( int i = 0; i < NV; i++ )
        {
            sum_dv += v_delta[i];
            sum_tv += v_tau[i];
        }
        if ( sum_tv ) tG = sum_dv / sum_tv;

#if DEBUG
        cout << " sum_dv = " << sum_dv
        << " sum_tv = " << sum_tv
        << " v_delta.size() = " << v_delta.size()
        << " v_tau.size() = " << v_tau.size()
	<< " tG = " << tG
        << endl;
#endif

        SEXP tcoef;
        PROTECT(tcoef = NEW_NUMERIC(1));
        REAL(tcoef)[0] = tG;
        UNPROTECT(1);
        return(tcoef);
    }

    // uniformly sample in [1, n]
    static inline int uniformRandomNumber(const int n)
    {
	int j = (int)(n * unif_rand()) + 1;   // unif_rand in [0, 1)
	return j;
    }

    static inline int findIndex(const int r, const vector<int>& W)
    {
	unsigned int i;
	for ( i = 1; i < W.size(); i++ ) if ( r <= W[i] ) break;
	return i;
    }

    // input: a node in graph g
    // output: one neighbor of node n chosen uniformly randomly
    static inline void uniformRandomAdjacentNode
	(const Graph_ud::vertex_descriptor& v, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u,
	 Graph_ud::vertex_descriptor& w)
    {
	int nc = out_degree(v, g);

	Graph_ud::adjacency_iterator vi, v_end;
	tie(vi, v_end) = adjacent_vertices(v, g);

	switch (nc)
	{
	case 0:
	case 1: u = w = *vi;
		break;
	case 2:
		u = *vi; vi++;
		w = *vi;
		break;
	default:
		{
		int r1 = uniformRandomNumber(nc);
		int r2 = uniformRandomNumber(nc);

		while ( r1 == r2 ) r2 = uniformRandomNumber(nc);

		for ( int i = 0; vi != v_end; vi++, i++ )
		{
		    if ( i == r1 ) u = *vi;
		    if ( i == r2 ) w = *vi;
		}

		break;
		}
	}
#if DEBUG
	cout << " uniformRandomAdjacentNode: " << endl;
	cout << " n = " << n << endl;
	cout << " nc = " << nc << endl;
	cout << " *vi = " << *vi << endl;
	cout << " u = " << u << endl;
	cout << " w = " << w << endl;
#endif
    }

    static inline void uniformRandomAdjacentNode_i
	(const int n, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u,
	 Graph_ud::vertex_descriptor& w)
    {
	Graph_ud::vertex_descriptor v = vertex(n, g);
	uniformRandomAdjacentNode(v, g, u, w);
    }

    // Approximating Cw
    //    Outline of the algorithm:
    //    Input: integer k;
    //           array A[1..|V'|] of nodes V' = {v in V: d(v) >= 2}
    //           node weights w: V' -> N>0;
    //           adjacentcy array for each node
    //    Output: approximation of Cw
    //    Data: node variables: u, w;
    //           integer variables: r, l, j, W[0..|V'|]
    //    Algorithm:
    //    W[0] = 0
    //    for i = (1, ..., |V'|) do
    //       W[i] = W[i-1] + w(A[i])
    //    l = 0
    //    for i in (1, ..., k) do
    //    {
    //       r = UniformRandomNumber( {1,...,W[|V'|]} )
    //       j = FindIndex( j: W[j-1] < r <= W[j] )
    //       u = UniformRandomAdjacentNode(A[j])
    //       repeat
    //         w = UniformRandomAdjacentNode(A[j])
    //       until u != w
    //       if ( EdgeExists(u, w) then
    //          l = l + 1
    //    }
    //    return l/k

    SEXP clusteringCoefAppr(SEXP k_in,
        SEXP num_verts_in, SEXP num_edges_in, SEXP R_edges_in,
        SEXP weighted, SEXP R_v_weights_in)
    {
	// prepare for later unif_rand call
	GetRNGstate();

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

        int i, l, r, j;

        int k = INTEGER(k_in)[0];
        int NV = INTEGER(num_verts_in)[0];
        vector<int> v_weight(NV, 1);
        vector<int> W(NV+1, 0);

#if DEBUG
	cout << " inside clusteringCoefAppr "
		<< " k = " << k
		<< " NV = " << NV
		<< endl;
#endif

        if ( INTEGER(weighted)[0] )
        {
            double* weights = REAL(R_v_weights_in);
            for ( i = 0; i < NV; i++ ) v_weight[i] = (int) weights[i];
        }

        Graph_ud::vertex_descriptor u=Graph_ud::null_vertex(), w=Graph_ud::null_vertex();

	W[0] = 0;
	for ( i = 1; i < NV+1; i++ ) W[i] = W[i-1] + v_weight[i-1];

	// TODO: limit nodes to those w/ degree >= 2
	//       pick a number within range uniformaly
	//	 pick an adjacent node uniformaly randomly
	for ( l = 0, i = 0; i < k; i++ )
	{
	   r = uniformRandomNumber(W[NV]);
	   j = findIndex(r, W);
	   uniformRandomAdjacentNode_i(j-1, g, u, w);

	   if ( edge(u, w, g).second ) l++;

#if DEBUG
	   cout << " i = " << i;
	   cout << " r = " << r;
	   cout << " j = " << j;
	   cout << " l = " << l << endl;
#endif
	}

	double cG = double(l) / double(k);

        SEXP ccoef;
        PROTECT(ccoef = NEW_NUMERIC(1));
        REAL(ccoef)[0] = cG;
        UNPROTECT(1);
        return(ccoef);

    }

    inline bool prob_cmp(const simple_point<int>& p1,
			 const simple_point<int>& p2)
	{ return p1.y > p2.y; }

    //  To find a random node w/ probability d(u) / sum(d(V))
    //  The following closely mirrors the codes on
    //  Unequal probability sampling; without-replacement case
    //
    //    /* Record element identities */
    //    for (i = 0; i < n; i++)
    //        perm[i] = i + 1;
    //
    //    //* Sort probabilities into descending order */
    //    //* Order element identities in parallel */
    //    revsort(p, perm, n);
    //
    //    //* Compute the sample */
    //    totalmass = 1;
    //    for (i = 0, n1 = n-1; i < nans; i++, n1--) {
    //        rT = totalmass * unif_rand();
    //        mass = 0;
    //        for (j = 0; j < n1; j++) {
    //            mass += p[j];
    //            if (rT <= mass)
    //                break;
    //        }
    //        ans[i] = perm[j];
    //        totalmass -= p[j];
    //        for(k = j; k < n1; k++) {
    //            p[k] = p[k + 1];
    //            perm[k] = perm[k + 1];
    //        }
    //    }
    static void ProbRandomNode
	(const Graph_ud::vertex_descriptor& v, const Graph_ud& g,
	 Graph_ud::vertex_descriptor& u)
    {

	typedef graph_traits<Graph_ud>::vertex_iterator vertex_iterator;

	vertex_iterator vi, v_end;

	int NV = num_vertices(g);
	std::vector < simple_point<int> > pp(num_vertices(g));

	int i = 0, totalmass = 0;
	for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++, i++ )
	{
	   pp[i].x = i+1;
	   pp[i].y = out_degree(*vi, g);
	   totalmass += pp[i].y;
	}

	std::stable_sort(pp.begin(), pp.end(), prob_cmp);

	int j, k, n1, rT, mass;
	for ( i = 0, n1 = NV-1; i < NV; i++, n1-- )
	{
            rT = (int)(totalmass * unif_rand());
            mass = 0;
            for (j = 0; j < n1; j++) {
                mass += pp[j].y;
                if (rT <= mass) break;
            }
	    u = vertex(i, g);
	    if ( !edge(v, u, g).second ) break;

	    totalmass -= pp[j].y;
	    for ( k = j; k < n1; k++ ) pp[k] = pp[k+1];
	}
    }

    // Graph Generator:
    //    Outline of algorithm:
    //    Input: initial graph G: two connected nodes
    //           integer: n >= 3, d >= 2, o
    //    Output: graph G
    //    Algorithm:
    //    for ( i = (3, ..., n) do
    //    {
    //       v = NewNode()
    //       for 1, ..., Min(i-1, d) do
    //       {
    //          repeat
    //            u = RandomNode( with prob du / sum(d(v) )
    //          until node EdgeExists(v, u)
    //          AddEdge(v, u)
    //       }
    //       for 1, ..., o do
    //       {
    //          u = RandomAdjacentNode(v)
    //          repeat
    //             w = RandomAdjacentNode(v)
    //          until w != u
    //          if ( node EdgeExists(u, w) then
    //             AddEdge(u, w)
    //       }
    //    }

    SEXP graphGenerator(SEXP n_in, SEXP d_in, SEXP o_in)
    {
	int i, j;
        int n = INTEGER(n_in)[0];
        int d = INTEGER(d_in)[0];
        int o = INTEGER(o_in)[0];

	GetRNGstate();	// get random number generator ready

	// initial graph with 2 connected nodes
	Graph_ud g(2);
	boost::add_edge(0, 1, g);

        Graph_ud::vertex_descriptor v, u, w=Graph_ud::null_vertex();

	for ( i = 3; i <= n; i++ )
	{
	   // generate a new node
	   v = boost::add_vertex(g);

	   for ( j = 1; j <= min(i-1, d); j++ )
	   {
		ProbRandomNode(v, g, u);
		boost::add_edge(v, u, g);
	   }

	   for ( j = 1; j <= o; j++ )
	   {
	        uniformRandomAdjacentNode(v, g, u, w);

		if ( !edge(u, w, g).second )
		   boost::add_edge(u, w, g);
	   }
	}

#if DEBUG
	typedef graph_traits<Graph_ud>::vertex_iterator vertex_iterator;
	vertex_iterator vi, v_end;
	cout << " no. of vertices: " << num_vertices(g)
	     << " no. of edges:    " << num_edges(g)
	     << endl;

	for ( tie(vi, v_end) = vertices(g); vi != v_end; vi++, i++ )
	{
	   cout << " vertex: " << *vi
		<< " has degree: " << out_degree(*vi, g)
		<< endl;
	}
#endif

        int NE = num_edges(g);
        SEXP anslst, ncnt, ecnt, enlst;
        PROTECT(anslst = allocVector(VECSXP, 3));
        PROTECT(ncnt = NEW_INTEGER(1));
        PROTECT(ecnt = NEW_INTEGER(1));
        PROTECT(enlst = allocMatrix(INTSXP, 2, NE));

        INTEGER(ncnt)[0] = num_vertices(g);
        INTEGER(ecnt)[0] = NE;

	typedef graph_traits<Graph_ud>::edge_iterator edge_iterator;
	edge_iterator ei, e_end;
	for ( i = 0, tie(ei, e_end) = edges(g); ei != e_end ; ei++ )
        {
            INTEGER(enlst)[i++] = source(*ei, g);
            INTEGER(enlst)[i++] = target(*ei, g);
        }

	SET_VECTOR_ELT(anslst,0,ncnt);
	SET_VECTOR_ELT(anslst,1,ecnt);
        SET_VECTOR_ELT(anslst,2,enlst);
        UNPROTECT(4);
        return(anslst);
    }

}


#pragma once
#include "Optimizer.hpp"

#include <Eigen/Dense>
#include <array>
#include <nlohmann/json.hpp>

namespace yavque
{
class Adam : public Optimizer
{
private:
	const double alpha_;
	const double beta1_;
	const double beta2_;
	const double eps_;

	int t_ = 0;
	Eigen::VectorXd m_;
	Eigen::VectorXd v_;

public:
	static constexpr std::array<double, 4> DEFAULT_PARAMS = {1e-3, 0.9, 0.999, 1e-8};

	explicit Adam(double alpha = DEFAULT_PARAMS[0], double beta1 = DEFAULT_PARAMS[1],
	              double beta2 = DEFAULT_PARAMS[2], double eps = DEFAULT_PARAMS[3])
		: alpha_(alpha), beta1_(beta1), beta2_(beta2), eps_(eps)
	{
	}

	explicit Adam(const nlohmann::json& params)
		: alpha_(params.value("alpha", DEFAULT_PARAMS[0])),
		  beta1_(params.value("beta1", DEFAULT_PARAMS[1])),
		  beta2_(params.value("beta2", DEFAULT_PARAMS[2])),
		  eps_(params.value("eps", DEFAULT_PARAMS[3]))
	{
	}

	static nlohmann::json defaultParams()
	{
		return nlohmann::json{
			{"name", "Adam"},
			{"alhpa", DEFAULT_PARAMS[0]},
			{"beta1", DEFAULT_PARAMS[1]},
			{"beta2", DEFAULT_PARAMS[2]},
			{"eps", DEFAULT_PARAMS[3]},
		};
	}

	[[nodiscard]] nlohmann::json desc() const override
	{
		return nlohmann::json{
			{"name", "Adam"},  {"alhpa", alpha_}, {"beta1", beta1_},
			{"beta2", beta2_}, {"eps", eps_},
		};
	}

	Eigen::VectorXd getUpdate(const Eigen::VectorXd& grad) override
	{
		if(t_ == 0)
		{
			m_ = Eigen::VectorXd::Zero(grad.rows());
			v_ = Eigen::VectorXd::Zero(grad.rows());
		}
		++t_;

		m_ *= beta1_;
		m_ += (1 - beta1_) * grad;

		Eigen::VectorXd g2 = grad.array().square();
		v_ *= beta2_;
		v_ += (1 - beta2_) * g2;

		double epsnorm = eps_ * sqrt(1.0 - pow(beta2_, t_));
		Eigen::VectorXd denom
			= v_.unaryExpr([epsnorm](double x) { return sqrt(x) + epsnorm; });

		double alphat = alpha_ * sqrt(1.0 - pow(beta2_, t_)) / (1.0 - pow(beta1_, t_));

		return -alphat * m_.cwiseQuotient(denom);
	}
};
} // namespace yavque


/**
 * \file ex7.cxx
 * \author Ray Chew
 * \date 7 July 2017
 * \brief Dijkstra graph algorithm for .gph graphs
 */

/**
 * \mainpage Ex7
 *
 * \section Description
 *
 * Shortest-longest path distance calculator using the Dijkstra algorithm for `.gph` graphs.

 * Reads `.gph` graph file and prints the node number and distance of the node corresponding to the end of longest shortest path.

 * Flag `-m1` uses the Boost Graph Library Dijkstra Algorithm.

 * Flag `-m2` uses a self-implemented Dijkstra Algorithm for undirected graphs.

 *
 * * compile: `g++ -std=c++14 -O3 ex7.cxx -o ex7 -lboost_timer -Wall`

 * * run: `./ex7 filepath/graph.gph [-m1/-m2]`

 * * flags: `-m1` for boost algorithm, `-m2` for self-implemented algorithm.
 */

/* -- Includes -- */
/* C++ includes. */
#include <iostream> /* for std::cout, std::ofstream */
#include <fstream> /* for fstream::app */
#include <utility> /* for std::pair */
#include <vector> /* for std::vector */

/* Boost Dijkstra Algorithm includes. */
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

/* Boost qi parser and timer includes. */
#include <boost/spirit/include/qi.hpp>
#include <boost/timer/timer.hpp>

/* Boost Bimap Container includes for own algorithm. */
#include <boost/bimap.hpp>
#include <boost/bimap/multiset_of.hpp>
#include <boost/bimap/support/lambda.hpp> /* for bimap::_data and bimap::_key */
//#include <boost/bind.hpp> /* for boost::bind in sorting of vector<pair<int,int>> */

namespace bm = boost::bimaps;
using namespace boost::spirit;
using qi::int_;
using qi::double_;
using qi::parse;

/**
 *  \brief A method that calculates the longest-shortest path from the source node with name `1` using the boost library dijkstra algorithm.
 *  \param n number of edges as int.
 *  \param file pointer to graph file opened.
 *  \return MaxVertex and MaxDistance, the node name and distance of the longest-shortest path as `std::pair<int,int>`.
 */
std::pair<int,int> m1 (int& n, std::ifstream& file){

  /* start get list of edges and weights */
  using Edge = std::pair<int, int>;
  std::vector<Edge> Edges; // vector to store std::pair of edges.
  std::vector<int> Weights; // vector to weights as integers.
  std::string str; // string to store line of graph file.

  while (getline(file,str)){ /// get graph line-by-line.
    int Vert1;
    int Vert2;
    int Weight;

    auto it = str.begin(); /// initializes iterator for qi::parse.

    parse(it, str.end(), int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](int i){Weight = i;})]);

    Edge edge = std::make_pair(Vert1, Vert2);  /// make edge-pair out of vertices.
    Edges.push_back(edge);
    Weights.push_back(Weight);
  }
  /* end get list of edges and weights */

  /* start building graph */
  /// initialize type to store weights on edges.
  typedef boost::property<boost::edge_weight_t, int> EdgeWeightProperty;
  // adjacency_list<out-edges, vertex_set, directedness, vertex properties, edge properties>
  /// create graph.
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty> Graph;

  Graph g(Edges.begin(),Edges.end(), Weights.begin(), n);   /// populate graph.

  /* end building graph */

  /* start finding shortest path from source node. */
  typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  vertex_descriptor source = boost::vertex(1, g);   /// define source vertex as vertex with index == 1.
  /// initialize vectors for predecessor and distances.
  std::vector<vertex_descriptor> parents(boost::num_vertices(g));
  std::vector<int> distances(boost::num_vertices(g));

  boost::dijkstra_shortest_paths(g, source, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));

  /* end finding shortest path of from source node. */

  /* start finding longest-shortest path from source node. */
  signed int maxDistance = 0;
  unsigned int maxVertex = 0;

  /// create iterator over vertices.
  // vertexPair.first is the iterated element, and .second is the end-index of all vertices.
  typedef boost::graph_traits <Graph>::vertex_iterator vertex_iter;
  std::pair<vertex_iter, vertex_iter> vertexPair;

  // vertexPair = boost::vertices loops over all vertices in g.
  for (vertexPair = boost::vertices(g); vertexPair.first != vertexPair.second; ++vertexPair.first){
    /// replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((distances[*vertexPair.first] > maxDistance) && (distances[*vertexPair.first] < std::numeric_limits<int>::max())){
      maxDistance = distances[*vertexPair.first];
      maxVertex = *vertexPair.first;
    }
    /// if distance == maxDistance, check if vertex index is smaller.
    if ((distances[*vertexPair.first] == maxDistance) && (*vertexPair.first < maxVertex)){
      maxDistance = distances[*vertexPair.first];
    }
  }
  /* end finding longest-shortest path from source node. */

  std::pair<int,int> Final = std::make_pair(maxVertex,maxDistance);
  return Final;
}

/**
 *  \brief A method that calculates the longest-shortest path from the source node with name `1` using a self-implemented dijkstra algorithm.
 *  \param n number of edges as int.
 *  \param file pointer to graph file opened.
 *  \return MaxVertex and MaxDistance, the node name and distance of the longest-shortest path as `std::pair<int,int>`.
 */
std::pair<int,int> m2 (int& n, std::ifstream& file){

  /* start populating adjacency list */
  using vertex =  std::pair<int, int>;
  std::vector<std::vector<vertex>> adjList(n); // empty adjacency list.

  std::cout << "pass initialize adjList." << std::endl;
  std::string str; // string to store line of graph.

  while (getline(file,str)){
    int Vert1;
    int Vert2;
    int Weight;

    auto it = str.begin(); /// initialize iterator for qi::parse.

    parse(it, str.end(), int_[([&Vert1](int i){Vert1 = i;})] >> qi::space >> int_[([&Vert2](int i){Vert2 = i;})] >> qi::space >> double_[([&Weight](double i){Weight = i;})]);

    vertex VertexWeight1(Vert2,Weight);
    adjList[Vert1].push_back(VertexWeight1);

    vertex VertexWeight2(Vert1,Weight);
    adjList[Vert2].push_back(VertexWeight2);
  }
  std::cout << "pass build adjList." << std::endl;
  /* end populating adjacency list */

  using bimap = bm::bimap<int, boost::bimaps::multiset_of<int,std::less<int>>>;
  bimap Unvisited; /// define unvisited set as a boost::bimap container.
  std::vector<vertex> finalWeights(n); // vector to store all calculated weights/distances.

  /* start initializing unvisited set */
  for(int i=1; i<n; i++){
    Unvisited.left.insert(bimap::left_value_type(i,std::numeric_limits<int>::max()));
    finalWeights[i] = std::make_pair(i, std::numeric_limits<int>::max());
  }
  bimap::right_iterator itr = Unvisited.right.begin();
  Unvisited.right.replace_key(itr, 0);
  finalWeights[1].second = 0;

  std::cout << "pass initialize unvisited set." << std::endl;
  /* end initializing unvisited set */

  /* start calculating and updating distances */
  while(Unvisited.size()>0){
    auto minPair = Unvisited.right.begin();
    int minIdx = minPair->second;
    int minDist = minPair->first;

    signed int adjListSize = adjList[minIdx].size();

    for(int j=0; j<adjListSize; j++){
      int neighbour = adjList[minIdx][j].first;
      int dist = adjList[minIdx][j].second;
      int newDist = minDist + dist;
      int nPWeight = finalWeights[neighbour].second;

      if (newDist < nPWeight){
	auto toBeReplaced = Unvisited.left.find(neighbour);
	Unvisited.left.modify_data(toBeReplaced, bm::_data=newDist);
	finalWeights[neighbour].second = newDist;
      }
    }
    Unvisited.left.erase(minIdx);
  }
  std::cout << "pass distance calculation." << std::endl;
  /* end calculating and updating distances */

  /* start find node of the longest-shortest path */
  //std::sort(finalWeights.begin(), finalWeights.end(), [](auto &left, auto &right) {
  //    return left.second < right.second;});

  int maxDistance = 0;
  int maxVertex = 0;

  for(auto ita = finalWeights.begin(); ita != finalWeights.end(); ita++){
    /// replace maxDistance if a greater distance is found, and maxDistance must be less than "infinity" (of 32-bit signed integer).
    if ((ita->second > maxDistance) && (ita->second < std::numeric_limits<int>::max())){
      maxDistance = ita->second;
      maxVertex = ita->first;
    }
    /// if distance == maxDistance, check if vertex index is smaller.
    if ((ita->second == maxDistance) && (ita->first < maxVertex)){
      maxVertex = ita->first;
    }
  }
  /* end find node of the longest-shortest path */

  vertex Final = std::make_pair(maxVertex,maxDistance);
  return Final;
}

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

  if (argc < 3){ // must have filename of graph and a flag of some sort...
    std::cerr << "No file or flag (-m1 or -m2)!!!" << std::endl;
    return -1;
  }

  std::ifstream file(argv[1]);  // read graph file.
  std::string str; /// read graph file line by line.

  /* start get number of edges */
  getline(file, str);
  int n; /// store n as int for number of edges.

  auto it = str.begin();
  parse(it, str.end(), int_[([&n](int i){n = i;})] >> int_);
  n = n + 1; // No?
  /* end get number of edges */

  /* start dijkstra algorithm according to flag */
  std::pair<int,int> f =std::make_pair(0,0); /// initialize f as (int,int) pair for final node number and distance.
  boost::timer::cpu_timer timer;
  for(int i = 0; i < argc; i++){
    if (std::string(argv[i]) == "-m1"){
      f = m1(n,file);
    }
    else if(std::string(argv[i]) == "-m2"){
      f = m2(n,file);
    }
  }
  boost::timer::cpu_times times = timer.elapsed();
  /* end dijkstra algorithm according to flag */

  /// output vertex and distance of the longest-shortest path.
  std::cout << "RESULT VERTEX " << f.first << std::endl;
  std::cout << "RESULT DIST " << f.second <<  std::endl;

  /// print CPU- and Wall-Time.
  // boost::timer::cpu_times returns tuple of wall, system and user times in nanoseconds.
  std::cout << std::endl;
  std::cout << "WALL-CLOCK " << times.wall / 1e9 << "s" << std::endl;
  std::cout << "USER TIME " << times.user / 1e9 << "s" << std::endl;

  file.close();
  return 0;
}


/*
 *  lengthdistribution.cpp
 *  MetaQuant
 *
 *  Kevin McLoughlin
 *  Based on code from eXpress, created by Adam Roberts in 2013.
 *  Copyright 2014 Kevin McLoughlin, Adam Roberts. All rights reserved.
 */

#include "lengthdistribution.h"
#include "main.h"
#include <numeric>
#include <boost/assign.hpp>
#include <iostream>
#include <fstream>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp>

using namespace std;

LengthDistribution::LengthDistribution(double alpha, size_t max_val,
                                       size_t prior_mu, size_t prior_sigma,
                                       size_t kernel_n, double kernel_p,
                                       size_t bin_size)
    : _hist(max_val/bin_size+1),
      _tot_mass(LOG_0),
      _sum(LOG_0),
      _min(max_val/bin_size),
      _bin_size(bin_size) {

  max_val = max_val/bin_size;
  kernel_n = kernel_n/bin_size;
  assert(kernel_n % 2 == 0);

  double tot = log(alpha);

  // Set to prior distribution
  if (prior_mu) {
    boost::math::normal norm(prior_mu/bin_size,
                             prior_sigma/(bin_size*bin_size));

    for (size_t i = 0; i <= max_val; ++i) {
      double norm_mass = boost::math::cdf(norm, i+0.5) -
                         boost::math::cdf(norm, i-0.5);
      double mass = LOG_EPSILON;
      if (norm_mass != 0) {
        mass = tot + log(norm_mass);
      }
      _hist[i] = mass;
      _sum = log_add(_sum, log((double)i)+mass);
      _tot_mass = log_add(_tot_mass, mass);
    }
  } else {
    _hist = vector<double>(max_val + 1, tot - log((double)max_val));
    _hist[0] = LOG_0;
    _sum = _hist[1] + log((double)(max_val * (max_val + 1))) - log(2.);
    _tot_mass = tot;
  }

  // Define kernel
  boost::math::binomial_distribution<double> binom(kernel_n, kernel_p);
  _kernel = vector<double>(kernel_n + 1);
  for (size_t i = 0; i <= kernel_n; i++) {
    _kernel[i] = log(boost::math::pdf(binom, i));
  }
}

LengthDistribution::LengthDistribution(string param_file_name,
                                       string length_type) :
    _bin_size(1) {
  ifstream infile (param_file_name.c_str());
  const size_t BUFF_SIZE = 99999;
  char line_buff[BUFF_SIZE];

  if (!infile.is_open()) {
    logger.severe("Unable to open paramater file '%s'.",
                  param_file_name.c_str());
  }

  do {
    infile.getline (line_buff, BUFF_SIZE, '\n');
  } while (strncmp(line_buff + 1, length_type.c_str(), length_type.size()));

  infile.getline (line_buff, BUFF_SIZE, '\n');
  char *p = strtok(line_buff, "\t");
  size_t i = 0;

  _tot_mass = 0;
  _sum = 0;
  do {
    double val = strtod(p,NULL);
    _hist.push_back(log(val));
    _tot_mass += val;
    _sum += i*val;
    i++;
    p = strtok(NULL, "\t");
  } while (p);

  _tot_mass = log(_tot_mass);
  _sum = log(_sum);
  _min = max_val();;
}

size_t LengthDistribution::max_val() const {
  return (_hist.size()-1) * _bin_size;
}

size_t LengthDistribution::min_val() const {
  if (_min == _hist.size() - 1) {
    return 1;
  }
  return _min;
}

void LengthDistribution::add_val(size_t len, double mass) {
  assert(!isnan(mass));
  assert(_kernel.size());

  len /= _bin_size;

  if (len > max_val()) {
      len = max_val();
  }
  if (len < _min) {
    _min = len;
  }

  size_t offset = len - _kernel.size()/2;

  for (size_t i = 0; i < _kernel.size(); i++) {
    if (offset > 0 && offset < _hist.size()) {
      double k_mass = mass + _kernel[i];
      _hist[offset] = log_add(_hist[offset], k_mass);
      _sum = log_add(_sum, log((double)offset)+k_mass);
      _tot_mass = log_add(_tot_mass, k_mass);
    }
    offset++;
  }
}

double LengthDistribution::pmf(size_t len) const {
  len /= _bin_size;
  if (len > max_val()) {
    len = max_val();
  }
  return _hist[len]-_tot_mass;
}

double LengthDistribution::cmf(size_t len) const {
  double cum = LOG_0;
  vector<double> cdf(_hist.size());
  for (size_t i = 0; i < _hist.size(); ++i) {
    cum = log_add(cum, _hist[i]);

  }
  return cum - _tot_mass;
}

vector<double> LengthDistribution::cmf() const {
  double cum = LOG_0;
  vector<double> cdf(_hist.size());
  for (size_t i = 0; i < _hist.size(); ++i) {
    cum = log_add(cum, _hist[i]);
    cdf[i] = cum - _tot_mass;
  }
  assert(approx_eq(cum, _tot_mass));

  return cdf;
}

double LengthDistribution::tot_mass() const {
  return _tot_mass;
}

double LengthDistribution::mean() const {
  return _sum - tot_mass();
}

string LengthDistribution::to_string() const {
  string s = "";
  char buffer[50];
  for(size_t i = 0; i < _hist.size(); i++) {
    sprintf(buffer, "%e\t",sexp(pmf(i*_bin_size)));
    s += buffer;
  }
  s.erase(s.length()-1,1);
  return s;
}

void LengthDistribution::append_output(ofstream& outfile,
                                       string length_type) const {
  outfile << ">" << length_type << " Length Distribution (0-" << max_val()*_bin_size;
  outfile << ")\n" << to_string() << endl;
}


#ifndef MATHTOOLBOX_GRADIENT_DESCENT_HPP
#define MATHTOOLBOX_GRADIENT_DESCENT_HPP

#include <Eigen/Core>

namespace mathtoolbox
{
    namespace optimization
    {
        /// \brief Run a simple gradient descent to find a local minimizer of the specified function
        ///
        /// \details This algorithm uses the Backtracking Line Search algorithm to determine an appropriate step size.
        ///
        /// \param lower_bound The lower bound values. If this is a zero-length empty vector, the algorithm just ignores
        /// the lower bound condition.
        ///
        /// \param upper_bound The upper bound values. If this is a zero-length empty vector, the algorithm just ignores
        /// the upper bound condition.
        ///
        /// \param default_alpha The default step size that the algorithm first tries.
        void RunGradientDescent(const Eigen::VectorXd&                                        x_init,
                                const std::function<double(const Eigen::VectorXd&)>&          f,
                                const std::function<Eigen::VectorXd(const Eigen::VectorXd&)>& g,
                                const Eigen::VectorXd&                                        lower_bound,
                                const Eigen::VectorXd&                                        upper_bound,
                                const double                                                  epsilon,
                                const double                                                  default_alpha,
                                const unsigned int                                            max_num_iters,
                                Eigen::VectorXd&                                              x_star,
                                unsigned int&                                                 num_iters);
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_GRADIENT_DESCENT_HPP


#include <benchmark.hpp>
#include <pi_helpers.hpp>

#include <boost/mpi.hpp>
#include <gmpxx.h>

#include <cassert>
#include <cmath>
#include <iostream>
#include <stdexcept>

namespace mpi = boost::mpi;

namespace
{

static constexpr int ROOT_ID = 0;
static constexpr int TAG = 0;

void send_mpf_pi_part(const mpf_class& pi_part, const mpi::communicator& world)
{
    mpf_srcptr pi_part_raw = pi_part.get_mpf_t();

    world.send(ROOT_ID, TAG, pi_part_raw->_mp_prec);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_size);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_exp);
    world.send(ROOT_ID, TAG, pi_part_raw->_mp_d, std::abs(pi_part_raw->_mp_size));
}

void recv_and_add_mpf_pi_part(mpf_class& pi, int rank, const mpi::communicator& world)
{
    mpf_t another_pi_part;

    world.recv(rank, TAG, another_pi_part->_mp_prec);
    world.recv(rank, TAG, another_pi_part->_mp_size);
    world.recv(rank, TAG, another_pi_part->_mp_exp);

    auto mp_d = std::make_unique<mp_limb_t[]>(another_pi_part->_mp_prec + 1);
    another_pi_part->_mp_d = mp_d.get();
    world.recv(rank, TAG, another_pi_part->_mp_d, std::abs(another_pi_part->_mp_size));

    mpf_add(pi.get_mpf_t(), pi.get_mpf_t(), another_pi_part);
}

void pi_sum_reduce(const mpf_class& pi_part, mpf_class& pi, const mpi::communicator& world)
{
    // TODO: create a user-defined type for GMP float
    if (world.rank() == ROOT_ID)
    {
        pi = std::move(pi_part);
        std::size_t process_count = world.size();
        for (std::size_t rank = 1; rank < process_count; ++rank)
        {
            recv_and_add_mpf_pi_part(pi, rank, world);
        }
    }
    else
    {
        send_mpf_pi_part(pi_part, world);
    }
}

mpf_class pi_leibniz_mpi(std::size_t summand_count, mp_bitcnt_t precision,
                         const mpi::communicator& world)
{
    mpi::broadcast(world, summand_count, ROOT_ID);

    mpf_class pi_part = my::pi::pi_part_leibniz_mpi(summand_count, precision, world.rank(), world.size());
    world.barrier();

    mpf_class pi(0.0, precision);
    pi_sum_reduce(pi_part, pi, world);

    return 4 * pi;
}

mpf_class pi_bellard_mpi(std::size_t summand_count, mp_bitcnt_t precision,
                         const mpi::communicator& world)
{
    mpi::broadcast(world, summand_count, ROOT_ID);

    mpf_class pi_part = my::pi::pi_part_bellard_mpi(summand_count, precision, world.rank(), world.size());
    world.barrier();

    mpf_class pi(0.0, precision);
    pi_sum_reduce(pi_part, pi, world);

    pi /= (1 << 6);
    return pi;
}

template <typename RegularPiCalculationFunction,
          typename MPIPiCalculationFunction>
void benchmark(std::size_t summand_count,
               mp_bitcnt_t precision,
               const mpi::communicator& world,
               RegularPiCalculationFunction pi_regular,
               MPIPiCalculationFunction pi_mpi)
{
    static constexpr std::size_t ITERATIONS_COUNT = 100;

    if (summand_count < static_cast<std::size_t>(world.size()))
    {
        throw std::runtime_error("Summand count is less than processor count, please decrease number of processors.");
    }

    if (world.rank() == ROOT_ID)
    {
        auto pi_regular_wrapper = [summand_count, precision, pi_regular]()
        {
            return pi_regular(summand_count, precision);
        };
        double pi_regular_result = my::benchmark_function(pi_regular_wrapper, ITERATIONS_COUNT);
        my::print_result("Regular time: ", pi_regular_result);
    }

    double pi_mpi_result;
    {
        auto pi_mpi_wrapper = [summand_count, precision, pi_mpi, world]()
        {
            return pi_mpi(summand_count, precision, world);
        };
        pi_mpi_result = my::benchmark_function(pi_mpi_wrapper, ITERATIONS_COUNT);
    }
    if (world.rank() == ROOT_ID)
    {
        my::print_result("    MPI time: ", pi_mpi_result);
    }
}

template <typename MPIPiCalculationFunction>
void calculate(std::size_t summand_count,
               mp_bitcnt_t precision,
               const mpi::communicator& world,
               MPIPiCalculationFunction pi_mpi)
{
    if (summand_count < static_cast<std::size_t>(world.size()))
    {
        throw std::runtime_error("Summand count is less than processor count, please decrease number of processors.");
    }

    mpf_class pi_mpi_result = pi_mpi(summand_count, precision, world);

    if (world.rank() == ROOT_ID)
    {
        mp_exp_t exp;
        std::string pi_string = pi_mpi_result.get_str(exp);
        assert(exp == 1);
        std::cout << std::string_view(pi_string.data(), 1);
        std::cout << '.';
        std::cout << std::string_view(pi_string.data() + 1, pi_string.size()) << std::endl;
    }
}

}  // namespace

int main(int argc, char* argv[]) try
{
    mpi::environment env(argc, argv);
    mpi::communicator world;

    struct AlgorithmInfo
    {
        const std::function<mpf_class(std::size_t summand_count, mp_bitcnt_t precision)> pi_regular;
        const std::function<mpf_class(std::size_t summand_count, mp_bitcnt_t precision, const mpi::communicator& world)> pi_mpi;
        my::pi::AlgorithmParams params;
    };

    std::unordered_map<my::pi::AlgorithmType, AlgorithmInfo>
    algorithm_info_map =
    {
        {
            my::pi::AlgorithmType::BELLARD,
            {
                .pi_regular = my::pi::pi_bellard_regular,
                .pi_mpi = pi_bellard_mpi,
                .params =
                {
                    .precision = (1 << 26),
                    .benchmark_summand_count = std::size_t{1} << 8,
                    .calculation_summand_count = std::size_t{1} << 22
                }
            }
        },
        {
            my::pi::AlgorithmType::LEIBNIZ,
            {
                .pi_regular = my::pi::pi_leibniz_regular,
                .pi_mpi = pi_leibniz_mpi,
                .params =
                {
                    .precision = (1 << 7),
                    .benchmark_summand_count = std::size_t{1} << 26,
                    .calculation_summand_count = std::size_t{1} << 45
                }
            }
        },
    };

    bool do_benchmark = true;
    auto algorithm = my::pi::AlgorithmType::LEIBNIZ;

    const AlgorithmInfo& algorithm_info = algorithm_info_map.at(algorithm);

    if (do_benchmark)
    {
        benchmark(algorithm_info.params.benchmark_summand_count,
                  algorithm_info.params.precision,
                  world,
                  algorithm_info.pi_regular,
                  algorithm_info.pi_mpi);
    }
    else
    {
        calculate(algorithm_info.params.calculation_summand_count,
                  algorithm_info.params.precision,
                  world,
                  algorithm_info.pi_mpi);
    }

    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    std::cerr << "Exception caught: " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (...)
{
    std::cerr << "An unknown exception caught" << std::endl;
    return EXIT_FAILURE;
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP
#define STAN_MATH_PRIM_SCAL_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP

#include <boost/math/special_functions/bessel.hpp>

namespace stan {
  namespace math {

    /**
     *
       \f[
       \mbox{modified\_bessel\_second\_kind}(v, z) =
       \begin{cases}
         \textrm{error} & \mbox{if } z \leq 0 \\
         K_v(z) & \mbox{if } z > 0 \\[6pt]
         \textrm{NaN} & \mbox{if } z = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       \frac{\partial\, \mbox{modified\_bessel\_second\_kind}(v, z)}{\partial z} =
       \begin{cases}
         \textrm{error} & \mbox{if } z \leq 0 \\
         \frac{\partial\, K_v(z)}{\partial z} & \mbox{if } z > 0 \\[6pt]
         \textrm{NaN} & \mbox{if } z = \textrm{NaN}
       \end{cases}
       \f]

       \f[
       {K_v}(z)
       =
       \frac{\pi}{2}\cdot\frac{I_{-v}(z) - I_{v}(z)}{\sin(v\pi)}
       \f]

       \f[
       \frac{\partial \, K_v(z)}{\partial z} = -\frac{v}{z}K_v(z)-K_{v-1}(z)
       \f]
     *
     */
    template<typename T2>
    inline T2
    modified_bessel_second_kind(int v, const T2 z) {
      return boost::math::cyl_bessel_k(v, z);
    }

  }
}

#endif


#include "stratton-chu/distorted-surface.hpp"

#include <boost/math/special_functions/legendre.hpp>

using namespace boost::math;

double legendre(int num, double arg)
{
    if (arg > 1.0)
        return legendre_p(num, 1.0);
    if (arg < -1.0)
        return legendre_p(num, -1.0);
    return legendre_p(num, arg);
}

double legendre_derivative(int num, double arg)
{
    if (arg > 1.0)
        return 0.0;
    if (arg < -1.0)
        return 0.0;
    return legendre_p_prime(num, arg);
}

SurfaceDistortionHarmonic::SurfaceDistortionHarmonic(
        const ISurface& pure_surface,
        const Vector& v,
        const std::vector<DistortionHarmonic>& harmonics) :  // v - unit vector
    m_pure_surface(pure_surface), m_harmonics(harmonics), m_v(v)
{}

Position SurfaceDistortionHarmonic::point(const Vector2D& pos) const
{
    Position result = m_pure_surface.point(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * cos(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta = m_v * shift;
        result += delta;
    }

    return result;
}

Vector SurfaceDistortionHarmonic::tau1(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau1(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = - m_harmonics[i].ampl * m_harmonics[i].kx * sin(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}

Vector SurfaceDistortionHarmonic::tau2(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau2(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = - m_harmonics[i].ampl * m_harmonics[i].ky * sin(m_harmonics[i].kx * pos[0] + m_harmonics[i].ky * pos[1]);
        Vector delta = m_v * shift;
        result += delta;
    }
    return result;
}

SurfaceDistortionLegendre::DistortionPolinom::DistortionPolinom(double ampl, double alpha, int number) :
    ampl(ampl), direction(cos(alpha), sin(alpha)), number(number)
{
}

SurfaceDistortionLegendre::SurfaceDistortionLegendre(
        const ISurface& pure_surface,
        const Vector& v,
        double radius,
        Vector2D center,
        const std::vector<DistortionPolinom>& harmonics) :  // v - unit vector
    m_pure_surface(pure_surface), m_harmonics(harmonics), m_v(v), m_center(center), m_radius(radius)
{
}

Position SurfaceDistortionLegendre::point(const Vector2D& pos) const
{
    Position result = m_pure_surface.point(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * legendre(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta = m_v * shift;
        result += delta;
    }

    return result;
}

Vector SurfaceDistortionLegendre::tau1(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau1(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * m_harmonics[i].direction[0] / m_radius * legendre_derivative(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}

Vector SurfaceDistortionLegendre::tau2(const Vector2D& pos) const
{
    Vector result = m_pure_surface.tau2(pos);
    for (size_t i = 0; i < m_harmonics.size(); i++)
    {
        double shift = m_harmonics[i].ampl * m_harmonics[i].direction[1] / m_radius * legendre_derivative(m_harmonics[i].number, m_harmonics[i].direction * (pos - m_center) / m_radius);
        Vector delta =  m_v * shift;
        result += delta;
    }
    return result;
}


#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

#include <boost/optional.hpp>

#include <algorithm>
#include <utility>
#include <vector>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr long double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr long double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856;
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

// get the length of a full coordinate vector, using one of our basic functions to compute distances
template <class BinaryOperation>
double getLength(const std::vector<Coordinate> &coordinates, BinaryOperation op)
{
    if (coordinates.empty())
        return 0.;

    double result = 0;
    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
        result += op(lhs, rhs);
        return false;
    };
    // side-effect find adding up distances
    std::adjacent_find(coordinates.begin(), coordinates.end(), functor);

    return result;
}

// Find the closest distance and location between coordinate and the line connecting source and
// target:
//             coordinate
//                 |
//                 |
// source -------- x -------- target.
// returns x as well as the distance between source and x as ratio ([0,1])
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude{1.0 - clamped_ratio} * source.lon +
                    target.lon * FloatLongitude{clamped_ratio},
                FloatLatitude{1.0 - clamped_ratio} * source.lat +
                    target.lat * FloatLatitude{clamped_ratio},
            }};
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate);

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <igl/read_triangle_mesh.h>
#include <igl/write_triangle_mesh.h>
#include <igl/writeOFF.h>
#include <igl/per_vertex_normals.h>
#include <igl/cotmatrix.h>
#include <igl/cotmatrix_entries.h>
#include <igl/adjacency_list.h>
#include <igl/triangle_triangle_adjacency.h>
#include <igl/barycenter.h>
#include <igl/massmatrix.h>
#include <igl/writeOBJ.h>
#include <fstream>
#include <cmath>
#include <array>

void findRotations(const Eigen::MatrixXd& N0,
                   const Eigen::MatrixXd& N1,
                   std::vector<Eigen::Matrix3d>& rot) {

    const auto n = N0.rows();
    rot.resize(n);

    for(int i = 0; i < n; ++i) {
        Eigen::Vector3d n1 = N0.row(i);
        Eigen::Vector3d n2 = N1.row(i);
        Eigen::Vector3d v = n1.cross(n2);
        const double c = n1.dot(n2);

        if(c > -1 + 1e-8) {
            const double coeff = 1 / (1 + c);
            Eigen::Matrix3d v_x;
            v_x << 0.0, -v(2), v(1), v(2), 0.0, -v(0), -v(1), v(0), 0.0;
            rot[i] = Eigen::Matrix3d::Identity() + v_x + coeff * v_x * v_x;
        } else{
            rot[i] = -Eigen::Matrix3d::Identity();
        }
    }
}

std::vector<std::vector<int>> collectNeighbours(const std::vector<std::vector<int>>& adj,
                                                const Eigen::MatrixXd& V,
                                                const Eigen::MatrixXd& N,
                                                const double r,
                                                const double nr) {

    std::vector<int> stack;
    std::vector<int> flag(V.rows(), -1);
    std::vector<std::vector<int>> result(V.rows());
    const double normalConeThreshold = cos(nr * M_PI / 180.);

    for(int i = 0; i < V.rows(); ++i) {

        stack.push_back(i);
        flag[i] = i;

        while(!stack.empty()) {
            auto id = stack.back();
            stack.pop_back();

            result[i].push_back(id);

            for (int j : adj[id]) {
                if(flag[j] != i && (V.row(i) - V.row(j)).norm() < r && (N.row(i).dot(N.row(j))) > normalConeThreshold) {
                    stack.push_back(j);
                    flag[j] = i;
                }
            }
        }
    }

    return result;
}

void fitNormals(const std::vector<std::vector<int>>& nbh,
                const Eigen::MatrixXd& V,
                const Eigen::MatrixXd& N,
                Eigen::MatrixXd& N2,
                const double cosineThreshold,
                const double sigma = 1.) {

    const auto nv = nbh.size();
    N2.resize(nv, 3);
    double angleThreshold = cosineThreshold * M_PI / 180.;

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

        const auto& nbi = nbh[i];

        Eigen::MatrixXd NN(nbi.size(), 3);

        for (int k = 0; k < nbi.size(); ++k) {
            NN.row(k) = N.row(nbi[k]);
        }

        Eigen::DiagonalMatrix<double, -1> W(nbi.size());

        if(sigma < 10.) {
            for(int i = 0; i < W.diagonal().size(); ++i) {
                double dot = NN.row(0).dot(NN.row(i));
                if (dot >= 1.){
                    W.diagonal()(i) = 1;
                } else if(dot < 0) {
                    W.diagonal()(i) = 0;
                } else {
                    W.diagonal()(i) = std::exp(-std::pow(acos(dot) / angleThreshold / sigma, 2));
                }
            }
        } else {
            W.diagonal().setOnes();
        }

        Eigen::JacobiSVD<Eigen::Matrix3d> svd(NN.transpose() * W * NN, Eigen::ComputeFullV);
        Eigen::Matrix3d frame = svd.matrixV();
        N2.row(i) = (frame.leftCols(2) * frame.leftCols(2).transpose() * N.row(i).transpose()).normalized();
    }
}

void assembleRHS(const Eigen::MatrixXd& C,
                 const Eigen::MatrixXd& V,
                 const Eigen::MatrixXi& F,
                 const std::vector<Eigen::Matrix3d>& R,
                 Eigen::MatrixXd& rhs) {

    const auto nv = V.rows();
    rhs.resize(nv, 3);
    rhs.setZero();

    for(int i = 0; i < F.rows(); ++i)  {
        for(int j = 0; j < 3; ++j)  {
            int v0 = F(i, (j + 1) % 3);
            int v1 = F(i, (j + 2) % 3);

            Eigen::Vector3d b = C(i,j) * R[i] * (V.row(v0) - V.row(v1)).transpose();
            rhs.row(v0) -= b.transpose();
            rhs.row(v1) += b.transpose();
        }
    }
}

std::vector<std::vector<int>> triangleAdjacency(const Eigen::MatrixXi& F, const size_t nv) {

    std::vector<std::vector<int>> vnbhs(nv);
    const auto nf = F.rows();

    for(int i = 0; i < nf; ++i) {
        for(int j = 0; j < 3; ++j) {
            vnbhs[F(i, j)].push_back(i);
        }
    }

    std::vector<int> flags(nf, -1);
    std::vector<std::vector<int>> ret(nf);

    for(int i = 0; i < nf; ++i) {
        for(int j = 0; j < 3; ++j) {
            for(int k : vnbhs[F(i, j)]) {
                if(k != i && flags[k] != i) {
                    ret[i].push_back(k);
                    flags[k] = i;
                }
            }
        }
    }

    return ret;
}

void center(Eigen::MatrixXd& V) {
    V.rowwise() -= V.colwise().mean();;
    V /= 2. * V.rowwise().norm().maxCoeff();
}

void gaussThinning(const std::string &mesh_folder,
                   const Eigen::MatrixXd &V_in,
                   const Eigen::MatrixXi &F,
                   Eigen::MatrixXd &V,
                   const int number_iterations = 100,
                   double minConeAngle = 2.5,
                   double smooth = 1e-5,
                   double start_angle = 25,
                   double radius = 0.1,
                   double sigma = 2.) {

    double coneAngle = start_angle;
    double r = radius;
    double eps = 1e-3;

    V = V_in;
    const auto nv = V.rows();
    center(V);

    igl::writeOFF(mesh_folder + "/normalized.off", V, F);

    Eigen::SparseMatrix<double> I(nv, nv);
    I.setIdentity();

    Eigen::MatrixXi TT;
    Eigen::MatrixXd B, b, C, N, N2;
    std::vector<Eigen::Matrix3d> rot;
    Eigen::SparseMatrix<double> L, M;
    std::vector<std::vector<int>> nbhs;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> chol;

    auto tt = triangleAdjacency(F, nv);
    igl::triangle_triangle_adjacency(F, TT);
    igl::cotmatrix_entries(V, F, C);
    igl::cotmatrix(V, F, L);
    igl::massmatrix(V, F, igl::MASSMATRIX_TYPE_BARYCENTRIC, M);

    if(smooth) {
        chol.compute(-L + smooth * L.transpose() * L + eps * M);
    } else {
        chol.compute(-L + eps * M);
    }

    for(int k = 0; k < number_iterations; ++k) {

        igl::per_face_normals(V, F, N);
        igl::barycenter(V, F, B);

        nbhs = collectNeighbours(tt, B, N, r, coneAngle);
        if(coneAngle > minConeAngle) coneAngle *= .95;

        fitNormals(nbhs, V, N, N2, coneAngle, sigma);
        findRotations(N, N2, rot);
        assembleRHS(C, V, F, rot, b);

        V = chol.solve(eps * M * V - b);

        if (k % std::max(1, (number_iterations / 10)) == 0) {
            std::cout << "writing " + mesh_folder + ": " << k << "\n";
            igl::writeOFF(mesh_folder + "/out" + std::to_string(k) + ".off", V, F);
        }
    }

    return;
}

void runExperiment(std::string folder, std::string inputFile, std::string outputFile, const int iters, const double minAngle, const double start_angle = 25, const double radius = 0.1, const double smooth = 1e-5) {
    Eigen::MatrixXd V_in, V_out;
    Eigen::MatrixXi F;
    igl::read_triangle_mesh(folder + "/" + inputFile, V_in, F);
    gaussThinning(folder, V_in, F, V_out, iters, minAngle, smooth, start_angle, radius);
    igl::write_triangle_mesh(folder + "/" + outputFile, V_out, F);
}

int main(int argc, const char * argv[]) {

    if(argc < 6) {
        std::cout << "Need input file, output file, output directory, number of iterations and minimum search cone. Running default experiments..." << std::endl;

        /* run default experiments here .... */
        runExperiment("./examples/architecture", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/boat", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/bumpy", "input.off", "out.obj", 150, 7.5);

        runExperiment("./examples/bunny", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/bunny_high", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/bunny_small", "input.off", "out.obj", 500, 5.0);

        runExperiment("./examples/coffee", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/cone", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/cone_high", "input.off", "out.obj", 100, 2.5, 25, 0.015);

        runExperiment("./examples/curved_fold", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/cylinder", "input.off", "out.obj", 300, 7.5);

        runExperiment("./examples/dog", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/dome", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/dress_high", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/drill", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/einstein", "input.off", "out.obj", 300, 7.5, 60, 0.015);

        runExperiment("./examples/face", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/fandisk", "input.off", "out.obj", 1000, 5.0);

        runExperiment("./examples/fertility", "input.off", "out.obj", 100, 7.5);

        runExperiment("./examples/guitar", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/lilium", "input.off", "out.obj", 100, 5.0);

        runExperiment("./examples/mask", "input.off", "out.obj", 500, 2.5);

        runExperiment("./examples/nut", "input.off", "out.obj", 100, 2.5);

        runExperiment("./examples/swing", "input.off", "out.obj", 500, 5.0);
    } else
    {
        std::string  infile = argv[1];
        std::string  outfile = argv[2];
        std::string  folder = argv[3];

        auto numIters = std::atoi(argv[4]);
        auto minAngle = std::stold(argv[5]);

        std::cout << "Processing " << infile << " with " << numIters << " iterations and mimimum cone angle " << minAngle << ". Output directory is " << folder << std::endl;

        runExperiment(folder, infile, outfile, numIters, minAngle);
    }

    return 0;
}


#include "segmatch/descriptors/eigenvalue_based.hpp"

#include <cfenv>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>
#include <pcl/common/common.h>

#pragma STDC FENV_ACCESS on

namespace segmatch {

/// \brief Utility function for swapping two values.
template<typename T>
bool swap_if_gt(T& a, T& b) {
  if (a > b) {
    std::swap(a, b);
    return true;
  }
  return false;
}

// EigenvalueBasedDescriptor methods definition
EigenvalueBasedDescriptor::EigenvalueBasedDescriptor(const DescriptorsParameters& parameters) {}

void EigenvalueBasedDescriptor::describe(const Segment& segment, Features* features) {
  CHECK_NOTNULL(features);
  std::feclearexcept(FE_ALL_EXCEPT);

  // Find the variances.
  const size_t kNPoints = segment.point_cloud.points.size();
  PointCloud variances;
  for (size_t i = 0u; i < kNPoints; ++i) {
    variances.push_back(PclPoint());
    variances.points[i].x = segment.point_cloud.points[i].x - segment.centroid.x;
    variances.points[i].y = segment.point_cloud.points[i].y - segment.centroid.y;
    variances.points[i].z = segment.point_cloud.points[i].z - segment.centroid.z;
  }

  // Find the covariance matrix. Since it is symmetric, we only bother with the upper diagonal.
  const std::vector<size_t> row_indices_to_access = {0,0,0,1,1,2};
  const std::vector<size_t> col_indices_to_access = {0,1,2,1,2,2};
  Eigen::Matrix3f covariance_matrix;
  for (size_t i = 0u; i < row_indices_to_access.size(); ++i) {
    const size_t row = row_indices_to_access[i];
    const size_t col = col_indices_to_access[i];
    double covariance = 0;
    for (size_t k = 0u; k < kNPoints; ++k) {
      covariance += variances.points[k].data[row] * variances.points[k].data[col];
    }
    covariance /= kNPoints;
    covariance_matrix(row,col) = covariance;
    covariance_matrix(col,row) = covariance;
  }

  // Compute eigenvalues of covariance matrix.
  constexpr bool compute_eigenvectors = false;
  Eigen::EigenSolver<Eigen::Matrix3f> eigenvalues_solver(covariance_matrix, compute_eigenvectors);
  std::vector<float> eigenvalues(3, 0.0);
  eigenvalues.at(0) = eigenvalues_solver.eigenvalues()[0].real();
  eigenvalues.at(1) = eigenvalues_solver.eigenvalues()[1].real();
  eigenvalues.at(2) = eigenvalues_solver.eigenvalues()[2].real();
  if (eigenvalues_solver.eigenvalues()[0].imag() != 0.0 ||
      eigenvalues_solver.eigenvalues()[1].imag() != 0.0 ||
      eigenvalues_solver.eigenvalues()[2].imag() != 0.0 ) {
    LOG(ERROR) << "Eigenvalues should not have non-zero imaginary component.";
  }

  // Sort eigenvalues from smallest to largest.
  swap_if_gt(eigenvalues.at(0), eigenvalues.at(1));
  swap_if_gt(eigenvalues.at(0), eigenvalues.at(2));
  swap_if_gt(eigenvalues.at(1), eigenvalues.at(2));

  // Normalize eigenvalues.
  double sum_eigenvalues = eigenvalues.at(0) + eigenvalues.at(1) + eigenvalues.at(2);
  double e1 = eigenvalues.at(0) / sum_eigenvalues;
  double e2 = eigenvalues.at(1) / sum_eigenvalues;
  double e3 = eigenvalues.at(2) / sum_eigenvalues;
  LOG_IF(ERROR, e1 == e2 || e2 == e3 || e1 == e3) << "Eigenvalues should not be equal.";

  // Store inside features.
  const double sum_of_eigenvalues = e1 + e2 + e3;
  constexpr double kOneThird = 1.0/3.0;
  CHECK_NE(e1, 0.0);
  CHECK_NE(sum_of_eigenvalues, 0.0);

  const double kNormalizationPercentile = 1.0;

  const double kLinearityMax = 28890.9 * kNormalizationPercentile;
  const double kPlanarityMax = 95919.2 * kNormalizationPercentile;
  const double kScatteringMax = 124811 * kNormalizationPercentile;
  const double kOmnivarianceMax = 0.278636 * kNormalizationPercentile;
  const double kAnisotropyMax = 124810 * kNormalizationPercentile;
  const double kEigenEntropyMax = 0.956129 * kNormalizationPercentile;
  const double kChangeOfCurvatureMax = 0.99702 * kNormalizationPercentile;

  const double kNPointsMax = 13200 * kNormalizationPercentile;

  Feature eigenvalue_feature;
  eigenvalue_feature.push_back(FeatureValue("linearity", (e1 - e2) / e1 / kLinearityMax));
  eigenvalue_feature.push_back(FeatureValue("planarity", (e2 - e3) / e1 / kPlanarityMax));
  eigenvalue_feature.push_back(FeatureValue("scattering", e3 / e1 / kScatteringMax));
  eigenvalue_feature.push_back(FeatureValue("omnivariance", std::pow(e1 * e2 * e3, kOneThird) / kOmnivarianceMax));
  eigenvalue_feature.push_back(FeatureValue("anisotropy", (e1 - e3) / e1 / kAnisotropyMax));
  eigenvalue_feature.push_back(FeatureValue("eigen_entropy",
                                            (e1 * std::log(e1)) + (e2 * std::log(e2)) + (e3 * std::log(e3)) / kEigenEntropyMax));
  eigenvalue_feature.push_back(FeatureValue("change_of_curvature", e3 / sum_of_eigenvalues / kChangeOfCurvatureMax));

  PointI point_min, point_max;

  pcl::getMinMax3D(segment.point_cloud, point_min, point_max);

  double diff_x, diff_y, diff_z;

  diff_x = point_max.x - point_min.x;
  diff_y = point_max.y - point_min.y;
  diff_z = point_max.z - point_min.z;

  if (diff_z < diff_x && diff_z < diff_y) {
    eigenvalue_feature.push_back(FeatureValue("pointing_up", 0.2));
  } else {
    eigenvalue_feature.push_back(FeatureValue("pointing_up", 0.0));
  }

  // eigenvalue_feature.push_back(FeatureValue("n_points", kNPoints / kNPointsMax));

  CHECK_EQ(eigenvalue_feature.size(), kDimension) << "Feature has the wrong dimension";
  features->push_back(eigenvalue_feature);

  // Check that there were no overflows, underflows, or invalid float operations.
  if (std::fetestexcept(FE_OVERFLOW)) {
    LOG(ERROR) << "Overflow error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_UNDERFLOW)) {
    LOG(ERROR) << "Underflow error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_INVALID)) {
    LOG(ERROR) << "Invalid Flag error in eigenvalue feature computation.";
  } else if (std::fetestexcept(FE_DIVBYZERO)) {
    LOG(ERROR) << "Divide by zero error in eigenvalue feature computation.";
  }
}

} // namespace segmatch