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

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <eigen3/Eigen/Dense>

#include <map>
#include <vector>
#include <random>
#include <iostream>
#include <functional>
#include <boost/progress.hpp>

#include "map_index.hpp"

namespace ss {

    template<typename DataType>
    class ITQIndex: public MapIndex<DataType, uint64_t > {

        using KeyType = uint64_t;
    public:

        explicit ITQIndex(const parameter & para):  MapIndex<DataType, uint64_t >(para) {}

        ~ITQIndex() {}

        void Train(const Matrix<DataType> & data) override;

    protected:
        KeyType Quantize(const DataType *data) override  {
            KeyType hash_value = 0;
            std::vector<DataType > v(_eigen_vectors.size());
            for (unsigned i = 0; i != v.size(); ++i) {
                v[i] = ss::DiffProduct(data, this->_means.data(), _eigen_vectors[i].data(), _eigen_vectors[i].size());
            }
            for (unsigned i = 0; i != v.size(); ++i) {
                DataType  product  = ss::InnerProduct(v.data(), _rotate_matrix[i].data(), _rotate_matrix[i].size());
                hash_value <<= 1;
                hash_value |= product > 0? 1 : 0;
            }
            return hash_value;
        }

    private:

        std::vector<std::vector<DataType > >  _eigen_vectors;
        std::vector<std::vector<DataType> >   _rotate_matrix;

    };
} // namespace ss

// ------------------------- implementation -------------------------

template<typename DataType>
void ss::ITQIndex<DataType>::Train(const Matrix<DataType> & data) {

    this->InitializeMeans(data); /// TODO(Xinyan): should avoid re-computing means

    std::mt19937 rng(unsigned(std::time(0)));
    std::normal_distribution<DataType > nd;
    std::uniform_int_distribution<unsigned> usBits(0, data.getSize() - 1);

    {
        /// 1. wrap data with eigen
        EigenMatrix< DataType > matrix_data = data.GetEigenMatrix();
        /// 2. zero-centered
        EigenMatrix< DataType > centered = matrix_data.rowwise() - matrix_data.colwise().mean();
        /// 3. use eigen-vectors to project data
        EigenMatrix< DataType > cov = (centered.transpose() * centered) / DataType (matrix_data.rows() - 1);
        Eigen::SelfAdjointEigenSolver<EigenMatrix< DataType >> eig(cov);
        EigenMatrix< DataType > eigen_vectors = eig.eigenvectors().rightCols(this->_para.num_bit);
        EigenMatrix< DataType > V = matrix_data * eigen_vectors;
        // 4. initialize R
        EigenMatrix< DataType > R(this->_para.num_bit, this->_para.num_bit);
        Eigen::JacobiSVD<EigenMatrix< DataType >> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
        R = svd.matrixU();

        boost::progress_display progress(this->_para.iteration);
        for (int iter = 0; iter < this->_para.iteration; ++iter && ++progress) {

            EigenMatrix< DataType > VR = V * R;
            EigenMatrix< DataType > B(VR.rows(), VR.cols()); // n * c
            assert(VR.rows() == this->_para.train_size);
            assert(VR.cols() == this->_para.num_bit);

            for (unsigned i = 0; i != VR.rows(); ++i) {
                for (unsigned j = 0; j != VR.cols(); ++j) {
                    B(i, j) = VR(i, j) > 0 ? 1 : -1;
                }
            }
            Eigen::JacobiSVD<EigenMatrix< DataType >> svd_tmp(B.transpose() * V, Eigen::ComputeThinU | Eigen::ComputeThinV);
            R = svd_tmp.matrixV() * svd_tmp.matrixU().transpose();
        }

        _rotate_matrix.resize(this->_para.num_bit);
        for (unsigned i = 0; i != _rotate_matrix.size(); ++i) {

            _rotate_matrix[i].resize(this->_para.num_bit);
            for (unsigned j = 0; j != _rotate_matrix[i].size(); ++j) {
                _rotate_matrix[i][j] = R(j, i);
            }
        }
        _eigen_vectors.resize(this->_para.num_bit);
        for (unsigned i = 0; i != _eigen_vectors.size(); ++i) {

            _eigen_vectors[i].resize(data.getDim());
            for (unsigned dimension = 0; dimension != data.getDim(); ++dimension) {
                _eigen_vectors[i][dimension] = eigen_vectors(dimension, i);
            }
        }
    }
}


//
// Created by kerin on 2019-12-01.
//
#include "mpi_utils.hpp"
#include "typedefs.hpp"
#include "variational_parameters.hpp"

#include "tools/eigen3.3/Dense"
#include "tools/eigen3.3/Eigenvalues"

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/fisher_f.hpp>

#include <cmath>

namespace boost_m  = boost::math;

void prep_lm(const Eigen::MatrixXd &H,
             const Eigen::MatrixXd &y,
             EigenRefDataMatrix HtH,
             EigenRefDataMatrix HtH_inv,
             EigenRefDataMatrix Hty,
             double &rss,
             EigenRefDataMatrix HtVH) {
	/*** All of the heavy lifting for linear hypothesis tests.
	 * Easier to have in one place if we go down the MPI route.
	 */

	HtH     = H.transpose() * H;
	HtH     = mpiUtils::mpiReduce_inplace(HtH);
	Hty     = H.transpose() * y;
	Hty     = mpiUtils::mpiReduce_inplace(Hty);
	HtH_inv = HtH.inverse();

	EigenDataVector resid = y - H * HtH_inv * Hty;
	HtVH = H.transpose() * resid.cwiseProduct(resid).asDiagonal() * H;
	HtVH = mpiUtils::mpiReduce_inplace(HtVH);

	rss = resid.squaredNorm();
	rss = mpiUtils::mpiReduce_inplace(&rss);
}

void prep_lm(const Eigen::MatrixXd &H,
             const Eigen::MatrixXd &y,
             EigenRefDataMatrix HtH,
             EigenRefDataMatrix HtH_inv,
             EigenRefDataMatrix Hty,
             double &rss) {
	/*** All of the heavy lifting for linear hypothesis tests.
	 * Easier to have in one place if we go down the MPI route.
	 */

	HtH     = H.transpose() * H;
	HtH     = mpiUtils::mpiReduce_inplace(HtH);
	Hty     = H.transpose() * y;
	Hty     = mpiUtils::mpiReduce_inplace(Hty);
	HtH_inv = HtH.inverse();

	EigenDataVector resid = y - H * HtH_inv * Hty;
	rss = resid.squaredNorm();
	rss = mpiUtils::mpiReduce_inplace(&rss);
}

