#include "profile.hpp"
#include "profile_python.hpp"
#include <boost/python.hpp>
#include <pyublas/numpy.hpp>
#include "optimization.hpp"
#include "rootfinder.hpp"

namespace gd {
using namespace boost::python;

// in km/s^2 Msun/kpc
const double GravConst = 4.30200406461e-06;

boost::python::tuple ProfileModel::Lmax_and_rcirc_at_E_(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return boost::python::make_tuple(Lmax, rcirc);
}
void ProfileModel::Lmax_and_rcirc_at_E(double E, double& Lmax, double& rcirc) {
	/*	def f(r, E=E):
			r = abs(r)
			return -r**2*(2*(E-self.potentialr(r)))
		rLmax, Lmaxsq = scipy.optimize.fmin(f, rtry, full_output=True, disp=False)[:2]
		return sqrt(abs(Lmaxsq)), abs(rLmax[0])
	*/
	auto f = [&](double logr) -> double {
		//r = fabs(r);
		double r = pow(10, logr);
		//printf("r = %f logr\n ", r);
		return -r*r*(2*(E-this->potentialr(r)));
	};
	MinimizerNLopt2<> opt(f);
	//double lower_bounds[1] = { 0};
	//nlopt_set_lower_bounds(opt.opt, lower_bounds);
	//opt.x = 1.;
	rcirc = pow(10, opt.optimize());
	//printf("r = %f Ekin = %f\n", r, 2*(E-this->potentialr(r)));
	Lmax = sqrt(2*(E-this->potentialr(rcirc)))*rcirc;
}
double ProfileModel::Lmax_at_E(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return Lmax;
}
double ProfileModel::rcirc_at_E(double E) {
	double Lmax, rcirc;
	Lmax_and_rcirc_at_E(E, Lmax, rcirc);
	return rcirc;
}
double ProfileModel::rmax_at_E(double E, double rcirc) {
	auto f = [&](double r) -> double {
		return E - this->potentialr(r);
	};
	double scale = 1.5;
	double r_far_way = rcirc * scale;
	assert(f(rcirc) >= 0);
	while(f(r_far_way) > 0) {
		r_far_way *= scale;
	}
	RootFinderGSL<> rootfinder(f);
	double rmax = rootfinder.findRoot(rcirc, r_far_way);
	return rmax;

	/*auto f = [&](double r) -> double {
		return E - this->potentialr(r);
	};
	auto root = [&](double x, double * y, double *dy) -> void {
		double r = pow(10, x) + rcirc;
		// x = log10(r-rcirc)
		// dx = 1/(r-rcirc) / log(10) dr
		*y = f(r);
		double gradient = -this->dphidr(r) * (r-rcirc) * log(10);
		*dy = gradient;
		//*
		double r1 = r;
		double dx = 1e-4;
		double r2 = pow(10, x+dx)+rcirc;
		double y1 = f(r1);
		double y2 = f(r2);
		printf("rmax grad: logr=%f r=%10.5f: erad=%10.5f [%10.5f %10.5f] (%f) \n", x, r, *y, (y2-y1)/dx, gradient, this->potentialr(r));
		//return g
		/**/
	/*};

	// find apo or peri center
	RootFinderGSLDerivative<> rootfinderFirst(root);
	double rstart = pow(10, rootfinderFirst.findRoot(1, 0, 1e-5))+rcirc; //pow(10, rootfinderFirst.findRoot(0));
	double scale = 1.+1e-4;
	assert(f(rstart*scale) < 0);
	assert(f(rstart/scale) > 0);
	return rstart;*/
}

boost::python::tuple ProfileModel::get_apo_peri(double E, double L, double rmin, double rcirc, double rmax) {

	auto ekinrad = [&](double r) -> double { // kinetic energy in radial direction
		return (-L*L/(2*r*r) - (this->potentialr(r) - E));
	};
	auto root = [&](double x, double * y, double *dy) -> void {
		if(x < 0) {
			//*y =FP_INFINITE;// inf;
			//*dy = FP_INFINITE;//inf;
			//return;
			//printf("grad: x=%f\n", x);
			//x = -x;
		}
		double r = pow(10, x);
		*y = ekinrad(r);
		double gradient = (-this->dphidr(r) + L*L/pow(r, 3)) * r * log(10);
		*dy = gradient;
		//*
		double r1 = r;
		double dx = 1e-4;
		double r2 = pow(10, x+dx);
		double y1 = ekinrad(r1);
		double y2 = ekinrad(r2);
		//printf("grad: logr=%f r=%10.5f: erad=%10.5f [%10.5f %10.5f] (%f) \n", x, r, *y, (y2-y1)/dx, gradient, this->potentialr(r));
		//return g
		/**/
	};

	// find apo or peri center
	//RootFinderGSLDerivative<> rootfinderFirst(root);
	//double rstart = pow(10, rootfinderFirst.findRoot(1)); //pow(10, rootfinderFirst.findRoot(0));

	double scale = (1+1e-4);
	double apo, peri;
	RootFinderGSL<> rootfinder(ekinrad);
	//printf("found a valid r %f (E=%f L=%f Eleft=%f (%f %f))\n", rstart, E, L, ekinrad(rstart), ekinrad(rstart*scale), ekinrad(rstart/scale));
	//printf("rmin : %f ekinrad: %f %f %f\n", rmin, ekinrad(rmin), ekinrad(rmin*scale), ekinrad(rmin/scale));
	//printf("rcirc: %f ekinrad: %f %f %f\n", rcirc, ekinrad(rcirc), ekinrad(rcirc*scale), ekinrad(rcirc/scale));
	//printf("rmax : %f ekinrad: %f %f %f\n", rmax, ekinrad(rmax), ekinrad(rmax*scale), ekinrad(rmax/scale));
	int n;
	n = 0;
	assert(ekinrad(rcirc) >= 0);
	while((n < 10) & (ekinrad(rmin) > 0)) {
		rmin /= scale;
		n++;
	}
	if(ekinrad(rmin) > 0) {
		throw std::range_error("rmin cannot be properly found");
	}

	n = 0;
	while((n < 10) & (ekinrad(rmax) > 0)) {
		rmax *= scale;
		n++;
	}
	if(ekinrad(rmax) > 0) {
		throw std::range_error("rmax cannot be properly found");
	}

	peri = rootfinder.findRoot(rmin, rcirc);
	apo = rootfinder.findRoot(rcirc, rmax);

		//printf("finding apocenter [%f]\n", rstart);
		//printf("found apocenter [%f]\n", apo);
		//printf("finding pericenter [%f] (%f %f %f)\n", rstart, ekinrad(1e-6), ekinrad(rstart/scale/scale), ekinrad(rstart*scale*scale));
		//printf("found pericenter [%f]\n", peri);
	scale = (1+1e-5);
	while(ekinrad(peri) < 0) {
		peri *= scale;
	}
	while(ekinrad(apo) < 0) {
		apo /= scale;
	}
	return boost::python::make_tuple(apo/scale, peri*scale);

	/*
	# just find apo or pericenter
	rstart = self.findR_at_EL(E, L, rtry)
	try:
		rstart = rstart[0]
	except:
		pass
	s = (1+1e-5) # scale factor for testing apo/peri
	r = rstart
	#print rstart, ekinrad(rstart/s), ekinrad(rstart*s), -L**2/(2*r**2) - (self.potentialr(r) - E), -L**2/(2*r**2) , (self.potentialr(r) - E)
	if (ekinrad(rstart/s) < 0) and (ekinrad(rstart*s) > 0): # we found pericenter
		rp = rstart
		ra = brentq(ekinrad, rstart*s, 1e9)
	else: # we found apocenter
		ra = rstart
		rp = brentq(ekinrad, 1e-9, rstart/s)

	# sanity checks
	assert ekinrad(ra*s) < 0, "available energy ar r > r_apo should be negative"
	assert ekinrad(rp/s) < 0, "available energy ar r < r_peri should be negative"
	assert ekinrad(ra/s) > 0, "available energy ar r < r_apo should be positive"
	assert ekinrad(rp*s) > 0, "available energy ar r > r_peri should be positive"
	assert ra > rp, "apocenter should be larger than pericenter"
	return ra/s, rp*s*/
}

void py_export_profile() {
	class_< ProfileModel, boost::noncopyable >("ProfileModel", no_init)
		.def("get_apo_peri", (&ProfileModel::get_apo_peri))
		.def("Lmax_at_E", (&ProfileModel::Lmax_at_E))
		.def("Lmax_and_rcirc_at_E", (&ProfileModel::Lmax_and_rcirc_at_E_))
		.def("rcirc_at_E", (&ProfileModel::rcirc_at_E))
		.def("rmax_at_E", (&ProfileModel::rmax_at_E))

		;
	class_<ProfileModel1C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel1C", init<Profile*>())
		.def("densityr", (&ProfileModel1C::densityr))
		.def("dphidr", (&ProfileModel1C::dphidr))
		.def("potentialr", (&ProfileModel1C::potentialr))
		;
	class_<ProfileModel2C, bases<ProfileModel>, boost::noncopyable  >("ProfileModel2C", init<Profile*,Profile*>())
		.def("densityr", (&ProfileModel2C::densityr))
		.def("dphidr", (&ProfileModel2C::dphidr))
		.def("potentialr", (&ProfileModel2C::potentialr))
		;

	class_< Profile, boost::noncopyable >("Profile", no_init)
		.def("densityr", pure_virtual(&Profile::densityr))
		;
	class_< Density, boost::noncopyable >("Density", no_init)
		.def("densityr", pure_virtual(&Density::densityr))
		;
	//py_export_profile_profile<Plummer, init<double, double, double>>("Plummer");
	//init<double, double, double>((boost::python::arg("b"), boost::python::arg("M"), boost::python::arg("G")))
	py_export_profile_profile_kw<Plummer>("Plummer", init<double, double, double>((boost::python::arg("M"), boost::python::arg("b"), boost::python::arg("G")=GravConst)));
	/*class_<Plummer, bases<Profile, Density> >("Plummer", init<double, double, double>())
		.def("potentialr", (&Plummer::potentialr))
		.def("densityr", (&Plummer::densityr))
		.def("densityR", (&Plummer::densityR))
		.def("dphidr", (&Plummer::dphidr))
		//.def("dphidr2", (&Plummer::dphidr2))
		;
	*/
	//py_export_profile_profile_kw<ProjectedExponential>("ProjectedExponential", init<double, double, double>((boost::python::arg("M"), boost::python::arg("scale"), boost::python::arg("G")=GravConst)));
	py_export_profile_profile_kw<ProjectedExponential>("ProjectedExponential", init<double, double, double>());

	class_<TestCase, bases<Profile, Density> >("TestCase", init<double, double, double>())
		.def("potentialr", (&TestCase::potentialr))
		.def("densityr", (&TestCase::densityr))
		.def("densityR", (&TestCase::densityR))
		.def("dphidr", (&TestCase::dphidr))
		//.def("dphidr2", (&TestCase::dphidr2))
		;
	class_<LogarithmicProfile, bases<Profile, Density> >("LogarithmicProfile", init<double, double>())
		.def("densityr", (&LogarithmicProfile::densityr))
		.def("densityR", (&LogarithmicProfile::densityR))
		.def("dphidr", (&LogarithmicProfile::dphidr))
		//.def("dphidr2", (&LogarithmicProfile::dphidr2))
	;
	class_<Isochrone, bases<Profile, Density> >("Isochrone", init<double, double, double>())
		.def("densityr", (&Isochrone::densityr))
		.def("densityR", (&Isochrone::densityR))
		.def("dphidr", (&Isochrone::dphidr))
		//.def("dphidr2", (&Isochrone::dphidr2))
		.def("potentialr", (&Isochrone::potentialr))
		;
	class_<NullProfile, bases<Profile, Density> >("NullProfile", init<>())
		.def("densityr", (&NullProfile::densityr))
		.def("densityR", (&NullProfile::densityR))
		.def("dphidr", (&NullProfile::dphidr))
		//.def("dphidr2", (&NullProfile::dphidr2))
	;
	class_<Hernquist, bases<Profile, Density> >("Hernquist", init<double, double, double>())
		.def("densityr", (&Hernquist::densityr))
		.def("densityR", (&Hernquist::densityR))
		.def("dphidr", (&Hernquist::dphidr))
		.def("potentialr", (&Hernquist::potentialr))
		;
	class_<Jaffe, bases<Profile, Density> >("Jaffe", init<double, double, double>())
		.def("densityr", (&Jaffe::densityr))
		.def("densityR", (&Jaffe::densityR))
		.def("dphidr", (&Jaffe::dphidr))
		.def("potentialr", (&Jaffe::potentialr))
		;
	class_<Einasto, bases<Profile, Density> >("Einasto", init<double, double, double, double>())
		.def("densityr", (&Einasto::densityr))
		.def("densityR", (&Einasto::densityR))
		.def("dphidr", (&Einasto::dphidr))
		.def("potentialr", (&Einasto::potentialr))
		;
	class_<Burkert, bases<Profile, Density> >("Burkert", init<double, double, double>())
		.def("densityr", (&Burkert::densityr))
		.def("densityR", (&Burkert::densityR))
		.def("dphidr", (&Burkert::dphidr))
		.def("potentialr", (&Burkert::potentialr))
		;
	class_<NFW, bases<Profile, Density> >("NFW", init<double, double, double, double>())
		.def("densityr", (&NFW::densityr))
		.def("potentialr", (&NFW::potentialr))
		.def("dphidr", (&NFW::dphidr))
		.def_readonly("c", &NFW::c)
		.def_readonly("mass200", &NFW::mass200)
		.def_readonly("r200", &NFW::r200)
		.def_readonly("rs", &NFW::rs)
		.def_readonly("rho0", &NFW::rho0);
	;
	class_<NFWCut, bases<Density> >("NFWCut", init<double, double, double>())
		.def("densityr", (&NFWCut::densityr))
		.def_readwrite("rs", &NFWCut::rs)
		.def_readwrite("rte", &NFWCut::rte)
		.def_readwrite("rho0", &NFWCut::rho0)
		;
	class_<TwoSlopeDensity, bases<Density> >("TwoSlopeDensity", init<double, double, double, double, double>())
		.def("densityr", (&TwoSlopeDensity::densityr))
		.def_readwrite("alpha", &TwoSlopeDensity::alpha)
		.def_readwrite("beta", &TwoSlopeDensity::beta)
		.def_readwrite("rho0", &TwoSlopeDensity::rho0)
		.def_readwrite("rs", &TwoSlopeDensity::rs)
		.def_readwrite("gamma", &TwoSlopeDensity::gamma)
		;
	class_<TwoSlopeDensityCut, bases<Density> >("TwoSlopeDensityCut", init<double, double, double, double, double, double>())
		.def("densityr", (&TwoSlopeDensityCut::densityr))
		.def_readwrite("alpha", &TwoSlopeDensityCut::alpha)
		.def_readwrite("beta", &TwoSlopeDensityCut::beta)
		.def_readwrite("rho0", &TwoSlopeDensityCut::rho0)
		.def_readwrite("rs", &TwoSlopeDensityCut::rs)
		.def_readwrite("gamma", &TwoSlopeDensityCut::gamma)
		.def_readwrite("rte", &TwoSlopeDensityCut::rte)
		;
	class_<BrokenPowerLawDensitySoft3, bases<Density> >("BrokenPowerLawDensitySoft3", init<double, double, double, double, double, double, double, double>())
		.def("densityr", (&BrokenPowerLawDensitySoft3::densityr))
		.def_readwrite("rho0", &BrokenPowerLawDensitySoft3::rho0)
		.def_readwrite("s1", &BrokenPowerLawDensitySoft3::s1)
		.def_readwrite("s2", &BrokenPowerLawDensitySoft3::s2)
		.def_readwrite("s3", &BrokenPowerLawDensitySoft3::s3)
		.def_readwrite("gamma1", &BrokenPowerLawDensitySoft3::gamma1)
		.def_readwrite("gamma2", &BrokenPowerLawDensitySoft3::gamma2)
		.def_readwrite("rs1", &BrokenPowerLawDensitySoft3::rs1)
		.def_readwrite("rs2", &BrokenPowerLawDensitySoft3::rs2)
		;

}

}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 5054)
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg.h>
#include <chrono>
#include <iostream>
#include <valarray>
#include <numeric>
#include <Eigen/Dense>

