#include<cstdlib>
#include<iostream>
#include<iomanip>
#include<fstream>
#include<cmath>
#include<vector>
#include <boost/filesystem/operations.hpp>
#include <boost/program_options.hpp>

using namespace std;
using namespace boost::program_options;

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

    /*User Inputs*/
    //string ligandNumText = "USERINPUT_LIGAND_NUMBER";
    //string npChargeText = "USERINPUT_NP_CHARGE";
    //string mpiText = "NODESIZE";

    string ligandNumFileNameText = "USERINPUT_LIGAND_FILENAME";
    string saltText = "USERINPUTSALT";
    string dataSetCountText = "USERINPUT_DATASETCOUNT";

    // Dev inputs
    string vlp1ChargeText = "DEVINPUT_VLP1_CHARGE";
    string vlp2ChargeText = "DEVINPUT_VLP2_CHARGE";
    string ligandChargeText = "DEVINPUT_LIGAND_CHARGE";

    const double pi = 3.141593;
    // big for NP, small for ligand
    int n, Q_vlp1, Q_vlp2;       //dev input
    int e2k2ratio; //user input
    double c;       //user input
    double D, d;    //dev input, but could be made user input
    int q;          //dev input

    int mpiProcs;

    //  Details on the datasets to be used (first dumpstep, number of directly subsequent steps):
    int initDumpStep, dataSetCount;

    // Specify variables via command line (-X x):
    options_description desc("Usage:\nrandom_mesh <options>");
    desc.add_options()
            ("help,h", "print usage message")
            ("Qvlp1,E", value<int>(&Q_vlp1)->default_value(-1500), "Q_E2 in e")
            ("Qvlp2,K", value<int>(&Q_vlp2)->default_value(-600), "Q_K2 in e")
            ("e2k2ratio,r", value<int>(&e2k2ratio)->default_value(1), "e2:k2 in multiples")
            ("NLigand,n", value<int>(&n)->default_value(100))
            ("Salt,c", value<double>(&c)->default_value(0.150), "c in Molars")
            ("qnp,q", value<int>(&q)->default_value(45), "q in e")
            ("NPDiameter,D", value<double>(&D)->default_value(56), "D in nm")
            ("LDiameter,d", value<double>(&d)->default_value(6.7), "d in nm")
            ("Mpi_procs,m", value<int>(&mpiProcs)->default_value(1), "Number of MPI procs for Lammps")
            ("initDumpStep,i", boost::program_options::value<int>(&initDumpStep)->default_value(0),
             "Specify the initial dump step to be used (dump step, not timestep).")
            ("dataSetCount,N", boost::program_options::value<int>(&dataSetCount)->default_value(150),
             "Specify the number of subsequent datasets to use after the initial dump step.");
    //hard code the ratio

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);

    /*************************Preprocessing*************************/
    /*Open a new the template file*/

    /******************computations of dev variables****************/

    double devInputQvlp1Charge = Q_vlp1 * (exp((1.64399 * 56) / sqrt(1 / c)) / (1 + ((1.64399 * 56) / sqrt(1 / c))));
    devInputQvlp1Charge = devInputQvlp1Charge * (1.6018 * 1e-19) / (1.60074 * 1e-19);

    double devInputQvlp2Charge = Q_vlp2 * (exp((1.64399 * 56) / sqrt(1 / c)) / (1 + ((1.64399 * 56) / sqrt(1 / c))));
    devInputQvlp2Charge = devInputQvlp2Charge * (1.6018 * 1e-19) / (1.60074 * 1e-19);

    double devInputLigandCharge = q * (exp((1.64399 * 6.7) / sqrt(1 / c)) / (1 + ((1.64399 * 6.7) / sqrt(1 / c))));
    devInputLigandCharge = devInputLigandCharge * (1.6018 * 1e-19) / (1.60074 * 1e-19);

    double userInputSalt = 1 / (1e3 * sqrt(1 / (2 * 4 * pi * 0.714295 * (6.022 * 1e23) * c)));
    //double userInputSalt = 3.2879795708993616*1e9/sqrt(1/c);
    userInputSalt = userInputSalt * 56 * 1e-9;

    std::string initcoords = "initCoords_" + std::to_string(n);
    if(e2k2ratio==1)
        initcoords = initcoords+"x_1-1";
    else if(e2k2ratio == 4)
        initcoords = initcoords+"x_1-4";
    initcoords = initcoords + ".assembly";
    cout<<"\n ===>"<<initcoords;
    ofstream inputScript("in.lammps", ios::trunc);
    if (inputScript.is_open()) {

        /*Open the template file*/
        string line;
        ifstream inputTemplate("infiles/in.lammps.template", ios::in);
        if (inputTemplate.is_open()) {
            while (getline(inputTemplate, line)) {
//                std::size_t found = line.find(ligandNumText);
//                if (found != std::string::npos)
//                    line.replace(found, ligandNumText.length(),  std::to_string(n));

//                found = line.find(npChargeText);
//                if (found != std::string::npos)
//                    line.replace(found, npChargeText.length(), std::to_string(userInputNpCharge));

                // Added for bnsl
                std::size_t found = line.find(vlp1ChargeText);
                if (found != std::string::npos)
                    line.replace(found, vlp1ChargeText.length(), std::to_string(devInputQvlp1Charge));
                found = line.find(vlp2ChargeText);
                if (found != std::string::npos)
                    line.replace(found, vlp2ChargeText.length(), std::to_string(devInputQvlp2Charge));

                found = line.find(ligandChargeText);
                if (found != std::string::npos)
                    line.replace(found, ligandChargeText.length(), std::to_string(devInputLigandCharge));

                found = line.find(saltText);
                if (found != std::string::npos)
                    line.replace(found, saltText.length(), std::to_string(userInputSalt));

                //unsure
                found = line.find(dataSetCountText);
                if (found != std::string::npos)
                    line.replace(found, dataSetCountText.length(), std::to_string(dataSetCount));

               found = line.find(ligandNumFileNameText);
               if (found != std::string::npos)
                   line.replace(found, ligandNumFileNameText.length(), initcoords);

                inputScript << line << endl;
            }
            inputTemplate.close();
        } else cout << "Unable to open the template input script" << endl;
        inputScript.close();
    } else cout << "Unable create a input Script" << endl;

    /*************************Lammps Call*************************/
/*
    //string lammpsExeCMD = "aprun -n NODESIZE lmp_mpi < in.lammps"; //This is Bigred and New Lammps
    //string lammpsExeCMD = "mpirun -n NODESIZE lmp_g++ < in.lammps"; //This is RedHat
    string lammpsExeCMD = "mpiexec -n NODESIZE lmp_mpi < in.lammps"; //This is windows and newest Lammps
    //string lammpsExeCMD = "aprun -n NODESIZE lmp_xe6  < in.lammps"; //This is Bigred and OLD lammps
    std::size_t found = lammpsExeCMD.find(mpiText);
    if (found != std::string::npos)
        lammpsExeCMD.replace(found, mpiText.length(), std::to_string(mpiProcs));

    //Creating the char array
    char lammpsExeCMD_array[lammpsExeCMD.length() + 1];
    strcpy(lammpsExeCMD_array, lammpsExeCMD.c_str());

    //System call
    //system("ls");
    int returnedCode;
    //Checking if processor is available
    if (!system(NULL))
        exit(EXIT_FAILURE);

    returnedCode = system(lammpsExeCMD_array);
    cout << "The value returned was: " << returnedCode << endl;

    if(returnedCode!=0)
        exit(EXIT_FAILURE);
*/
    cout << "Preprocessing completed." << endl;
    return 0;
}
// End of main


#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_face_normals.h>
#include <igl/barycenter.h>
#include <igl/pinv.h>
#include <igl/edges.h>
#include <Eigen/SparseCore>
#include <igl/adjacency_list.h>
#include <igl/adjacency_matrix.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/avg_edge_length.h>
#include <igl/edge_flaps.h>
#include <igl/unique_edge_map.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/principal_curvature.h>
#include <igl/collapse_edge.h>
#include <igl/is_edge_manifold.h>
#include <igl/C_STR.h>
#include <igl/circulation.h>
#include <igl/decimate.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/infinite_cost_stopping_condition.h>
using namespace std;

void collapse_edges(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::VectorXi & feature, Eigen::VectorXd & high, Eigen::VectorXd & low){
        using namespace Eigen;
    MatrixXi E,uE,EI,EF;
    VectorXi EMAP,I,J;
    VectorXd data;
    Eigen::MatrixXd U;
    Eigen::MatrixXi G;
    // VectorXd p;
    std::vector<std::vector<int>> uE2E;
    std::vector<std::vector<int>> vertex_face_adjacency;
    std::vector<int> small_edges;
    int e1,e2,f1,f2,e;
    int n = V.rows();

    int num_feature = feature.size();
    std::vector<std::vector<int>> A;
    igl::adjacency_list(F,A);

    std::vector<bool> is_feature_vertex;
    is_feature_vertex.resize(n);

    for (int s = 0; s < num_feature; s++) {
        is_feature_vertex[feature(s)] = true;
    }

    //igl::is_edge_manifold(F);

    std::function<bool(
                       const Eigen::MatrixXd &,
                       const Eigen::MatrixXi &,
                       const Eigen::MatrixXi &,
                       const Eigen::VectorXi &,
                       const Eigen::MatrixXi &,
                       const Eigen::MatrixXi &,
                       const igl::min_heap< std::tuple<double,int,int> > &,
                       const Eigen::VectorXi &,
                       const Eigen::MatrixXd &,
                       const int,
                       const int,
                       const int,
                       const int,
                       const int)>  stopping_condition;

    std::function<void(
                       const int,
                       const Eigen::MatrixXd &,
                       const Eigen::MatrixXi &,
                       const Eigen::MatrixXi &,
                       const Eigen::VectorXi &,
                       const Eigen::MatrixXi &,
                       const Eigen::MatrixXi &,
                       double &,
                       Eigen::RowVectorXd &)> shortest_edge_and_midpoint_lambda = [&A,&feature,&low,&high,&is_feature_vertex](
                                                                 const int e,
                                                                 const Eigen::MatrixXd & V,
                                                                 const Eigen::MatrixXi & F,
                                                                 const Eigen::MatrixXi & E,
                                                                 const Eigen::VectorXi & EMAP,
                                                                 const Eigen::MatrixXi & EF,
                                                                 const Eigen::MatrixXi & EI,
                                                                 double & cost,
                                                                 Eigen::RowVectorXd & p)->void{
        igl::shortest_edge_and_midpoint(e,V,F,E,EMAP,EF,EI,cost,p);
        if (is_feature_vertex[E(e,0)] || is_feature_vertex[E(e,1)] ) {
            cost = std::numeric_limits<double>::infinity();
            return;
        }
        if ( (V.row(E(e,0))-V.row(E(e,1))).norm() > ((low(E(e,0))+low(E(e,1)))/2) ) {
            cost = std::numeric_limits<double>::infinity();
            return;
        }
        for(int i = 0; i < A[E(e,1)].size(); i++){
            if((V.row(A[E(e,1)][i])-p).norm() > high(E(e,1))){
                cost = std::numeric_limits<double>::infinity();
                return;
            }
        }
        for(int r = 0; r < A[E(e,0)].size(); r++){
            if((V.row(A[E(e,0)][r])-p).norm() > high(E(e,0))){
                cost = std::numeric_limits<double>::infinity();
                return;
            }
        }
        //std::cout << "Mathing..." << std::endl;
        // consider both directions to circulate
        for(int direction = 0;direction<2;direction++)
        {
            // consider each face
            for(const int f : igl::circulation(e,direction,EMAP,EF,EI))
            {
                if (f < 0) {//?????
                    cost = std::numeric_limits<double>::infinity();
                    return;
                }
                //std::cout << f << std::endl;
                if( f == 0 || f ==  igl::circulation(e,direction,EMAP,EF,EI).size()-1)
                {

                    // skip
                    continue;
                }
                // Grab the three corners of the face
                Eigen::RowVector3d p_before[3], p_after[3];
                for(int c = 0;c<3;c++)
                {
                    // vertex index
//                    std::cout << e << std::endl;
//                    std::cout << f << std::endl;
//                    std::cout << c << std::endl;
                    const int v = F(f,c);
                    if( v == E(e,0) || v == E(e,1))
                    {
                        p_after[c] = p;
                    }else
                    {
                        p_after[c] = V.row(v);
                    }
                    p_before[c] = V.row(v);
                }
                const Eigen::RowVector3d n_before =
                ((p_before[1]- p_before[0]).cross(p_before[2]- p_before[0])).normalized();
                const Eigen::RowVector3d n_after =
                ((p_after[1]- p_after[0]).cross(p_after[2]- p_after[0])).normalized();
                if( n_before.dot(n_after) < n_after.norm()/2 )
                   {
                       cost = std::numeric_limits<double>::infinity();
                   }
                   }
                   }
        //std::cout << "Mathed!" << std::endl;

        };

    igl::infinite_cost_stopping_condition(shortest_edge_and_midpoint_lambda,stopping_condition);

    //std::cout << "??" << std::endl;
    igl::decimate(V,F,shortest_edge_and_midpoint_lambda,stopping_condition,U,G,J,I);
    //std::cout << "!!" << std::endl;

    Eigen::VectorXd high_new,low_new;
    Eigen::VectorXi feature_new;
    feature_new.resize(num_feature);
    high_new.resize(U.rows());
    low_new.resize(U.rows());
    int j = 0;
    for (int s = 0; s<U.rows(); s++) {
        high_new(s) = high(I(s));
        low_new(s) = low(I(s));
        if (is_feature_vertex[I(s)]) {
            feature_new(j) = s;
            j = j+1;
        }
    }

    // PLACEHOLDER

    V = U;
    F = G;
    high = high_new;
    low = low_new;
    feature = feature_new;

}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp remesh_botsch.cpp -o main


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