void student_t_test(long nn,
                    const Eigen::MatrixXd &HtH_inv,
                    const Eigen::MatrixXd &Hty,
                    double rss,
                    int jj,
                    double &stat,
                    double &pval) {
	/* 2-sided Student t-test on regression output
	   H0: beta[jj] != 0
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);
	nn = mpiUtils::mpiReduce_inplace(&nn);

	auto beta = HtH_inv * Hty;
	stat = beta(jj, 0);
	stat /= std::sqrt(rss * HtH_inv(jj, jj) / (double) (nn - pp));
//	if (std::isnan(stat)){
//		std::cout << "est = " << beta(jj, 0) << std::endl;
//		std::cout << "sd(est) = " << std::sqrt(rss * HtH_inv(jj, jj) / (double) (nn - pp)) << std::endl;
//		std::cout << "rss = " << rss << std::endl;
//	}

	boost_m::students_t t_dist(nn - pp);
	pval  = 2 * boost_m::cdf(boost_m::complement(t_dist, fabs(stat)));
}

void hetero_chi_sq(const Eigen::MatrixXd &HtH_inv,
                   const Eigen::MatrixXd &Hty,
                   const Eigen::MatrixXd &HtVH,
                   int jj,
                   double &stat,
                   double &pval) {
	/* Standard errors adjusted for Heteroscedasticity
	   https://en.wikipedia.org/wiki/Heteroscedasticity-consistent_standard_errors
	   HtVH = (H.transpose() * resid_sq.asDiagonal() * H)
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);

	auto beta = HtH_inv * Hty;
	auto var_beta = HtH_inv * HtVH * HtH_inv;
	stat = beta(jj, 0) * beta(jj, 0);
	stat /= var_beta(jj, jj);
	stat = std::abs(stat);
//	if (std::isnan(stat)){
//		std::cout << "est_sq = " << beta(jj, 0) * beta(jj, 0) << std::endl;
//		std::cout << "var(est) = " << var_beta(jj, jj) << std::endl;
//	}

	boost_m::chi_squared chi_dist(1);
	pval = boost_m::cdf(boost_m::complement(chi_dist, stat));
}

void homo_chi_sq(long nn,
                 const Eigen::MatrixXd &HtH_inv,
                 const Eigen::MatrixXd &Hty,
                 const double rss,
                 const int jj,
                 double &stat,
                 double &pval) {
	/* Essentially the square of the t-test from regression
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);
	nn = mpiUtils::mpiReduce_inplace(&nn);

	auto beta = HtH_inv * Hty;
	stat = beta(jj, 0) * beta(jj, 0);
	stat /= rss * HtH_inv(jj, jj) / (double) (nn - pp);
	stat = std::abs(stat);
//	if (std::isnan(stat)){
//		std::cout << "est_sq = " << beta(jj, 0) * beta(jj, 0) << std::endl;
//		std::cout << "var(est) = " << rss * HtH_inv(jj, jj) / (double) (nn - pp) << std::endl;
//	}

	boost_m::chi_squared chi_dist(1);
	pval = boost_m::cdf(boost_m::complement(chi_dist, stat));
}

double homo_chi_sq(const long nn,
                   const Eigen::MatrixXd &HtH_inv,
                   const Eigen::MatrixXd &Hty,
                   const double rss,
                   const int jj) {
	double tstat, pval;
	homo_chi_sq(nn, HtH_inv, Hty, rss, jj, tstat, pval);
	return pval;
}

double hetero_chi_sq(const Eigen::MatrixXd &HtH_inv,
                     const Eigen::MatrixXd &Hty,
                     const Eigen::MatrixXd &HtVH,
                     int jj) {
	double tstat, pval;
	hetero_chi_sq(HtH_inv, Hty, HtVH, jj, tstat, pval);
	return pval;
}

double student_t_test(long nn,
                      const Eigen::MatrixXd &HtH_inv,
                      const Eigen::MatrixXd &Hty,
                      double rss,
                      int jj) {
	double tstat, pval;
	student_t_test(nn, HtH_inv, Hty, rss, jj, tstat, pval);
	return pval;
}

template <typename GenoMat>
void compute_LOCO_pvals(const EigenDataVector &resid_pheno,
                        const GenoMat &Xtest,
                        Eigen::MatrixXd &neglogPvals,
                        Eigen::MatrixXd &testStats,
                        const EigenDataVector &eta) {
	bool isGxE     = eta.rows() > 0;
	long n_var     = Xtest.cols();
	long n_samples = Xtest.rows();
	long n_effects = (isGxE ? 2 : 1);
	double Nlocal  = n_samples;
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	neglogPvals.resize(n_var, (isGxE ? 4 : 1));
	testStats.resize(n_var, (isGxE ? 4 : 1));

	// Compute p-vals per variant (p=3 as residuals mean centered)
	Eigen::MatrixXd H(n_samples, 2 + 2 * (isGxE ? 1 : 0));
	H.col(0) = Eigen::VectorXd::Constant(n_samples, 1.0);
	if (isGxE) H.col(3) = eta.cast<double>();
	boost_m::students_t t_dist(n_samples - H.cols() - 1);
	boost_m::fisher_f f_dist(n_effects, n_samples - H.cols() - 1);
	for(std::uint32_t jj = 0; jj < n_var; jj++ ) {
		H.col(1) = Xtest.col(jj);

		double rss_alt, rss_null;
		Eigen::MatrixXd HtH(H.cols(), H.cols()), Hty(H.cols(), 1);
		Eigen::MatrixXd HtH_inv(H.cols(), H.cols()), HtVH(H.cols(), H.cols());
		if(!isGxE) {
			double beta_tstat, beta_pval;
			prep_lm(H, resid_pheno, HtH, HtH_inv, Hty, rss_alt);
			student_t_test(n_samples, HtH_inv, Hty, rss_alt, 1, beta_tstat, beta_pval);

			neglogPvals(jj,0) = -1 * log10(beta_pval);
			testStats(jj,0)   = beta_tstat;
		} else {
			H.col(2) = H.col(1).cwiseProduct(eta.cast<double>());
			try {
				// Single-var tests
				double beta_tstat, gam_tstat, rgam_stat, beta_pval, gam_pval, rgam_pval;
				prep_lm(H, resid_pheno, HtH, HtH_inv, Hty, rss_alt, HtVH);
				hetero_chi_sq(HtH_inv, Hty, HtVH, 2, rgam_stat, rgam_pval);
				student_t_test(n_samples, HtH_inv, Hty, rss_alt, 2, gam_tstat, gam_pval);
				student_t_test(n_samples, HtH_inv, Hty, rss_alt, 1, beta_tstat, beta_pval);

				// F-test over main+int effects of snp_j
				double joint_fstat, joint_pval;
				rss_null = resid_pheno.squaredNorm();
				rss_null = mpiUtils::mpiReduce_inplace(&rss_null);
				joint_fstat = (rss_null - rss_alt) / 2.0;
				joint_fstat /= rss_alt / (Nglobal - 3.0);
				joint_pval = 1.0 - boost_m::cdf(f_dist, joint_fstat);

				neglogPvals(jj, 0) = -1 * std::log10(beta_pval);
				neglogPvals(jj, 1) = -1 * std::log10(gam_pval);
				neglogPvals(jj, 2) = -1 * std::log10(rgam_pval);
				neglogPvals(jj, 3) = -1 * std::log10(joint_pval);
				testStats(jj, 0) = beta_tstat;
				testStats(jj, 1) = gam_tstat;
				testStats(jj, 2) = rgam_stat;
				testStats(jj, 3) = joint_fstat;
			} catch (...) {
				neglogPvals(jj, 0) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 1) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 2) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 3) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 0) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 1) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 2) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 3) = std::numeric_limits<double>::quiet_NaN();
			}
		}
	}
}

// Explicit instantiation
// https://stackoverflow.com/questions/2152002/how-do-i-force-a-particular-instance-of-a-c-template-to-instantiate
template void compute_LOCO_pvals(const EigenDataVector&, const EigenDataMatrix&,
                                 Eigen::MatrixXd&, Eigen::MatrixXd&,const EigenDataVector&);
template void compute_LOCO_pvals(const EigenDataVector&, const GenotypeMatrix&,
                                 Eigen::MatrixXd&, Eigen::MatrixXd&,const EigenDataVector&);


#include <iostream>
#include <tclap/CmdLine.h>

// SISL Main Include
#include <sisl/sisl.hpp>

// odd cartesian function spaces
#include <sisl/lattice/cartesian_odd.hpp>
#include <sisl/basis/tp3cubic.hpp>

// odd BCC function spaces
#include <sisl/lattice/bcc_odd.hpp>
#include <sisl/basis/quintic.hpp>
#include <sisl/basis/linear_rdod.hpp>

// utility functions
#include <poisson/pointset.hpp>
#include <sisl/utility/isosurface.hpp>
#include <sisl/utility/dualbcc.hpp>
#include <sisl/utility/dualfcc.hpp>
#include <sisl/utility/dualcc.hpp>
#include <sisl/utility/scattered.hpp>
#include <sisl/utility/ply_writer.hpp>

#include <Eigen/Dense>

#include <tuple>
#include <cmath>

#define VESION_STRING "0.1"

using namespace sisl;
using namespace std;
using namespace TCLAP;

template <class T>
class MarschnerLobb {
public:
	double a, fm;

	MarschnerLobb(double Fm, double alpha) : fm(Fm), a(alpha){ }
	double rho(double r){
		return cos(2*M_PI*fm*cos(r*M_PI/2.));
	}
	double f(const double &xx, const double &yy, const double &zz) {
		double x = (2.*xx-1.), y = (2.*yy-1.), z = (2.*zz-1.);
		double r = rho(sqrt(x*x + y*y));
		double ret = 1. - sin(M_PI*z/2.) + a * (1. + r);
		return ret/(2. + 2.*a);

	}

	double f(const vector3<T> &p) {return this->f(p.i, p.j, p.k);}
	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		double xx = 2.*x - 1.;
		double yy = 2.*y - 1.;
		double zz = 2.*z - 1.;
		return vector3<double> (
			M_PI*M_PI*a*fm*(xx)*sin(2.*M_PI*fm*cos(0.5*M_PI*sqrt(xx*xx + yy*yy))) *sin(0.5*M_PI*sqrt(xx*xx + yy*yy))/(sqrt(xx*xx + yy*yy)*(a + 1.)),
			M_PI*M_PI*a*fm*(yy)*sin(2.*M_PI*fm*cos(0.5*M_PI*sqrt(xx*xx + yy*yy))) *sin(0.5*M_PI*sqrt(xx*xx + yy*yy))/(sqrt(xx*xx + yy*yy)*(a + 1.)),
			-0.5*M_PI*cos(0.5*M_PI*zz)/(a + 1.)
		);
	}
};

template <class T>
class HamFunction {
public:
	HamFunction(){}
	double f(const double &x, const double &y, const double &z) {
		return -(sin(0.3141592654e1 * x) * sin(0.3141592654e1 * y) * sin(0.3141592654e1 * z) * (sqrt(0.25e0 + pow(0.9e1 * x - 0.45e1, 0.2e1) + pow(0.9e1 * y - 0.45e1, 0.2e1) + pow(0.9e1 * z - 0.45e1, 0.2e1)) - 0.2e1 * cos(0.8e1 * 0.3141592654e1 * (0.9e1 * z - 0.45e1) * pow(0.25e0 + pow(0.9e1 * x - 0.45e1, 0.2e1) + pow(0.9e1 * y - 0.45e1, 0.2e1) + pow(0.9e1 * z - 0.45e1, 0.2e1), -0.1e1 / 0.2e1)) - 0.2e1));
	}

	double f(const vector3<T> &p) {
		return this->f(p.i, p.j, p.k);
	}

	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		return vector3<double> (
			2.*(x-0.5),
			2.*(y-0.5),
			2.*(z-0.5)
		);
	}
};

template<class T>
class SphereFunction{
public:
	SphereFunction(){}
	double f(const double &x, const double &y, const double &z) {
		double xx = (x-0.5), yy = (y-0.5), zz = (z-0.5);
		return xx*xx + yy*yy + zz*zz - 0.25*0.25;
	}

	double f(const vector3<T> &p) {return this->f(p.i, p.j, p.k);}

	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		return vector3<double> (
			2*(x-0.5),
			2*(y-0.5),
			2*(z-0.5)
		);
	}
};

int main(int argc, char *argv[])
{
	try {
		CmdLine cmd("Dual marching cubes for CC/BCC/FCC lattices", ' ', VESION_STRING);
		sisl::utility::marchingCubes<double> mc;
		sisl::utility::dualbcc_isosurface<double> dbcc;
		sisl::utility::dualfcc_isosurface<double> dfcc;
		sisl::utility::dualcc_isosurface<double> dcc;

		ValueArg<std::string> outputArg("o", "output", "Output mesh name", true,"output", "filename");
		ValueArg<std::string> testFunction("t", "test_function", "test function", false, "lobner", "test function");
		ValueArg<double> isoValue("i", "iso_value", "Isovalue for contour", true, 0, "iso-value");
		ValueArg<double> gridGranularity("s", "grid_granularity", "Grid grid granularity",true, 0.25, "dh");

		cmd.add(testFunction);
		cmd.add(outputArg);
		cmd.add(isoValue);
		cmd.add(gridGranularity);

		cmd.parse(argc, argv);
		std::string output = outputArg.getValue();
		std::string function = testFunction.getValue();

		double levelset = isoValue.getValue();

		HamFunction<double> hf;
		SphereFunction<double> sf;
		MarschnerLobb<double> mf(6, 0.25);

		if(function == std::string("lobb")){
			dbcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}else if(function == std::string("ham")){
			dbcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}else if(function == std::string("sphere")){
			dbcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}

		dbcc.writeSurface(output + std::string(".bcc.ply"));
		dfcc.writeSurface(output + std::string(".fcc.ply"));
		dcc.writeSurface(output + std::string(".cc.ply"));

	}catch (ArgException &e) {
		cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
	}catch (char const* e) {
		cerr << e << endl;
	}
}


#include "runtime.h"

#include <cmath>
#include <time.h>
#include <chrono>
#include <vector>

#include "timers.h"
#include "stdio.h"

#ifdef EIGEN
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
using namespace Eigen;
#endif

extern "C" {
int loc(int v0, int v1, int *neighbors_start, int *neighbors) {
  int l = neighbors_start[v0];
  while(neighbors[l] != v1) l++;
  return l;
}

double atan2_f64(double y, double x) {
  return atan2(y, x);
}

float atan2_f32(float y, float x) {
  double d_y = y;
  double d_x = x;
  return (float)atan2(d_y, d_x);
}

double tan_f64(double x) {
  return tan(x);
}

float tan_f32(float x) {
  double d_x = x;
  return (float)tan(d_x);
}

double asin_f64(double x) {
  return asin(x);
}

float asin_f32(float x) {
  double d_x = x;
  return (float)asin(d_x);
}

double acos_f64(double x) {
  return acos(x);
}

float acos_f32(float x) {
  double d_x = x;
  return (float)acos(d_x);
}

double max_f64(double a,double b) {
  return max(a,b);
}

float max_f32(float a, float b) {
  double d_a = a;
  double d_b = b;
  return (float)max(d_a,d_b);
}

double min_f64(double a,double b) {
  return min(a,b);
}

float min_f32(float a, float b) {
  double d_a = a;
  double d_b = b;
  return (float)min(d_a,d_b);
}

double cbrt_f64(double x) {
  return cbrt(x);
}

float cbrt_f32(float x) {
  double d_x = x;
  return (float)cbrt(d_x);
}

double det3_f64(double * a){
  return a[0] * (a[4]*a[8]-a[5]*a[7])
       - a[1] * (a[3]*a[8]-a[5]*a[6])
       + a[2] * (a[3]*a[7]-a[4]*a[6]);
}

float det3_f32(float * a){
  return a[0] * (a[4]*a[8]-a[5]*a[7])
       - a[1] * (a[3]*a[8]-a[5]*a[6])
       + a[2] * (a[3]*a[7]-a[4]*a[6]);
}

double det2_f64(double * a){
  return a[0] * a[3] - a[1] * a[2];
}

float det2_f32(float * a){
  return a[0] * a[3] - a[1] * a[2];
}

double det4_f64(double * a){
  double det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  double det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  double det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  double det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  return a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;
}

float det4_f32(float * a){
  float det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  float det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  float det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  float det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  return a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;
}

void inv3_f64(double * a, double * inv){
  double cof00 = a[4]*a[8]-a[5]*a[7];
  double cof01 =-a[3]*a[8]+a[5]*a[6];
  double cof02 = a[3]*a[7]-a[4]*a[6];

  double cof10 =-a[1]*a[8]+a[2]*a[7];
  double cof11 = a[0]*a[8]-a[2]*a[6];
  double cof12 =-a[0]*a[7]+a[1]*a[6];

  double cof20 = a[1]*a[5]-a[2]*a[4];
  double cof21 =-a[0]*a[5]+a[2]*a[3];
  double cof22 = a[0]*a[4]-a[1]*a[3];

  double determ = a[0] * cof00 + a[1] * cof01 + a[2]*cof02;

  determ = 1.0/determ;
  inv[0] = cof00 * determ;
  inv[1] = cof10 * determ;
  inv[2] = cof20 * determ;

  inv[3] = cof01 * determ;
  inv[4] = cof11 * determ;
  inv[5] = cof21 * determ;

  inv[6] = cof02 * determ;
  inv[7] = cof12 * determ;
  inv[8] = cof22 * determ;
}

void inv3_f32(float * a, float * inv){
  float cof00 = a[4]*a[8]-a[5]*a[7];
  float cof01 =-a[3]*a[8]+a[5]*a[6];
  float cof02 = a[3]*a[7]-a[4]*a[6];

  float cof10 =-a[1]*a[8]+a[2]*a[7];
  float cof11 = a[0]*a[8]-a[2]*a[6];
  float cof12 =-a[0]*a[7]+a[1]*a[6];

  float cof20 = a[1]*a[5]-a[2]*a[4];
  float cof21 =-a[0]*a[5]+a[2]*a[3];
  float cof22 = a[0]*a[4]-a[1]*a[3];

  float determ = a[0] * cof00 + a[1] * cof01 + a[2]*cof02;

  determ = 1.0/determ;
  inv[0] = cof00 * determ;
  inv[1] = cof10 * determ;
  inv[2] = cof20 * determ;

  inv[3] = cof01 * determ;
  inv[4] = cof11 * determ;
  inv[5] = cof21 * determ;

  inv[6] = cof02 * determ;
  inv[7] = cof12 * determ;
  inv[8] = cof22 * determ;
}

void inv2_f64(double * a, double * inv){
  double determ = a[0] * a[3] - a[1] * a[2];

  determ = 1.0/determ;
  inv[0] = a[3] * determ;
  inv[1] = -a[1] * determ;
  inv[2] = -a[2] * determ;
  inv[3] = a[0] * determ;
}

void inv2_f32(float * a, float * inv){
  float determ = a[0] * a[3] - a[1] * a[2];

  determ = 1.0/determ;
  inv[0] = a[3] * determ;
  inv[1] = -a[1] * determ;
  inv[2] = -a[2] * determ;
  inv[3] = a[0] * determ;
}

void inv4_f64(double * a, double * inv){
  double det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  double det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  double det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  double det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  double determ = a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;

  determ = 1.0/determ;
  inv[0] = (a[5]*a[10]*a[15] + a[6]*a[11]*a[13] + a[7]*a[9]*a[14] - a[5]*a[11]*a[14] - a[6]*a[9]*a[15] - a[7]*a[10]*a[13]) * determ;
  inv[1] = (a[1]*a[11]*a[14] + a[2]*a[9]*a[15] + a[3]*a[10]*a[13] - a[1]*a[10]*a[15] - a[2]*a[11]*a[13] - a[3]*a[9]*a[14]) * determ;
  inv[2] = (a[1]*a[6]*a[15] + a[2]*a[7]*a[13] + a[3]*a[5]*a[14] - a[1]*a[7]*a[14] - a[2]*a[5]*a[15] - a[3]*a[6]*a[13]) * determ;
  inv[3] = (a[1]*a[7]*a[10] + a[2]*a[5]*a[11] + a[3]*a[6]*a[9] - a[1]*a[6]*a[11] - a[2]*a[7]*a[9] - a[3]*a[5]*a[10]) * determ;
  inv[4] = (a[4]*a[11]*a[14] + a[6]*a[8]*a[15] + a[7]*a[10]*a[12] - a[4]*a[10]*a[15] - a[6]*a[11]*a[12] - a[7]*a[8]*a[14]) * determ;
  inv[5] = (a[0]*a[10]*a[15] + a[2]*a[11]*a[12] + a[3]*a[8]*a[14] - a[0]*a[11]*a[14] - a[2]*a[8]*a[15] - a[3]*a[10]*a[12]) * determ;
  inv[6] = (a[0]*a[7]*a[14] + a[2]*a[4]*a[15] + a[3]*a[6]*a[12] - a[0]*a[6]*a[15] - a[2]*a[7]*a[12] - a[3]*a[4]*a[14]) * determ;
  inv[7] = (a[0]*a[6]*a[11] + a[2]*a[7]*a[8] + a[3]*a[4]*a[10] - a[0]*a[7]*a[10] - a[2]*a[4]*a[11] - a[3]*a[6]*a[8]) * determ;
  inv[8] = (a[4]*a[9]*a[15] + a[5]*a[11]*a[12] + a[7]*a[8]*a[13] - a[4]*a[11]*a[13] - a[5]*a[8]*a[15] - a[7]*a[9]*a[12]) * determ;
  inv[9] = (a[0]*a[11]*a[13] + a[1]*a[8]*a[15] + a[3]*a[9]*a[12] - a[0]*a[9]*a[15] - a[1]*a[11]*a[12] - a[3]*a[8]*a[13]) * determ;
  inv[10] = (a[0]*a[5]*a[15] + a[1]*a[7]*a[12] + a[3]*a[4]*a[13] - a[0]*a[7]*a[13] - a[1]*a[4]*a[15] - a[3]*a[5]*a[12]) *  determ;
  inv[11] = (a[0]*a[7]*a[9] + a[1]*a[4]*a[11] + a[3]*a[5]*a[8] - a[0]*a[5]*a[11] - a[1]*a[7]*a[8] - a[3]*a[4]*a[9]) * determ;
  inv[12] = (a[4]*a[10]*a[13] + a[5]*a[8]*a[14] + a[6]*a[9]*a[12] - a[4]*a[9]*a[14] - a[5]*a[10]*a[12] - a[6]*a[8]*a[13]) * determ;
  inv[13] = (a[0]*a[9]*a[14] + a[1]*a[10]*a[12] + a[2]*a[8]*a[13] - a[0]*a[10]*a[13] - a[1]*a[8]*a[14] - a[2]*a[9]*a[12]) * determ;
  inv[14] = (a[0]*a[6]*a[13] + a[1]*a[4]*a[14] + a[2]*a[5]*a[12] - a[0]*a[5]*a[14] - a[1]*a[6]*a[12] - a[2]*a[4]*a[13]) * determ;
  inv[15] = (a[0]*a[5]*a[10] + a[1]*a[6]*a[8] + a[2]*a[4]*a[9] - a[0]*a[6]*a[9] - a[1]*a[4]*a[10] - a[2]*a[5]*a[8]) * determ;
}

void inv4_f32(float * a, float * inv){
  float det0 = a[5] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[9]*a[15]-a[11]*a[13])
	   + a[7] * (a[9]*a[14]-a[10]*a[13]);
  float det1 = a[4] * (a[10]*a[15]-a[11]*a[14])
	   - a[6] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[14]-a[10]*a[12]);
  float det2 = a[4] * (a[9]*a[15]-a[11]*a[13])
	   - a[5] * (a[8]*a[15]-a[11]*a[12])
	   + a[7] * (a[8]*a[13]-a[9]*a[12]);
  float det3 = a[4] * (a[9]*a[14]-a[10]*a[13])
	   - a[5] * (a[8]*a[14]-a[10]*a[12])
	   + a[6] * (a[8]*a[13]-a[9]*a[12]);
  float determ = a[0]*det0 - a[1]*det1 + a[2]*det2 - a[3]*det3;

  determ = 1.0/determ;
  inv[0] = (a[5]*a[10]*a[15] + a[6]*a[11]*a[13] + a[7]*a[9]*a[14] - a[5]*a[11]*a[14] - a[6]*a[9]*a[15] - a[7]*a[10]*a[13]) * determ;
  inv[1] = (a[1]*a[11]*a[14] + a[2]*a[9]*a[15] + a[3]*a[10]*a[13] - a[1]*a[10]*a[15] - a[2]*a[11]*a[13] - a[3]*a[9]*a[14]) * determ;
  inv[2] = (a[1]*a[6]*a[15] + a[2]*a[7]*a[13] + a[3]*a[5]*a[14] - a[1]*a[7]*a[14] - a[2]*a[5]*a[15] - a[3]*a[6]*a[13]) * determ;
  inv[3] = (a[1]*a[7]*a[10] + a[2]*a[5]*a[11] + a[3]*a[6]*a[9] - a[1]*a[6]*a[11] - a[2]*a[7]*a[9] - a[3]*a[5]*a[10]) * determ;
  inv[4] = (a[4]*a[11]*a[14] + a[6]*a[8]*a[15] + a[7]*a[10]*a[12] - a[4]*a[10]*a[15] - a[6]*a[11]*a[12] - a[7]*a[8]*a[14]) * determ;
  inv[5] = (a[0]*a[10]*a[15] + a[2]*a[11]*a[12] + a[3]*a[8]*a[14] - a[0]*a[11]*a[14] - a[2]*a[8]*a[15] - a[3]*a[10]*a[12]) * determ;
  inv[6] = (a[0]*a[7]*a[14] + a[2]*a[4]*a[15] + a[3]*a[6]*a[12] - a[0]*a[6]*a[15] - a[2]*a[7]*a[12] - a[3]*a[4]*a[14]) * determ;
  inv[7] = (a[0]*a[6]*a[11] + a[2]*a[7]*a[8] + a[3]*a[4]*a[10] - a[0]*a[7]*a[10] - a[2]*a[4]*a[11] - a[3]*a[6]*a[8]) * determ;
  inv[8] = (a[4]*a[9]*a[15] + a[5]*a[11]*a[12] + a[7]*a[8]*a[13] - a[4]*a[11]*a[13] - a[5]*a[8]*a[15] - a[7]*a[9]*a[12]) * determ;
  inv[9] = (a[0]*a[11]*a[13] + a[1]*a[8]*a[15] + a[3]*a[9]*a[12] - a[0]*a[9]*a[15] - a[1]*a[11]*a[12] - a[3]*a[8]*a[13]) * determ;
  inv[10] = (a[0]*a[5]*a[15] + a[1]*a[7]*a[12] + a[3]*a[4]*a[13] - a[0]*a[7]*a[13] - a[1]*a[4]*a[15] - a[3]*a[5]*a[12]) *  determ;
  inv[11] = (a[0]*a[7]*a[9] + a[1]*a[4]*a[11] + a[3]*a[5]*a[8] - a[0]*a[5]*a[11] - a[1]*a[7]*a[8] - a[3]*a[4]*a[9]) * determ;
  inv[12] = (a[4]*a[10]*a[13] + a[5]*a[8]*a[14] + a[6]*a[9]*a[12] - a[4]*a[9]*a[14] - a[5]*a[10]*a[12] - a[6]*a[8]*a[13]) * determ;
  inv[13] = (a[0]*a[9]*a[14] + a[1]*a[10]*a[12] + a[2]*a[8]*a[13] - a[0]*a[10]*a[13] - a[1]*a[8]*a[14] - a[2]*a[9]*a[12]) * determ;
  inv[14] = (a[0]*a[6]*a[13] + a[1]*a[4]*a[14] + a[2]*a[5]*a[12] - a[0]*a[5]*a[14] - a[1]*a[6]*a[12] - a[2]*a[4]*a[13]) * determ;
  inv[15] = (a[0]*a[5]*a[10] + a[1]*a[6]*a[8] + a[2]*a[4]*a[9] - a[0]*a[6]*a[9] - a[1]*a[4]*a[10] - a[2]*a[5]*a[8]) * determ;
}

double complexNorm_f64(double r, double i) {
  return sqrt(r*r+i*i);
}

float complexNorm_f32(float r, float i) {
  return sqrt(r*r+i*i);
}

void storeTime(int i, double value) {
  simit::ir::TimerStorage::getInstance().storeTime(i, value);
}

double simitClock() {
  using namespace std::chrono;
  auto t = high_resolution_clock::now();
  time_point<high_resolution_clock,microseconds> usec = time_point_cast<microseconds>(t);
  return (double)(usec.time_since_epoch().count());
}
} // extern "C"

/// Temporary external spmm implementation until Simit supports assembling
/// matrix indices during computation.
template <typename Float>
int spmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
         int Bnn, int Bmm, Float* Bvals,
         int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
         int Cnn, int Cmm, Float* Cvals,
         int An,  int Am,  int** Arowptr, int** Acolidx,
         int Ann, int Amm, Float** Avals) {
#ifdef EIGEN
  auto B = csr2eigen<Float,RowMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  auto C = csr2eigen<Float,RowMajor>(Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals);

  SparseMatrix<Float,RowMajor> A(An, Am);
  A = B*C;
  eigen2csr(A, An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
#else
  simit_ierror << "extern spmm requires Eigen";
#endif
  return 0;
}
extern "C" int sspmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                     int Bnn, int Bmm, float* Bvals,
                     int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
                     int Cnn, int Cmm, float* Cvals,
                     int An,  int Am,  int** Arowptr, int** Acolidx,
                     int Ann, int Amm, float** Avals) {
  return spmm(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
              Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals,
              An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
}
extern "C" int dspmm(int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                     int Bnn, int Bmm, double* Bvals,
                     int Cn,  int Cm,  int* Crowptr, int* Ccolidx,
                     int Cnn, int Cmm, double* Cvals,
                     int An,  int Am,  int** Arowptr, int** Acolidx,
                     int Ann, int Amm, double** Avals) {
  return spmm(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
              Cn, Cm, Crowptr, Ccolidx, Cnn, Cmm, Cvals,
              An, Am, Arowptr, Acolidx, Ann, Amm, Avals);
}

// Solvers
#define SOLVER_ERROR                                            \
do {                                                            \
  simit_ierror << "Solvers require that Simit was built with Eigen."; \
} while (false)

template <typename Float>
void solve(int n,  int m,  int* rowptr, int* colidx,
           int nn, int mm, Float* Avals, Float* bvals, Float* xvals) {
#ifdef EIGEN
  auto A = csr2eigen<Float,ColMajor>(n, m, rowptr, colidx, nn, mm, Avals);
  auto x = new Map<Matrix<Float,Dynamic,1>>(xvals, m);
  auto b = new Map<Matrix<Float,Dynamic,1>>(bvals, n);

  SparseLU<SparseMatrix<Float, ColMajor>> solver;
  solver.compute(A);
  *x = solver.solve(*b);
#else
  SOLVER_ERROR;
#endif
}
extern "C" void cMatSolve_f64(int n,  int m,  int* rowptr, int* colidx,
                              int nn, int mm, double* A, double* x, double* b) {
  return solve(n, m, rowptr, colidx, nn, mm, A, x, b);
}
extern "C" void cMatSolve_f32(int n,  int m,  int* rowptr, int* colidx,
                              int nn, int mm, float* A, float* x, float* b) {
  return solve(n, m, rowptr, colidx, nn, mm, A, x, b);
}

/// LU factorization. Returns a solver object that can be used with
/// `lusolve` and `lumatsolve`. The solver object must be freed using
/// `lufree`.
template <typename Float>
int lu(int An,  int Am,  int* Arowptr, int* Acolidx,
       int Ann, int Amm, Float* Avals,
       void** solverPtr) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto solver = new SparseLU<SparseMatrix<Float,ColMajor>>();
  solver->compute(A);
  *solverPtr = static_cast<void*>(solver);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slu(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, float* Avals,
                   void** solver) {
  return lu(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}
extern "C" int dlu(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, double* Avals,
                   void** solver) {
  return lu(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}

/// Free an LU solver.
template <typename Float>
int lufree(void** solverPtr) {
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  delete solver;
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slufree(void** solverPtr) {
  return lufree<float>(solverPtr);
}
extern "C" int dlufree(void** solverPtr){
  return lufree<double>(solverPtr);
}

/// Solve `t=L^{-1}*b` and `x=L'^{-1}*t`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lusolve(void** solverPtr, int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = solver->solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" {
  int slusolve(void** solverPtr, int bn, float *bvals, int xn, float *xvals) {
    return lusolve(solverPtr, bn, bvals, xn, xvals);
  }
  int dlusolve(void** solverPtr, int bn, double *bvals, int xn, double *xvals){
    return lusolve(solverPtr, bn, bvals, xn, xvals);
  }
}

/// Solve `T=L^{-1}*B` and `X=L'^{-1}*T`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lumatsolve(void** solverPtr,
                int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                int Bnn, int Bmm, Float* Bvals,
                int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                int Xnn, int Xmm, Float** Xvals){
#ifdef EIGEN
  auto solver=static_cast<SparseLU<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto B = csr2eigen<Float,ColMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  SparseMatrix<Float> X(Xn, Xm);
  X = solver->solve(B);
  X = X.transpose();
  eigen2csr<Float>(X, Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slumatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, float* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, float** Xvals) {
  return lumatsolve(solverPtr,
                     Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                     Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}
extern "C" int dlumatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, double* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, double** Xvals) {
  return lumatsolve(solverPtr,
                     Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                     Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}

/// Cholesky factorization. Returns a solver object that can be used with
/// `lltsolve` and `lltmatsolve`. The solver object must be freed using
/// `cholfree`.
template <typename Float>
int chol(int An,  int Am,  int* Arowptr, int* Acolidx,
         int Ann, int Amm, Float* Avals,
         void** solverPtr) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto solver = new SimplicialCholesky<SparseMatrix<Float>>();
  solver->compute(A);
  *solverPtr = static_cast<void*>(solver);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int schol(int An,  int Am,  int* Arowptr, int* Acolidx,
                     int Ann, int Amm, float* Avals,
                     void** solver) {
  return chol(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}
extern "C" int dchol(int An,  int Am,  int* Arowptr, int* Acolidx,
                     int Ann, int Amm, double* Avals,
                     void** solver) {
  return chol(An, Am, Arowptr, Acolidx, Ann, Amm, Avals, solver);
}

/// Free a Cholesky solver.
template <typename Float>
int cholfree(void** solverPtr) {
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float>>*>(*solverPtr);
  delete solver;
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int scholfree(void** solverPtr) {
  return cholfree<float>(solverPtr);
}
extern "C" int dcholfree(void** solverPtr){
  return cholfree<double>(solverPtr);
}

/// Solve `t=L^{-1}*b` and `x=L'^{-1}*t`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lltsolve(void** solverPtr, int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float>>*>(*solverPtr);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = solver->solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" {
int slltsolve(void** solverPtr, int bn, float *bvals, int xn, float *xvals) {
  return lltsolve(solverPtr, bn, bvals, xn, xvals);
}
int dlltsolve(void** solverPtr, int bn, double *bvals, int xn, double *xvals){
  return lltsolve(solverPtr, bn, bvals, xn, xvals);
}
}

/// Solve `T=L^{-1}*B` and `X=L'^{-1}*T`, where `A=LL'` is the matrix that was
/// factorized with the provided solver using `chol`.
template <typename Float>
int lltmatsolve(void** solverPtr,
                 int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                 int Bnn, int Bmm, Float* Bvals,
                 int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                 int Xnn, int Xmm, Float** Xvals){
#ifdef EIGEN
  auto solver=static_cast<SimplicialCholesky<SparseMatrix<Float,ColMajor>>*>(*solverPtr);
  auto B = csr2eigen<Float,ColMajor>(Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals);
  SparseMatrix<Float> X(Xn, Xm);
  X = solver->solve(B);
  X = X.transpose();
  eigen2csr<Float>(X, Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int slltmatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, float* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, float** Xvals) {
  return lltmatsolve(solverPtr,
                      Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                      Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}
extern "C" int dlltmatsolve(void** solverPtr,
                            int Bn,  int Bm,  int* Browptr, int* Bcolidx,
                            int Bnn, int Bmm, double* Bvals,
                            int Xn,  int Xm,  int** Xrowptr, int** Xcolidx,
                            int Xnn, int Xmm, double** Xvals) {
  return lltmatsolve(solverPtr,
                      Bn, Bm, Browptr, Bcolidx, Bnn, Bmm, Bvals,
                      Xn, Xm, Xrowptr, Xcolidx, Xnn, Xmm, Xvals);
}

/// cross product between 2 vectors3D
template <typename Float>
void cross(int an, Float* a, int bn, Float* b, int cn, Float* c){
  assert(an==3 && bn==3);
  c[0] = a[1]*b[2]-a[2]*b[1];
  c[1] = a[2]*b[0]-a[0]*b[2];
  c[2] = a[0]*b[1]-a[1]*b[0];
}
extern "C" void scross(int an, float* a, int bn, float* b, int cn, float* c) {
  return cross(an, a, bn, b, cn, c);
}
extern "C" void dcross(int an, double* a, int bn, double* b, int cn, double* c) {
  return cross(an, a, bn, b, cn, c);
}

template <typename Float>
int triangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
			     int Ann, int Amm, Float* Avals,
				 int nb, Float *bvals, int nx, Float *xvals) {
#ifdef EIGEN
  auto A = csr2eigen<Float,Eigen::ColMajor>(An, Am, Arowptr, Acolidx,
                                            Ann, Amm, Avals);
  auto b = dense2eigen(nb, bvals);
  auto x = Eigen::Matrix<Float,Eigen::Dynamic,1>(nx);
  x = TriangularView<SparseMatrix<Float,ColMajor>,Lower>(A).solve(b);
  for (int i=0; i<nx; ++i) {
    xvals[i] = x(i);
  }
#else
  SOLVER_ERROR;
#endif
  return 0;
}
extern "C" int striangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, float* Avals,
				   int nb, float *bvals, int nx, float *xvals) {
  return triangularSolve(An, Am, Arowptr, Acolidx, Ann, Amm,
                         Avals, nb, bvals, nx, xvals);
}
extern "C" int dtriangularSolve(int An,  int Am,  int* Arowptr, int* Acolidx,
                   int Ann, int Amm, double* Avals,
				   int nb, double *bvals, int nx, double *xvals) {
  return triangularSolve(An, Am, Arowptr, Acolidx, Ann, Amm,
                         Avals, nb, bvals, nx, xvals);
}


// Some sort of comment up top describing the code
// This is a 1D Euler equation solver using the finite volume method
// Uses MUSCL scheme
// Think about what I want to use for time integration.

// This is the command needed to compile the code
// g++ -I ../../../eigen-3.4.0/ main.cpp fvm_1d_functions.cpp -o runSim.out && ./runSim.out

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

using namespace std;
using namespace Eigen;

int main() {

    // Set physical constants
    const double gasGamma = 5./3.;

    // Set important constants for code
    const int maxIter = 100;            // The maximum number of iterations
    const double CFL = 0.9;             // Set CFL number

    // Set grid parameters
    const int nCells = 4;      // Number of cells in the domain. Note these set based on their cell centered values
    const int numGhost = 2;      // Number of ghost cells on either side of the domain

    // Set domain parameters
    const double xLeft = 0;   // Left boundary
    const double xRight = 1;   // Right boundary

    // Calculate grid parameters
    const int nNodes = nCells + 1;  // There is one more node than there are cells
    const int totalNCells = nCells + 2*numGhost;   // Total number of cells, including ghost cells
    const int totalNNodes = totalNCells + 1; // Total number of nodes is one plus total number of cells

    // Calculate domain length
    const double L = xRight - xLeft; // Get total domain length

    // Calculate dx
    const double dx = L / nCells;

    // Make the cell centered grid
    ArrayXd x = makeGrid(totalNCells, numGhost, xLeft, dx);

    // Set initial conditions based on Sod paper on shock tube simulations
    // See https://doi.org/10.1016/0021-9991(78)90023-2
    // The simulation is done in normalized units
    // The initial states are presented in primitive variables (rho, u, p)
    Array3d leftStates;
    leftStates << 1.0, 0.0, 1.0;
    Array3d rightStates;
    rightStates << 0.125, 0.0, 0.1;

    // Set initial shock location
    double x0 = 0.5;

    // Initialize an array of primitive variables
    Array<ArrayXd, 3, 1> V;

    // There is an interface at some x0
    // To the left is one set of states, to the right is another set of states
    // Loop through the variables
    for (int var = 0; var < 3; var++) {
        // Initialize an array of zeros
        V(var) = ArrayXd::Zero(totalNCells);

        // Iterate through x
        for (int i = 0; i < totalNCells; i++) {
            // If we are less than x0, then the IC is the left state.
            // Note that this is a simplified IC in that it will move the initial shock location based on the resolution. This should have a minimal impact on sufficiently resolved sims
            if (x(i) <= x0) {
                V(var)(i) = leftStates(var);
            }
            else {
                V(var)(i) = rightStates(var);
            }
        }
    }

    // Maybe think about a way to specify what BCs to use here.

    // Initialize the conserved variables
    Array<ArrayXd, 3, 1> Q;
    for (int var = 0; var < 3; var++) {
        // Initialize an array of zeros
        Q(var) = ArrayXd::Zero(totalNCells);
    }

    // Calculate the conserved variables based on the primitive variables
    prim2cons(gasGamma, V, Q);

    // Next, start populating the run sim function
    // Choose which ode solver to use and figure out a nice way to do the time integration as a loop
    // Look into SSPRK methods. maybe 4 stage 3rd order? Gets us a nice CFL condition

    // Think about how I want to setup my variables. Do I use rho, u, p or one big variable? Maybe the big variable is the way to go?
    // Each conservation law is the same general idea, just the values internally will be different

    // Maybe put everything below this in some sort of "runSim" function?
    //runSimulation_FVM1D(gasGamma, maxIter, nCells, numGhost, xLeft, xRight);
    // std::cout << "Hello World!" << endl;

    Array<ArrayXd, 3, 1> test;

    test(0) = ArrayXd::Zero(5) + 5.;
    test(1) = ArrayXd::Zero(5) + 1.;
    test(2) = ArrayXd::Zero(5) + 2.;

    //test.setConstant(1.1);

    cout << CFL * dx / (abs(V(1)) + pow(gasGamma*V(2)/V(0),0.5)).minCoeff() << endl;

}


#include <iostream>
#include <fstream>
#include <array>
#include <random>
#include <armadillo>

using namespace std;
using namespace arma;

void MonteCarlo(vec &players, int MCSteps, int N, int transactions, double lambda, double alpha, double gamma, ofstream &outFile, vec &binCounts, double binSize, double m0, ofstream &outFileErr);
void outPut(vec &players, int MCSteps, int N, int transactions, mat &expectVal);
double findVariance(vec &players, int transaction, int N, double m0);
void makeBins(vec &players, vec &binCount, double binSize);

int main(int argc, char *argv[])
{
    if (argc < 8){
        cout << "To few arguments given. Expected number of persons, Monte Carlo cycles, transactions, start money, lambda, alpha and gamma" << endl;
    }
    int N = stoi(argv[1]);
    int MCSteps = stoi(argv[2]);
    int transactions = stoi(argv[3]);

    double startMoney = stod(argv[4]);
    double lambda = stod(argv[5]);
    double alpha = stod(argv[6]);
    double gamma = stod(argv[7]);

    double binSize = 0.01*startMoney;

    ofstream outFileVar = ofstream("variance.txt");
    ofstream outFileErr = ofstream("distError.txt");

    //outFileVar.open("variance.txt");
    ofstream outFileParameter;
    outFileParameter.open("parameters.txt");

    ofstream binParameters = ofstream("binParameters.txt");

    outFileParameter << "N " << N << "\n";
    outFileParameter << "MCSteps " << MCSteps << "\n";
    outFileParameter << "Trasactions " << transactions << "\n";
    outFileParameter << "StartingMoney " << startMoney << "\n";
    outFileParameter << "Lambda " << lambda << "\n";
    outFileParameter << "Alpha " << alpha << "\n";
    outFileParameter << "Gamma " << gamma;

    double binEnd;
    if (alpha > 0 || gamma > 0){
        binEnd = 2*startMoney/(sqrt(lambda + 0.1)) + startMoney;
    }
    else{
        binEnd = 2*startMoney/(sqrt(lambda + 0.1)) + startMoney;
    }

    int binNum = int(binEnd/double(binSize));
    cout << binNum << endl;
    vec binCounts = zeros(binNum);
    vec players = ones(N)*startMoney;

    cout << alpha << " " << gamma << endl;

    binParameters << MCSteps << " " << N << " " << startMoney << " " << binSize << " " << binNum << " " << (binEnd) << " " << lambda << " " << alpha << " " << gamma << endl;
    binParameters.close();

    MonteCarlo(players, MCSteps,  N,transactions,lambda,alpha,gamma,outFileVar,binCounts,binSize,startMoney,outFileErr);
    //outPut(players, MCSteps, N, transactions, expectVal);

    binCounts.save("bins.bin",raw_binary);
    players.save("data.bin",raw_binary);

    outFileErr.close();
    outFileVar.close();

    //players.save("data.bin",raw_binary);

    cout << "Finished" << endl;

}

void MonteCarlo(vec &players, int MCSteps, int N, int transactions, double lambda,double alpha,double gamma, ofstream &outFile, vec &binCounts,double binSize,double m0,ofstream &outFileErr){

    random_device rd;
    mt19937_64 gen(rd());

    uniform_real_distribution<double> distribution(0.0,N);
    uniform_real_distribution<double> eps(0.0,1.0);

    int writingFreq = 100;
    double p = 0;

    mat c = zeros(N,N);
    double maxTransactions = 1;

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

        players.fill(m0);

        for (int j = 0; j < transactions; j++){
            int index_i = distribution(gen);
            int index_j = distribution(gen);

            double epsFac = eps(gen);

            if (players(index_i) - players(index_j) == 0){
                p = 1.;
            }
            else{
                p = 2*pow(fabs((players(index_i) - players(index_j))/double(m0)),-alpha)*(pow((c(index_i,index_j)+1)/(maxTransactions+1),gamma));
            }

            if (eps(gen) < p && (index_i != index_j)){

                double m1 = lambda*players(index_i) + (1-lambda)*epsFac*    (players(index_i) + players(index_j));
                double m2 = lambda*players(index_j) + (1-lambda)*(1-epsFac)*(players(index_i) + players(index_j));

                //cout << "hei" << endl;

                players(index_i) = m1;
                players(index_j) = m2;

                c(index_j,index_i) += 1;
                c(index_i,index_j) += 1;

                if (c(index_j,index_i) > maxTransactions){
                    maxTransactions = c(index_j,index_i);
                }

                else if (c(index_i,index_j) > maxTransactions){
                    maxTransactions = c(index_i,index_j);
                }

            }

            if (MCSteps == 1){
                if (j%writingFreq == 0){

                    double mean = 0;
                    for (int i = 0; i < N; i++){
                        mean += players(i)/m0;

                    }

                    mean /= (N);

                    outFile << (j+1) << " " << findVariance(players,j+1,N,m0) << " " << mean << "\n";
                }

            }
        }

    vec tempCounts = binCounts;
    makeBins(players,binCounts,binSize);

    if (i > 1){
        outFileErr << i+1 << " " << norm(tempCounts/double(i-1) - binCounts/(double(i)) ) << endl;
    }

    }

}

//void outPut(vec &players, int MCSteps, int N, int transactions, mat &expectVal){

//    vec means = zeros(N);
//    for (int k = 0; k < N;k++){
//        means(k) = expectVal(0,k) / MCSteps;
//    }

//    means.save("data.bin",raw_binary);
//}

double findVariance(vec &players, int transaction, int N,double m0){

    double mean = 0;
    double secondMoment = 0;

    for (int i = 0; i < N; i++){
        mean += players(i)/m0;
        secondMoment += players(i)/m0*players(i)/m0;
    }

    mean /= (N);
    //cout << secondMoment << endl;
    secondMoment /= (N);

    return (secondMoment - mean*mean);

}

void makeBins(vec &players, vec &binCount, double binSize){

    for (int i = 0; i < players.size();i++){
        for (int j = 0; j < binCount.size();j++){
            if(players(i)> (j-1)*binSize && players(i)< (j)*binSize){
                binCount(j) += 1;
            }
        }
    }

    //binCount.print();

}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TWO_PROD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing two_prod capabilities

    For any two reals @c x and @c y two_prod computes two reals (in an std::pair)
    @c r0 and @c r1 so that:

    @code
    r0 = x * y
    r1 = r0 -(x * y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

  **/
  std::pair<Value, Value> two_prod(Value const& x, Value const& y);

} }
#endif