using Timer = std::chrono::duration<double, std::micro>;

void print(int n, const Timer& t_eigen, const Timer& t_sci, const Timer& t_val)
{
    std::cout << "Dot product:\n"
              << "------------\n"
              << "size =            " << n << '\n'
              << "scilib/eigen =    " << t_sci.count() / t_eigen.count() << "\n"
              << "scilib/valarray = " << t_sci.count() / t_val.count()
              << "\n\n";
}

void benchmark(int n)
{
    Eigen::VectorXd aa(n);
    Eigen::VectorXd ab(n);

    aa.fill(1.0);
    ab.fill(2.0);

    auto t1 = std::chrono::high_resolution_clock::now();
    double dot_eigen;
    for (int it = 0; it < 10000; ++it) {
        dot_eigen = aa.dot(ab);
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    Timer t_eigen = t2 - t1;
    (void) dot_eigen; // ignore unused result

    Sci::Vector<double> na(n);
    Sci::Vector<double> nb(n);
    na = 1.0;
    nb = 2.0;
    t1 = std::chrono::high_resolution_clock::now();
    double sci;
    for (int it = 0; it < 10000; ++it) {
        sci = Sci::Linalg::dot(na.view(), nb.view());
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_sci = t2 - t1;
    (void) sci;

    std::valarray<double> va(1.0, n);
    std::valarray<double> vb(2.0, n);
    t1 = std::chrono::high_resolution_clock::now();
    double val;
    for (int it = 0; it < 10000; ++it) {
        val = std::inner_product(std::begin(va), std::end(va), std::begin(vb),
                                 0.0);
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_val = t2 - t1;
    (void) val;

    print(n, t_eigen, t_sci, t_val);
}

int main()
{
    int n = 10;
    benchmark(n);

    n = 100;
    benchmark(n);

    n = 1000;
    benchmark(n);

    n = 10000;
    benchmark(n);

    n = 100000;
    benchmark(n);
}

#ifdef _MSC_VER
#pragma warning(pop)
#endif


/**
 * DD cut generation for CPLEX
 */

#include <cassert>
#include <queue>
#include <stack>
#include <boost/unordered_set.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/any.hpp>
#include "cut_cplex.hpp"
#include "flow_decomp.hpp"
#include "../bdd/bdd.hpp"
#include "../util/graph.hpp"
#include "../util/stats.hpp"

using boost::any_cast;
using boost::unordered_set;

/**
 * Generates a cut for the set of points represented by the given BDD towards the direction of the given point.
 * All input must be in layer space. Store additional information in cut_info unless it is NULL (default).
 */
Inequality* generate_bdd_inequality(BDD* bdd, const vector<double>& x, const vector<double>& interior_point, Options* options,
                                    CutInfo* cut_info /* = NULL */)
{
	Inequality* facet;
	int nvars = bdd->layers.size() - 1;

	Stats stats;
	stats.register_name("cut-time");
	stats.start_timer(0);
	try {

		IloEnv env;
		IloModel model(env);

		IloNumVarArray u(env, nvars);
		IloArray<IloNumVarArray> v(env, nvars+1);

		// Create variables
		for (int i = 0; i < nvars; ++i) {
			u[i] = IloNumVar(env, -IloInfinity, IloInfinity, IloNumVar::Float, ("u" + to_string(i)).c_str());
		}

		for (int i = 0; i < nvars + 1; ++i) {
			int size = bdd->layers[i].size();
			v[i] = IloNumVarArray(env, size);
			for (int j = 0; j < size; ++j) {
				v[i][j] = IloNumVar(env, -IloInfinity, IloInfinity, IloNumVar::Float,
				                    ("v" + to_string(i) + "," + to_string(j)).c_str());
			}
		}

		// Define objective
		IloObjective obj = IloAdd(model, IloMaximize(env));
		for (int i = 0; i < nvars; ++i) {
			obj.setLinearCoef(u[i], x[i] - interior_point[i]);
		}

		// Initialize constraint arrays
		IloArray<IloRangeArray> zero_arc_constrs(env, nvars + 1);
		IloArray<IloRangeArray> one_arc_constrs(env, nvars + 1);
		for (int i = 0; i < nvars + 1; ++i) {
			int size = bdd->layers[i].size();
			zero_arc_constrs[i] = IloRangeArray(env, size);
			one_arc_constrs[i] = IloRangeArray(env, size);
		}

		// Add constraints
		for (int i = 0; i < nvars; ++i) {
			int size = bdd->layers[i].size();
			for (int j = 0; j < size; ++j) {
				Node* zero_node = bdd->layers[i][j]->zero_arc;
				Node* one_node = bdd->layers[i][j]->one_arc;

				if (zero_node != NULL) {
					// cout << i << "," << j << " / " << zero_node->layer << "," << zero_node->id << endl;
					// model.add( v[zero_node->layer][zero_node->id] <= v[i][j] );
					zero_arc_constrs[i][j] = IloRange(env, v[zero_node->layer][zero_node->id] - v[i][j], 0,
					                                  ("a0_" + to_string(i) + "," + to_string(j)).c_str());
					model.add(zero_arc_constrs[i][j]);
				}
				if (one_node != NULL) {
					// cout << i << "," << j << " / " << one_node->layer << "," << one_node->id << endl;
					// model.add( v[one_node->layer][one_node->id] <= v[i][j] - u[i] );
					one_arc_constrs[i][j] = IloRange(env, v[one_node->layer][one_node->id] - v[i][j] + u[i], 0,
					                                 ("a1_" + to_string(i) + "," + to_string(j)).c_str());
					model.add(one_arc_constrs[i][j]);
				}
			}
		}

		// v_s = 1 + u^T interior_point
		IloExpr vs(env);
		vs += 1;
		for (int i = 0; i < nvars; ++i) {
			vs += u[i] * interior_point[i];
		}
		assert(bdd->layers[0].size() == 1);
		model.add(v[0][0] == vs);

		// v_t = 0
		assert(bdd->layers[nvars].size() == 1);
		model.add(v[nvars][0] == 0);

		// Create CPLEX object
		IloCplex cplex(model);
		cplex.setParam(IloCplex::Threads, 1);
		cplex.setParam(IloCplex::AggInd, 100); // Important parameter for efficiency; there is often a lot to aggregate
		// cplex.setOut(env.getNullStream()); // Suppress output

		stats.end_timer(0);
		cout << "Time to construct LP model: " << stats.get_time(0) << endl;
		cout << cplex.getNrows() << " rows and " << cplex.getNcols() << " columns" << endl;
		// cplex.exportModel("cutlp.lp");

		// Solve cut LP
		stats.start_timer(0);
		cplex.solve();
		stats.end_timer(0);

		cout << "Time to solve LP: " << stats.get_time(0) << endl;

		double bound = cplex.getObjValue();
		cout << "Polar opt obj: " << bound << endl;

		// // Debugging info, if unbounded
		// cout << "Status: " << cplex.getStatus() << endl;
		// IloNumArray ray_vals(env);
		// IloNumVarArray ray_vars(env);
		// cplex.getRay(ray_vals, ray_vars);
		// cout << "Ray: ";
		// for (int i = 0; i < ray_vars.getSize(); ++i) {
		//   cout << ray_vars[i] << " " << ray_vals[i] << endl;
		// }
		// cout << endl;

		// Restrict to optimal face and perturb to obtain extreme point of the polar
		if (options->cut_perturbation_iterative) {
			perturb_bdd_cut_iterative(env, cplex, model, u, obj, nvars, x, interior_point);
		} else if (options->cut_perturbation_random) {
			perturb_bdd_cut_random(env, cplex, model, u, obj, nvars, x, interior_point);
		}

		vector<double> coeffs(nvars);
		double rhs;

		// Retrieve left-hand side coefficients: u
		for (int i = 0; i < nvars; ++i) {
			if (cplex.isExtracted(u[i])) {
				coeffs[i] = cplex.getValue(u[i]);
			} else {
				// If u[i] not extracted, this means it was not added to model: objective is zero and not added to constraints
				// Could be a bug, but it also may be the valid scenario of the variable being fixed to zero in the DD
				coeffs[i] = 0;
			}
		}

		// Retrieve right-hand side: 1 + u^T interior
		rhs = 1;
		for (int i = 0; i < nvars; ++i) {
			rhs += coeffs[i] * interior_point[i];
		}

		// Inequality u^T x <= 1 + u^T interior
		facet = new Inequality();
		facet->coeffs = coeffs;
		facet->rhs = rhs;

		// cout << "Relaxation facet generated (normalized), BDD order: ";
		// for( int i = 0; i < nvars; ++i ) {
		//   cout << facet->coeffs[i] / facet->rhs << " ";
		// }
		// cout << "<= 1" << endl;
		// cout << "Relaxation facet generated, BDD order: ";
		// for( int i = 0; i < nvars; ++i ) {
		//   cout << facet->coeffs[i] << " ";
		// }
		// cout << "<= " << facet->rhs << endl;

		double lhs = 0;
		for (int i = 0; i < nvars; ++i) {
			lhs += facet->coeffs[i] * x[i];
			// cout << "coeff = " << facet->coeffs[i] << ", x = " << x[i] << endl;
		}
		cout << "LHS = " << lhs << " / Violation: " << lhs - facet->rhs << endl;

		cout << "Distance = " << get_distance_hyperplane_point(facet, x) << endl;

		// // Debugging info: Assert complementary slackness
		// int n_tight = 0;
		// int n_nzdual = 0;
		// for (int i = 0; i < nvars; ++i) {
		//  	int size = bdd->layers[i].size();
		//  	// cout << "u[" << i << "] = " << cplex.getValue(u[i]) << endl;
		//  	for (int j = 0; j < size; ++j) {
		//  		Node* zero_node = bdd->layers[i][j]->zero_arc;
		//  		Node* one_node = bdd->layers[i][j]->one_arc;
		//  		if (zero_node != NULL) {
		//  			cout << "Dual (" << i << "," << j << ") -0-> (" << zero_node->layer << "," << zero_node->id << "): " << cplex.getDual(zero_arc_constrs[i][j]);
		//  			cout << " / slack: " << cplex.getValue(v[zero_node->layer][zero_node->id]) - cplex.getValue(v[i][j]) << endl;
		//  			assert(DBL_EQ(cplex.getDual(zero_arc_constrs[i][j]), 0) || DBL_EQ(cplex.getValue(v[zero_node->layer][zero_node->id]), cplex.getValue(v[i][j])));
		//  			if (DBL_EQ(cplex.getValue(v[zero_node->layer][zero_node->id]), cplex.getValue(v[i][j])))
		//  				n_tight++;
		//  			if (!DBL_EQ(cplex.getDual(zero_arc_constrs[i][j]), 0))
		//  				n_nzdual++;
		//  		}
		//  		if (one_node != NULL) {
		//  			cout << "Dual (" << i << "," << j << ") -1-> (" << one_node->layer << "," << one_node->id << "): " << cplex.getDual(one_arc_constrs[i][j]);
		//  			cout << " / slack: " << cplex.getValue(v[one_node->layer][one_node->id]) - (cplex.getValue(v[i][j]) - cplex.getValue(u[i])) << endl;
		//  			assert(DBL_EQ(cplex.getDual(one_arc_constrs[i][j]), 0) || DBL_EQ(cplex.getValue(v[one_node->layer][one_node->id]), cplex.getValue(v[i][j]) - cplex.getValue(u[i])));
		//  			if (DBL_EQ(cplex.getValue(v[one_node->layer][one_node->id]), cplex.getValue(v[i][j]) - cplex.getValue(u[i])))
		//  				n_tight++;
		//  			if (!DBL_EQ(cplex.getDual(one_arc_constrs[i][j]), 0))
		//  				n_nzdual++;
		//  		}
		//  		// cout << "v[" << i << "][" << j << "] = " << cplex.getValue(v[i][j]) << endl;
		//  	}
		// }
		// // cout << "Tight arcs = " << n_tight << " / Nonzero duals = " << n_nzdual << endl;

		// Store additional information in cut_info
		if (cut_info != NULL) {
			int bdd_size = bdd->layers.size();
			cut_info->zero_arc_flow.clear();
			cut_info->zero_arc_flow.resize(bdd_size);
			cut_info->one_arc_flow.clear();
			cut_info->one_arc_flow.resize(bdd_size);
			for (int layer = 0; layer < bdd_size; ++layer) {
				int size = bdd->layers[layer].size();
				for (int k = 0; k < size; ++k) {
					Node* node = bdd->layers[layer][k];
					double zero_flow = 0;
					if (node->zero_arc != NULL) {
						zero_flow = cplex.getDual(zero_arc_constrs[layer][k]);
					}
					double one_flow = 0;
					if (node->one_arc != NULL) {
						one_flow = cplex.getDual(one_arc_constrs[layer][k]);
					}
					cut_info->zero_arc_flow[layer].push_back(zero_flow);
					cut_info->one_arc_flow[layer].push_back(one_flow);
				}
			}

			// // Uncomment for debugging purposes
			// print_all_paths_in_flow(bdd, cut_info->zero_arc_flow, cut_info->one_arc_flow);
		}

		obj.end();
		zero_arc_constrs.end();
		one_arc_constrs.end();
		env.end();

	} catch (IloException& ex) {
		cout << "error: " << ex << endl;
		exit(1);
	}

	return facet;
}

void perturb_bdd_cut_iterative(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point)
{
	// cplex.setOut(env.getNullStream()); // Suppress output
	cout << "Before perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;

	IloExpr obj_expr(env);
	for (int i = 0; i < nvars; ++i) {
		obj_expr += u[i] * (x[i] - interior_point[i]);
	}
	double bound = cplex.getObjValue();
	// model.add( obj_expr == bound );
	model.add(obj_expr <= bound + 1e-5);
	model.add(obj_expr >= bound - 1e-5);
	obj_expr.end();

	for (int k = 0; k < nvars; ++k) {
		for (int i = 0; i < nvars; ++i) {
			obj.setLinearCoef(u[i], 0);
		}
		obj.setLinearCoef(u[k], 1);
		cplex.solve();
		if (k != nvars - 1) {
			cout << "u[" << k << "] = " << cplex.getObjValue() << endl;
			if (DBL_EQ(cplex.getObjValue(), 0)) {
				u[k].setBounds(0, 0);
			} else {
				// u[k].setBounds(cplex.getObjValue(), cplex.getObjValue());
				u[k].setBounds(cplex.getObjValue() - 1e-5, cplex.getObjValue() + 1e-5);
			}
		}
	}

	cout << "After perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;
}

void perturb_bdd_cut_random(IloEnv env, IloCplex cplex, IloModel model, const IloNumVarArray u, IloObjective obj, int nvars,
	const vector<double>& x, const vector<double>& interior_point)
{
	// Perturb the objective slightly
	cout << "Before perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;

	IloExpr obj_expr(env);
	for (int i = 0; i < nvars; ++i) {
		obj_expr += u[i] * (x[i] - interior_point[i]);
	}
	double bound = cplex.getObjValue();
	model.add(obj_expr == bound);
	// model.add( obj_expr <= bound + 1e-5 );
	// model.add( obj_expr >= bound - 1e-5 );
	obj_expr.end();

	for (int i = 0; i < nvars; ++i) {
		double pert = ((double) rand() / (double) RAND_MAX - 0.5) * 2 * 1e-4;
		double new_coeff = x[i] - interior_point[i] + pert;
		if (new_coeff < 0) {
			new_coeff = x[i] - interior_point[i] - pert;
		}
		obj.setLinearCoef(u[i], new_coeff);
	}

	cplex.solve();

	cout << "After perturbation: ";
	for (int i = 0; i < nvars; ++i) {
		cout << cplex.getValue(u[i]) << " ";
	}
	cout << endl;
}


#ifndef EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP
#define EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP

#include <cassert>

#include <armadillo>

#include "arma/lapack_extra.hpp"

namespace expsum
{

//
// Compute accurate con-eigenvalue decomposition of the matrix $A=XD^{2}X^{H}$
//
// This class computes the con-eigenvalue decomposition
//
// ``` math
//   A U = \Lambda \overline{U},
// ```
//
// of a real symmetric (or complex Hermitian) and positive-definite matrix $A$
// having a rank-revealing decomposition of the form, $A = X D^{2} X^{H}$.
//
// @X  A matrix of type `T` of dimension $m \times n$.
//     On entry, the rank-revealing factor $X$.
//     On exit, orthonormal con-eigenvectors of the matrix $A$.
// @d  A vector of real values with size $n$.
//     On entry, the diagonal elements of rank-revealing factor $D$. The
//     diagonal of $D$ must be all positive and decreasing.
//     On exit, con-eigenvalues of the matrix $A$.
// @threshold threshold value for con-eigenvalues.
//

template <typename T>
class coneig_sym_rrd
{
public:
    // Scalar types
    using size_type    = arma::uword;
    using value_type   = T;
    using real_type    = typename arma::get_pod_type<T>::result;
    using complex_type = std::complex<real_type>;

    // Matrix/Vector types
    using vector_type         = arma::Col<value_type>;
    using matrix_type         = arma::Mat<value_type>;
    using index_vector_type   = arma::uvec;
    using real_vector_type    = arma::Col<real_type>;
    using real_matrix_type    = arma::Mat<real_type>;
    using complex_vector_type = arma::Col<complex_type>;
    using complex_matrix_type = arma::Mat<complex_type>;

    static size_type run(matrix_type& X, real_vector_type& d,
                         real_type threshold, value_type* work,
                         real_type* rwork);

private:
    //
    // Compute QR factorization of matrix G = Q * R.
    //
    static void qr_factorization(matrix_type& G, value_type* work)
    {
        auto n          = static_cast<arma::blas_int>(G.n_rows);
        auto lwork      = n * n;
        value_type* tau = work + n * n;
        arma::blas_int info;
        arma::lapack::geqrf(&n, &n, G.memptr(), &n, tau, work, &lwork, &info);
        if (info)
        {
            std::ostringstream msg;
            msg << "(coneig_sym_rrd) xGEQRF failed with info " << info;
            throw std::runtime_error(msg.str());
        }
    }
    //
    // Solve R * Y = X, where R is upper triangular matrix.
    //
    static void tri_solve(matrix_type& R, matrix_type& X)
    {
        char uplo  = 'U';
        char trans = 'N';
        char diag  = 'N';
        auto m     = static_cast<arma::blas_int>(R.n_rows);
        auto nrhs  = static_cast<arma::blas_int>(X.n_cols);
        arma::blas_int info;
        arma::lapack::trtrs(&uplo, &trans, &diag, &m, &nrhs, R.memptr(), &m,
                            X.memptr(), &m, &info);
        if (info)
        {
            std::ostringstream msg;
            msg << "(coneig_sym_rrd) xTRTRS failed with info " << info;
            throw std::runtime_error(msg.str());
        }
    }
    //
    // Compute singular values and corresponding left singular vectors of upper
    // triangular matrix R using one-sided Jacobi method.
    //
    // --- for real matrix
    static void jacobi_svd(matrix_type& R, real_vector_type& sigma,
                           real_type* work, real_type* /*dummy*/)
    {
        assert(R.n_rows == R.n_cols);
        assert(sigma.n_elem == R.n_cols);

        char joba = 'U'; // Input matrix R is upper triangular matrix
        char jobu = 'U'; // Compute left singular vectors
        char jobv = 'N'; // Do not compute right singular vectors
        auto n    = static_cast<arma::blas_int>(R.n_cols);
        auto mv   = arma::blas_int();
        auto ldv  = arma::blas_int(2);

        real_type dummy_v[2];
        auto lwork = 2 * n;

        arma::blas_int info;

        arma::lapack::gesvj(&joba, &jobu, &jobv, &n, &n, R.memptr(), &n,
                            sigma.memptr(), &mv, &dummy_v[0], &ldv, work,
                            &lwork, &info);

        if (info < arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[s/d]GESVJ error: " << -info
                << " th argument had an illegal value";
            throw std::logic_error(msg.str());
        }

        if (info > arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[s/d]GESVJ did not converge in the maximal allowed number "
                << info << " of sweeps";
            throw std::runtime_error(msg.str());
        }
    }
    // --- for complex matrix
    static void jacobi_svd(matrix_type& R, real_vector_type& sigma,
                           complex_type* work, real_type* rwork)
    {
        assert(R.n_rows == R.n_cols);
        assert(sigma.n_elem == R.n_cols);

        char joba = 'U'; // Input matrix R is upper triangular matrix
        char jobu = 'U'; // Compute left singular vectors
        char jobv = 'N'; // Do not compute right singular vectors
        auto n    = static_cast<arma::blas_int>(R.n_cols);
        auto mv   = arma::blas_int();
        auto ldv  = arma::blas_int(2);

        complex_type dummy_v[2];
        auto lwork  = 2 * n;
        auto lrwork = std::max(arma::blas_int(6), 2 * n);

        arma::blas_int info;

        arma::lapack::gesvj(&joba, &jobu, &jobv, &n, &n, R.memptr(), &n,
                            sigma.memptr(), &mv, &dummy_v[0], &ldv, work,
                            &lwork, rwork, &lrwork, &info);

        if (info < arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[c/z]GESVJ error: " << -info
                << " th argument had an illegal value";
            throw std::logic_error(msg.str());
        }

        if (info > arma::blas_int())
        {
            std::ostringstream msg;
            msg << "[c/z]GESVJ did not converge in the maximal allowed number "
                << info << " of sweeps";
            throw std::runtime_error(msg.str());
        }
    }
};

//
// Required memory for workspace
//
// work size:
//    for matrix G : n * n
//    for matrix Y : n * n
//    for workspace: 2 * n
//    ------------------------------
//    Total        : 2 * n * (n + 1)
//
// rwork size:
//    if `T` is real type   : n
//    if `T` is complex type: 3 * n
//
template <typename T>
typename coneig_sym_rrd<T>::size_type
coneig_sym_rrd<T>::run(matrix_type& X, real_vector_type& d, real_type threshold,
                       value_type* work, real_type* rwork)
{
    // const size_type m = X.n_rows;
    const size_type n = X.n_cols;

    value_type* ptr1 = work;
    value_type* ptr2 = work + n * n;
    value_type* ptr3 = work + 2 * n * n;

    real_vector_type dinv(rwork, n, false, true);
    dinv = real_type(1) / d;

    //
    // Form G = D * (X.st() * X) * D
    //
    matrix_type G(ptr1, n, n, false, true);
    G = X.st() * X;
    for (size_type j = 0; j < n; ++j)
    {
        for (size_type i = 0; i < n; ++i)
        {
            G(i, j) *= d(i) * d(j);
        }
    }
    //
    // Compute G = Q * R by Householder QR factorization
    //
    qr_factorization(G, ptr2);
    matrix_type& R = G; // R = trimatu(G), on exit
    //
    // Compute SVD of R = U * S * V.t() using one-sided Jacobi method.
    // We need singular values and left singular vectors here.
    //
    real_vector_type& sigma = d;
    matrix_type U(ptr2, n, n, false, true);
    U = arma::trimatu(R); // make a copy of matrix R
    jacobi_svd(U, sigma, ptr3, rwork + n);

    //-------------------------------------------------------------------------
    // Truncation: discard con-eigenvalues if negligibly small
    //-------------------------------------------------------------------------
    auto sum_d     = real_type();
    size_type nvec = n;
    while (nvec)
    {
        sum_d += d(nvec - 1);
        if (2 * sum_d > threshold)
        {
            break;
        }
        --nvec;
    }

    if (nvec == size_type(0))
    {
        return size_type();
    }

    //-------------------------------------------------------------------------
    //
    // The eigenvectors of A * conj(A) are given as
    //
    //   conj(X') = X * D * V * S^{-1/2}.
    //
    // However, direct evaluation of eigenvectors with this formula might be
    // inaccurate since D is ill-conditioned. The following formula are used
    // instead.
    //
    //   conj(X') = X * D * R^(-1) * U * S^{1/2}
    //            = X * (D.inv() * R * D.inv())^(-1) * (D.inv() * U * S^{1/2})
    //            = X * R1.inv() * X1
    //
    //-------------------------------------------------------------------------
    //
    // Compute X1 = D^(-1) * U * S^{1/2} [in-place]
    //
    matrix_type U_trunc(ptr2, n, nvec, false, true);
    for (size_type j = 0; j < nvec; ++j)
    {
        const auto sj = std::sqrt(sigma(j));
        for (size_type i = 0; i < n; ++i)
        {
            U_trunc(i, j) *= sj * dinv(i);
        }
    }
    //
    // Compute R1 = D^(-1) * R * D^(-1) [in-place]
    //
    for (size_type j = 0; j < n; ++j)
    {
        for (size_type i = 0; i <= j; ++i)
        {
            R(i, j) *= dinv(i) * dinv(j);
        }
    }
    //
    // Solve R1 * Y1 = X1 in-place
    //
    tri_solve(R, U_trunc);
    //
    // Compute con-eigenvectors U = conj(X) * conj(Y).
    //
    X.head_cols(nvec) = arma::conj(X) * arma::conj(U_trunc);
    //
    // Adjust phase factor of each con-eigenvectors, so that U^{T} * U = I
    //
    if (arma::is_complex<T>::value)
    {
        for (size_type j = 0; j < nvec; ++j)
        {
            auto xj          = X.col(j);
            const auto t     = arma::dot(xj, xj);
            const auto phase = t / std::abs(t);
            const auto scale = std::sqrt(arma::access::alt_conj(phase));
            xj *= scale;
        }
    }

    return nvec;
}

} // namespace: expsum

#endif /* EXPSUM_REDUCTION_CONEIG_SYM_RRD_HPP */


/*
Copyright 2013-2015 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef MATH_MAX_SEMIRING_HPP_INCLUDED
#define MATH_MAX_SEMIRING_HPP_INCLUDED

#include <limits>
#include <iosfwd>
#include <type_traits>

#include <boost/mpl/bool.hpp>

#include <boost/functional/hash_fwd.hpp>

#include "magma.hpp"

#include "detail/is_close.hpp"

namespace math {

/**
Semiring whose \ref plus (and \ref choose) operation picks the maximum of the
two values.
\ref times performs multiplication on the underlying value.

The underlying value must always be non-negative, so that the additive identity
has value 0.
The multiplicative identity has value 1.

Division is only implemented for non-integer types.

This type supports Boost.Hash, if \c boost/functional/hash.hpp is included.

\tparam Type
    Underlying type that represents the value.
*/
template <class Type> class max_semiring;

template <class Type> struct max_semiring_tag;

template <class Type> struct decayed_magma_tag <max_semiring <Type>>
{ typedef max_semiring_tag <Type> type; };

template <class Type> class max_semiring {
private:
    Type value_;
public:
    /**
    Initialise with 0.
    */
    max_semiring() : value_()
    { assert (value_ >= Type (0)); }

    /**
    Initialise with \a value as the value.
    */
    explicit max_semiring (Type const & value) : value_ (value) {}

    /**
    Return the underlying value.
    */
    Type const & value() const { return value_; }
};

namespace detail {

    template <class Type> struct is_max_semiring_tag : boost::mpl::false_ {};
    template <class Type> struct is_max_semiring_tag <max_semiring_tag <Type>>
    : boost::mpl::true_ {};

} // namespace detail

MATH_MAGMA_GENERATE_OPERATORS (detail::is_max_semiring_tag)

namespace operation {

    /* Queries. */

    template <class Type>
        struct is_member <max_semiring_tag <Type>, typename std::enable_if <
            std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        // Detect NaN.
        bool operator() (max_semiring <Type> const & v) const { return v == v; }
    };

    template <class Type> struct equal <max_semiring_tag <Type>> {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        { return left.value() == right.value(); }
    };

    template <class Type> struct approximately_equal <max_semiring_tag <Type>,
        typename std::enable_if <!std::numeric_limits <Type>::is_exact>::type>
    {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        {
            static Type const tolerance = 1e-5;
            return math::detail::is_close (left.value(), right.value(),
                tolerance);
        }
    };

    // compare always orders smaller values first.
    template <class Type> struct compare <max_semiring_tag <Type>> {
        bool operator() (
            max_semiring <Type> const & left, max_semiring <Type> const & right)
            const
        { return left.value() < right.value(); }
    };

    // "choose" and "plus" both select the maximal element.
    template <class Type>
    struct order <max_semiring_tag <Type>, callable::choose>
    : reverse_order <compare <max_semiring_tag <Type>>> {};

    template <class Type>
    struct order <max_semiring_tag <Type>, callable::plus>
    : reverse_order <compare <max_semiring_tag <Type>>> {};

    /* Produce. */

    // Return the not-a-number value, not-a-number, if it is available.
    template <class Type> struct non_member <max_semiring_tag <Type>, typename
        std::enable_if <std::numeric_limits <Type>::has_quiet_NaN>::type>
    {
        max_semiring <Type> operator() () const {
            return max_semiring <Type> (
                std::numeric_limits <Type>::quiet_NaN());
        }
    };

    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::times>
    {
        max_semiring <Type> operator() () const
        { return max_semiring <Type> (1); }
    };

    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::choose>
    {
        max_semiring <Type> operator() () const
        { return max_semiring <Type> (0); }
    };

    // Additive identity: same as "choose".
    template <class Type>
        struct identity <max_semiring_tag <Type>, callable::plus>
    : identity <max_semiring_tag <Type>, callable::choose> {};

    // Multiplicative annihilator: forward to additive identity.
    template <class Type>
        struct annihilator <max_semiring_tag <Type>, callable::times>
    : identity <max_semiring_tag <Type>, callable::plus> {};

    /* Operations. */

    // "choose" and "plus" are implemented automatically, since "order" is
    // implemented for them.

    template <class Type> struct times <max_semiring_tag <Type>>
    : associative, commutative, approximate_if <
        boost::mpl::bool_ <!std::numeric_limits <Type>::is_exact>>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & left,
            max_semiring <Type> const & right) const
        { return max_semiring <Type> (left.value() * right.value()); }
    };

    // Semiring in both directions.
    template <class Type> struct is_semiring <
        max_semiring_tag <Type>, either, callable::times, callable::choose>
    : rime::true_type {};

    template <class Type> struct is_semiring <
        max_semiring_tag <Type>, either, callable::times, callable::plus>
    : rime::true_type {};

    // Division is only implemented for non-integer types.
    template <class Type> struct divide <max_semiring_tag <Type>, either,
        typename std::enable_if <!std::numeric_limits <Type>::is_integer>::type>
    : approximate_if <boost::mpl::bool_ <!std::numeric_limits <Type>::is_exact>>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & left,
            max_semiring <Type> const & right) const
        { return max_semiring <Type> (left.value() / right.value()); }
    };

    // Inversion is only implemented for non-integer types.
    template <class Type>
        struct invert <max_semiring_tag <Type>, either, callable::times,
        typename std::enable_if <!std::numeric_limits <Type>::is_integer>::type>
    {
        max_semiring <Type> operator() (max_semiring <Type> const & c) const
        { return max_semiring <Type> (1 / c.value()); }
    };

    template <class Type> struct print <max_semiring_tag <Type>> {
        template <class Stream>
            void operator() (Stream & stream, max_semiring <Type> const & c)
            const
        { stream << c.value(); }
    };

} // namespace operation