#include <iostream>
#include <fstream>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/objdetect.hpp>
#include <boost/program_options.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <list>

template<typename T>
std::vector<T> polyfit( const std::vector<T>& oX, const std::vector<T>& oY, int nDegree ) {
	using namespace boost::numeric::ublas;

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

	// more intuative this way
	nDegree++;

	size_t nCount =  oX.size();
	matrix<T> oXMatrix( nCount, nDegree );
	matrix<T> oYMatrix( nCount, 1 );

	// copy y matrix
	for ( size_t i = 0; i < nCount; i++ )
	{
		oYMatrix(i, 0) = oY[i];
	}

	// create the X matrix
	for ( size_t nRow = 0; nRow < nCount; nRow++ )
	{
		T nVal = 1.0f;
		for ( int nCol = 0; nCol < nDegree; nCol++ )
		{
			oXMatrix(nRow, nCol) = nVal;
			nVal *= oX[nRow];
		}
	}

	// transpose X matrix
	matrix<T> oXtMatrix( trans(oXMatrix) );
	// multiply transposed X matrix with X matrix
	matrix<T> oXtXMatrix( prec_prod(oXtMatrix, oXMatrix) );
	// multiply transposed X matrix with Y matrix
	matrix<T> oXtYMatrix( prec_prod(oXtMatrix, oYMatrix) );

	// lu decomposition
	permutation_matrix<T> pert(oXtXMatrix.size1());
	const std::size_t singular = lu_factorize(oXtXMatrix, pert);
	// must be singular
	if( singular != 0 ) {
		// then the regression cannot be done... return an empty vector
		return std::vector<T>();
	}

	// backsubstitution
	lu_substitute(oXtXMatrix, pert, oXtYMatrix);

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

float simpleShape(cv::Mat &image) {
	if(image.empty()) return 0;

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

	cv::Mat edges;
	cv::Canny(gray, edges, 200, 400);

	cv::Mat result(image.size(), CV_8UC3, cv::Scalar(0,0,0));

	std::vector<std::vector<cv::Point> > contours0;
	std::vector<std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarchy;
    cv::findContours( edges, contours0, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

	std::cout << contours0.size() << " ";

	cv::drawContours( result, contours, 3, cv::Scalar(128,0,255),
                  3, cv::LINE_AA, hierarchy, std::abs(3) );

    contours.resize(contours0.size());
    for( size_t k = 0; k < contours0.size(); k++ )
        cv::approxPolyDP(cv::Mat(contours0[k]), contours[k], 1, true);

	cv::drawContours( result, contours, 3, cv::Scalar(128,255,255),
                  3, cv::LINE_AA, hierarchy, std::abs(3) );

	cv::imshow("result", edges);
	cv::waitKey();

	return 0;
}

float  horizonLine(cv::Mat &image) {
	if(image.empty()) return 0;

	std::vector<float> x, y;

	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

	cv::Mat edges;
	cv::Canny(gray, edges, 110, 220);

	std::vector<cv::Vec4i> lines;
    cv::HoughLinesP( edges, lines, 1, CV_PI/2, 0, 30, 1 );
    float meanY = 0; int nbLines = 0;
    float meanY2 = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        cv::line( image, cv::Point(lines[i][0], lines[i][1]),
            cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0,0,255), 3, 8 );

        x.push_back(lines[i][0]);
		x.push_back(lines[i][2]);
		y.push_back(lines[i][1]);
		y.push_back(lines[i][3]);

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

		meanY2 += lines[i][1] * lines[i][1];
		meanY2 += lines[i][3] * lines[i][3];

    }
    meanY /= nbLines;
    meanY2 /= nbLines;

    float stdev = std::sqrt(meanY2 - meanY*meanY);
    float meanValue = meanY;

    meanY = 0; nbLines = 0;
    for( size_t i = 0; i < lines.size(); i++ )
    {
    	if(lines[i][0] == lines[i][2]) continue;

		if(lines[i][1] > (.7 * image.rows)) continue;
		if(lines[i][1] < (.3 * image.rows)) continue;

        if(std::abs(lines[i][1] - meanValue) > 1.3 * stdev) continue;

		meanY += lines[i][1]; ++nbLines;
		meanY += lines[i][3]; ++nbLines;

    }
    meanY /= nbLines;

    if(nbLines == 0) {
    	meanY = image.rows/2;
    } else {
    	std::vector<float> model = polyfit<float>(x, y, 1);

	    if(std::abs(model[0]- image.rows/2) < std::abs(meanY - image.rows/2))
	    	meanY = model[0];
    }

	// cv::line(image, cv::Point_<int>(0, meanY), cv::Point_<int>(image.cols, meanY), cv::Scalar( 0, 255, 0 ), 2);

return meanY;

}

float faceLine(cv::Mat &image) {
	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

	cv::CascadeClassifier face_cascade;
	cv::CascadeClassifier faceProfil_cascade;
	bool faceCascadeEnabled = false;
	bool faceProfilCascadeEnabled = false;

	// ------------------------------------------------------------------------------------------------
	// init haar cascades framework

	if(face_cascade.load("haarcascade_frontalface_alt.xml")) {
		faceCascadeEnabled = true;
	} else {
		std::cerr<< "[I] cannot open: haarcascade_frontalface_alt.xml" << std::endl;
	}

	if(faceProfil_cascade.load("haarcascade_profileface.xml")) {
		faceProfilCascadeEnabled = true;
	} else {
		std::cerr<< "[I] cannot open: haarcascade_profileface.xml" << std::endl;
	}

	// ------------------------------------------------------------------------------------------------
	// find features

	std::list<cv::Rect> allFeatures;
	std::vector<cv::Rect> faceFeatures;
	if(faceCascadeEnabled)
		face_cascade.detectMultiScale( gray, faceFeatures, 2, 2, 0| cv::CASCADE_SCALE_IMAGE, cv::Size(15, 15) );

	for(size_t i = 0 ; i < faceFeatures.size() ; ++i) {
		cv::rectangle(image, faceFeatures[i], cv::Scalar( 0, 0, 255 ));
		allFeatures.push_back(faceFeatures[i]);
	}

	faceFeatures.clear();
	if(faceProfilCascadeEnabled)
		faceProfil_cascade.detectMultiScale( gray, faceFeatures, 2, 2, 0|cv::CASCADE_SCALE_IMAGE, cv::Size(15, 15) );

	for(size_t i = 0 ; i < faceFeatures.size() ; ++i) {
		cv::rectangle(image, faceFeatures[i], cv::Scalar( 255, 255, 0 ));
		allFeatures.push_back(faceFeatures[i]);
	}

	if(allFeatures.size() < 2) return -1;

	float mnX = image.cols;
	float mxX = 0;
	for( std::list<cv::Rect>::iterator it = allFeatures.begin() ; it != allFeatures.end() ; ++it ) {
		float x = it->x + it->width /2;

		if(x > mxX) mxX = x;
		if(x < mnX) mnX = x;
	}

	if((mxX-mnX)/image.cols < 0.2) return -1;	// if the detected faces are collocated in one area, then skip it.

	// ------------------------------------------------------------------------------------------------
	// find equatorial line

	float meanY = 0; int nbLines = 0;
    float meanY2 = 0;
    for( std::list<cv::Rect>::iterator it = allFeatures.begin() ; it != allFeatures.end() ; ++it ) {
    	meanY += it->y + it->height /2;
    	meanY2 += (it->y + it->height /2)*(it->y + it->height /2);
    	++nbLines;
    }

    if(nbLines == 0) return -1;

    meanY /= nbLines;
    meanY2 /= nbLines;

    float stdev = std::sqrt(meanY2 - meanY*meanY);
    float meanValue = meanY;

    meanY = 0; nbLines = 0;

    for( std::list<cv::Rect>::iterator it = allFeatures.begin() ; it != allFeatures.end() ; ++it ) {
    	float y = it->y + it->height /2;

    	if(std::abs(y-meanValue) > 1.3 * stdev) continue;

    	meanY += y;
    	++nbLines;
    }

    if(nbLines == 0) return meanValue;

	meanY /= nbLines;

	if(meanY < (gray.rows * 1 / 3) && allFeatures.size() < 2) { // we need at least two faces to justify an equatorial line lower higher the first third of the image
		return -1;
	}

	return meanY;

}

float percentageSaliency(const cv::Mat& image, float slicePosition, float sliceHeightDegree = 10.f) {

	double sumImage = cv::sum( image )[0];
	float sumSlice = 0;

	float sliceHeight = sliceHeightDegree * (image.rows / 180);

	for(int i = static_cast<int>(std::max(slicePosition - sliceHeight, 0.f)) ; i < std::min(static_cast<int>(slicePosition + sliceHeight), image.rows) ; ++i) {
		for(int j = 0 ; j < image.cols ; ++j) {
			sumSlice += image.at<float>(i,j);
		}
	}

	return sumSlice / sumImage;

}