#include <boost/simd/function/scalar/two_prod.hpp>
#include <boost/simd/function/simd/two_prod.hpp>

#endif


#ifndef _KERNEL_INDUCTION_HPP_
#define _KERNEL_INDUCTION_HPP_

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SparseCore>
#include <cstdarg>
#include <mimkl/definitions.hpp>
#include <spdlog/spdlog.h>

using mimkl::definitions::Index;

namespace mimkl
{
namespace induction
{

auto logger = spdlog::stdout_color_mt("kernel_induction");

//! If only the diagonal entries of \f$XLX^T\f$ are needed
/*!  one full matrix multiplication cannot be avoided with matrix induction,
    but we only compute the diagonal of \f$(X*L)*X^T\f$
\param matrix NxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param diagonal a  Nx1 Matrix.
\sa test/diagonal_from_square_induction
*/
template <typename MatrixDerived, typename InducerDerived, typename DiagonalDerived>
void get_diagonal_from_square_induction(
const Eigen::MatrixBase<MatrixDerived> &matrix,
const Eigen::SparseMatrixBase<InducerDerived> &inducer,
const Eigen::EigenBase<DiagonalDerived> &diagonal)
{
    const Eigen::Matrix<typename MatrixDerived::Scalar, MatrixDerived::RowsAtCompileTime,
                        MatrixDerived::ColsAtCompileTime>
    matrix_inducer =
    matrix * inducer.template selfadjointView<Eigen::Lower>(); // no .noalias()
    // possible here:
    // matrix_inducer =
    // ;

    Eigen::EigenBase<DiagonalDerived> &diagonal_ =
    const_cast<Eigen::EigenBase<DiagonalDerived> &>(diagonal);
    const Index n = matrix_inducer.rows(); // == matrix.rows()
    if (diagonal.rows() < n)
    {
        spdlog::get("kernel_induction")
        ->critical("error in get_diagonal_from_square_induction():\n rows of "
                   "diagonal {}, rows needed: {}",
                   diagonal.rows(), matrix.rows());
    }
    //  try {
    for (Index i = 0; i < n; i++)
    {
        diagonal_.derived()(i, 0) =
        matrix_inducer.row(i) *
        matrix.adjoint().col(i); // matrix.row(i) for RealScalars
    }
    //  } catch (...){ //
    //		spdlog::get("kernel_induction")->critical("error in
    // get_diagonal_from_square_induction():\n rows of diagonal {}, rows needed:
    //[]", diagonal.rows(), matrix.rows());
    //		throw;
    //  }
}

//! Matrix induction of a linear kernel with the extended kernel K = X*L*Y_t
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse, symmetric) MxM matrix.
\returns kernel_matrix the similarity NxK matrix.
\sa test/linear_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_linear_kernel(LhsDerived &lhs, RhsDerived &rhs, LDerived inducer)
{
    return lhs * inducer.template selfadjointView<Eigen::Lower>() *
           rhs.adjoint(); // symmetry allows optimization Lower vs. Upper?
}

//! Matrix induction of a polynomial kernel with the extended kernel K =
//! (X*L*Y_t + c)^p
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed (unconjugated)  right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param degree polynomial degree.
\param offset "free parameter trading off the influence of higher-order versus
lower-order terms in the polynomial".
\returns kernel_matrix the similarity NxK matrix.
\sa TODO
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_polynomial_kernel(LhsDerived &lhs,
                                  RhsDerived &rhs,
                                  LDerived inducer,
                                  const double degree,
                                  const double offset)
{
    return (induce_linear_kernel<KDerived>(lhs, rhs, inducer).array() + offset)
    .pow(degree)
    .matrix();
}

//! Matrix induction of a gaussian kernel with the extended kernel k = exp(-
//! (x-y)_t*L*(x-y) / ( 2*s^2 )).
/*!  This squared pairwise euclidean distance cannot be expressed in a concise
matrix multiplication.
The squared distance of xi to yj = \f$(x_i^T*L*x_i) -(x_i^T*L*y_j)
-(y_j^T*L*x_i) + (y_j^T*L*y_j) \f$
This means next to \f$XLY^T\f$ (the linear kernel) only the diagonal entries of
\f$XLX^T\f$ and \f$YLY^T\f$ are needed.
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param sigma_square variance of the bell curve.
\returns kernel_matrix the similarity NxK matrix.
\sa gaussian_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_gaussian_kernel(LhsDerived &lhs,
                                RhsDerived &rhs,
                                LDerived inducer,
                                const double sigma_square)
{
    // we only need the diagonal entries of X*L*X_t, so only fill the diagonal
    // of
    // (X*L)*X_t
    Eigen::Matrix<typename LhsDerived::Scalar, LhsDerived::RowsAtCompileTime, 1>
    diag_lhs_inducer_lhs_t; // Length N
    if (LhsDerived::RowsAtCompileTime == Eigen::Dynamic)
        diag_lhs_inducer_lhs_t.resize(lhs.rows());
    get_diagonal_from_square_induction(lhs, inducer, diag_lhs_inducer_lhs_t);
    // same for (Y*L)*Y_t
    Eigen::Matrix<typename RhsDerived::Scalar, RhsDerived::RowsAtCompileTime, 1>
    diag_rhs_inducer_rhs_t; // Length K
    if (RhsDerived::RowsAtCompileTime == Eigen::Dynamic)
        diag_rhs_inducer_rhs_t.resize(rhs.rows());
    get_diagonal_from_square_induction(rhs, inducer, diag_rhs_inducer_rhs_t);

    // X*L*Y_t
    return (-(
            /*! -2* lhs_inducer_rhs only in case of scalar matrices.
             with complex numbers we need: -lhs_inducer_rhs
             -lhs_inducer_rhs.adjoint
             imaginary parts cancel out! */
            ((-2 * induce_linear_kernel<KDerived>(lhs, rhs, inducer).real()).colwise() +
             diag_lhs_inducer_lhs_t)
            .rowwise() +
            diag_rhs_inducer_rhs_t.transpose()) /
            (2 * sigma_square))
    .array()
    .exp()
    .matrix();
}