template <class Type>
    inline std::size_t hash_value (max_semiring <Type> const & m)
{ return boost::hash <Type>() (m.value()); }

} // namespace math

#endif // MATH_MAX_SEMIRING_HPP_INCLUDED


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

#include "data.hpp"
#include "../helper.hpp"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"

using namespace std;

typedef g2o::BlockSolver< g2o::BlockSolverTraits<1, 1> >  ScaleBlockSolver;
typedef g2o::LinearSolverCSparse<ScaleBlockSolver::PoseMatrixType> ScaleLinearSolver;

class VertexScale : public g2o::BaseVertex<1, double>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexScale()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      // cout<<_estimate<<" + "<<*update<<endl;
      _estimate += *update;
    }
};

class EdgeScaleDirect : public g2o::BaseUnaryEdge<1, Eigen::VectorXd, VertexScale>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgeScaleDirect(const Eigen::Vector3d& point3d, const Eigen::Matrix3d& K, double tx, const cv::Mat& _img1)
    : img1(_img1)
    {
      Eigen::Vector3d KX = K * point3d;

      u_base = KX(0)/KX(2);
      v = KX(1)/KX(2);
      u_inc = tx*K(0,0)/point3d(2);
      au_as << u_inc, 0;
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError();
    void linearizeOplus();

  private:

    double u_base;
    double v;
    double u_inc;
    Eigen::Matrix<double,2,1> au_as;
    const cv::Mat& img1;
};