float salientCenter(const cv::Mat& image, int step = 5) {

	std::vector<float> histogram(180/step);
	int offset = 0; //static_cast<int>(30.f*(step/180.f));

	float sumHist = 0;
	for(int i = offset ; i < 180-offset ; i+=step) {
		float top    = image.rows*static_cast<float>(i)/180;
		float bottom = image.rows*static_cast<float>(i+step)/180;

		float sum = 0;
		for(int ii = top ; ii < bottom ; ++ii) {
			for(int jj = 0 ; jj < image.cols ; ++jj) {
				sum += image.at<float>(ii,jj);
			}
		}

		sum /= (bottom-top) * image.cols;
		histogram[i/step] = sum;
		sumHist += sum;
	}

	float loc = 0;
	for(size_t i = offset ; i < histogram.size() - offset ; ++i) {
		loc += i*step*histogram[i]/sumHist;
	}

	return image.rows * static_cast<float>(loc)/180.f;

}

void applyGaussianEquatorialPrior(cv::Mat& image, float gaussianM = 0.f, float gaussianSD = 500.f, bool central = false) {

	for(int i = 0 ; i < image.rows ; ++i) {
		float lat = (.5f - static_cast<float>(i)/image.rows) * 180.f;
		float prior = 0.f;

		if(central)
			prior =  (.01 + .40 * exp(-((lat-gaussianM)*(lat-gaussianM))/(gaussianSD))) / .41f;
		else
			prior =  (.01 + .40 * exp(-((lat-gaussianM)*(lat-gaussianM))/(gaussianSD)) + .75 * exp(-((lat-100)*(lat-100))/(200)) + .75 * exp(-((lat+90)*(lat+90))/(200))) / .41f;

		prior = std::min(1.f, prior);

		for(int j = 0 ; j < image.cols ; ++j) {
			image.at<float>(i,j) = prior * image.at<float>(i,j);
		}
	}

}

void applyEquatorialPrior(cv::Mat& image, cv::Mat& colorImage, bool central = false, bool print = false) {
	float scaling_factor = static_cast<float>(image.cols) / 1400.f;	// normalize the size of the images

	cv::resize(colorImage, colorImage, cv::Size(colorImage.size().width/scaling_factor, colorImage.size().height/scaling_factor));
	float hz = colorImage.rows / 2;
	float fc = faceLine(colorImage);

	float equatorialLine = hz;

	if(fc > 0) {
		if(fc > colorImage.rows / 3)	// prior: faces are not higher than the first third of the image
			equatorialLine = fc;
	}

	float slCenter = salientCenter(image);

	slCenter = std::max(std::min((slCenter - image.rows/2)/image.rows, 0.1f), -0.1f);
	slCenter = slCenter * image.rows + image.rows/2;
	slCenter = (slCenter/scaling_factor + colorImage.rows / 2) / 2;

	if(fc > 0) {
		if(fc > colorImage.rows / 3)	// prior: faces are not higher than the first third of the image
			slCenter = fc;
	}

	equatorialLine = (.5f - slCenter / colorImage.rows) * 180.f;

	if(print) std::cout << equatorialLine << std::endl;

	applyGaussianEquatorialPrior(image, equatorialLine, 700.f, central);

}

void bottomThresholdSaliency(cv::Mat &saliency) {
	cv::Mat uCharSalmap;
	uCharSalmap = 255 * saliency;
	uCharSalmap.convertTo(uCharSalmap, CV_8UC1);

	int histSize[] = {255};
	int channels[] = {0};
	float range[] = { 0, 256 } ;
 	const float* histRange = { range };
	cv::Mat hist;

	cv::calcHist(&uCharSalmap, 1, channels, cv::Mat(), hist, 1, histSize, &histRange, true, true);

	float sum = 0;
	float mid = static_cast<float>(saliency.cols*saliency.rows) * .7;
	int theshold = 0;
	for(int i = 0 ; i < hist.rows ; ++i) {
		sum += hist.at<float>(i,0);

		if(sum > mid) {
			theshold = i;
			break;
		}
	}

	for(int i = 0 ; i < uCharSalmap.rows ; ++i) {
		for(int j = 0 ; j < uCharSalmap.cols ; ++j) {
			if(uCharSalmap.at<unsigned char>(i,j) < theshold) {
				uCharSalmap.at<unsigned char>(i,j) = theshold;
			}
		}
	}

	uCharSalmap.convertTo(saliency, CV_32FC1);
	uCharSalmap /= 255;

}

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

	// ------------------------------------------------------------------------------------------------------------------------------------------
	// parse parameters

	namespace po = boost::program_options;

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

	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "produce help message")
			("input-file,i", po::value< std::string >(), "Saliency input image.")
			("color-file,c", po::value< std::string >(), "Color image corresponding to the saliency map.")
			("line,l", po::value< float >(), "Add the ground truth equatorial line (in degree [-90,+90]).")
			("output-file,o", po::value< std::string >(), "Saliency map with the equatorial prior.")
			("central", "Consider only the central part of the Gaussian mixture")
			("no-adapt", "No adaptation")
			("print-horizon", "Output in the terminal the position of the horizon line")
	;

	po::variables_map vm;
	try {
		po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
		po::notify(vm);
	} catch(boost::exception &) {
		std::cerr << "Error incorect program options. See --help... \n";
		return 0;
	}

	if (vm.count("help")) {
		std::cout << "--------------------------------------------------------------------------------\n";
		std::cout << "\t\tApply the equatorial prior on saliency maps\n";
		std::cout << "--------------------------------------------------------------------------------\n";
		std::cout << "\n\n";
		std::cout << desc << "\n";
		return 1;
	}

	std::string inputImagePath;
	std::string inputColorImagePath;
	std::string outputPath;

	bool central = false;
	bool noAdapt = false;

	if (vm.count("input-file")) {
		inputImagePath = vm["input-file"].as< std::string >();
	} else {
		std::cerr << "It is required to provide the an input saliency map. See --help\n";
		return 0;
	}

	if (vm.count("color-file")) {
		inputColorImagePath = vm["color-file"].as< std::string >();
	} else {
		std::cerr << "It is required to provide the an input color image. See --help\n";
		return 0;
	}

	if (vm.count("output-file")) {
		outputPath = vm["output-file"].as< std::string >();
	}

	if(vm.count("central")) {
		central = true;
	}

	if(vm.count("no-adapt")) {
		noAdapt = true;
	}

	bool printHz = false;
	if(vm.count("print-horizon")) {
		printHz = true;
	}

	// ---------------------------------------------------------------------------------------------------
	// Get statistics

	cv::Mat image = cv::imread(inputImagePath);
	if(image.empty()) {
		std::cerr << "Cannot open: " << inputImagePath << std::endl;
		return 0;
	}

	cv::Mat colorImage = cv::imread(inputColorImagePath);
	if(colorImage.empty()) {
		std::cerr << "Cannot open: " << inputColorImagePath << std::endl;
		return 0;
	}

	cv::cvtColor(image, image, cv::COLOR_BGR2GRAY);

	image.convertTo(image, CV_32FC1);
	image /= 255;

	if(vm.count("second-file") && vm.count("weight")) {
		cv::Mat imageW = cv::imread(vm["second-file"].as< std::string >());
		if(imageW.empty()) {
			std::cerr << "cannot open: " << vm["second-file"].as< std::string >() << std::endl ;
		} else {
			cv::cvtColor(imageW, imageW, cv::COLOR_BGR2GRAY);
			imageW.convertTo(imageW, CV_32FC1);
			imageW /= 255;

			float w = vm["weight"].as< float >();
			image = image * w  + (1 - w) * imageW;
		}
	}

	// image = cv::Mat(image.size(), CV_32FC1, cv::Scalar(1.0f));

	if(printHz) std::cout << inputColorImagePath << ", ";

	if(!noAdapt)
		applyEquatorialPrior(image, colorImage, central, printHz);
	else
		applyGaussianEquatorialPrior(image, 0, 700, central);

	// bottomThresholdSaliency(image);

	// if (vm.count("line")) {
	// 	float line = vm["line"].as< float >();

	// 	line =  image.rows * (90 - line) / 180.f ;

	// 	// std::cout << line << std::endl;
	// applyGaussianEquatorialPrior(image, 0, 500);

	double mn, mx;
	cv::minMaxLoc(image, &mn, &mx);

	image = (image - mn) / (mx - mn);

	// 	// line =  colorImage.rows * (90 - line) / 180.f ;

	// 	// if(line > 0 && line < colorImage.rows)
	// 	// 	cv::line(colorImage, cv::Point_<int>(0, line), cv::Point_<int>(colorImage.cols, line), cv::Scalar( 255, 0, 255), 2);
	// }

	// cv::imshow("colorImage", colorImage);
	// cv::waitKey();

	if(!outputPath.empty()) {
		cv::Mat tmp;
		cv::cvtColor(image, tmp, cv::COLOR_GRAY2BGR);
		tmp *= 255;
		tmp.convertTo(tmp, CV_8UC3);
		cv::imwrite(outputPath, tmp);

		return 0;
	} else {
		cv::imshow("with equatorial prior", image);
		cv::waitKey();
	}

	return 0;
}


#pragma once
#include <Eigen/Core>

#include "smoothness_matrix.hpp"
#include "total_derivative.hpp"
#include "../fixed_size_container_type_trait.hpp"
#include "../value_type_trait.hpp"

namespace ubs {
/** @brief Internal namespace */
namespace internal {

/**
 * @brief Implementation to evaluate a spline at the specified position.
 *
 * This is implemented using template meta programming (TMP) as one needs to have one sum per dimension. A spline
 * can be evaluated using the following formula.
 * @f[
 * \mathbf{f}(\mathbf{t}) = \sum_{i_1} N_{i_1} \sum_{i_2} N_{i_2} \cdots \sum_{i_M} N_{i_M}
 * \mathbf{r}(\mathbf{t})
 * @f]
 *
 * @note As this class uses the internal of the spline, it must be a friend of the UniformBSpline class.
 * @tparam InputDims_ The total number of input dimensions.
 * @tparam CurDim_ The current dimension.
 */
template <typename Spline_, int InputDims_, int CurDim_>
struct EvaluateBSpline {
    /**
     * @brief Evaluates the spline at the specified position.
     *
     * @param[in] spline The spline.
     * @param[in] strides The strides of the control points.
     * @param[in] idx The current flattened index in the control points.
     * @param[in] basisVal The current basis value.
     * @param[in] pos The position.
     * @param[in] basisFunc The basis function.
     * @param[in] evalFunc The evaluation function.
     */
    template <typename T, typename EvalFunc>
    static void apply(const std::array<typename Spline_::BasisType, Spline_::InputDims>& fullBasis,
                      const std::array<int, Spline_::InputDims>& strides,
                      int idx,
                      T basisVal,
                      EvalFunc evalFunc) {
        const auto basis = fullBasis[CurDim_];
        for (int i = 0; i < Spline_::Order; ++i, idx += strides[CurDim_]) {
            const T newBasisVal = basisVal * basis[i];
            EvaluateBSpline<Spline_, InputDims_, CurDim_ + 1>::apply(fullBasis, strides, idx, newBasisVal, evalFunc);
        }
    }
};

/**
 * @brief 'Recursion' end, if the current dimension matches the input dimensions.
 * @tparam InputDims_ The input dimensions.
 */
template <typename Spline_, int InputDims_>
struct EvaluateBSpline<Spline_, InputDims_, InputDims_> {
    /**
     * @brief Call the evaluate function.
     */
    template <typename T, typename EvalFunc>
    static void apply(const std::array<typename Spline_::BasisType, Spline_::InputDims>& /*fullBasis*/,
                      const std::array<int, Spline_::InputDims>& /*strides*/,
                      int idx,
                      T basisVal,
                      EvalFunc evalFunc) {
        evalFunc(idx, basisVal);
    }
};

/**
 * @brief Implementation to evaluate the spline smoothness.
 * @note As this class uses the internal of the spline, it must be a friend of the UniformBSpline class.
 * @tparam TotalDerivative_ The order of the total derivative.
 * @tparam PartialDerivativeIdx_ The current partial derivative index.
 * @tparam InputDims_ The total number of input dimensions.
 * @tparam CurDim_ The current dimension.
 */
template <int TotalDerivative_, int PartialDerivativeIdx_, int InputDims_, int CurDim_>
class EvaluateBSplineSmoothness {
private:
    static constexpr auto FullPartialDerivatives_ =
        TotalDerivative<InputDims_, TotalDerivative_>::getPartialDerivatives();