//! Matrix induction of a sigmoidal kernel with the extended kernel k = tanh(a*
//! (x_t*L*y) +b).
/*!
\param lhs left hand side NxM data-matrix.
\param rhs untransposed  (unconjugated) right hand side KxM data-matrix.
\param inducer pathway specific (sparse) MxM matrix.
\param a
\param b
\returns kernel_matrix the similarity NxK matrix.
\sa test/sigmoidal_induction
*/
template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
KDerived induce_sigmoidal_kernel(LhsDerived &lhs,
                                 RhsDerived &rhs,
                                 LDerived inducer,
                                 const double a,
                                 const double b)
{
    return (a * induce_linear_kernel<KDerived>(lhs, rhs, inducer).array() + b)
    .tanh()
    .matrix();
}

template <typename KDerived, typename LhsDerived, typename RhsDerived, typename LDerived>
std::vector<std::function<KDerived(LhsDerived &, RhsDerived &)>>
inducer_combination(std::function<KDerived(LhsDerived &, RhsDerived &, LDerived)>
                    kernel_function, // parameter specification done
                                     // previously
                    std::vector<LDerived> inducers)
{
    std::vector<std::function<KDerived(LhsDerived &, RhsDerived &)>> lambda_expressions;
    lambda_expressions.reserve(inducers.size());

    for (LDerived inducer : inducers)
    {
        lambda_expressions.push_back(
        [&, inducer](const LhsDerived &lhs, const RhsDerived &rhs) {
            return kernel_function(lhs, rhs, inducer);
        });
    }

    return lambda_expressions;
}

} // namespace induction
} // namespace mimkl