/*
Copyright 2009-2021 Nicolas Colombe

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.
*/

#pragma once

#include "math.hpp"
#include "vector2.hpp"

#include <boost/optional.hpp>

namespace eXl
{
  template <class Real>
  class AABB2D
  {
  public:
    inline AABB2D(){}
    inline AABB2D(Real iMinX, Real iMinY, Real iMaxX, Real iMaxY)
    {
      m_Data[0] = Vector2<Real>(iMinX,iMinY);
      m_Data[1] = Vector2<Real>(iMaxX,iMaxY);
    }

    static inline AABB2D FromCenterAndSize(Vector2<Real> iCenter,Vector2<Real> iSize)
    {
      AABB2D ret;
      ret.m_Data[0] = iCenter - iSize / 2.0;
      ret.m_Data[1] = iCenter + iSize / 2.0;
      return ret;
    }

    inline AABB2D(Vector2<Real> iMin,Vector2<Real> iSize)
    {
      m_Data[0] = iMin;
      m_Data[1] = iMin + iSize;
    }

    template <class OtherReal>
    explicit inline AABB2D(AABB2D<OtherReal> const& iOther)
    {
      m_Data[0] = Vector2<Real>(iOther.m_Data[0]);
      m_Data[1] = Vector2<Real>(iOther.m_Data[1]);
    }