    static constexpr int PartialDerivative_ =
        FullPartialDerivatives_[PartialDerivativeIdx_].partialDerivatives[CurDim_];

public:
    /**
     * @brief Evaluates the spline smoothness.
     * @param[in] spline The spline.
     * @param[in] idx1 The first index.
     * @param[in] idx2 The seconds index.
     * @param[in] strides The strides of the control points.
     * @param[in] factor The multiplication factor of the smoothness matrix.
     * @param[out] res The resulting smoothness.
     */
    template <typename Spline>
    static void apply(const Spline& spline,
                      int idx1,
                      int idx2,
                      const std::array<int, Spline::InputDims>& strides,
                      typename Spline::ValueType factor,
                      typename Spline::OutputType& res) {

        const auto smoothnessMatrix = UniformBSplineSmoothnessBasis<Spline::Order, PartialDerivative_>::matrix()
                                          .template cast<typename Spline::ValueType>()
                                          .eval();

        for (int p = 0; p < spline.getNumControlPoints(CurDim_) - Spline::Degree;
             ++p, idx1 += strides[CurDim_], idx2 += strides[CurDim_]) {
            int newIdx1 = idx1;
            for (int i = 0; i < Spline::Order; ++i, newIdx1 += strides[CurDim_]) {
                int newIdx2 = idx2;
                for (int j = 0; j < Spline::Order; ++j, newIdx2 += strides[CurDim_]) {
                    auto newFactor = factor * smoothnessMatrix(i, j);
                    EvaluateBSplineSmoothness<TotalDerivative_, PartialDerivativeIdx_, InputDims_, CurDim_ + 1>::apply(
                        spline, newIdx1, newIdx2, strides, newFactor, res);
                }
            }
        }
    }
};

/**
 * @brief 'Recursion' end, if the current dimension matches the input dimensions.
 * @tparam TotalDerivative_ The order of the total derivative.
 * @tparam PartialDerivativeIdx_ The current partial derivative index.
 * @tparam InputDims_ The input dimensions.
 */
template <int TotalDerivative_, int PartialDerivativeIdx_, int InputDims_>
class EvaluateBSplineSmoothness<TotalDerivative_, PartialDerivativeIdx_, InputDims_, InputDims_> {
public:
    template <typename Spline>
    static void apply(const Spline& spline,
                      int idx1,
                      int idx2,
                      const std::array<int, Spline::InputDims>& /*strides*/,
                      typename Spline::ValueType factor,
                      typename Spline::OutputType& res) {
        assert(idx1 < int(spline.controlPoints_.getNumElements()));
        assert(idx2 < int(spline.controlPoints_.getNumElements()));

        res += FixedSizeContainerTypeTrait<typename Spline::OutputType>::evalSmoothness(
            factor, spline.controlPoints_.at(idx1), spline.controlPoints_.at(idx2));
    }
};

/**
 * @brief Compute the smoothness with respect to the total derivative.
 *
 * To calculate the smoothness of the total derivative, the problem can be split up by calculating the smoothness for
 * each partial derivative, multiply that value by the multiplicity of that derivative and sum them up.
 *
 * @tparam InputDims_ The number of input dimensions.
 * @tparam TotalDerivative_ The order of the total derivative.
 * @tparam NumPartialDerivatives_ The total number of distinct partial derivatives.
 * @tparam PartialDerivativeIdx_ The current number of the partial derivative to compute the smoothness.
 */
template <int InputDims_, int TotalDerivative_, int NumPartialDerivatives_, int PartialDerivativeIdx_>
struct ComputeSmoothness {
private:
    static constexpr auto FullPartialDerivatives_ =
        TotalDerivative<InputDims_, TotalDerivative_>::getPartialDerivatives();
    static constexpr Array<int, InputDims_> PartialDerivatives_ =
        FullPartialDerivatives_[PartialDerivativeIdx_].partialDerivatives;
    static constexpr double PartialDerivativesMultiplicity_ =
        FullPartialDerivatives_[PartialDerivativeIdx_].multiplicity;

public:
    /**
     * @brief Compute the smoothness value of the partial derivatives needed to calculate the total derivative
     *        smoothness value
     * @param[in] spline The spline.
     * @param[in] strides The strides of the control points.
     * @param[out] res The smoothness value for each dimension.
     */
    template <typename Spline>
    static void apply(const Spline& spline,
                      const std::array<int, InputDims_>& strides,
                      typename Spline::OutputType& res) {
        static_assert(InputDims_ == Spline::InputDims, "Input dimension missmatch.");
        using T = typename Spline::ValueType;
        using OutputType = typename Spline::OutputType;

        // Calculate the scaling factor. The scaling factor depends on the multiplicity of the partial derivative,
        // the control points scale and the partial derivative.
        T scale = T(PartialDerivativesMultiplicity_);

        for (int dim = 0; dim < InputDims_; ++dim) {
            using ValueTypeTrait = ValueTypeTrait<T>;
            scale *= ValueTypeTrait::pow(spline.getScale(dim), 2 * PartialDerivatives_[dim] - 1);
        }

        // Calculate the smoothness value.
        OutputType smoothness = FixedSizeContainerTypeTrait<OutputType>::zero();
        EvaluateBSplineSmoothness<TotalDerivative_, PartialDerivativeIdx_, InputDims_, 0>::apply(
            spline, 0, 0, strides, T(1.0), smoothness);
        res += scale * smoothness;

        // Compute smoothness value for the next partial derivative.
        ComputeSmoothness<InputDims_, TotalDerivative_, NumPartialDerivatives_, PartialDerivativeIdx_ + 1>::apply(
            spline, strides, res);
    }
};

/**
 * @brief 'Loop' end, if the current dimension matches the input dimensions.
 * @tparam InputDims_ The input dimensions.
 */
template <int InputDims_, int TotalDerivative_, int NumPartialDerivatives_>
struct ComputeSmoothness<InputDims_, TotalDerivative_, NumPartialDerivatives_, NumPartialDerivatives_> {
    template <typename Spline>
    static void apply(const Spline& /*spline*/,
                      const std::array<int, InputDims_>& /*strides*/,
                      const typename Spline::OutputType& /*res*/) {
        // No more partial derivatives left. Nothing more to do.
    }
};

} // namespace internal
} // namespace ubs

#include "uniform_bspline_eval_impl.hpp"


#include "catch.hpp"

#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>

#include <arlib/details/arlib_utils.hpp>
#include <arlib/details/esx_impl.hpp>
#include <arlib/esx.hpp>
#include <arlib/graph_utils.hpp>
#include <arlib/routing_kernels/types.hpp>
#include <arlib/terminators.hpp>

#include "cittastudi_graph.hpp"
#include "test_types.hpp"
#include "utils.hpp"

#include <kspwlo_ref/algorithms/kspwlo.hpp>
#include <kspwlo_ref/exploration/graph_utils.hpp>

#include <chrono>
#include <experimental/filesystem>
#include <memory>
#include <string>
#include <string_view>

using namespace arlib::test;

template <typename ForwardIt, typename Edge>
bool contains(ForwardIt first, ForwardIt last, Edge e) {
  if (auto search = std::find(first, last, e); search != last) {
    return true;
  } else {
    return false;
  }
}

TEST_CASE("Edge priority computation", "[esx]") {
  using namespace boost;
  using arlib::details::compute_priority;

  auto G = arlib::read_graph_from_string<Graph>(std::string(graph_gr_esx));
  Vertex s = 0, t = 6;

  // Compute shortest path from s to t
  auto weight_map = get(edge_weight, G);
  auto sp_path = *arlib::details::compute_shortest_path(G, weight_map, s, t);

  // Compute lower bounds for AStar
  auto heuristic = arlib::details::distance_heuristic<Graph, Length>(G, t);

  // We keep a set of deleted-edges
  using Edge = typename graph_traits<Graph>::edge_descriptor;
  auto deleted_edges = std::unordered_set<Edge, boost::hash<Edge>>{};

  // Check that (0, 3) was computed in shortest path
  REQUIRE(contains(sp_path.begin(), sp_path.end(), edge(0, 3, G).first));
  auto s_n3 = edge(0, 3, G).first;

  int prio_s_n3 = compute_priority(G, s_n3, weight_map, deleted_edges);
  REQUIRE(prio_s_n3 == 0);

  // Check that (3, 5) was computed in shortest path
  REQUIRE(contains(sp_path.begin(), sp_path.end(), edge(3, 5, G).first));
  auto n3_n5 = edge(3, 5, G).first;

  int prio_n3_n5 = compute_priority(G, n3_n5, weight_map, deleted_edges);
  REQUIRE(prio_n3_n5 == 3);

  // Check that (5, 6) was computed in shortest path
  REQUIRE(contains(sp_path.begin(), sp_path.end(), edge(5, 6, G).first));
  auto n5_t = edge(5, 6, G).first;

  int prio_n5_t = compute_priority(G, n5_t, weight_map, deleted_edges);
  REQUIRE(prio_n5_t == 0);
}

TEST_CASE("esx kspwlo algorithm runs on Boost::Graph", "[esx]") {
  auto G = arlib::read_graph_from_string<Graph>(std::string{graph_gr_esx});
  Vertex s = 0, t = 6;
  auto predecessors = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors, s, t, 3, 0.5);
  auto res = arlib::to_paths(G, predecessors, s, t);

  // Create a new tmp file out of graph_gr_esx
  namespace fs = std::experimental::filesystem;
  auto path = fs::temp_directory_path() / std::string("graph_gr_esx_file.gr");
  auto of = std::ofstream(path.string());
  of << graph_gr_esx;
  of.close();

  auto G_regr = std::make_unique<RoadNetwork>(path.c_str());
  auto res_regression = esx(G_regr.get(), 0, 6, 3, 0.5);

  using boost::edges;
  using boost::source;
  using boost::target;
  std::cout << "Esx boost::graph result:\n";
  for (auto &p : res) {
    for (auto it = edges(p).first; it != edges(p).second; ++it) {
      std::cout << "(" << source(*it, p) << ", " << target(*it, p) << ") ";
    }
    std::cout << "\n";
  }

  std::cout << "Esx regression result:\n";
  for (auto &regPath : res_regression) {
    auto es = regPath.getEdges();
    // Cleaning loops coming from dijkstra algorithm (for no reason)
    remove_self_loops(es.begin(), es.end());

    for (auto edge : es) {
      std::cout << "(" << edge.first << ", " << edge.second << ") ";
    }
    std::cout << "\n";
  }

  // Same number of paths are computed
  REQUIRE(res.size() == res_regression.size());

  // For each k-spwlo check if its edges are in a solution of the regression
  // test
  for (auto &p : res) {
    REQUIRE(one_regression_path_have_edges(res_regression, p));
  }

  using boost::edge_weight;
  using boost::get;
  REQUIRE(alternative_paths_are_dissimilar(res, get(edge_weight, G), 0.5));
}