#endif /*_KERNEL_INDUCTION_HPP_*/


//    boost asinh.hpp header file

//  (C) Copyright Eric Ford & Hubert Holin 2001. Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_ASINH_HPP
#define BOOST_ASINH_HPP

#include <cmath>
#include <limits>
#include <string>
#include <stdexcept>

#include <boost/config.hpp>

// This is the inverse of the hyperbolic sine function.

namespace boost
{
    namespace math
    {
#if defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::log;

        using    ::std::numeric_limits;
#endif

        template<typename T>
        inline T    asinh(const T x)
        {
            using    ::std::abs;
            using    ::std::sqrt;
            using    ::std::log;

            using    ::std::numeric_limits;

            T const            one = static_cast<T>(1);
            T const            two = static_cast<T>(2);

            static T const    taylor_2_bound = sqrt(numeric_limits<T>::epsilon());
            static T const    taylor_n_bound = sqrt(taylor_2_bound);
            static T const    upper_taylor_2_bound = one/taylor_2_bound;
            static T const    upper_taylor_n_bound = one/taylor_n_bound;

            if        (x >= +taylor_n_bound)
            {
                if        (x > upper_taylor_n_bound)
                {
                    if        (x > upper_taylor_2_bound)
                    {
                        // approximation by laurent series in 1/x at 0+ order from -1 to 0
                        return( log( x * two) );
                    }
                    else
                    {
                        // approximation by laurent series in 1/x at 0+ order from -1 to 1
                        return( log( x*two + (one/(x*two)) ) );
                    }
                }
                else
                {
                    return( log( x + sqrt(x*x+one) ) );
                }
            }
            else if    (x <= -taylor_n_bound)
            {
                return(-asinh(-x));
            }
            else
            {
                // approximation by taylor series in x at 0 up to order 2
                T    result = x;

                if    (abs(x) >= taylor_2_bound)
                {
                    T    x3 = x*x*x;

                    // approximation by taylor series in x at 0 up to order 4
                    result -= x3/static_cast<T>(6);
                }

                return(result);
            }
        }
    }
}