    inline bool Empty() const
    {
      return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
    }

    bool CircleTest(Vector2f const& iPos, Real iRadius)
    {
      if(Contains(iPos))
      {
        return true;
      }
      else
      {
        //Test segment X == X0
        Real dist = m_Data[0].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment X == X1
        dist = m_Data[1].X() - iPos.X();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real y = Math<Real>::Sqrt(dist) + iPos.Y();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].Y() - y && y - m_Data[0].Y() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }

        //TestSegment Y == Y0
        dist = m_Data[0].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
        //Test segment Y == Y1
        dist = m_Data[1].Y() - iPos.Y();   //(x0-xc)
        dist = (iRadius + dist)*(iRadius - dist); //R^2 - (x0-xc)^2
        if(dist > 0)
        {
          Real x = Math<Real>::Sqrt(dist) + iPos.X();
          if(Math<Real>::ZERO_TOLERANCE < m_Data[1].X() - x && x - m_Data[0].X() > Math<Real>::ZERO_TOLERANCE)
            return true;
        }
      }
      return false;
    }

    inline void Rotate(Real iAngle)
    {
      Vector2<Real> center = GetCenter();
      Vector2<Real> size = GetSize();
      Vector2<Real> points[4];
      points[0] = size * -0.5;
      points[1] = Vector2<Real>(size.X() * -0.5, size.Y() *  0.5) ;
      points[2] = Vector2<Real>(size.X() *  0.5, size.Y() * -0.5) ;
      points[3] = size * 0.5;
      points[0] = Vector2<Real>(points[0].X() * Math<Real>::Cos(iAngle) + points[0].Y() * Math<Real>::Sin(iAngle)
                         , points[0].Y() * Math<Real>::Cos(iAngle) - points[0].X() * Math<Real>::Sin(iAngle));
      points[1] = Vector2<Real>(points[1].X() * Math<Real>::Cos(iAngle) + points[1].Y() * Math<Real>::Sin(iAngle)
                         , points[1].Y() * Math<Real>::Cos(iAngle) - points[1].X() * Math<Real>::Sin(iAngle));
      points[2] = Vector2<Real>(points[2].X() * Math<Real>::Cos(iAngle) + points[2].Y() * Math<Real>::Sin(iAngle)
                         , points[2].Y() * Math<Real>::Cos(iAngle) - points[2].X() * Math<Real>::Sin(iAngle));
      points[3] = Vector2<Real>(points[3].X() * Math<Real>::Cos(iAngle) + points[3].Y() * Math<Real>::Sin(iAngle)
                         , points[3].Y() * Math<Real>::Cos(iAngle) - points[3].X() * Math<Real>::Sin(iAngle));

      m_Data[0] = m_Data[1] = points[0];
      for (unsigned int i = 1; i < 4; ++i)
      {
        m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), points[i].X());
        m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), points[i].Y());
        m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), points[i].X());
        m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), points[i].Y());
      }
      m_Data[0] += center;
      m_Data[1] += center;
    }

    inline Vector2<Real> GetCenter()const
    {
      return (m_Data[0] + m_Data[1])/2;
    }

    inline Vector2<Real> GetSize()const
    {
      return m_Data[1] - m_Data[0];
    }

    inline void Inflate(Real iCoeff)
    {
      Vector2<Real> center = (m_Data[1] + m_Data[0]) / 2;
      Vector2<Real> size = m_Data[1] - m_Data[0];
      size = size * iCoeff;
      m_Data[0] = center - size / 2.0;
      m_Data[1] = center + size / 2.0;
    }

    inline Real MinX()const{return m_Data[0].X();}
    inline Real MinY()const{return m_Data[0].Y();}
    inline Real MaxX()const{return m_Data[1].X();}
    inline Real MaxY()const{return m_Data[1].Y();}

    inline Real& MinX(){return m_Data[0].X();}
    inline Real& MinY(){return m_Data[0].Y();}
    inline Real& MaxX(){return m_Data[1].X();}
    inline Real& MaxY(){return m_Data[1].Y();}

    inline bool operator ==(AABB2D const& iOther) const
    {
      return m_Data[0]==iOther.m_Data[0] && m_Data[1] == iOther.m_Data[1];
    }

    inline bool operator !=(AABB2D const& iOther) const
    {
      return !((*this) == iOther);
    }

	  inline bool Empty()
	  {
		  return m_Data[0].X() == m_Data[1].X() || m_Data[0].Y() == m_Data[1].Y();
	  }

    inline void SetCommonBox(AABB2D<Real> const& iBox1,AABB2D<Real> const& iBox2)
    {
      m_Data[0].X() = Math<Real>::Max(iBox1.m_Data[0].X(),iBox2.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Max(iBox1.m_Data[0].Y(),iBox2.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].X(),iBox2.m_Data[1].X()),m_Data[0].X());
      m_Data[1].Y() = Math<Real>::Max(Math<Real>::Min(iBox1.m_Data[1].Y(),iBox2.m_Data[1].Y()),m_Data[0].Y());
    }

    inline void Absorb(Vector2<Real> const& iPoint)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(), iPoint.X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(), iPoint.Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(), iPoint.X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(), iPoint.Y());
    }

    inline void Absorb(AABB2D<Real> const& iBox)
    {
      m_Data[0].X() = Math<Real>::Min(m_Data[0].X(),iBox.m_Data[0].X());
      m_Data[0].Y() = Math<Real>::Min(m_Data[0].Y(),iBox.m_Data[0].Y());
      m_Data[1].X() = Math<Real>::Max(m_Data[1].X(),iBox.m_Data[1].X());
      m_Data[1].Y() = Math<Real>::Max(m_Data[1].Y(),iBox.m_Data[1].Y());
    }

    inline bool IsInside(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return (m_Data[0].X() - iOther.m_Data[0].X() >= iEpsilon && m_Data[0].Y() - iOther.m_Data[0].Y() >= iEpsilon
           && iOther.m_Data[1].X() - m_Data[1].X() >= iEpsilon && iOther.m_Data[1].Y() - m_Data[1].Y() >= iEpsilon );
    }

    inline bool Intersect(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon
          && iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon;
    }

    //0 No, 1 Left, 2 Right, 3 Down, 4 Up
    inline int Touch(AABB2D const& iOther, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      if(iOther.m_Data[1].Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iOther.m_Data[0].Y() >= iEpsilon)
      {
        Real val = m_Data[0].X() - iOther.m_Data[1].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 1;

        val = m_Data[1].X() - iOther.m_Data[0].X();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 2;
      }
      if(iOther.m_Data[1].X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iOther.m_Data[0].X() >= iEpsilon)
      {
        Real val = m_Data[0].Y() - iOther.m_Data[1].Y();

        if(val <= iEpsilon && val >= -iEpsilon)
          return 3;

        val = m_Data[1].Y() - iOther.m_Data[0].Y();
        if(val <= iEpsilon && val >= -iEpsilon)
          return 4;
      }
      return 0;
    }

    boost::optional<Vector2<Real>> SegmentTest(Vector2<Real> const& iOrigin, Vector2<Real> const& iDir, Real iEpsilon = Math<Real>::ZERO_TOLERANCE)
    {
      Real const scaleX = 1.0 / iDir.X();
      Real const scaleY = 1.0 / iDir.Y();
      Real const signX = Math<Real>::Sign(scaleX);
      Real const signY = Math<Real>::Sign(scaleY);

      Real const nearTimeX = ((signX > 0 ? m_Data[0].X() : m_Data[1].X()) - iOrigin.X()) * scaleX;
      Real const nearTimeY = ((signY > 0 ? m_Data[0].Y() : m_Data[1].Y()) - iOrigin.Y()) * scaleY;
      Real const farTimeX = ((signX > 0 ? m_Data[1].X() : m_Data[0].X()) - iOrigin.X()) * scaleX;
      Real const farTimeY = ((signY > 0 ? m_Data[1].Y() : m_Data[0].Y()) - iOrigin.Y()) * scaleY;

      if (nearTimeX > farTimeY || nearTimeY > farTimeX)
      {
        return boost::none;
      }

      Real const nearTime = nearTimeX > nearTimeY ? nearTimeX : nearTimeY;
      Real const farTime = farTimeX < farTimeY ? farTimeX : farTimeY;

      if (nearTime > 1 - iEpsilon || farTime < -iEpsilon)
      {
        return boost::none;
      }

      return iOrigin + iDir * nearTime;
    }

    inline bool Contains(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      return iPoint.X() - m_Data[0].X() >= iEpsilon && m_Data[1].X() - iPoint.X() >= iEpsilon
          && iPoint.Y() - m_Data[0].Y() >= iEpsilon && m_Data[1].Y() - iPoint.Y() >= iEpsilon;
    }

    // Neg value -> distance to min, Pos value -> distance to max, 0, inside
    Vector2f Classify(Vector2<Real>const& iPoint, Real iEpsilon = Math<Real>::ZERO_TOLERANCE) const
    {
      Vector2f res(iPoint.X() - m_Data[0].X(), iPoint.Y() - m_Data[0].Y());
      for(uint32_t axis = 0; axis < 2; ++axis)
      {
        if(!(res.m_Data[axis] < -iEpsilon))
        {
          res.m_Data[axis] = m_Data[1].m_Data[axis] - iPoint.m_Data[axis];
          if(res.m_Data[axis] < -iEpsilon)
          {
            res.m_Data[axis] *= -1;
          }
          else
          {
            res.m_Data[axis] = 0;
          }
        }
      }

      return res;
    }

    inline Vector2<Real> GetDistX(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].X() - m_Data[0].X(),m_Data[1].X() - iOther.m_Data[1].X());
    }

    inline Vector2<Real> GetDistY(AABB2D const& iOther) const
    {
      return Vector2<Real>(iOther.m_Data[0].Y() - m_Data[0].Y(),m_Data[1].Y() - iOther.m_Data[1].Y());
    }

    Vector2<Real> m_Data[2];
  };

  typedef AABB2D<int> AABB2Di;
  typedef AABB2D<float> AABB2Df;
  typedef AABB2D<double> AABB2Dd;
}