TEST_CASE("ESX running with bidirectional dijkstra returns same result as "
          "unidirectional dijkstra",
          "[esx]") {
  using namespace boost;

  auto G = arlib::read_graph_from_string<Graph>(std::string(graph_gr_esx));

  Vertex s = 0, t = 6;
  int k = 3;
  double theta = 0.5;

  auto predecessors_uni = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_uni, s, t, 3, 0.5);
  auto res_paths_uni = arlib::to_paths(G, predecessors_uni, s, t);

  auto predecessors_bi = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_bi, s, t, 3, 0.5,
             arlib::routing_kernels::bidirectional_dijkstra);
  auto res_paths_bi = arlib::to_paths(G, predecessors_bi, s, t);

  REQUIRE(res_paths_uni.size() == res_paths_bi.size());

  for (std::size_t i = 0; i < res_paths_uni.size(); ++i) {
    REQUIRE(res_paths_uni[i].length() == res_paths_bi[i].length());
  }
}

TEST_CASE("ESX running with plain dijkstra returns same result as astar",
          "[esx]") {
  using namespace boost;

  auto G = arlib::read_graph_from_string<Graph>(std::string(graph_gr_esx));

  Vertex s = 0, t = 6;
  int k = 3;
  double theta = 0.5;

  auto predecessors_uni = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_uni, s, t, 3, 0.5);
  auto res_paths_uni = arlib::to_paths(G, predecessors_uni, s, t);

  auto predecessors_bi = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_bi, s, t, 3, 0.5,
             arlib::routing_kernels::dijkstra);
  auto res_paths_bi = arlib::to_paths(G, predecessors_bi, s, t);

  REQUIRE(res_paths_uni.size() == res_paths_bi.size());

  for (std::size_t i = 0; i < res_paths_uni.size(); ++i) {
    REQUIRE(res_paths_uni[i].length() == res_paths_bi[i].length());
  }
}

TEST_CASE("ESX times-out on large graph", "[esx]") {
  using namespace boost;
  using namespace std::chrono_literals;

  auto G = arlib::read_graph_from_string<Graph>(std::string(cittastudi_gr));

  Vertex s = 0, t = 20;
  auto k = 3;
  auto theta = 0.5;
  auto predecessors = arlib::multi_predecessor_map<Vertex>{};

  REQUIRE_THROWS_AS(arlib::esx(G, predecessors, s, t, k, theta,
                               arlib::routing_kernels::astar,
                               arlib::timer{1us}),
                    arlib::terminator_stop_error);
}

TEST_CASE(
    "ESX with Betweeness-Centrality is functionally equivalent to original one",
    "[esx]") {
  using namespace boost;

  auto G = arlib::read_csr_graph_from_string(std::string(graph_gr_esx));

  Vertex s = 0, t = 6;
  int k = 3;
  double theta = 0.5;

  using EdgeCentralityProperty =
      exterior_edge_property<arlib::CSRGraph, double>;
  using EdgeCentralityContainer =
      typename EdgeCentralityProperty::container_type;
  using EdgeCentralityMap = typename EdgeCentralityProperty::map_type;

  // Compute Edge betweeness-centrality
  auto edge2centrality = EdgeCentralityContainer(num_edges(G));
  auto edge_centrality_map = EdgeCentralityMap(edge2centrality, G);
  boost::brandes_betweenness_centrality(
      G, boost::edge_centrality_map(edge_centrality_map));

  // ARP with Betweeness-Centrality
  auto predecessors_bc = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_bc, edge_centrality_map, s, t, k, theta);
  auto res_paths_bc = arlib::to_paths(G, predecessors_bc, s, t);

  // ARP for vanilla ESX
  auto predecessors_bi = arlib::multi_predecessor_map<Vertex>{};
  arlib::esx(G, predecessors_bi, s, t, k, theta,
             arlib::routing_kernels::dijkstra);
  auto res_paths_bi = arlib::to_paths(G, predecessors_bi, s, t);

  // Require solutions to match
  REQUIRE(res_paths_bc.size() == res_paths_bi.size());

  for (std::size_t i = 0; i < res_paths_bc.size(); ++i) {
    REQUIRE(res_paths_bc[i].length() == res_paths_bi[i].length());
  }
}


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" step
 * planner. The pdf can be found in https://arxiv.org/abs/1704.01271, and in the
 * `doc/` folder in this repository.
 */

#pragma once

#include <eigen-quadprog/QuadProg.h>
#include <Eigen/Eigen>
#include <cmath>
#include <iostream>
#include <pinocchio/spatial/se3.hpp>
#include <sstream>
#include <stdexcept>
#define RESET "\033[0m"
#define RED "\033[31m"  /* Red */
#define BLUE "\033[34m" /* Blue */

namespace Eigen
{
/** @brief Column vector of size 9 which correspond to the number of
 * optimization variables. */
typedef Matrix<double, 9, 1> Vector9d;
}  // namespace Eigen

namespace reactive_planners
{
/**
 * @brief Implements the "Walking Control Based on Step Timing Adaptation" step
 * planner. The pdf can be found in https://arxiv.org/abs/1704.01271, and in the
 * `doc/` folder in this repository.
 *
 * All quantities are here expressed in the local frame. Which means for a robot
 * that the quantities are expressed in the "base" frame.
 *
 * @todo write here the formulation of the QP.
 */
class DcmVrpPlanner
{
public:
    /**
     * @brief Construct a new DcmVrpPlanner object and initialize it.
     *
     * @param l_min [in] Minimum step length in the x direction (in the
     * direction of forward motion).
     * @param l_max [in] Maximum step length in the x direction (in the
     * direction of forward motion).
     * @param w_min [in] Minimum step length in the y direction (in the lateral
     * direction).
     * @param w_max [in] Maximum step length in the y direction (in the lateral
     * direction).
     * @param t_min [in] Minimum step time.
     * @param t_max [in] Maximum step time.
     * @param v_des [in] Desired average velocity in the x and y ([v_x, v_y]) 2d
     * vector.
     * @param l_p [in] Default lateral step length. Typically useful for
     * humanoid robot where this value refer to the distance between the 2 feet
     * while in the half-sitting/neutral position.
     * @param ht [in] Average desired height of the com above the ground.
     * @param cost_weights_local [in] Weights of the QP cost expressed in the
     * local frame.
     */
    DcmVrpPlanner(const double& l_min,
                  const double& l_max,
                  const double& w_min,
                  const double& w_max,
                  const double& t_min,
                  const double& t_max,
                  const double& l_p,
                  const double& ht,
                  const Eigen::Ref<const Eigen::Vector9d>& cost_weights_local);

    /**
     * @brief Construct a new DcmVrpPlanner object with some default parameters.
     */
    DcmVrpPlanner()
    {
        initialize(
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, Eigen::Vector9d::Zero());
    }

    /**
     * @brief Initialize The inner variable that are not time varying.
     *
     * @copydoc DcmVrpPlanner::DcmVrpPlanner()
     */
    void initialize(
        const double& l_min,
        const double& l_max,
        const double& w_min,
        const double& w_max,
        const double& t_min,
        const double& t_max,
        const double& l_p,
        const double& ht,
        const Eigen::Ref<const Eigen::Vector9d>& cost_weights_local);

    /**
     * @brief Set the nominal steptime.
     */
    void set_steptime_nominal(double t_nom);

    /**
     * @brief Computes adapted step location.
     *
     * We use a QP formulation with the following notation:
     * \f{eqnarray*}{
     *    minimize   & \\
     *       x       & (1/2) x^T Q x + q^T x \\
     *    subject to & \\
     *               & A_{ineq} x \leq b_{ineq} \\
     *               & A_{eq} x = b_{eq} \\
     *               & x_{opt_{lb}} \leq x \leq x_{opt_{ub}}
     * \f}
     * We use the off-the-shelf QP solver eigen-quadprog in order to solve it.
     * @see DcmVrpPlanner for the full formulation.
     *
     * @param current_step_location is the location of the previous foot step
     * location (2d vector) [ux, uy].
     * @param time_from_last_step_touchdown is the time elapsed since the last
     * foot step landed.
     * @param is_left_leg_in_contact is true is the current foot is on the left
     * side, false if it is on the right side.
     * @param v_des
     * @param com is the CoM position.
     * @param com_vel is the CoM velocity.
     * @param world_M_base SE3 position of the robot base expressed in the world
     * frame.
     */
    void update(const Eigen::Ref<const Eigen::Vector3d>& current_step_location,
                const double& time_from_last_step_touchdown,
                const bool& is_left_leg_in_contact,
                const Eigen::Ref<const Eigen::Vector3d>& v_des,
                const Eigen::Ref<const Eigen::Vector3d>& com,
                const Eigen::Ref<const Eigen::Vector3d>& com_vel,
                const pinocchio::SE3& world_M_base,
                const double& new_t_min);
    /**
     * @brief Computes adapted step location for python3.
     *
     * We use a QP formulation with the following notation:
     * \f{eqnarray*}{
     *    minimize   & \\
     *       x       & (1/2) x^T Q x + q^T x \\
     *    subject to & \\
     *               & A_{ineq} x \leq b_{ineq} \\
     *               & A_{eq} x = b_{eq} \\
     *               & x_{opt_{lb}} \leq x \leq x_{opt_{ub}}
     * \f}
     * We use the off-the-shelf QP solver eigen-quadprog in order to solve it.
     * @see DcmVrpPlanner for the full formulation.
     *
     * @param current_step_location is the location of the previous foot step
     * location (2d vector) [ux, uy].
     * @param time_from_last_step_touchdown is the time elapsed since the last
     * foot step landed.
     * @param is_left_leg_in_contact is true is the current foot is on the left
     * side, false if it is on the right side.
     * @param v_des
     * @param com is the CoM position.
     * @param com_vel is the CoM velocity.
     * @param yaw
     */
    void update(const Eigen::Ref<const Eigen::Vector3d>& current_step_location,
                const double& time_from_last_step_touchdown,
                const bool& is_left_leg_in_contact,
                const Eigen::Ref<const Eigen::Vector3d>& v_des,
                const Eigen::Ref<const Eigen::Vector3d>& com,
                const Eigen::Ref<const Eigen::Vector3d>& com_vel,
                const double& yaw,
                const double& new_t_min);

    /**
     * @brief Solve the Quadratic program and extract the solution. Use
     * DcmVrpPlanner::get_next_step_location() and
     * DcmVrpPlanner::get_duration_before_step_landing() to access the results.
     */
    bool solve();