#endif /* BOOST_ASINH_HPP */


/**
 * @author Elie Khoury <Elie.Khoury@idiap.ch>
 * @author Laurent El Shafey <Laurent.El-Shafey@idiap.ch>
 * @date Tue Apr 2 21:08:00 2013 +0200
 *
 * Copyright (C) Idiap Research Institute, Martigny, Switzerland
 */

#include <boost/make_shared.hpp>
#include <bob.math/inv.h>
#include <bob.math/lu.h>
#include <bob.math/stats.h>

#include <bob.learn.linear/wccn.h>

namespace bob { namespace learn { namespace linear {

  WCCNTrainer::WCCNTrainer() {
  }

  WCCNTrainer::WCCNTrainer(const WCCNTrainer& other) {
  }

  WCCNTrainer::~WCCNTrainer() {}

  WCCNTrainer& WCCNTrainer::operator= (const WCCNTrainer& other) {
    return *this;
  }

  bool WCCNTrainer::operator== (const WCCNTrainer& other) const {
    return true;
  }

  bool WCCNTrainer::operator!= (const WCCNTrainer& other) const {
    return !(this->operator==(other));
  }

  void WCCNTrainer::train(Machine& machine,
      const std::vector<blitz::Array<double, 2> >& data) const {

    const size_t n_classes = data.size();
    // if #classes < 2, then throw
    if (n_classes < 2) {
      boost::format m("number of classes should be >= 2, but you passed %u");
      m % n_classes;
      throw std::runtime_error(m.str());
    }

    // checks for data type and shape once
    const int n_features = data[0].extent(1);

    for (size_t cl=0; cl<n_classes; ++cl) {
      if (data[cl].extent(1) != n_features) {
        boost::format m("number of features (columns) of array for class %u (%d) does not match that of array for class 0 (%d)");
        m % cl % data[cl].extent(1) % n_features;
        throw std::runtime_error(m.str());
      }
    }

    // machine dimensions
    const size_t n_inputs = machine.inputSize();
    const size_t n_outputs = machine.outputSize();

    // Checks that the dimensions are matching
    if ((int)n_inputs != n_features) {
      boost::format m("machine input size (%u) does not match the number of columns in input array (%d)");
      m % n_inputs % n_features;
      throw std::runtime_error(m.str());
    }
    if ((int)n_outputs != n_features) {
      boost::format m("machine output size (%u) does not match the number of columns in output array (%d)");
      m % n_outputs % n_features;
      throw std::runtime_error(m.str());
    }

    // 1. Computes the mean vector and the Scatter matrix Sw and Sb
    blitz::Array<double,1> mean(n_features);
    blitz::Array<double,2> buf1(n_features, n_features); // Sw
    blitz::Array<double,2> buf2(n_features, n_features); // Sb
    bob::math::scatters(data, buf1, buf2, mean); // buf1 = Sw; buf2 = Sb

    // 2. Computes the inverse of (1/N * Sw), Sw is the within-class covariance matrix
    buf1 /= n_classes;
    bob::math::inv(buf1, buf2); // buf2 = (1/N * Sw)^{-1}

  // 3. Computes the Cholesky decomposition of the inverse covariance matrix
  bob::math::chol(buf2, buf1); //  buf1 = cholesky(buf2)

  // 4. Updates the linear machine
  machine.setInputSubtraction(0); // we do not substract the mean
  machine.setInputDivision(1.);
  machine.setWeights(buf1);
  machine.setBiases(0);
  machine.setActivation(boost::make_shared<bob::learn::activation::IdentityActivation>());

  }

}}}