#ifndef SKYLARK_GEMV_HPP
#define SKYLARK_GEMV_HPP

#include <boost/mpi.hpp>
#include "exception.hpp"

// Defines a generic Gemv function that recieves a wider set of matrices

namespace skylark { namespace base {

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& x,
    T beta, elem::Matrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, beta, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::Matrix<T>& A, const elem::Matrix<T>& x,
    elem::Matrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& x,
    T beta, elem::DistMatrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, beta, y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T>& A, const elem::DistMatrix<T>& x,
    elem::DistMatrix<T>& y) {
    elem::Gemv(oA, alpha, A, x, y);
}

/**
 * The following combinations is not offered by Elemental, but is useful for us.
 * We implement it partially.
 */

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& x,
    T beta, elem::DistMatrix<T, elem::STAR, elem::STAR>& y) {
    // TODO verify sizes etc.
    // TODO verify matching grids.

    if (oA == elem::TRANSPOSE) {
        boost::mpi::communicator comm(y.Grid().Comm(), boost::mpi::comm_attach);
        elem::Matrix<T> ylocal(y.Matrix());
        elem::Gemv(elem::TRANSPOSE,
            alpha, A.LockedMatrix(), x.LockedMatrix(),
            beta / T(comm.size()), ylocal);
        boost::mpi::all_reduce(comm,
            ylocal.Buffer(), ylocal.MemorySize(), y.Buffer(),
            std::plus<T>());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::VC, elem::STAR>& x,
    elem::DistMatrix<T, elem::STAR, elem::STAR>& y) {

    int y_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    elem::Zeros(y, y_height, 1);
    base::Gemv(oA, alpha, A, x, T(0), y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& x,
    T beta, elem::DistMatrix<T, elem::VC, elem::STAR>& y) {
    // TODO verify sizes etc.

    if (oA == elem::NORMAL) {
        elem::Gemv(elem::NORMAL,
            alpha, A.LockedMatrix(), x.LockedMatrix(),
            beta, y.Matrix());
    } else {
        SKYLARK_THROW_EXCEPTION(base::unsupported_base_operation());
    }
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const elem::DistMatrix<T, elem::VC, elem::STAR>& A,
    const elem::DistMatrix<T, elem::STAR, elem::STAR>& x,
    elem::DistMatrix<T, elem::VC, elem::STAR>& y) {

    int y_height = (oA == elem::NORMAL ? A.Height() : A.Width());
    elem::Zeros(y, y_height, 1);
    base::Gemv(oA, alpha, A, x, T(0), y);
}

template<typename T>
inline void Gemv(elem::Orientation oA,
    T alpha, const sparse_matrix_t<T>& A, const elem::Matrix<T>& x,
    T beta, elem::Matrix<T>& y) {
    // TODO verify sizes etc.

    const int* indptr = A.indptr();
    const int* indices = A.indices();
    const double *values = A.locked_values();
    double *yd = y.Buffer();
    const double *xd = x.LockedBuffer();

    int n = A.width();

    if (oA == elem::NORMAL) {
        elem::Scal(beta, y);

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int col = 0; col < n; col++) {
            T xv = alpha * xd[col];
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     yd[row] += val * xv;
                 }
        }

    } else {

#       if SKYLARK_HAVE_OPENMP
#       pragma omp parallel for
#       endif
        for(int col = 0; col < n; col++) {
            double yv = beta * yd[col];
            for (int j = indptr[col]; j < indptr[col + 1]; j++) {
                     int row = indices[j];
                     T val = values[j];
                     yv += alpha * val * xd[row];
                 }
            yd[col] = yv;
        }

    }
}

} } // namespace skylark::base