    /**
     * @brief Perform an internal checks on the solver matrices. A warning
     * message is displayed in case of problems.
     *
     * @return true is everything seems fine.
     * @return false is something wrong.
     */
    bool internal_checks();

    /**
     * @brief Display the matrices of the Problem.
     */
    void print_solver() const;

    /**
     * @brief Convert the inner data to a string format.
     */
    std::string to_string() const;

    /*
     * Getters
     */

    /**
     * @brief @copydoc DcmVrpPlanner::t_nom_
     *
     * @return const double&
     */
    const double& get_t_nom() const
    {
        return t_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::tau_nom_
     *
     * @return const double&
     */
    const double& get_tau_nom() const
    {
        return tau_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::l_nom_
     *
     * @return const double&
     */
    const double& get_l_nom() const
    {
        return l_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::w_nom_
     *
     * @return const double&
     */
    const double& get_w_nom() const
    {
        return w_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::bx_nom_
     *
     * @return const double&
     */
    const double& get_bx_nom() const
    {
        return bx_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::by_nom_
     *
     * @return const double&
     */
    const double& get_by_nom() const
    {
        return by_nom_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::world_M_local_
     *
     * @return const pinocchio::SE3&
     */
    const pinocchio::SE3& get_world_M_local() const
    {
        return world_M_local_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::dcm_local_
     *
     * @return Eigen::Ref<const Eigen::Vector3d>
     */

    Eigen::Vector3d dcm_local_REF;
    Eigen::Ref<const Eigen::Vector3d> get_dcm_local()
    {
        dcm_local_REF = world_M_local_.act(dcm_local_);
        dcm_local_REF = dcm_local_;
        return dcm_local_REF;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::current_step_location_local_
     *
     * @return Eigen::Ref<const Eigen::Vector3d>
     */
    Eigen::Ref<const Eigen::Vector3d> get_current_step_location_local() const
    {
        return current_step_location_local_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::v_des_local_
     *
     * @return Eigen::Ref<const Eigen::Vector3d>
     */
    Eigen::Ref<const Eigen::Vector3d> get_v_des_local() const
    {
        return v_des_local_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::dcm_nominal_
     *
     * @return Eigen::Ref<const Eigen::Vector3d>
     */
    Eigen::Ref<const Eigen::Vector3d> get_dcm_nominal() const
    {
        return dcm_nominal_;
    }

    /*
     * Output
     */

    /**
     * @brief @copydoc DcmVrpPlanner::next_step_location_
     *
     * @return Eigen::Ref<const Eigen::Vector3d>
     */
    Eigen::Ref<const Eigen::Vector3d> get_next_step_location() const
    {
        return next_step_location_;
    }

    /**
     * @brief Return the slack values from the last solution.
     **/
    const Eigen::Vector4d& get_slack_variables() const
    {
        return slack_variables_;
    }

    /**
     * @brief @copydoc DcmVrpPlanner::duration_before_step_landing_
     *
     * @return const double&
     */
    const double& get_duration_before_step_landing() const
    {
        return duration_before_step_landing_;
    }

    /**
     * @brief Get the desired com height.
     *
     * @return const double&
     */
    const double& get_com_height() const
    {
        return ht_;
    }

    /**
     * @brief return cost.
     *
     * @return double
     */
    double cost()
    {
        return (0.5 * x_opt_.transpose() * Q_ * x_opt_ +
                q_.transpose() * x_opt_)(0, 0);
    }

    /**
     * @brief add time equation for fixing tau.
     */
    void add_t_eq(double time)
    {
        A_eq_(2, 2) = 1;
        B_eq_(2) = exp(omega_ * time);
    }

    /*
     * Private methods
     */
private:
    /**
     * @brief Compute the nominal step location from the user input.
     *
     * @param is_left_leg_in_contact [in] is used notably to define which
     * surface to use for the next contact.
     * - 1 if left leg is in contact
     * - 2 if right leg is in contact
     * @param v_des_local in the local frame.
     */
    void compute_nominal_step_values(
        const bool& is_left_leg_in_contact,
        const Eigen::Ref<const Eigen::Vector3d>& v_des_local);

    /*
     * Attributes
     */
private:
    /** @brief Minimum step length in the x direction (in the direction of
     * forward motion). */
    double l_min_;

    /** @brief Maximum step length in the x direction (in the direction of
     * forward motion). */
    double l_max_;

    /** @brief Nominal step length in the x direction (in the direction of
     * forward motion). */
    double l_nom_;

    /** @brief Minimum step length in the y direction (in the lateral
     * direction).
     */
    double w_min_;

    /** @brief Maximum step length in the y direction (in the lateral
     * direction).
     */
    double w_max_;

    /** @brief Nominal step length in the y direction (in the lateral
     * direction).
     */
    double w_nom_;

    /** @brief Minimum step time. */
    double t_min_;

    /** @brief Maximum step time. */
    double t_max_;

    /** @brief Nominal step time. */
    double t_nom_ = 0.1;

    /** @brief Minimum step time in logarithmic scale:
     * \f$ e^{\omega t_{nom}} \f$*/
    double tau_min_;

    /** @brief Maximum step time in logarithmic scale:
     * \f$ e^{\omega t_{nom}} \f$*/
    double tau_max_;

    /** @brief Nominal step time in logarithmic scale:
     * \f$ e^{\omega t_{nom}} \f$*/
    double tau_nom_;

    /** @brief Default step width. */
    double l_p_;

    /** @brief Average desired height of the com above the ground. */
    double ht_;

    /** @brief Natural frequency of the pendulum: \f$ \omega =
     * \sqrt{\frac{g}{z_0}} \f$. */
    double omega_;

    /** @brief Maximum DCM offset along the X-axis. */
    double bx_max_;

    /** @brief Minimum DCM offset along the X-axis. */
    double bx_min_;

    /** @brief Nominal DCM offset along the Y-axis. */
    double bx_nom_;

    /** @brief Maximum DCM offset along the Y-axis. */
    double by_max_out_;

    /** @brief Minimum DCM offset along the Y-axis. */
    double by_max_in_;

    /** @brief Nominal DCM offset along the Y-axis. */
    double by_nom_;

    /** @brief SE3 position of the robot base in the world frame. */
    pinocchio::SE3 world_M_local_;

    /** @brief Current DCM computed from the CoM estimation. */
    Eigen::Vector3d dcm_local_;

    /** @brief Nominal DCM computed from the CoM estimation and nominal time. */
    Eigen::Vector3d dcm_nominal_;

    /** @brief Current DCM computed from the CoM estimation. */
    Eigen::Vector3d current_step_location_local_;

    /** @brief Current DCM computed from the CoM estimation. */
    Eigen::Vector3d v_des_local_;

    /** @brief Store the time from last step touchdown in order to stop
     * optimizing after t_min_ is passed. */
    double time_from_last_step_touchdown_;

    /*
     * Problem results
     */

    /** @brief Next step location expressed in the world frame. */
    Eigen::Vector3d next_step_location_;

    /** @brief Slack variable values corresponding to last solution. */
    Eigen::Vector4d slack_variables_;

    /** @brief Remaining time before the step landing. */
    double duration_before_step_landing_;

    /**
     * QP variables
     */

    /** @brief Number of variabes in the optimization problem. */
    int nb_var_;

    /** @brief Number of equality constraints in the optimization problem. */
    int nb_eq_;

    /** @brief Number of inequality in the optimization problem. */
    int nb_ineq_;

    /** @brief Quadratic program solver.
     *
     * This is an eigen wrapper around the quad_prog fortran solver.
     */
    Eigen::QuadProgDense qp_solver_;

    /** @brief Solution of the optimization problem.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd x_opt_;

    /** @brief Lower Bound on the solution of the optimization problem.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd x_opt_lb_;

    /** @brief Upper Bound on the solution of the optimization problem.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd x_opt_ub_;

    /** @brief Quadratic term of the quadratic cost.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::MatrixXd Q_;

    /** @brief Cost weights expressed in the local frame. */
    Eigen::Vector9d cost_weights_local_;

    /** @brief Linear term of the quadratic cost.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd q_;

    /** @brief Linear equality matrix.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::MatrixXd A_eq_;

    /** @brief Linear equality vector.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd B_eq_;

    /** @brief Linear inequality matrix.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::MatrixXd A_ineq_;

    /** @brief Linear inequality vector.
     * @see DcmVrpPlanner::compute_adapted_step_locations. */
    Eigen::VectorXd B_ineq_;
};

}  // namespace reactive_planners


#include <boost/python/def.hpp>
#include <boost/python/args.hpp>

#include <scitbx/math/interpolation.h>
#include <scitbx/vec3.h>
#include <scitbx/vec2.h>

namespace scitbx { namespace math {

namespace {

  template <typename PointType>
  void wrap_splines()
  {
    using namespace boost::python;
    def("interpolate_catmull_rom_spline",
      (af::shared<PointType>(*)(
        PointType const&,
        PointType const&,
        PointType const&,
        PointType const&,
        unsigned)) interpolate_catmull_rom_spline, (
          arg("p0"),
          arg("p1"),
          arg("p2"),
          arg("p3"),
          arg("n_points")));
  }

} // namespace <anonymous>

namespace boost_python {

  void wrap_interpolation()
  {
    wrap_splines< scitbx::vec2<double> >();
    wrap_splines< scitbx::vec3<double> >();
  }

}}} // namespace scitbx::math::boost_python


#pragma once

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/mesh_algorithms.hpp>
#include <crest/geometry/biscale_mesh.hpp>
#include <crest/basis/quasi_interpolation.hpp>
#include <crest/basis/lagrange_basis2d.hpp>
#include <crest/basis/detail/homogenized_basis_detail.hpp>
#include <crest/util/eigen_extensions.hpp>
#include <crest/util/stat.hpp>
#include <crest/util/timer.hpp>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>

#include <set>
#include <cassert>
#include <unordered_map>

#include <iostream>

namespace crest
{
    template <typename Scalar>
    class HomogenizedBasis : public Basis<Scalar, HomogenizedBasis<Scalar>>
    {
    public:
        explicit HomogenizedBasis(const BiscaleMesh<Scalar, int> & mesh,
                                  Eigen::SparseMatrix<double> weights);

        virtual std::vector<int> boundary_nodes() const override { return _mesh.coarse_mesh().boundary_vertices(); }

        virtual std::vector<int> interior_nodes() const override { return _mesh.coarse_mesh().compute_interior_vertices(); }

        virtual Assembly<Scalar> assemble() const override;

        virtual int num_dof() const override { return _mesh.coarse_mesh().num_vertices(); }

        template <typename Function2d>
        VectorX<Scalar> interpolate(const Function2d & f) const;

        template <typename Function2d>
        VectorX<Scalar> interpolate_boundary(const Function2d & f) const;

        template <int QuadStrength, typename Function2d>
        VectorX<Scalar> load(const Function2d & f) const;

        template <int QuadStrength, typename Function2d>
        Scalar error_l2(const Function2d & f, const VectorX<Scalar> & weights) const;

        template <int QuadStrength, typename Function2d_x, typename Function2d_y>
        Scalar error_h1_semi(const Function2d_x & f_x,
                             const Function2d_y & f_y,
                             const VectorX<Scalar> & weights) const;

        const Eigen::SparseMatrix<Scalar> & basis_weights() const { return _basis_weights; }

    private:
        Eigen::SparseMatrix<Scalar> _basis_weights;
        const BiscaleMesh<Scalar, int> & _mesh;
    };

    /**
     * Base class for solvers that compute localized correctors for a given BiscaleMesh.
     */
    template <typename Scalar>
    class CorrectorSolver
    {
    public:
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix;

        virtual ~CorrectorSolver() {}

        crest::HomogenizedBasis<Scalar> compute_basis(const BiscaleMesh<Scalar, int> & mesh,
                                                      unsigned int oversampling);

        Eigen::SparseMatrix<Scalar> compute_correctors(const BiscaleMesh<Scalar, int> & mesh,
                                                       unsigned int oversampling);

        virtual std::vector<Eigen::Triplet<Scalar>> resolve_element_correctors_for_patch(
                const BiscaleMesh<Scalar, int> & mesh,
                const std::vector<int> & coarse_patch_interior,
                const std::vector<int> & fine_patch_interior,
                const Eigen::SparseMatrix<Scalar> & global_coarse_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_fine_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_quasi_interpolator,
                int coarse_element) const = 0;

        const std::unordered_map<std::string, AccumulatedDensityHistogram> & stats() const;

        void set_iterative_tolerance(Scalar tol) { _tol = tol; }
        Scalar iterative_tolerance() const { return _tol; }

    protected:
        explicit CorrectorSolver<Scalar>() : _tol(Scalar(10) * std::numeric_limits<Scalar>::epsilon()) {}

        VectorX<Scalar> local_rhs(const BiscaleMesh<Scalar, int> & mesh,
                                  const std::vector<int> & fine_patch_interior,
                                  int coarse_element,
                                  int local_index) const;

    private:
        void set_stats(std::unordered_map<std::string, AccumulatedDensityHistogram> stats);
        std::unordered_map<std::string, AccumulatedDensityHistogram> _stats;
        Scalar _tol;
    };

    /**
     * The default corrector solver, which uses SparseLU to compute correctors. It is slow, but very robust.
     */
    template <typename Scalar>
    class SparseLuCorrectorSolver : public CorrectorSolver<Scalar>
    {
    public:
        virtual std::vector<Eigen::Triplet<Scalar>> resolve_element_correctors_for_patch(
                const BiscaleMesh<Scalar, int> & mesh,
                const std::vector<int> & coarse_patch_interior,
                const std::vector<int> & fine_patch_interior,
                const Eigen::SparseMatrix<Scalar> & global_coarse_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_fine_stiffness,
                const Eigen::SparseMatrix<Scalar> & global_quasi_interpolator,
                int coarse_element) const override
        {
            (void) global_coarse_stiffness;

            const auto I_H = sparse_submatrix(global_quasi_interpolator,
                                              coarse_patch_interior,
                                              fine_patch_interior);
            const auto A = sparse_submatrix(global_fine_stiffness,
                                            fine_patch_interior,
                                            fine_patch_interior);

            std::vector<Eigen::Triplet<Scalar>> triplets;
            const auto C = detail::construct_saddle_point_problem(A, I_H);

            Eigen::SparseLU<Eigen::SparseMatrix<Scalar>> solver;
            solver.analyzePattern(C);
            solver.factorize(C);
            assert(solver.info() == Eigen::Success);

            for (int i = 0; i < 3; ++i)
            {
                const auto b_local = this->local_rhs(mesh, fine_patch_interior, coarse_element, i);

                VectorX<Scalar> c(C.rows());
                c << b_local, VectorX<Scalar>::Zero(I_H.rows());

                // Recall that the solution is of the form [x, kappa], where kappa is merely a Lagrange multiplier, so
                // we extract x as the corrector.
                const VectorX<Scalar> corrector = solver.solve(c).topRows(A.rows());

                assert(static_cast<size_t>(corrector.rows()) == fine_patch_interior.size());
                const auto global_index = mesh.coarse_mesh().elements()[coarse_element].vertex_indices[i];
                for (size_t k = 0; k < fine_patch_interior.size(); ++k)
                {
                    const auto component = corrector(k);
                    // Due to rounding issues, some components that should perhaps be zero in exact arithmetic
                    // may be non-zero, and as such we might end up with a denser matrix than we should actually
                    // have. To prevent this, we introduce a threshold which determines whether to keep the entry.
                    // TODO: Make this threshold configurable?
                    if (std::abs(component) > 1e-12)
                    {
                        triplets.emplace_back(Eigen::Triplet<Scalar>(global_index, fine_patch_interior[k], component));
                    }
                }
            }

            return triplets;
        }
    };

    template <typename Scalar>
    VectorX<Scalar> CorrectorSolver<Scalar>::local_rhs(const BiscaleMesh<Scalar, int> & mesh,
                                                       const std::vector<int> & fine_patch_interior,
                                                       int coarse_element,
                                                       int local_index) const
    {
        assert(local_index >= 0 && local_index < 3);
        // TODO: Simplify this function
        assert(std::is_sorted(fine_patch_interior.cbegin(), fine_patch_interior.cend()));

        const auto coarse_triangle = mesh.coarse_mesh().triangle_for(coarse_element);
        const Eigen::Matrix<Scalar, 3, 3> coarse_coeff = detail::basis_coefficients_for_triangle(coarse_triangle);

        const auto coarse_grad_x = coarse_coeff(0, local_index);
        const auto coarse_grad_y = coarse_coeff(1, local_index);

        VectorX<Scalar> rhs(fine_patch_interior.size());
        rhs.setZero();
        for (auto k : mesh.descendants_for(coarse_element))
        {
            const auto vertex_indices = mesh.fine_mesh().elements()[k].vertex_indices;
            const auto fine_triangle = mesh.fine_mesh().triangle_for(k);
            const Eigen::Matrix<Scalar, 3, 3> fine_coeff = detail::basis_coefficients_for_triangle(fine_triangle);

            for (size_t j = 0; j < 3; ++j)
            {
                const auto fine_grad_x = fine_coeff(0, j);
                const auto fine_grad_y = fine_coeff(1, j);

                // TODO: Fix this
                const auto product = [&] (auto  , auto  )
                {
                    return coarse_grad_x * fine_grad_x + coarse_grad_y * fine_grad_y;
                };

                // At this point, we only know the index of the vertex in the global mesh,
                // but we need the index of the vertex with respect to the patch interior. For now,
                // we just perform a binary search to recover it, though there may be much more efficient ways.
                // For example, we can construct a hashmap in the beginning of this function (which may
                // or may not be more efficient).
                const auto vertex_index = vertex_indices[j];
                const auto range = std::equal_range(fine_patch_interior.cbegin(),
                                                    fine_patch_interior.cend(),
                                                    vertex_index);

                if (range.first != range.second)
                {
                    const auto inner_product = triquad<2>(product,
                                                          fine_triangle.a,
                                                          fine_triangle.b,
                                                          fine_triangle.c);

                    const auto local_index = range.first - fine_patch_interior.cbegin();
                    rhs(local_index) += inner_product;
                }

            }
        }
        return rhs;
    }

    template <typename Scalar>
    Eigen::SparseMatrix<Scalar> CorrectorSolver<Scalar>::compute_correctors(
            const BiscaleMesh<Scalar, int> & mesh,
            unsigned int oversampling)
    {
        AccumulatedDensityHistogramBuilder timing_density;

        const auto I_H = quasi_interpolator(mesh);

        Eigen::SparseMatrix<Scalar> A_fine(mesh.fine_mesh().num_vertices(), mesh.fine_mesh().num_vertices());
        Eigen::SparseMatrix<Scalar> A_coarse(mesh.coarse_mesh().num_elements(), mesh.coarse_mesh().num_elements());
        {
            // Currently we needlessly construct the mass matrix here too. For now we put this in a block scope
            // so that it will be deallocated shortly thereafter, but in the long-term this should
            // be remedied so that we don't redundantly compute it. TODO
            const auto fine_assembly = LagrangeBasis2d<Scalar>(mesh.fine_mesh()).assemble();
            A_fine = std::move(fine_assembly.stiffness);

            const auto coarse_assembly = LagrangeBasis2d<Scalar>(mesh.coarse_mesh()).assemble();
            A_coarse = std::move(coarse_assembly.stiffness);
        }

        std::vector<Eigen::Triplet<Scalar>> basis_triplets;
        for (int coarse_element = 0; coarse_element < mesh.coarse_mesh().num_elements(); ++coarse_element)
        {
            Timer element_patch_timer;

            const auto coarse_patch = mesh.coarse_element_patch(coarse_element, oversampling);
            const auto coarse_patch_interior = coarse_patch.interior();
            const auto fine_patch = mesh.fine_patch_from_coarse(coarse_patch);
            const auto fine_patch_interior = fine_patch.interior();

            if (!fine_patch_interior.empty())
            {
                const auto corrector_contributions = resolve_element_correctors_for_patch(
                        mesh,
                        coarse_patch_interior,
                        fine_patch_interior,
                        A_coarse,
                        A_fine,
                        I_H,
                        coarse_element);

                std::copy(corrector_contributions.cbegin(),
                          corrector_contributions.cend(),
                          std::back_inserter(basis_triplets));
            }

            timing_density.add_sample(static_cast<double>(fine_patch_interior.size()), element_patch_timer.elapsed());
        }

        std::unordered_map<std::string, AccumulatedDensityHistogram> stats;
        stats["timing_distribution"] = timing_density.with_bin_count(100).build();
        set_stats(stats);

        Eigen::SparseMatrix<Scalar> basis(mesh.coarse_mesh().num_vertices(), mesh.fine_mesh().num_vertices());
        basis.setFromTriplets(basis_triplets.cbegin(), basis_triplets.cend());
        return basis;
    }

    template <typename Scalar>
    HomogenizedBasis<Scalar> CorrectorSolver<Scalar>::compute_basis(const BiscaleMesh<Scalar, int> & mesh,
                                                                    unsigned int oversampling)
    {
        const auto corrector_weights = compute_correctors(mesh, oversampling);
        const auto lagrange_basis_weights = detail::standard_coarse_basis_in_fine_space(mesh);
        const auto basis_weights = lagrange_basis_weights - corrector_weights;
        return HomogenizedBasis<Scalar>(mesh, basis_weights);
    }

    template <typename Scalar>
    const std::unordered_map<std::string, AccumulatedDensityHistogram> &
    CorrectorSolver<Scalar>::stats() const
    {
        return _stats;
    };

    template <typename Scalar>
    void CorrectorSolver<Scalar>::set_stats(std::unordered_map<std::string, AccumulatedDensityHistogram> stats)
    {
        _stats = stats;
    }

    template <typename Scalar>
    HomogenizedBasis<Scalar>::HomogenizedBasis(const BiscaleMesh<Scalar, int> & mesh,
                                               Eigen::SparseMatrix<double> weights)
            : _mesh(mesh)
    {
        if (weights.rows() == mesh.coarse_mesh().num_vertices() && weights.cols() == mesh.fine_mesh().num_vertices())
        {
            _basis_weights = std::move(weights);
        } else
        {
            throw std::invalid_argument("Dimensions of basis weights are not compatible with "
                                                "supplied coarse and fine meshes.");
        }
    }

    template <typename Scalar>
    Assembly<Scalar> HomogenizedBasis<Scalar>::assemble() const
    {
        LagrangeBasis2d<Scalar> fine_basis(_mesh.fine_mesh());
        const auto fine_assembly = fine_basis.assemble();

        const auto & M = fine_assembly.mass;
        const auto & A = fine_assembly.stiffness;
        const auto & W = _basis_weights;

        Assembly<Scalar> assembly;
        assembly.mass = W * M * W.transpose();
        assembly.stiffness = W * A * W.transpose();
        return assembly;
    }

    template <typename Scalar>
    template <typename Function2d>
    VectorX<Scalar> HomogenizedBasis<Scalar>::interpolate(const Function2d & f) const
    {
        // A slightly more accurate way of interpolating would perhaps be to interpolate the function
        // in the fine space, and then quasi-interpolate it to weights in the coarse space.
        // However, it seems this has some unfortunate effects on the implementation of inhomogeneous
        // Dirichlet boundary conditions?
        return LagrangeBasis2d<Scalar>(_mesh.coarse_mesh()).interpolate(f);
    }

    template <typename Scalar>
    template <typename Function2d>
    VectorX<Scalar> HomogenizedBasis<Scalar>::interpolate_boundary(const Function2d & f) const
    {
        return LagrangeBasis2d<Scalar>(_mesh.fine_mesh()).interpolate_boundary(f);
    }

    template <typename Scalar>
    template <int QuadStrength, typename Function2d>
    VectorX<Scalar> HomogenizedBasis<Scalar>::load(const Function2d & f) const
    {
        const LagrangeBasis2d<Scalar> fine_basis(_mesh.fine_mesh());
        const auto fine_load = fine_basis.template load<QuadStrength>(f);
        const auto & W = _basis_weights;
        return W * fine_load;
    };

    template <typename Scalar>
    template <int QuadStrength, typename Function2d>
    Scalar HomogenizedBasis<Scalar>::error_l2(const Function2d & f, const VectorX<Scalar> & weights) const
    {
        const LagrangeBasis2d<Scalar> fine_basis(_mesh.fine_mesh());
        const auto & W = _basis_weights;
        const VectorX<Scalar> fine_weights = W.transpose() * weights;
        return fine_basis.template error_l2<QuadStrength>(f, fine_weights);
    };

    template <typename Scalar>
    template <int QuadStrength, typename Function2d_x, typename Function2d_y>
    Scalar HomogenizedBasis<Scalar>::error_h1_semi(const Function2d_x & f_x,
                                                   const Function2d_y & f_y,
                                                   const VectorX<Scalar> & weights) const
    {
        const LagrangeBasis2d<Scalar> fine_basis(_mesh.fine_mesh());
        const auto & W = _basis_weights;
        const VectorX<Scalar> fine_weights = W.transpose() * weights;
        return fine_basis.template error_h1_semi<QuadStrength>(f_x, f_y, fine_weights);
    };
}


#include <iostream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace Eigen;
using namespace std;

#ifndef REPEAT
#define REPEAT 1000000
#endif

enum func_opt
{
    TV,
    TMATV,
    TMATVMAT,
};

template <class res, class arg1, class arg2, int opt>
struct func;

template <class res, class arg1, class arg2>
struct func<res, arg1, arg2, TV>
{
    static EIGEN_DONT_INLINE res run( arg1& a1, arg2& a2 )
    {
	asm ("");
	return a1 * a2;
    }
};

template <class res, class arg1, class arg2>
struct func<res, arg1, arg2, TMATV>
{
    static EIGEN_DONT_INLINE res run( arg1& a1, arg2& a2 )
    {
	asm ("");
	return a1.matrix() * a2;
    }
};

template <class res, class arg1, class arg2>
struct func<res, arg1, arg2, TMATVMAT>
{
    static EIGEN_DONT_INLINE res run( arg1& a1, arg2& a2 )
    {
	asm ("");
	return res(a1.matrix() * a2.matrix());
    }
};

template <class func, class arg1, class arg2>
struct test_transform
{
    static void run()
    {
	arg1 a1;
	a1.setIdentity();
	arg2 a2;
	a2.setIdentity();

	BenchTimer timer;
	timer.reset();
	for (int k=0; k<10; ++k)
	{
	    timer.start();
	    for (int k=0; k<REPEAT; ++k)
		a2 = func::run( a1, a2 );
	    timer.stop();
	}
	cout << setprecision(4) << fixed << timer.value() << "s  " << endl;;
    }
};

#define run_vec( op, scalar, mode, option, vsize ) \
    std::cout << #scalar << "\t " << #mode << "\t " << #option << " " << #vsize " "; \
    {\
	typedef Transform<scalar, 3, mode, option> Trans;\
	typedef Matrix<scalar, vsize, 1, option> Vec;\
	typedef func<Vec,Trans,Vec,op> Func;\
	test_transform< Func, Trans, Vec >::run();\
    }

#define run_trans( op, scalar, mode, option ) \
    std::cout << #scalar << "\t " << #mode << "\t " << #option << "   "; \
    {\
	typedef Transform<scalar, 3, mode, option> Trans;\
	typedef func<Trans,Trans,Trans,op> Func;\
	test_transform< Func, Trans, Trans >::run();\
    }

int main(int argc, char* argv[])
{
    cout << "vec = trans * vec" << endl;
    run_vec(TV, float,  Isometry, AutoAlign, 3);
    run_vec(TV, float,  Isometry, DontAlign, 3);
    run_vec(TV, float,  Isometry, AutoAlign, 4);
    run_vec(TV, float,  Isometry, DontAlign, 4);
    run_vec(TV, float,  Projective, AutoAlign, 4);
    run_vec(TV, float,  Projective, DontAlign, 4);
    run_vec(TV, double, Isometry, AutoAlign, 3);
    run_vec(TV, double, Isometry, DontAlign, 3);
    run_vec(TV, double, Isometry, AutoAlign, 4);
    run_vec(TV, double, Isometry, DontAlign, 4);
    run_vec(TV, double, Projective, AutoAlign, 4);
    run_vec(TV, double, Projective, DontAlign, 4);

    cout << "vec = trans.matrix() * vec" << endl;
    run_vec(TMATV, float,  Isometry, AutoAlign, 4);
    run_vec(TMATV, float,  Isometry, DontAlign, 4);
    run_vec(TMATV, double, Isometry, AutoAlign, 4);
    run_vec(TMATV, double, Isometry, DontAlign, 4);

    cout << "trans = trans1 * trans" << endl;
    run_trans(TV, float,  Isometry, AutoAlign);
    run_trans(TV, float,  Isometry, DontAlign);
    run_trans(TV, double, Isometry, AutoAlign);
    run_trans(TV, double, Isometry, DontAlign);
    run_trans(TV, float,  Projective, AutoAlign);
    run_trans(TV, float,  Projective, DontAlign);
    run_trans(TV, double, Projective, AutoAlign);
    run_trans(TV, double, Projective, DontAlign);

    cout << "trans = trans1.matrix() * trans.matrix()" << endl;
    run_trans(TMATVMAT, float,  Isometry, AutoAlign);
    run_trans(TMATVMAT, float,  Isometry, DontAlign);
    run_trans(TMATVMAT, double, Isometry, AutoAlign);
    run_trans(TMATVMAT, double, Isometry, DontAlign);
}


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_MAPPING_SSF_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_MAPPING_SSF_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
#include <boost/geometry/strategies/spherical/ssf.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

// An enumeration type defining types of mapping of geographical
// latitude to spherical latitude.
// See: http://en.wikipedia.org/wiki/Great_ellipse
//      http://en.wikipedia.org/wiki/Latitude#Auxiliary_latitudes
enum mapping_type { mapping_geodetic, mapping_reduced, mapping_geocentric };

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename Spheroid, mapping_type Mapping>
struct mapper
{
    explicit inline mapper(Spheroid const& /*spheroid*/) {}

    template <typename CalculationType>
    static inline CalculationType const& apply(CalculationType const& lat)
    {
        return lat;
    }
};

template <typename Spheroid>
struct mapper<Spheroid, mapping_reduced>
{
    typedef typename promote_floating_point
        <
            typename radius_type<Spheroid>::type
        >::type fraction_type;

    explicit inline mapper(Spheroid const& spheroid)
    {
        fraction_type const a = geometry::get_radius<0>(spheroid);
        fraction_type const b = geometry::get_radius<2>(spheroid);
        b_div_a = b / a;
    }

    template <typename CalculationType>
    inline CalculationType apply(CalculationType const& lat) const
    {
        return atan(static_cast<CalculationType>(b_div_a) * tan(lat));
    }

    fraction_type b_div_a;
};

template <typename Spheroid>
struct mapper<Spheroid, mapping_geocentric>
{
    typedef typename promote_floating_point
        <
            typename radius_type<Spheroid>::type
        >::type fraction_type;

    explicit inline mapper(Spheroid const& spheroid)
    {
        fraction_type const a = geometry::get_radius<0>(spheroid);
        fraction_type const b = geometry::get_radius<2>(spheroid);
        sqr_b_div_a = b / a;
        sqr_b_div_a *= sqr_b_div_a;
    }

    template <typename CalculationType>
    inline CalculationType apply(CalculationType const& lat) const
    {
        return atan(static_cast<CalculationType>(sqr_b_div_a) * tan(lat));
    }

    fraction_type sqr_b_div_a;
};

}
#endif // DOXYGEN_NO_DETAIL

/*!
\brief Check at which side of a geographical segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0).
         The check is performed by mapping the geographical coordinates
         to spherical coordinates and using spherical_side_formula.
\ingroup strategies
\tparam Spheroid The reference spheroid model
\tparam Mapping The type of mapping of geographical to spherical latitude
\tparam CalculationType \tparam_calculation
 */
template <typename Spheroid,
          mapping_type Mapping = mapping_geodetic,
          typename CalculationType = void>
class mapping_spherical_side_formula
{

public :
    inline mapping_spherical_side_formula()
        : m_mapper(Spheroid())
    {}

    explicit inline mapping_spherical_side_formula(Spheroid const& spheroid)
        : m_mapper(spheroid)
    {}

    template <typename P1, typename P2, typename P>
    inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calculation_type;

        calculation_type lon1 = get_as_radian<0>(p1);
        calculation_type lat1 = m_mapper.template apply<calculation_type>(get_as_radian<1>(p1));
        calculation_type lon2 = get_as_radian<0>(p2);
        calculation_type lat2 = m_mapper.template apply<calculation_type>(get_as_radian<1>(p2));
        calculation_type lon = get_as_radian<0>(p);
        calculation_type lat = m_mapper.template apply<calculation_type>(get_as_radian<1>(p));

        return detail::spherical_side_formula(lon1, lat1, lon2, lat2, lon, lat);
    }

private:
    side::detail::mapper<Spheroid, Mapping> const m_mapper;
};

// The specialization for geodetic latitude which can be used directly
template <typename Spheroid,
          typename CalculationType>
class mapping_spherical_side_formula<Spheroid, mapping_geodetic, CalculationType>
{

public :
    inline mapping_spherical_side_formula() {}
    explicit inline mapping_spherical_side_formula(Spheroid const& /*spheroid*/) {}

    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        return spherical_side_formula<CalculationType>::apply(p1, p2, p);
    }
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_MAPPING_SSF_HPP