#pragma once

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

using namespace std;
using namespace Eigen;

template<class T, unsigned int batch_size, unsigned int input_size, unsigned int output_size>
class Layer
{
private:
  // matrix to store outputs of neurons after activation sigma(W*X + B)
  Matrix<T, batch_size, output_size> output_ {};
  // matrix to store outputs of neurons before activation W*X + B
  Matrix<T, batch_size, output_size> unactivated_output_ {};
  // matrix to store weights of the connections  (W)*X + (B)
  Matrix<T, input_size, output_size> weights_ {};
  // matrix to store biases of the layer W*X + (B)
  Matrix<T, 1, output_size> biases_ {};

  // matrix to store errors at intermediate nodes for given input i.e. target activation - current activation
  Matrix<T, batch_size, output_size> deltas_ {};
  // matrix to store gradients w.r.t. weights
  Matrix<T, input_size, output_size> grad_weights_ {};
  // matrix to store gradients w.r.t. biases
  Matrix<T, 1, output_size> grad_biases_ {};

public:
  Layer() {}

  void initializeWeightsRandomly()
  {
    weights_ = Matrix<T, input_size, output_size>::Random();
    biases_ = Matrix<T, 1, output_size>::Random();
  }

  void apply( const Matrix<T, batch_size, input_size>& input )
  {
    unactivated_output_ = ( input * weights_ ).rowwise() + biases_;
    output_ = unactivated_output_.cwiseMax( 0 );
  }