#endif // SKYLARK_GEMV_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "determinant.hpp"
#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"
#include "SiconosException.hpp"

using namespace Siconos;

//=======================
//       get norm
//=======================

double SimpleMatrix::normInf() const
{
  if(_num == DENSE)
    return norm_inf(*mat.Dense);
  else if(_num == TRIANGULAR)
    return norm_inf(*mat.Triang);
  else if(_num == SYMMETRIC)
    return norm_inf(*mat.Sym);
  else if(_num == SPARSE)
    return norm_inf(*mat.Sparse);
  else if(_num == SPARSE_COORDINATE)
    return norm_inf(*mat.SparseCoordinate);
  else if(_num == BANDED)
    return norm_inf(*mat.Banded);
  else if(_num == ZERO)
    return 0;
  else if(_num == IDENTITY)
    return 1;

  THROW_EXCEPTION("Matrix type not supported");
  return std::numeric_limits<double>::infinity();
}

void SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
  if(_num == DENSE)
  {
    if(vIn->size() != size(1))
      THROW_EXCEPTION("the given vector does not have the right length");
    DenseVect tmpV = DenseVect(size(0));
    for(unsigned int i = 0; i < size(1); i++)
    {
      ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
      (*vIn)(i) = norm_inf(tmpV);
    }
  }
  else
    THROW_EXCEPTION("not implemented for data other than DenseMat");
}
//=======================
//       determinant
//=======================

double SimpleMatrix::det() const
{
  if(_num == DENSE)
    return determinant(*mat.Dense);
  else if(_num == TRIANGULAR)
    return determinant(*mat.Triang);
  else if(_num == SYMMETRIC)
    return determinant(*mat.Sym);
  else if(_num == SPARSE)
    return determinant(*mat.Sparse);
  else if(_num == SPARSE_COORDINATE)
    return determinant(*mat.Sparse);
  else if(_num == BANDED)
    return determinant(*mat.Banded);
  else if(_num == ZERO)
    return 0;
  else  if(_num == IDENTITY)
    return 1;
  THROW_EXCEPTION("Matrix type not supported");
  return std::numeric_limits<double>::infinity();
}

void SimpleMatrix::trans()
{
  switch(_num)
  {
  case DENSE:
    *mat.Dense = ublas::trans(*mat.Dense);
    break;
  case TRIANGULAR:
    THROW_EXCEPTION("failed, the matrix is triangular matrix and can not be transposed in place.");
    break;
  case SYMMETRIC:
    break;
  case SPARSE:
    *mat.Sparse = ublas::trans(*mat.Sparse);
    break;
  case SPARSE_COORDINATE:
    *mat.Sparse = ublas::trans(*mat.Sparse);
    break;
  case BANDED:
    *mat.Banded = ublas::trans(*mat.Banded);
    break;
  case Siconos::ZERO:
    break;
  case Siconos::IDENTITY:
    break;
  default:
    THROW_EXCEPTION("Matrix type not supported");
  }
  resetFactorizationFlags();
}

void SimpleMatrix::trans(const SiconosMatrix &m)
{
  if(m.isBlock())
    THROW_EXCEPTION("not yet implemented for m being a BlockMatrix.");

  if(&m == this)
    trans();
  else
  {
    Siconos::UBLAS_TYPE numM = m.num();
    switch(numM)
    {
    case DENSE:
      if(_num != DENSE)
        THROW_EXCEPTION("try to transpose a dense matrix into another type.");
      noalias(*mat.Dense) = ublas::trans(*m.dense());
      break;
    case TRIANGULAR:
      if(_num != DENSE)
        THROW_EXCEPTION("try to transpose a triangular matrix into a non-dense one.");
      noalias(*mat.Dense) = ublas::trans(*m.triang());
      break;
    case SYMMETRIC:
      *this = m;
      break;
    case SPARSE:
      if(_num == DENSE)
        noalias(*mat.Dense) = ublas::trans(*m.sparse());
      else if(_num == SPARSE)
        noalias(*mat.Sparse) = ublas::trans(*m.sparse());
      else if(_num == SPARSE_COORDINATE)
        noalias(*mat.SparseCoordinate) = ublas::trans(*m.sparse());
      else
        THROW_EXCEPTION("try to transpose a sparse matrix into a forbidden type (not dense nor sparse).");
      break;
    case SPARSE_COORDINATE:
      if(_num == DENSE)
        noalias(*mat.Dense) = ublas::trans(*m.sparseCoordinate());
      else if(_num == SPARSE)
        noalias(*mat.Sparse) = ublas::trans(*m.sparseCoordinate());
      else if(_num == SPARSE_COORDINATE)
        noalias(*mat.SparseCoordinate) = ublas::trans(*m.sparseCoordinate());
      else
        THROW_EXCEPTION("try to transpose a sparse coordinate matrix into a forbidden type (not dense nor sparse coordinate).");
      break;
    case BANDED:
      if(_num == DENSE)
        noalias(*mat.Dense) = ublas::trans(*m.banded());
      else if(_num == BANDED)
        noalias(*mat.Banded) = ublas::trans(*m.banded());
      else
        THROW_EXCEPTION("try to transpose a banded matrix into a forbidden type (not dense nor banded).");
      break;
    case ZERO:
      *this = m;
      break;
    case IDENTITY:
      *this = m;
      break;
    default:
      THROW_EXCEPTION("");
    }
    resetFactorizationFlags();
  }
}

/*
The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.

you can solve Ax=b using three lines of ublas code:

permutation_matrix<> piv;
lu_factorize(A, piv);
lu_substitute(A, piv, x);

*/
#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/* Matrix inversion routine.
Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T, class U, class V>
bool InvertMatrix(const ublas::matrix<T, U, V>& input, ublas::matrix<T, U, V>& inverse)
{
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
// create a working copy of the input
  matrix<T, U, V> A(input);
// create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

// perform LU-factorization
  int res = lu_factorize(A,pm);
  if(res != 0) return false;

// create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<T>(A.size1()));

// backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);

  return true;
}

#endif //INVERT_MATRIX_HPP

// Note FP: never used. Comment before removal ?
// void invertMatrix(const SimpleMatrix& input, SimpleMatrix& output)
// {
//   InvertMatrix(*input.dense(), *output.dense());
// }


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

#ifndef BOOST_MATH_INTERPOLATORS_BARYCENTRIC_RATIONAL_DETAIL_HPP
#define BOOST_MATH_INTERPOLATORS_BARYCENTRIC_RATIONAL_DETAIL_HPP