  void apply_without_activation( const Matrix<T, batch_size, input_size>& input )
  {
    unactivated_output_ = ( input * weights_ ).rowwise() + biases_;
    output_ = unactivated_output_;
  }

  void print( const unsigned int layer_num ) const
  {
    const IOFormat CleanFmt( 4, 0, ", ", "\n", "[", "]" );

    cout << "Layer " << layer_num << endl;
    cout << "input_size: " << input_size << " -> "
         << "output_size: " << output_size << endl
         << endl;

    cout << "weights:" << endl << weights_.format( CleanFmt ) << endl << endl;
    cout << "biases:" << endl << biases_.format( CleanFmt ) << endl << endl;
    cout << "unactivated_output:" << endl << unactivated_output_.format( CleanFmt ) << endl << endl;
    cout << "output:" << endl << output_.format( CleanFmt ) << endl << endl;

    cout << "deltas:" << endl << deltas_.format( CleanFmt ) << endl << endl;
    cout << "grad_weights:" << endl << grad_weights_.format( CleanFmt ) << endl << endl;
    cout << "grad_biases:" << endl << grad_biases_.format( CleanFmt ) << endl << endl << endl;
  }

  void perturbWeight( const unsigned int weight_num, const T epsilon )
  {
    const unsigned int i = weight_num / output_size;
    const unsigned int j = weight_num % output_size;
    if ( i < input_size ) {
      weights_( i, j ) += epsilon;
    } else {
      biases_( 0, j ) += epsilon;
    }
  }

  unsigned int getNumParams() const { return ( input_size + 1 ) * output_size; }
  unsigned int getInputSize() const { return input_size; }
  unsigned int getOutputSize() const { return output_size; }

  T getEvaluatedGradient( const unsigned int paramNum )
  {
    const unsigned int i = paramNum / output_size;
    const unsigned int j = paramNum % output_size;
    if ( i < input_size ) {
      return grad_weights_( i, j );
    } else {
      return grad_biases_( 0, j );
    }
  }

  const Matrix<T, batch_size, input_size> computeDeltas( Matrix<T, batch_size, output_size> nextLayerDeltas )
  {
    // activated nodes is the matrix that stores 0/1 corresponding to whether the output node was activated
    Matrix<T, batch_size, output_size> activated_nodes
      = ( unactivated_output_.array() > 0 ).template cast<T>().matrix();
    deltas_ = nextLayerDeltas.cwiseProduct( activated_nodes );
    return deltas_ * weights_.transpose();
  }

  const Matrix<T, batch_size, input_size> computeDeltasLastLayer(
    Matrix<T, batch_size, output_size> nextLayerDeltas )
  {
    deltas_ = nextLayerDeltas;
    return deltas_ * weights_.transpose();
  }

  void evaluateGradients( const Matrix<T, batch_size, input_size>& input )
  {
    grad_weights_ = Matrix<T, input_size, output_size>::Zero();
    // grad_biases_ = Matrix<T, 1, output_size>::Zero();
    for ( unsigned int b = 0; b < batch_size; b++ ) {
      // for ( unsigned int j = 0; j < output_size; j++ ) {
      //   // for ( unsigned int i = 0; i < input_size; i++ ) {
      //   //   grad_weights_( i, j ) += input( b, i ) * deltas_( b, j );
      //   // }
      //   grad_weights_.col( j ) += input.row( b ) * deltas_( b, j );
      // }
      grad_weights_.noalias() += input.row( b ).transpose() * deltas_.row( b );
      // grad_biases_.noalias() += deltas_.row( b );
      // noalias is an eigen optimisation - otherwise becomes slower than for loops
    }
    grad_biases_ = deltas_.colwise().sum();
  }

  const Matrix<T, input_size, output_size>& weights() const { return weights_; }
  const Matrix<T, batch_size, output_size>& output() const { return output_; }
  const Matrix<T, 1, output_size>& biases() const { return biases_; }

  // accessors for mutable access to weights and biases
  Matrix<T, input_size, output_size>& weights() { return weights_; }
  Matrix<T, 1, output_size>& biases() { return biases_; }
};


#include "ElGamalSignature.h"
#include <ctime>
#include <NTL/BasicThreadPool.h>

using namespace ElGamal;

PublicKey::PublicKey(const ZZ &p, const ZZ &alpha, const ZZ &beta)
        : p(p), alpha(alpha), beta(beta) {}

PrivateKey::PrivateKey(const ZZ &a) : a(a) {
}

ElGamalSignature::ElGamalSignature() : pk(nullptr), sk(nullptr) {
    SetSeed(conv<ZZ>(static_cast<long>(time(nullptr))));
}

void ElGamalSignature::generateKeyPair(int len) {
    ZZ p = findPrime(len);
    ZZ alpha = findPrimitiveRoot(p);
    ZZ a = RandomBnd(p - 2) + 1;//[1,p-1]
    ZZ beta = PowerMod(alpha, a, p);
    pk = new PublicKey(p, alpha, beta);
    sk = new PrivateKey(a);
}

ZZ ElGamalSignature::sig(const ZZ &x, PublicKey *pk, PrivateKey *sk) {
    ZZ k = RandomBnd(pk->p - 3) + 1;//[1,p-2]
    while (GCD(k, pk->p - 1) != 1) {
        k = RandomBnd(pk->p - 3) + 1;
    }
    ZZ gamma = PowerMod(pk->alpha, k, pk->p);
    //delta = (x-a*gamma)*k^(-1) mod (p-1)
    ZZ delta = MulMod(x - sk->a * gamma, InvMod(k, pk->p - 1), pk->p - 1);
    return gamma * pk->p + delta;
}

ZZ ElGamalSignature::sig(const ZZ &x) const {
    return this->sig(x, pk, sk);
}

ZZ ElGamalSignature::sig(const string &x) const {
    return this->sig(stringToNumber(x), pk, sk);
}

bool ElGamalSignature::ver(const ZZ &x, const ZZ &y, PublicKey *pk) {
    ZZ gamma = y / pk->p;
    ZZ delta = y % pk->p;
    //beta^(gamma)*gamma^delta==alpha^x (mod p)
    return (PowerMod(pk->beta, gamma, pk->p) * PowerMod(gamma, delta, pk->p)) % pk->p == PowerMod(pk->alpha, x, pk->p);
}

ElGamalSignature::~ElGamalSignature() {
    delete pk;
    delete sk;
}

PublicKey *ElGamalSignature::getPK() const {
    return pk;
}

PrivateKey *ElGamalSignature::getSK() const {
    return sk;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// With contributions from Cornelius Steinhardt

#ifndef MTL_VECTOR_SECULAR_INCLUDE
#define MTL_VECTOR_SECULAR_INCLUDE

#include <cmath>
#include <boost/utility.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/operation/minimal_increase.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

/// Class for the secular equation( to solve eigenvalue problems)
template <typename Vector>
class secular_f
{
    typedef typename Collection<Vector>::value_type   value_type;
    typedef typename Collection<Vector>::size_type    size_type;

  public:
    /// Constructor needs 2 Vectors z(nummerator), d(dominator) and sigma as factor before the sum
    secular_f(const Vector& z, const Vector& d, value_type sigma)
      : z(z), d(d), sigma(sigma) {}

    /// secular_f equation as function, evaluates the function value
    /** \f$f(x)=1+\sigma * sum_{i=1}^{n}\frac{z_i}{d_i-x} \f$**/
    value_type f(const value_type& lamb)
    {
	value_type fw= 1;
	for(size_type i=0; i<size(z); i++)
	    fw+= sigma*z[i]*z[i]/(d[i]-lamb);
	return fw;
    }

    value_type square(value_type x) const { return x*x; }

    /// gradient of secular_f equation as function, evaluates the gradientfunction value
    /** \f$gradf(x)=\sigma * sum_{i=1}^{n}\frac{z_i}{(d_i-x)^2} \f$**/
    value_type grad_f(const value_type& lamb)
    {
	value_type gfw= 0.0;
	for(size_type i=0; i<size(z); i++)
	    gfw+= square(z[i] / (d[i] - lamb)); // , std::cout << "gfw = " << gfw << '\n';  //TODO
	return sigma*gfw;
    }

    /// Evaluates the roots of secular_f equation =0 with newton algo.
    /** Computes mixed Newton and interval nesting. d must be sorted. **/
    Vector roots()
    {
	assert(size(z) > 1);
	const double tol= 1.0e-6;
	Vector       start(resource(z)), lambda(resource(z));

	for (size_type i= 0; i < size(z); i++) {
	    // Equal poles -> eigenvalue
	    if (i < size(z) - 1 && d[i] == d[i+1]) {
		lambda[i]= d[i]; continue; }

	    // Check if root is too close to pole (i.e. d[i]+eps > 0) then take this because we can't reach the root
	    value_type next= minimal_increase(d[i]), lamb, old;
	    if (f(next) >= value_type(0)){
		lambda[i]= next; continue; }

	    if (i < size(z) - 1)
		old= lamb= start[i]= (d[i] + d[i+1]) / 2;  //start points between pols
	    else
		old= lamb= start[i]= 1.5 * d[i] - 0.5 * d[i-1];  // last start point plus half the distance to second-last

   	    while (std::abs(f(lamb)) > tol) {
		if (lamb <= d[i])
		    start[i]= lamb= (d[i] + start[i]) / 2;
		else
		    lamb-= f(lamb) / grad_f(lamb);
		if (old == lamb) break;
		old= lamb;
	    }
	    lambda[i]= lamb;
	}
	return lambda;
    }

 private:
    Vector     z, d;
    value_type sigma;
};

template <typename Vector, typename Value>
inline Vector secular(const Vector& z, const Vector& d, Value sigma)
{
	vampir_trace<3030> tracer;
    secular_f<Vector> functor(z, d, sigma);
    return functor.roots();
}

}}// namespace vector

#endif // MTL_VECTOR_SECULAR_INCLUDE