#include <vector>
#include <utility> // for std::move
#include <algorithm> // for std::is_sorted
#include <boost/lexical_cast.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/core/demangle.hpp>
#include <boost/assert.hpp>

namespace boost{ namespace math{ namespace detail{

template<class Real>
class barycentric_rational_imp
{
public:
    template <class InputIterator1, class InputIterator2>
    barycentric_rational_imp(InputIterator1 start_x, InputIterator1 end_x, InputIterator2 start_y, size_t approximation_order = 3);

    barycentric_rational_imp(std::vector<Real>&& x, std::vector<Real>&& y, size_t approximation_order = 3);

    Real operator()(Real x) const;

    Real prime(Real x) const;

    // The barycentric weights are not really that interesting; except to the unit tests!
    Real weight(size_t i) const { return m_w[i]; }

    std::vector<Real>&& return_x()
    {
        return std::move(m_x);
    }

    std::vector<Real>&& return_y()
    {
        return std::move(m_y);
    }

private:

    void calculate_weights(size_t approximation_order);

    std::vector<Real> m_x;
    std::vector<Real> m_y;
    std::vector<Real> m_w;
};

template <class Real>
template <class InputIterator1, class InputIterator2>
barycentric_rational_imp<Real>::barycentric_rational_imp(InputIterator1 start_x, InputIterator1 end_x, InputIterator2 start_y, size_t approximation_order)
{
    std::ptrdiff_t n = std::distance(start_x, end_x);

    if (approximation_order >= (std::size_t)n)
    {
        throw std::domain_error("Approximation order must be < data length.");
    }

    // Big sad memcpy.
    m_x.resize(n);
    m_y.resize(n);
    for(unsigned i = 0; start_x != end_x; ++start_x, ++start_y, ++i)
    {
        // But if we're going to do a memcpy, we can do some error checking which is inexpensive relative to the copy:
        if(boost::math::isnan(*start_x))
        {
            std::string msg = std::string("x[") + boost::lexical_cast<std::string>(i) + "] is a NAN";
            throw std::domain_error(msg);
        }

        if(boost::math::isnan(*start_y))
        {
           std::string msg = std::string("y[") + boost::lexical_cast<std::string>(i) + "] is a NAN";
           throw std::domain_error(msg);
        }

        m_x[i] = *start_x;
        m_y[i] = *start_y;
    }
    calculate_weights(approximation_order);
}

template <class Real>
barycentric_rational_imp<Real>::barycentric_rational_imp(std::vector<Real>&& x, std::vector<Real>&& y,size_t approximation_order) : m_x(std::move(x)), m_y(std::move(y))
{
    BOOST_ASSERT_MSG(m_x.size() == m_y.size(), "There must be the same number of abscissas and ordinates.");
    BOOST_ASSERT_MSG(approximation_order < m_x.size(), "Approximation order must be < data length.");
    BOOST_ASSERT_MSG(std::is_sorted(m_x.begin(), m_x.end()), "The abscissas must be listed in increasing order x[0] < x[1] < ... < x[n-1].");
    calculate_weights(approximation_order);
}

template<class Real>
void barycentric_rational_imp<Real>::calculate_weights(size_t approximation_order)
{
    using std::abs;
    int64_t n = m_x.size();
    m_w.resize(n, 0);
    for(int64_t k = 0; k < n; ++k)
    {
        int64_t i_min = (std::max)(k - (int64_t) approximation_order, (int64_t) 0);
        int64_t i_max = k;
        if (k >= n - (std::ptrdiff_t)approximation_order)
        {
            i_max = n - approximation_order - 1;
        }

        for(int64_t i = i_min; i <= i_max; ++i)
        {
            Real inv_product = 1;
            int64_t j_max = (std::min)(static_cast<int64_t>(i + approximation_order), static_cast<int64_t>(n - 1));
            for(int64_t j = i; j <= j_max; ++j)
            {
                if (j == k)
                {
                    continue;
                }

                Real diff = m_x[k] - m_x[j];
                using std::numeric_limits;
                if (abs(diff) < (numeric_limits<Real>::min)())
                {
                   std::string msg = std::string("Spacing between  x[")
                      + boost::lexical_cast<std::string>(k) + std::string("] and x[")
                      + boost::lexical_cast<std::string>(i) + std::string("] is ")
                      + boost::lexical_cast<std::string>(diff) + std::string(", which is smaller than the epsilon of ")
                      + boost::core::demangle(typeid(Real).name());
                    throw std::logic_error(msg);
                }
                inv_product *= diff;
            }
            if (i % 2 == 0)
            {
                m_w[k] += 1/inv_product;
            }
            else
            {
                m_w[k] -= 1/inv_product;
            }
        }
    }
}

template<class Real>
Real barycentric_rational_imp<Real>::operator()(Real x) const
{
    Real numerator = 0;
    Real denominator = 0;
    for(size_t i = 0; i < m_x.size(); ++i)
    {
        // Presumably we should see if the accuracy is improved by using ULP distance of say, 5 here, instead of testing for floating point equality.
        // However, it has been shown that if x approx x_i, but x != x_i, then inaccuracy in the numerator cancels the inaccuracy in the denominator,
        // and the result is fairly accurate. See: http://epubs.siam.org/doi/pdf/10.1137/S0036144502417715
        if (x == m_x[i])
        {
            return m_y[i];
        }
        Real t = m_w[i]/(x - m_x[i]);
        numerator += t*m_y[i];
        denominator += t;
    }
    return numerator/denominator;
}

/*
 * A formula for computing the derivative of the barycentric representation is given in
 * "Some New Aspects of Rational Interpolation", by Claus Schneider and Wilhelm Werner,
 * Mathematics of Computation, v47, number 175, 1986.
 * http://www.ams.org/journals/mcom/1986-47-175/S0025-5718-1986-0842136-8/S0025-5718-1986-0842136-8.pdf
 * and reviewed in
 * Recent developments in barycentric rational interpolation
 * Jean-Paul Berrut, Richard Baltensperger and Hans D. Mittelmann
 *
 * Is it possible to complete this in one pass through the data?
 */

template<class Real>
Real barycentric_rational_imp<Real>::prime(Real x) const
{
    Real rx = this->operator()(x);
    Real numerator = 0;
    Real denominator = 0;
    for(size_t i = 0; i < m_x.size(); ++i)
    {
        if (x == m_x[i])
        {
            Real sum = 0;
            for (size_t j = 0; j < m_x.size(); ++j)
            {
                if (j == i)
                {
                    continue;
                }
                sum += m_w[j]*(m_y[i] - m_y[j])/(m_x[i] - m_x[j]);
            }
            return -sum/m_w[i];
        }
        Real t = m_w[i]/(x - m_x[i]);
        Real diff = (rx - m_y[i])/(x-m_x[i]);
        numerator += t*diff;
        denominator += t;
    }

    return numerator/denominator;
}
}}}
#endif


#pragma once

#include "../../util/assert.hh"
#include "../../util/meta.hh"

#include <Eigen/Core>
#include <stdexcept>
#include <sstream>

namespace bold
{
  template<typename T,int dim>
  struct LineSegment : Eigen::Matrix<T,dim,2>
  {
  public:
    typedef Eigen::Matrix<T,dim,2> Base;
    typedef Eigen::Matrix<T,dim,1> PointType;

    template<typename OtherDerived>
    LineSegment(Eigen::MatrixBase<OtherDerived> const& other)
      :  Base(other)
    {}

    LineSegment(PointType const& p1,
                PointType const& p2)
    {
      static_assert(std::is_arithmetic<T>::value, "Must be an arithmetic type");

      this->col(0) = p1;
      this->col(1) = p2;

      ASSERT((p2 - p1).cwiseAbs().maxCoeff() != 0);
    }

    template<typename OtherDerived>
    LineSegment& operator=(Eigen::MatrixBase<OtherDerived> const& other)
    {
      this->Base::operator=(other);
      return *this;
    }

    PointType p1() const { return this->col(0); }
    PointType p2() const { return this->col(1); }

    /** Returns the vector formed by p2() - p1() */
    PointType delta() const { return p2() - p1(); }

    /** Returns the vector formed by (p2() + p1()) / 2.0 */
    PointType mid() const { return (p2() + p1()) / 2.0; }

    T length() const { return delta().norm(); }

    double normalisedDot(LineSegment<T,dim> other) const
    {
      return delta().normalized().dot( other.delta().normalized() );
    }

    /** Calculate the smallest angle between the two line segments.
     *
     * The output will be between 0 and PI/2, inclusive.
     */
    double smallestAngleBetween(LineSegment<T,dim> other)
    {
      auto a = fabs(acos(normalisedDot(other)));
      if (a >= M_PI/2)
        a = M_PI - a;
      return a;
    }

    template<int newDim>
    LineSegment<T,newDim> to() const
    {
      auto newLineSegment = LineSegment<T,newDim>{LineSegment<T,newDim>::Zero()};
      newLineSegment.col(0).template head< meta::min<dim,newDim>::value >() =
        this->col(0).template head< meta::min<dim,newDim>::value >();
      newLineSegment.col(1).template head< meta::min<dim,newDim>::value >() =
        this->col(1).template head< meta::min<dim,newDim>::value >();
      return newLineSegment;
    }

    bool operator==(LineSegment<T,dim> const& other) const
    {
      const double epsilon = 0.0000004;
      return (this->array() - other.array()).abs().sum() < epsilon;
    }

    LineSegment<T,dim> operator+(PointType const& delta) const
    {
      return LineSegment<T,dim>{this->colwise() + delta};
    }

    LineSegment<T,dim> operator-(PointType const& delta) const
    {
      return LineSegment<T,dim>{this->colwise() - delta};
    }

    friend std::ostream& operator<<(std::ostream& stream, LineSegment<T,dim> const& lineSegment)
    {
      return stream << "LineSegment (P1=" << lineSegment.p1().transpose() << " P2=" << lineSegment.p2().transpose() << ")";
    }

  };

  typedef LineSegment<double,3> LineSegment3d;
  typedef LineSegment<int,3> LineSegment3i;
}