// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab

#define EIGEN_DONT_VECTORIZE
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#include <Eigen/Eigen>
#include <Eigen/LU>
#include <iostream>
#include <fstream>
#include <cmath>
#include <limits>
#include <vector>
#include <map>

using namespace std;

static double inlierRatio(0.8);
static int restartCount(1);
static int generationCount(64);

double uniformRand()
{
	return double(rand())/RAND_MAX;
}

double gaussianRand(double mean, double sigm)
{
	// Generation using the Polar (Box-Mueller) method.
	// Code inspired by GSL, which is a really great math lib.
	// http://sources.redhat.com/gsl/
	// C++ wrapper available.
	// http://gslwrap.sourceforge.net/
	double r, x, y;

	// Generate random number in unity circle.
	do
	{
		x = uniformRand()*2 - 1;
		y = uniformRand()*2 - 1;
		r = x*x + y*y;
	}
	while (r > 1.0 || r == 0);

	// Box-Muller transform.
	return sigm * y * sqrt (-2.0 * log(r) / r) + mean;
}

struct TrainingEntry
{
	double timeStamp;
	Eigen::Vector3d odom_tr;
	Eigen::Quaterniond odom_rot;
	Eigen::Vector3d icp_tr;
	Eigen::Quaterniond icp_rot;

	TrainingEntry(){}
	TrainingEntry(std::istream& is)
	{
		is >> timeStamp;
		double t_x = 0, t_y = 0, t_z = 0, q_x = 0, q_y = 0, q_z = 0, q_w = 1;
		is >> t_x;
		is >> t_y;
		is >> t_z;
		icp_tr = Eigen::Vector3d(t_x, t_y, t_z);
		is >> q_x;
		is >> q_y;
		is >> q_z;
		is >> q_w;
		icp_rot = Eigen::Quaterniond(q_w, q_x, q_y, q_z).normalized();
		is >> t_x;
		is >> t_y;
		is >> t_z;
		odom_tr = Eigen::Vector3d(t_x, t_y, t_z);
		is >> q_x;
		is >> q_y;
		is >> q_z;
		is >> q_w;
		odom_rot = Eigen::Quaterniond(q_w, q_x, q_y, q_z).normalized();
		//odom_tr = odom_rot*odom_tr;
		//cerr << icp_rot.x() << " " << icp_rot.y() << " " << icp_rot.z() << " " << icp_rot.w() <<endl;
		//cerr << icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << endl;
		// FIXME: bug in Eigen ?
		//icp_tr = icp_rot*icp_tr;
		//cerr << icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << endl;
	}

	void dump(ostream& stream) const
	{
		stream <<
			timeStamp << " : " <<
			icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << " " <<
			icp_rot.x() << " " << icp_rot.y() << " " << icp_rot.z() << " " << icp_rot.w() << " " <<
			odom_tr.x() << " " << odom_tr.y() << " " << odom_tr.z() << " " <<
			odom_rot.x() << " " << odom_rot.y() << " " << odom_rot.z() << " " << odom_rot.w();
	}
};

struct TrainingSet: public std::vector<TrainingEntry>
{
	void dump()
	{
		for (TrainingSet::const_iterator it(begin()); it != end(); ++it)
		{
			const TrainingEntry& entry(*it);
			entry.dump(cout);
			cout << "\n";
		}
	}
};

TrainingSet trainingSet;

struct Params
{
	Eigen::Vector3d tr;
	Eigen::Quaterniond rot;

	Params():tr(0,0,0),rot(1,0,0,0) {}
	Params(const Eigen::Vector3d& _tr, const Eigen::Quaterniond& _rot):
		tr(_tr),
		rot(_rot)
		{}
	Params(const double transVariance):
		tr(
			gaussianRand(0, transVariance),
			gaussianRand(0, transVariance),
			gaussianRand(0, transVariance)
		),
		rot(
			Eigen::Quaterniond(1,0,0,0) *
			Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitX())) *
			Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitY())) *
			Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitZ()))
		)
		{}

	// add random noise
	const Params& mutate(const double amount = 1.0)
	{
		const double count = fabs(gaussianRand(0, 1));
		for (int i = 0; i < int(count + 1); i++)
		{
			const int toMutate = rand() % 6;
			switch (toMutate)
			{
				case 0: tr.x() += gaussianRand(0, 0.1) * amount; break;
				case 1: tr.y() += gaussianRand(0, 0.1) * amount; break;
				case 2: tr.z() += gaussianRand(0, 0.1) * amount; break;
				case 3: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitX())); break;
				case 4: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitY())); break;
				case 5: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitZ())); break;
				default: break;
			};
		}
		/*
		tr.x() += gaussianRand(0, 0.1) * amount;
		tr.y() += gaussianRand(0, 0.1) * amount;
		tr.z() += gaussianRand(0, 0.1) * amount;
		rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitX()));
		rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitY()));
		rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitZ()));
		*/
		return *this;
	}

	// renormalize quaternion
	void normalize()
	{
		rot.normalize();
		if (rot.x() < 0)
		{
			rot.x() = -rot.x();
			rot.y() = -rot.y();
			rot.z() = -rot.z();
			rot.w() = -rot.w();
		}
	}

	// dump content of this transform
	void dumpTransform(ostream& stream) const
	{
		stream << tr.x() << " " << tr.y() << " " << tr.z() << " " <<
				rot.x() << " " << rot.y() << " " << rot.z() << " " << rot.w();
	}

	// dump what to copy/paste
	void dumpCopyPaste(ostream& stream) const
	{
		/*stream <<
			"translation: x=" << tr.x() << " y=" << tr.y() << " z=" << tr.z() << " " <<
			"quaternion: x=" << rot.x() << " y=" << rot.y() << " z=" << rot.z() << " w=" << rot.w();
		*/
		stream << "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"base_link_to_kinect\" args=\"";
		dumpTransform(stream);
		stream << " /base_link /kinect 100\"/>"
		" \n\n OR \n\n"
		"rosrun tf static_transform_publisher ";
		dumpTransform(stream);
		stream << " /base_link /kinect 100\n";
	}
};

// we have two typedefs for the same physical object because they are conceptually different
typedef vector<Params> Genome;
typedef vector<Params> ParamsVector;

void normalizeParams(ParamsVector& params)
{
	for (size_t i = 0; i < params.size(); ++i)
		params[i].normalize();
}

void dumpParamsStats(ostream& stream, const ParamsVector& params)
{
	Eigen::Vector3d trMean(0,0,0);
	Eigen::Quaterniond::Coefficients rotMean(0,0,0,0);
	for (size_t i = 0; i < params.size(); ++i)
	{
		trMean += params[i].tr;
		rotMean += params[i].rot.coeffs();
	}
	trMean /= double(params.size());
	rotMean /= double(params.size());

	Params mean(trMean,Eigen::Quaterniond(rotMean));
	mean.normalize();
	mean.dumpCopyPaste(stream); stream << "\n";

	Eigen::Vector3d trVar(0,0,0);
	Eigen::Quaterniond::Coefficients rotVar(0,0,0,0);
	for (size_t i = 0; i < params.size(); ++i)
	{
		trVar += (params[i].tr - trMean).cwiseProduct(params[i].tr - trMean);
		rotVar += (params[i].rot.coeffs() - rotMean).cwiseProduct(params[i].rot.coeffs() - rotMean);
	}
	trVar /= double(params.size());
	rotVar /= double(params.size());

	stream << "\nVariance:\n";
	Params var(trVar,Eigen::Quaterniond(rotVar));
	var.dumpTransform(stream); stream << "\n";

	stream << "\nValues:\n";
	for (size_t i = 0; i < params.size(); ++i)
	{
		params[i].dumpTransform(stream); stream << "\n";
	}
	stream << endl;
}

double computeError(const Params& p, const TrainingEntry& e)
{
	/*
	// version with Eigen::Matrix4d
	Eigen::Matrix4d blk(Eigen::Matrix4d::Identity());
	blk.corner(Eigen::TopLeft,3,3) = p.rot.toRotationMatrix();
	blk.corner(Eigen::TopRight,3,1) = p.tr;

	Eigen::Matrix4d odom(Eigen::Matrix4d::Identity());
	odom.corner(Eigen::TopLeft,3,3) = e.odom_rot.toRotationMatrix();
	odom.corner(Eigen::TopRight,3,1) = e.odom_tr;

	Eigen::Matrix4d blk_i(blk.inverse());

	//const Eigen::Matrix4d pred_icp = blk * odom * blk_i;
	const Eigen::Matrix4d pred_icp = blk_i * odom * blk;

	const Eigen::Matrix3d pred_icp_rot_m = pred_icp.corner(Eigen::TopLeft,3,3);
	const Eigen::Quaterniond pred_icp_rot = Eigen::Quaterniond(pred_icp_rot_m);
	const Eigen::Vector3d pred_icp_tr = pred_icp.corner(Eigen::TopRight,3,1);
	*/

	// version with Eigen::Transform3d
	typedef Eigen::Transform<double, 3, Eigen::Affine> Transform3d;
	typedef Eigen::Translation<double, 3> Translation3d;

	const Transform3d blk = Translation3d(p.tr) * p.rot;
	const Transform3d blk_i = Transform3d(blk.inverse(Eigen::Isometry));
	const Transform3d odom = Translation3d(e.odom_tr) * e.odom_rot;
	//const Eigen::Transform3d pred_icp = blk * odom * blk_i;
	const Transform3d pred_icp = blk_i * odom * blk;

	const Eigen::Matrix3d pred_icp_rot_m = pred_icp.matrix().topLeftCorner(3,3);
	const Eigen::Quaterniond pred_icp_rot = Eigen::Quaterniond(pred_icp_rot_m);
	const Eigen::Vector3d pred_icp_tr = pred_icp.translation();

	// identity checked ok
	//cerr << "mat:\n" << blk.matrix() * blk_i.matrix() << endl;

	//cout << "dist: " << (e.icp_tr - pred_icp.translation()).norm() << endl;
	//cout << "ang dist: " << e.icp_rot.angularDistance(pred_icp_rot) << endl;
	//cout << "tr pred icp:\n" << pred_icp.translation() << "\nicp:\n" << e.icp_tr << "\n" << endl;
	//cout << "rot pred icp:\n" << pred_icp_rot << "\nicp:\n" << e.icp_rot << "\n" << endl;
	// FIXME: tune coefficient for rot vs trans

	const double e_tr((e.icp_tr - pred_icp_tr).norm());
	const double e_rot(e.icp_rot.angularDistance(pred_icp_rot));
	/*if (e_tr < 0)
		abort();
	if (e_rot < 0)
		abort();*/
	//cerr << e_tr << " " << e_rot << endl;
	return e_tr + e_rot;
}

// compute errors with outlier rejection
double computeError(const Params& p)
{
	vector<double> errors;
	errors.reserve(trainingSet.size());
	for (TrainingSet::const_iterator it(trainingSet.begin()); it != trainingSet.end(); ++it)
	{
		const TrainingEntry& entry(*it);
		errors.push_back(computeError(p, entry));
	}
	sort(errors.begin(), errors.end());
	const size_t inlierCount(errors.size() * inlierRatio);
	double error = 0;
	for (size_t i = 0; i < inlierCount; ++i)
		error += errors[i];
	return error;
}

double evolveOneGen(Genome& genome, double annealing = 1.0, Params* bestParams = 0)
{
	typedef multimap<double, Params> EvaluationMap;
	typedef EvaluationMap::iterator EvaluationMapIterator;
	EvaluationMap evalutationMap;

	double totalError = 0;
	double bestError = numeric_limits<double>::max();
	int bestInd = 0;
	for (size_t ind = 0; ind < genome.size(); ind++)
	{
		const double error = computeError(genome[ind]);
		if (error < bestError)
		{
			bestError = error;
			bestInd = ind;
		}

		totalError += error;
		evalutationMap.insert(make_pair(error, genome[ind]));

		/*cout << "E " << ind << " : ";
		genome[ind].dump(cout);
		cout << " = " << error << "\n";*/
	}

	if (bestParams)
		*bestParams = genome[bestInd];

	assert((genome.size() / 4) * 4 == genome.size());

	size_t ind = 0;
	for (EvaluationMapIterator it = evalutationMap.begin(); ind < genome.size() / 4; ++it, ++ind )
	{
		//cout << "S " << it->first << "\n";
		genome[ind * 4] = it->second;
		genome[ind * 4 + 1] = it->second.mutate(annealing);
		genome[ind * 4 + 2] = it->second.mutate(annealing);
		genome[ind * 4 + 3] = it->second.mutate(annealing);
	}

	return bestError;
}

int main(int argc, char** argv)
{
	srand(time(0));

	if (argc < 2)
	{
		cerr << "Usage " << argv[0] << " LOG_FILE_NAME [INLIER_RATIO] [RESTART_COUNT] [GEN_COUNT]\n";
		cerr << "  LOG_FILE_NAME   name of file to load containing delta tf\n";
		cerr << "  INLIER_RATIO    ratio of inlier to use for error computation (range: ]0:1], default: 0.8)\n";
		cerr << "  RESTART_COUNT   number of random restart (default: 1)\n";
		cerr << "  GEN_COUNT       number of generation for the ES (default: 64)\n";
		cerr << endl;
		return 1;
	}
	if (argc >= 3)
	{
		inlierRatio = atof(argv[2]);
		if (inlierRatio <= 0 || inlierRatio > 1)
		{
			cerr << "Invalid inline ratio: " << inlierRatio << ", must be within range ]0:1]" << endl;
			return 2;
		}
	}
	cout << "Inlier ratio: " << inlierRatio << endl;
	if (argc >= 4)
	{
		restartCount = atoi(argv[3]);
		if (restartCount < 1)
		{
			cerr << "Invalid restart count: " << restartCount << ", must be greater than 0" << endl;
			return 3;
		}
	}
	cout << "Restart count: " << restartCount << endl;
	if (argc >= 5)
	{
		generationCount = atoi(argv[4]);
		if (generationCount < 1)
		{
			cerr << "Invalid generation count: " << generationCount << ", must be greater than 0" << endl;
			return 4;
		}
	}
	cout << "Generation count: " << generationCount << endl;

	ifstream ifs(argv[1]);
	while (ifs.good())
	{
		TrainingEntry e(ifs);
		if (ifs.good())
		{
			trainingSet.push_back(e);
			//e.dump(cerr); cerr << endl;
		}
	}
	cout << "Loaded " << trainingSet.size() << " training entries" << endl;
	//trainingSet.dump();

	ParamsVector bests;
	for (int i = 0; i < restartCount; ++i)
	{
		cout << "Starting " << i << " restart on " << restartCount << endl;
		Genome genome(1024);
		for (size_t g = 0; g < genome.size(); ++g)
			genome[g] = Params(0.5);
		for (int g = 0; g < generationCount; ++g)
		{
			cout << "\r" << g << " best has error " << evolveOneGen(genome, 2. * (double)(generationCount - g) / (double)(generationCount)) << "            ";
			cout.flush();
		}
		Params best;
		cout << "\r  Best error of restart " << i << ": " << evolveOneGen(genome, 1.0, &best) << endl;
		bests.push_back(best);
	}

	cout << "\nOptimization completed, code to COPY-PASTE in to use the transformation:\n\n";
	if (restartCount == 1)
	{
		bests[0].dumpCopyPaste(cout);
	}
	else
	{
		normalizeParams(bests);
		dumpParamsStats(cout, bests);
	}

	return 0;
}


#include "stochasticInput.hpp"
#include <boost/scoped_ptr.hpp>
#include <cmath>
#include "ClpBALPInterface.hpp"
#include "ClpRecourseSolver.hpp"
#include "rawInput.hpp"

using namespace std;
using boost::scoped_ptr;

template <typename BALPSolver> void ucCoefficientDiving(stochasticInput &input, const std::string &LPBasis,
	const vector<int> &fixed,
	MPI_Comm comm = MPI_COMM_WORLD) {

	using namespace std;

	BAContext ctx(comm);
	ctx.initializeAssignment(input.nScenarios());

	int nvar1 = input.nFirstStageVars();

	BALPSolver solver(input, ctx, BALPSolver::useDual);
	solver.loadStatus(LPBasis);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);
	for (unsigned i = 0; i < fixed.size(); i++) {
		solver.setFirstStageColLB(fixed[i],1.0);
	}
	solver.go();

	int nIter = 0;
	while (true) {
		const vector<double>& firstSol = solver.getFirstStagePrimalColSolution();
		int atZero = 0, atOne = 0, fractional = 0;
		double bestDist = 1.;
		int bestIdx = -1;
		for (int i = 0; i < nvar1; i++) {
			if (fabs(firstSol[i]-0.0) < 1e-5) {
				atZero++;
			} else if (fabs(firstSol[i]-1.0) < 1e-5) {
				atOne++;
			} else {
				assert(solver.getFirstStageColState(i) == Basic);
				fractional++;
				if (fabs(firstSol[i]-1.0) < bestDist) {
					bestDist = fabs(firstSol[i]-1.0);
					bestIdx = i;
				}
			}
		}
		printf("Iteration %d\n",nIter++);
		printf("%d at zero, %d at one, %d fractional\n",atZero,atOne,fractional);
		if (fractional == 0) break;
		printf("Candidate for diving: %d %f\n",bestIdx,firstSol.at(bestIdx));

		solver.setFirstStageColLB(bestIdx,1.0);
		solver.go();
		assert(solver.getStatus() == Optimal);

	}
	printf("Objective from coefficient diving: %f\n", solver.getObjective());

}

template <typename BALPSolver> void ucFractionalDiving(stochasticInput &input, const std::string &LPBasis,
	const vector<int> &fixed,
	MPI_Comm comm = MPI_COMM_WORLD) {

	using namespace std;

	BAContext ctx(comm);
	ctx.initializeAssignment(input.nScenarios());

	int nvar1 = input.nFirstStageVars();

	BALPSolver solver(input, ctx, BALPSolver::useDual);
	solver.loadStatus(LPBasis);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);
	solver.setDualObjectiveLimit(1e9);
	for (unsigned i = 0; i < fixed.size(); i++) {
		solver.setFirstStageColLB(fixed[i],1.0);
	}
	solver.go();

	int nIter = 0;
	while (true) {
		const vector<double>& firstSol = solver.getFirstStagePrimalColSolution();
		int atZero = 0, atOne = 0, fractional = 0;
		double bestDist = 1.;
		int bestIdx = -1;
		for (int i = 0; i < nvar1; i++) {
			if (fabs(firstSol[i]-0.0) < 1e-5) {
				atZero++;
			} else if (fabs(firstSol[i]-1.0) < 1e-5) {
				atOne++;
			} else {
				assert(solver.getFirstStageColState(i) == Basic);
				fractional++;
				double fractionality = fabs(firstSol[i]-floor(firstSol[i]+0.5));
				if (fractionality < bestDist) {
					bestDist = fractionality;
					bestIdx = i;
				}
			}
		}
		printf("Iteration %d\n",nIter++);
		printf("%d at zero, %d at one, %d fractional\n",atZero,atOne,fractional);
		if (fractional == 0) break;
		printf("Candidate for diving: %d %f\n",bestIdx,firstSol.at(bestIdx));

		if (firstSol[bestIdx] < 0.5) {
			solver.setFirstStageColUB(bestIdx,0.0);
		} else {
			solver.setFirstStageColLB(bestIdx,1.0);
		}
		solver.go();
		if (solver.getStatus() == ProvenInfeasible) {
			assert(firstSol[bestIdx] < 0.5);
			solver.setFirstStageColUB(bestIdx,1.0);
			solver.setFirstStageColLB(bestIdx,1.0);
			solver.go();
		}
		assert(solver.getStatus() == Optimal);

	}
	printf("Objective from fractional diving: %f\n", solver.getObjective());

}

template <typename BALPSolver, typename RecourseSolver> void ucRounding(stochasticInput &input, const std::string &LPBasis,
	const vector<int> &fixed,
	MPI_Comm comm = MPI_COMM_WORLD) {

	using namespace std;

	BAContext ctx(comm);
	ctx.initializeAssignment(input.nScenarios());

	int nvar1 = input.nFirstStageVars();

	BALPSolver solver(input, ctx, BALPSolver::useDual);
	solver.loadStatus(LPBasis);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);

	for (unsigned i = 0; i < fixed.size(); i++) {
		solver.setFirstStageColLB(fixed[i],1.0);
	}
	solver.go();

	vector<double> firstSol = solver.getFirstStagePrimalColSolution();
	for (int i = 0; i < nvar1; i++) {
		if (firstSol[i] < 0.01) {
			firstSol[i] = 0.0;
		} else {
			firstSol[i] = 1.0;
		}
		/*
		if (fabs(firstSol[i]-0.0) < fabs(firstSol[i]-1.0)) {
			firstSol[i] = 0.0;
		} else {
			firstSol[i] = 1.0;
		}*/
	}

	int nvar2 = input.nSecondStageVars(0);
	int nscen = input.nScenarios();
	int ncons2 = input.nSecondStageCons(0);

	const vector<double> &obj1 = input.getFirstStageObj();

	vector<variableState> rowSave(ncons2), colSave(nvar2);
	bool havesave = false;
	double sum = 0.0;
	bool infeas = false;
	for (int k = 0; k < nvar1; k++) sum += firstSol[k]*obj1[k];
	for (int scen = 0; scen < nscen; scen++) {
		RecourseSolver rsol(input, scen, firstSol);
		rsol.setDualObjectiveLimit(1e7);

		if (havesave) {
			for (int r = 0; r < nvar2; r++) {
				rsol.setSecondStageColState(r,colSave[r]);
			}
			for (int r = 0; r < ncons2; r++) {
				rsol.setSecondStageRowState(r,rowSave[r]);
			}
		}
		rsol.go();
		sum += rsol.getObjective();

		if (rsol.getStatus() == ProvenInfeasible) {
			printf("got infeasible 1st stage\n");
			sum = COIN_DBL_MAX;
			infeas = true; break;
		}
		assert(rsol.getStatus() == Optimal);
		if (!havesave) {
			for (int r = 0; r < nvar2; r++) {
				colSave[r] = rsol.getSecondStageColState(r);
			}
			for (int r = 0; r < ncons2; r++) {
				rowSave[r] = rsol.getSecondStageRowState(r);
			}
			havesave = true;
		}

	}

	printf("Rounding solution: %f\n", sum);

}

// improve the lower bound by fixing generators to on if they are infeasible off
template <typename BALPSolver> void ucFakeStrongBranching(stochasticInput &input, const std::string &LPBasis,
	MPI_Comm comm = MPI_COMM_WORLD) {

	using namespace std;

	BAContext ctx(comm);
	ctx.initializeAssignment(input.nScenarios());

	int nvar1 = input.nFirstStageVars();

	BALPSolver solver(input, ctx, BALPSolver::useDual);
	solver.loadStatus(LPBasis);
	solver.setPrimalTolerance(1e-6);
	solver.setDualTolerance(1e-6);
	solver.go();

	solver.setDualObjectiveLimit(10.*solver.getObjective());

	BADimensions dims(input,ctx);
	BADimensionsSlacks dimsSlacks(dims);
	BAFlagVector<variableState> states;
	states.allocate(dimsSlacks,ctx, PrimalVector);
	solver.getStates(states);

	const vector<double>& firstSol = solver.getFirstStagePrimalColSolution();
	vector<int> fixed;
	for (int i = 0; i < nvar1; i++) {
		if (fabs(firstSol[i]-0.0) < 1e-5) {
		} else if (fabs(firstSol[i]-1.0) < 1e-5) {
		} else {
			solver.setFirstStageColUB(i,0.0);
			solver.go();

			solver.setFirstStageColUB(i,1.0);
			solver.setStates(states);
			if (solver.getStatus() == ProvenInfeasible) {
				solver.setFirstStageColLB(i,1.0);
				fixed.push_back(i);
				solver.go();
				solver.getStates(states);
				printf("New Lower Bound: %f\n", solver.getObjective());
			} else {
			}
		}
	}
	cout << fixed.size() << " fixed at UB\n";
	for (unsigned i = 0; i < fixed.size(); i++) {
		printf("%d,",fixed[i]);
	}
	printf("\n");

}

// 194 fixed
int fixed4h10s[] = {36,63,64,93,96,116,117,120,121,122,123,143,144,145,147,151,157,158,159,164,167,168,169,170,171,172,191,192,193,194,195,196,197,198,204,205,206,207,212,213,214,215,218,219,220,221,238,241,257,296,297,298,299,325,354,355,357,377,378,381,382,383,384,408,412,418,419,420,424,425,428,429,430,431,432,433,452,453,454,455,456,457,458,459,465,466,467,468,473,474,475,476,479,480,481,482,499,518,557,558,559,560,586,615,616,618,638,639,642,643,644,645,669,673,679,680,681,685,686,689,690,691,692,693,694,713,714,715,716,717,718,719,720,726,727,728,729,734,735,736,737,740,741,742,743,760,779,818,819,820,821,847,876,879,899,900,903,904,905,906,930,934,940,941,942,946,947,950,951,952,953,954,955,974,975,976,977,978,979,980,981,987,988,989,990,995,996,997,998,1001,1002,1003,1004,1040};

class fakeIntegerWrapper : public rawInput {
public:
	fakeIntegerWrapper(const std::string &datarootname, int overrideScenarioNumber = 0, MPI_Comm comm = MPI_COMM_WORLD) :
		rawInput(datarootname, overrideScenarioNumber, comm) {}

	virtual bool isFirstStageColInteger(int col) { return true; }

};

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

	MPI_Init(&argc, &argv);

	int mype;
	MPI_Comm_rank(MPI_COMM_WORLD,&mype);

	if (argc != 4) {
		if (mype == 0) printf("Usage: %s [rawdump root name] [num scenarios] [LP relaxation basis]\n",argv[0]);
		return 1;
	}

	string datarootname(argv[1]);
	int nscen = atoi(argv[2]);
	string LPBasis(argv[3]);

	if (mype == 0) printf("Initializing data interface\n");
	scoped_ptr<fakeIntegerWrapper> s(new fakeIntegerWrapper(datarootname,nscen));

	//ucFakeStrongBranching<ClpBALPInterface>(*s,LPBasis);

	//ucCoefficientDiving<ClpBALPInterface>(*s,LPBasis, vector<int>(fixed4h10s,fixed4h10s+194));
	//ucFractionalDiving<ClpBALPInterface>(*s,LPBasis, vector<int>(fixed4h10s,fixed4h10s+194));
	ucRounding<ClpBALPInterface,ClpRecourseSolver>(*s,LPBasis, vector<int>(fixed4h10s,fixed4h10s+194));

	MPI_Finalize();

	return 0;

}


/*  $Id: ballcutter.cpp 680 2011-03-23 20:26:43Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "ballcutter.h"
#include "numeric.h"

namespace ocl
{

BallCutter::BallCutter() {
    std::cout << " usage: BallCutter( double diameter, double length )\n";
    assert(0);
}

BallCutter::BallCutter(double d, double l) {
    diameter = d;               assert( diameter>0.0 );
    radius = d/2.0;
    length = l;                 assert( length>0.0 );
    normal_length = radius;
    xy_normal_length = 0.0;
    center_height = radius;
}

// drop-cutter methods: vertex and facet are handled in base-class

// drop-cutter edgeDrop
CC_CLZ_Pair BallCutter::singleEdgeDropCanonical(const Point& u1, const Point& u2) const {
    // the plane of the line will slice the spherical cutter at
    // a distance d==u1.y==u2.y from the center of the cutter
    // here the radius of the circular section is s:
    double s = sqrt( square(radius) - square( u1.y ) );
    Point normal(u2.z - u1.z, -(u2.x - u1.x), 0); // (dz, -du) is a normal to the line
    normal.xyNormalize();
    if (normal.y < 0)  // flip normal so it points upward
        normal = -1*normal;
    Point cc( -s*normal.x, u1.y, 0); // from (0,u1.y,0) we go a distance -s in the normal direction
    cc.z_projectOntoEdge(u1,u2);
    double cl_z = cc.z + s*normal.y - radius;
    return CC_CLZ_Pair( cc.x , cl_z);
}

// push-cutter: vertex and facet handled in base-class
bool BallCutter::generalEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    bool result = false;
    if ( isZero_tol( (p2-p1).xyNorm() ) ) { // this would be a vertical edge
        return result;
    }

    const Point ufp1 = f.p1 + Point(0,0,radius); // take a fiber which is raised up by radius
    const Point ufp2 = f.p2 + Point(0,0,radius); // and intersect it with a cylinder around the edge p1-p2
    // Ray : P(t) = O + t*V    from point O, in direction V
    // Cylinder [A, B, r]   from point A to point B, radius r
    // Point P on infinite cylinder if ((P - A) x (B - A))^2 = r^2 * (B - A)^2
    // expand : ((O - A) x (B - A) + t * (V x (B - A)))^2 = r^2 * (B - A)^2
    // equation in the form (X + t * Y)^2 = d , where:
    //  X = (O - A) x (B - A)
    //  Y = V x (B - A)
    //  d = r^2 * (B - A)^2
    // expand the equation :
    // t^2 * (Y . Y) + t * (2 * (X . Y)) + (X . X) - d = 0
    // => second order equation in the form : a*t^2 + b*t + c = 0 where
    // a = (Y . Y)
    // b = 2 * (X . Y)
    // c = (X . X) - d
    Point ab = p2-p1;                           // axis of the cylinder
    Point ao = (ufp1 - p1);                     // cyl start to ray start
    Point ao_x_ab = ao.cross(ab);               // cross product
    Point v_x_ab  = (ufp2-ufp1).cross(ab);      // cross product
    double ab2 = ab.dot(ab);                    // dot product
    double a = v_x_ab.dot(v_x_ab);              // dot product
    double b = 2 * ( v_x_ab.dot(ao_x_ab) );     // dot product
    double c = ao_x_ab.dot(ao_x_ab) - (radius*radius * ab2);
    // solve second order equation : a*t^2 + b*t + c = 0
    // t = (-b +/- sqrt( b^2 - 4ac ) )   / 2a
    double discr = b*b-4*a*c;
    double t1, t2;
    if ( isZero_tol( discr ) ) { // tangent case, only one root
        t1= -b/(2*a);
        if ( calcCCandUpdateInterval( t1, p1, p2, f, i ) )
            result = true;
    } else if ( discr > 0.0 ) { // two roots
        t1 = (-b + sqrt( discr))/(2*a);
        t2 = (-b - sqrt( discr))/(2*a);
        if ( calcCCandUpdateInterval( t1, p1, p2, f, i ) )
            result = true;
        if ( calcCCandUpdateInterval( t2, p1, p2, f, i ) )
            result = true;
    }
    return result;
}

// t is a position along the fiber
// p1-p2 is the edge
// Interval& i is updated
bool BallCutter::calcCCandUpdateInterval( double t, const Point& p1, const Point& p2, const Fiber& f, Interval& i) const {
    Point cl_center = f.point(t) + Point(0,0,radius); // center of ball is here
    CCPoint cc_tmp = cl_center.closestPoint(p1,p2); // cc-point on the edge, point on edge closest to center
    cc_tmp.type = EDGE_BALL;
    // require contact with lower hemishphere
    return i.update_ifCCinEdgeAndTrue( t, cc_tmp, p1, p2, ((cl_center-cc_tmp).z >=0) );
}

std::string BallCutter::str() const {
    std::ostringstream o;
    o << *this;
    return o.str();
}

std::ostream& operator<<(std::ostream &stream, BallCutter c) {
  stream << "BallCutter(d=" << c.diameter << ", r=" << c.radius << ", L=" << c.length << ")";
  return stream;
}

} // end namespace
// end file ballcutter.cpp


#ifndef NEPOMUK_CONTAINER_SEARCH_HEAP_HPP_
#define NEPOMUK_CONTAINER_SEARCH_HEAP_HPP_

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#include <boost/heap/d_ary_heap.hpp>
#pragma GCC diagnostic pop

#include <boost/assert.hpp>

#include <unordered_map>
#include <utility>

namespace nepomuk
{
namespace tool
{
namespace container
{

// a wrapper for the boost d-ary heap that allows decrase key operations to implement variations of
// Dijkstras algorithm
template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
class KAryHeap
{
    // define how to identify
    using HeapEntry = std::pair<key_type_t, value_type_t>;
    using HeapContainer = boost::heap::d_ary_heap<HeapEntry,
                                                  boost::heap::arity<arity>,
                                                  boost::heap::mutable_<true>,
                                                  boost::heap::compare<std::greater<HeapEntry>>>;
    // access into the heap to allow decreasing keys
    using HeapHandle = typename HeapContainer::handle_type;

    struct DataByValue
    {
        key_type_t key;
        HeapHandle handle;
        value_meta_data meta_data;
    };
    using DataContainer = std::unordered_map<value_type_t, DataByValue>;

  public:
    using key_type = key_type_t;
    using value_type = value_type_t;

    KAryHeap() {}

    void clear();

    bool empty() const;

    void insert(value_type const value, key_type const key, value_meta_data data);

    // mark element reached, but don't add it to the heap. Can be used to also decrease its key
    void reach_non_inserting(value_type const value, key_type const key, value_meta_data data);

    void decrease_key(value_type const value, key_type const key, value_meta_data data);

    bool contains(value_type const value) const;

    value_meta_data const &data(value_type const value) const;

    bool reached(const value_type value) const;

    key_type const &key(value_type value) const;

    key_type min_key() const;

    value_type min() const;

    value_type delete_min();

  private:
    DataContainer container;
    HeapContainer heap;
};

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
void KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::clear()
{
    heap.clear();
    container.clear();
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
bool KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::empty() const
{
    return heap.empty();
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
void KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::insert(value_type const value,
                                                                        key_type const key,
                                                                        value_meta_data data)
{
    auto handle = heap.push(std::make_pair(key, value));
    container[value] = {key, handle, std::move(data)};
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
void KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::reach_non_inserting(
    value_type const value, key_type const key, value_meta_data data)
{
    // add to the reached data, but not into the heap (non-transferring stop)
    container[value] = {key, HeapHandle(), std::move(data)};
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
void KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::decrease_key(
    value_type const value, key_type const key, value_meta_data data)
{
    auto reference = container.find(value);
    heap.increase(reference->second.handle, std::make_pair(key, value));
    reference->second.key = key;
    reference->second.meta_data = data;
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
bool KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::contains(
    value_type const value) const
{
    return container.find(value) != container.end() &&
           container.find(value)->second.handle != HeapHandle();
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
value_meta_data const &
KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::data(value_type const value) const
{
    BOOST_ASSERT(container.find(value) != container.end());
    return container.find(value)->second.meta_data;
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
bool KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::reached(
    const value_type value) const
{
    return container.find(value) != container.end();
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
key_type_t const &
KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::key(value_type_t value) const
{
    return container.find(value)->second.key;
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
key_type_t KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::min_key() const
{
    BOOST_ASSERT(!heap.empty());
    return heap.top().first;
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
value_type_t KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::min() const
{
    BOOST_ASSERT(!heap.empty());
    return heap.top().second;
}

template <typename value_type_t, typename key_type_t, int arity, typename value_meta_data>
value_type_t KAryHeap<value_type_t, key_type_t, arity, value_meta_data>::delete_min()
{
    BOOST_ASSERT(!heap.empty());

    auto const value = heap.top().second;
    heap.pop();
    container[value].handle = HeapHandle();

    return value;
}

} // namespace container
} // namespace tool
} // namespace nepomuk

#endif // NEPOMUK_CONTAINER_SEARCH_HEAP_HPP_


#include <vector>
#include <algorithm>
#include <fstream>
#include <boost/gil/image.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/gil/color_convert.hpp>
#include <boost/gil/extension/io/jpeg_io.hpp>
using namespace std;
using namespace boost;
using namespace boost::gil;

template <typename GrayView, typename R>
void gray_image_hist(const GrayView &img_view, R &hist)
{
    //    for_each_pixel(img_view,++lambda::var(hist)[lambda::_1]);
    for (typename GrayView::iterator it = img_view.begin(); it != img_view.end(); ++it)
        ++hist[*it];
}

template <typename V, typename R>
void get_hist(const V &img_view, R &hist)
{
    gray_image_hist(color_converted_view<gray8_pixel_t>(img_view), hist);
}

int main()
{
    rgb8_image_t img;
    jpeg_read_image("my.jpg", img);

    int histogram[256];
    std::fill(histogram, histogram + 256, 0);
    get_hist(const_view(img), histogram);

    std::fstream histo_file("out-histogram.txt", std::ios::out);
    for (std::size_t ii = 0; ii < 256; ++ii)
        histo_file << histogram[ii] << std::endl;
    histo_file.close();

    return 0;
}


// Copyright 2019 Erik Teichmann <kontakt.teichmann@gmail.com>

#ifndef INCLUDE_SAM_SYSTEM_RK4_SYSTEM_HPP_
#define INCLUDE_SAM_SYSTEM_RK4_SYSTEM_HPP_

#include <vector>

#include <boost/numeric/odeint/integrate/check_adapter.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>

#include "./generic_system.hpp"

namespace sam {

/*! \brief A system that is integrated with a 4th order Runge-Kutta method.
 *
 * A system that is integrated with a Runge-Kutta method of 4th order. The
 * integrator is implemented by the odeint library in boost
 * (https://www.boost.org/doc/libs/1_72_0/libs/numeric/odeint/doc/html/index.html).
 */
template<typename ODE, typename state_type = std::vector<double>>
class RK4System: public GenericSystem<ODE, state_type> {
 public:
  template<typename... Ts>
  explicit RK4System(unsigned int system_size, unsigned int dimension,
                     Ts... parameters);

  template<typename observer_type = boost::numeric::odeint::null_observer>
  void Integrate(double dt, unsigned int number_steps,
                 observer_type observer
                     = boost::numeric::odeint::null_observer());

 private:
  boost::numeric::odeint::runge_kutta4<state_type> stepper_;
};

// Implementation

template<typename ODE, typename state_type>
template<typename... Ts>
RK4System<ODE, state_type>::RK4System(unsigned int system_size,
                                      unsigned int dimension,
                                      Ts... parameters)
    : GenericSystem<ODE, state_type>(system_size, dimension, parameters...) {}

template<typename ODE, typename state_type>
template<typename observer_type>
void RK4System<ODE, state_type>::Integrate(double dt, unsigned int number_steps,
                                           observer_type observer) {
      this->t_ = boost::numeric::odeint::integrate_n_steps(
          stepper_, *(this->ode_), this->x_, this->t_, dt, number_steps,
          observer);
}

}  // namespace sam

#endif  // INCLUDE_SAM_SYSTEM_RK4_SYSTEM_HPP_


/*
 * Copyright 2009-2011 The VOTCA Development Team (http://www.votca.org)
 *
 * 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 <math.h>
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <sstream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <votca/tools/cubicspline.h>
#include <votca/csg/nblistgrid.h>
#include <votca/csg/beadlist.h>
#include "csg_fmatch.h"
#include <votca/tools/table.h>
#include <votca/tools/linalg.h>

int main(int argc, char** argv)
{
    CGForceMatching app;
    return app.Exec(argc, argv);
}

void CGForceMatching::Initialize(void)
{
    CsgApplication::Initialize();
    AddProgramOptions()
        ("options", boost::program_options::value<string>(), "  options file for coarse graining")
        ("trj-force", boost::program_options::value<string>(), "  coarse-grained trajectory containing forces of already known interactions");
}

bool CGForceMatching::EvaluateOptions()
{
    CsgApplication::EvaluateOptions();
    CheckRequired("trj", "no trajectory file specified");
    CheckRequired("options", "need to specify options file");
    LoadOptions(OptionsMap()["options"].as<string>());

    _has_existing_forces = false;
    if(OptionsMap().count("trj-force"))
        _has_existing_forces = true;
    return true;
}

void CGForceMatching::BeginEvaluate(Topology *top, Topology *top_atom)
{
    // set counters to zero value:
    _nblocks = 0;
    _line_cntr = _col_cntr = 0;

    // Number of CG beads in topology
    _nbeads = top->BeadCount();
    // Set frame counter to zero
    _frame_counter = 0;

    // read _nframes from input file
    _nframes = _options.get("cg.fmatch.frames_per_block").as<int>();
    // read _constr_least_sq from input file
    _constr_least_sq = _options.get("cg.fmatch.constrainedLS").as<bool>();

    // initializing bonded interactions
    for (list<Property*>::iterator iter = _bonded.begin();
            iter != _bonded.end(); ++iter) {
        SplineInfo *i = new SplineInfo(_splines.size(), true, _col_cntr, *iter);
        //adjust initial matrix dimensions:
        _line_cntr += i->num_gridpoints;
        _col_cntr += 2 * i->num_gridpoints;

        // add spline to container
        _splines.push_back(i);
    }

    // initializing non-bonded interactions
    for (list<Property*>::iterator iter = _nonbonded.begin();
            iter != _nonbonded.end(); ++iter) {
        SplineInfo *i = new SplineInfo(_splines.size(), false, _col_cntr, *iter);
        //adjust initial matrix dimensions:
        _line_cntr += i->num_gridpoints;
        _col_cntr += 2 * i->num_gridpoints;

        // add spline to container
        _splines.push_back(i);
    }

    cout << "\nYou are using VOTCA!\n";
    cout << "\nhey, somebody wants to forcematch!\n";

    // now initialize _A, _b, _x and probably _B_constr
    // depending on least-squares algorithm used
    if (_constr_least_sq) { // Constrained Least Squares

        cout << "\nUsing constrained Least Squares!\n " << endl;

        // assign _least_sq_offset
        _least_sq_offset = 0;

        // resize and clear _B_constr
        _B_constr.resize(_line_cntr, _col_cntr, false);
        _B_constr.clear();

        // resize matrix _A
        _A.resize(3 * _nbeads *_nframes, _col_cntr, false);
        // resize vector _b
        _b.resize(3 * _nbeads *_nframes, false);

        // in case of constrained least squares smoothing conditions
        // are assigned to matrix _B_constr
        FmatchAssignSmoothCondsToMatrix(_B_constr);
    } else { // Simple Least Squares

        cout << "\nUsing simple Least Squares! " << endl;
        // assign _least_sq_offset
        _least_sq_offset = _line_cntr;

        // resize matrix _A
        _A.resize(_line_cntr + 3 * _nbeads *_nframes, _col_cntr, false);
        // resize vector _b
        _b.resize(_line_cntr + 3 * _nbeads *_nframes, false);

        // in case of simple least squares smoothing conditions
        // are assigned to matrix _A
        FmatchAssignSmoothCondsToMatrix(_A);
        // clear _b (only necessary in simple least squares)
        _b.clear();
    }
    // resize and clear _x
    _x.resize(_col_cntr);
    _x.clear();

    if(_has_existing_forces) {
        _top_force.CopyTopologyData(top);
        _trjreader_force = TrjReaderFactory().Create(_op_vm["trj-force"].as<string>());
        if(_trjreader_force == NULL)
            throw runtime_error(string("input format not supported: ") + _op_vm["trj-force"].as<string>());
        // open the trajectory
        _trjreader_force->Open(_op_vm["trj-force"].as<string>());
        // read in first frame
        _trjreader_force->FirstFrame(_top_force);
    }
}

CGForceMatching::SplineInfo::SplineInfo(int index, bool bonded_, int matr_pos_, Property *options)
{
    // initialize standard data
    splineIndex = index;
    _options = options;
    splineName = options->get("name").value();
    bonded = bonded_;

    // get non-bonded information
    if (!bonded) {
        type1 = options->get("type1").value();
        type2 = options->get("type2").value();
    }

    // initialize the grid
    double grid_min = options->get("fmatch.min").as<double>();
    double grid_max = options->get("fmatch.max").as<double>();
    double grid_step = options->get("fmatch.step").as<double>();

    // GenerateGrid returns number of grid points. We subtract 1 to get
    // the number of spline functions
    num_gridpoints = Spline.GenerateGrid(grid_min, grid_max, grid_step);
    num_splinefun = num_gridpoints - 1;

    cout << "Number of spline functions for the interaction " << splineName << ":" << num_splinefun << endl;

    matr_pos = matr_pos_;

    // initialize grid for block averaging
    dx_out = options->get("fmatch.out_step").as<double>();
    // number of output grid points
    num_outgrid = 1 + (int)((grid_max-grid_min)/dx_out);
    result.resize(num_outgrid, false);
    result.clear();
    error.resize(num_outgrid, false);
    error.clear();
    resSum.resize(num_outgrid, false);
    resSum.clear();
    resSum2.resize(num_outgrid, false);
    resSum2.clear();
    block_res_f.resize(num_gridpoints, false);
    block_res_f2.resize(num_gridpoints, false);

}
void CGForceMatching::EndEvaluate()
{
    cout << "\nWe are done, thank you very much!" << endl;
     if(_has_existing_forces) {
         _trjreader_force->Close();
        delete _trjreader_force;
     }
}

void CGForceMatching::WriteOutFiles()
{
    // sanity check
    if (_nblocks == 0) {
        cerr << "\nERROR in csg_fmatch::EndCG - No blocks have been processed so far" << endl;
        cerr << "It might be that you are using trajectory, which is smaller than needed for one block" << endl;
        cerr << "Check your input!" << endl;
        exit(-1);
    }

    string file_extension = ".force";
    string file_name;
    Table force_tab;

    // table with error column
    force_tab.SetHasYErr(true);

    SplineContainer::iterator is;

    for (is = _splines.begin(); is != _splines.end(); ++is) {
        // construct meaningful outfile name
        file_name = (*is)->splineName;
        file_name = file_name + file_extension;

        // resize table
        force_tab.resize((*is)->num_outgrid, false);

        // print output file names on stdout
        cout << "Updating file: " << file_name << endl;

        // loop over output grid points
        for (int i = 0; i < (*is)->num_outgrid; i++) {
            // average value
            (*is)->result[i] = (*is)->resSum[i] / _nblocks;
            // standard deviation of the average
            (*is)->error[i] = sqrt( ((*is)->resSum2[i] / _nblocks - (*is)->result[i] * (*is)->result[i])/_nblocks );
        }

        // first output point = first grid point
        double out_x = (*is)->Spline.getGridPoint(0);
        // loop over output grid
        for (int i = 0; i < (*is)->num_outgrid; i++) {
            // put point, result, flag and accuracy at point out_x into the table
            force_tab.set(i, out_x, (-1.0) * (*is)->result[i], 'i', (*is)->error[i]);
            // update out_x for the next iteration
            out_x += (*is)->dx_out;
        }
        // save table in the file
        force_tab.Save(file_name);
        // clear the table for the next spline
        force_tab.clear();
    }
}

void CGForceMatching::EvalConfiguration(Topology *conf, Topology *conf_atom)
{
    SplineContainer::iterator spiter;
    if(_has_existing_forces) {
        if(conf->BeadCount() != _top_force.BeadCount())
            throw std::runtime_error("number of beads in topology and force topology does not match");
        for(int i=0; i<conf->BeadCount(); ++i) {
            conf->getBead(i)->F() -= _top_force.getBead(i)->getF();
            vec d = conf->getBead(i)->getPos() - _top_force.getBead(i)->getPos();
            if(abs(d) > 1e-6)
                throw std::runtime_error("One or more bead positions in mapped and reference force trajectory differ by more than 1e-6");
        }
    }

    for (spiter = _splines.begin(); spiter != _splines.end(); ++spiter) {
        SplineInfo *sinfo = *spiter;
        if (sinfo->bonded) // bonded interaction
            EvalBonded(conf, sinfo);
        else // non-bonded interaction
            EvalNonbonded(conf, sinfo);
    }

    // loop for the forces vector:
    // hack, chage the Has functions..
    if (conf->getBead(0)->HasF()) {
        vec Force(0., 0., 0.);
        for (int iatom = 0; iatom < _nbeads; ++iatom) {
            Force = conf->getBead(iatom)->getF();
            _b(_least_sq_offset + 3 * _nbeads * _frame_counter + iatom) = Force.x();
            _b(_least_sq_offset + 3 * _nbeads * _frame_counter + _nbeads + iatom) = Force.y();
            _b(_least_sq_offset + 3 * _nbeads * _frame_counter + 2 * _nbeads + iatom) = Force.z();
        }
    } else {
        cerr << "\nERROR in csg_fmatch::EvalConfiguration - No forces in configuration!\n" << endl;
        exit(-1);
    }
    // update the frame counter
    _frame_counter += 1;

    if (_frame_counter % _nframes == 0) { // at this point we processed _nframes frames, which is enough for one block
        // update block counter
        _nblocks++;
        // solve FM equations and accumulate the result
        FmatchAccumulateData();
        // print status information
        cout << "\nBlock No" << _nblocks << " done!" << endl;
        // write results to output files
        WriteOutFiles();

        // we must count frames from zero again for the next block
        _frame_counter = 0;
        if (_constr_least_sq) { //Constrained Least Squares
            // Matrices should be cleaned after each block is evaluated
            _A.clear();
            _b.clear();
            // clear and assign smoothing conditions to _B_constr
            FmatchAssignSmoothCondsToMatrix(_B_constr);
        } else { // Simple Least Squares
            // Matrices should be cleaned after each block is evaluated
            // clear and assign smoothing conditions to _A
            FmatchAssignSmoothCondsToMatrix(_A);
            _b.clear();
        }
    }
    if(_has_existing_forces)
        _trjreader_force->NextFrame(_top_force);
}

void CGForceMatching::FmatchAccumulateData()
{
    _x.clear();
    if (_constr_least_sq) { // Constrained Least Squares
        // Solving linear equations system
        ub::matrix<double> B_constr = _B_constr;
        votca::tools::linalg_constrained_qrsolve(_x, _A, _b, B_constr);
        _x = -_x;

    } else { // Simple Least Squares

        ub::vector<double> residual(_b.size());
        votca::tools::linalg_qrsolve(_x, _A, _b, &residual);

        // calculate FM residual - quality of FM
        // FM residual is initially calculated in (kJ/(mol*nm))^2
        double fm_resid = 0;

        for (size_t i = 0; i < _b.size(); i++)
            fm_resid += residual(i) * residual(i);

        // strange number is units conversion -> now (kcal/(mol*angstrom))^2
        fm_resid /= 3 * _nbeads * _frame_counter * 1750.5856;

        cout << endl;
        cout << "#### Force matching residual ####" << endl;
        cout << "     Chi_2 = " << fm_resid << endl;
        cout << "#################################" << endl;
        cout << endl;
    }

    SplineContainer::iterator is;
    for (is = _splines.begin(); is != _splines.end(); ++is) {
        int &mp = (*is)->matr_pos;
        int &ngp = (*is)->num_gridpoints;

        // _x contains results for all splines. Here we cut the results for one spline
        for (int i = 0; i < ngp; i++) {
            (*is)->block_res_f[i] = _x[ i + mp ];
            (*is)->block_res_f2[i] = _x[ i + mp + ngp];
        }
        // result cutted before is assigned to the corresponding spline
        (*is)->Spline.setSplineData((*is)->block_res_f, (*is)->block_res_f2);

        // first output point = first grid point
        double out_x = (*is)->Spline.getGridPoint(0);

        // point in the middle of the output grid for printing debug information
        int grid_point_debug = (*is)->num_outgrid / 2;

        // loop over output grid
        for (int i = 0; i < (*is)->num_outgrid; i++) {
            // update resSum (add result of a particular block)
            (*is)->resSum[i] += (*is)->Spline.Calculate(out_x);
            // print useful debug information
            if (i == grid_point_debug) cout << "This should be a number: " << (*is)->Spline.Calculate(out_x) << " " << endl;
            // update resSum2 (add result of a particular block)
            (*is)->resSum2[i] += (*is)->Spline.Calculate(out_x) * (*is)->Spline.Calculate(out_x);
            // output point for the next iteration
            out_x += (*is)->dx_out;
        }
    }
}

void CGForceMatching::FmatchAssignSmoothCondsToMatrix(ub::matrix<double> &Matrix)
{
// This function assigns Spline smoothing conditions to the Matrix.
// For the simple least squares the function is used for matrix _A
// For constrained least squares - for matrix _B_constr
    int line_tmp, col_tmp;
    line_tmp = 0;
    col_tmp = 0;

    Matrix.clear();

    SplineContainer::iterator is;
    for (is = _splines.begin(); is != _splines.end(); ++is) {
        int sfnum = (*is)->num_splinefun;
        (*is)->Spline.AddBCToFitMatrix(Matrix, line_tmp, col_tmp);
        // update counters
        line_tmp += sfnum + 1;
        col_tmp += 2 * (sfnum + 1);
    }
}

void CGForceMatching::LoadOptions(const string &file)
{
    load_property_from_xml(_options, file);
    _bonded = _options.Select("cg.bonded");
    _nonbonded = _options.Select("cg.non-bonded");
}

void CGForceMatching::EvalBonded(Topology *conf, SplineInfo *sinfo)
{
    std::list<Interaction *> interList;
    std::list<Interaction *>::iterator interListIter;

    interList = conf->InteractionsInGroup(sinfo->splineName);

    for (interListIter = interList.begin(); interListIter != interList.end(); ++interListIter) {

        int beads_in_int = (*interListIter)->BeadCount(); // 2 for bonds, 3 for angles, 4 for dihedrals

        CubicSpline &SP = sinfo->Spline;

        int &mpos = sinfo->matr_pos;

        double var = (*interListIter)->EvaluateVar(*conf); // value of bond, angle, or dihedral

        for (int loop = 0; loop < beads_in_int; loop++) {
            int ii = (*interListIter)->getBeadId(loop);
            vec gradient = (*interListIter)->Grad(*conf, loop);

            SP.AddToFitMatrix(_A, var,
                    _least_sq_offset + 3 * _nbeads * _frame_counter + ii, mpos, -gradient.x());
            SP.AddToFitMatrix(_A, var,
                    _least_sq_offset + 3 * _nbeads * _frame_counter + _nbeads + ii, mpos, -gradient.y());
            SP.AddToFitMatrix(_A, var,
                    _least_sq_offset + 3 * _nbeads * _frame_counter + 2 * _nbeads + ii, mpos, -gradient.z());
        }
    }
}

void CGForceMatching::EvalNonbonded(Topology *conf, SplineInfo *sinfo)
{
    // generate the neighbour list
            // generate the neighbour list
        NBList *nb;

        bool gridsearch=false;

        if(_options.exists("cg.nbsearch")) {
            if(_options.get("cg.nbsearch").as<string>() == "grid")
                gridsearch=true;
            else if(_options.get("cg.nbsearch").as<string>() == "simple")
                gridsearch=false;
            else throw std::runtime_error("cg.nbsearch invalid, can be grid or simple");
        }
        if(gridsearch)
            nb = new NBListGrid();
        else
            nb = new NBList();

   nb->setCutoff(sinfo->_options->get("fmatch.max").as<double>()); // implement different cutoffs for different interactions!

    // generate the bead lists
    BeadList beads1, beads2;
    beads1.Generate(*conf, sinfo->type1);
    beads2.Generate(*conf, sinfo->type2);

    // is it same types or different types?
    if (sinfo->type1 == sinfo->type2)
        nb->Generate(beads1, true);
    else
        nb->Generate(beads1, beads2, true);

    NBList::iterator pair_iter;
    // iterate over all pairs
    for (pair_iter = nb->begin(); pair_iter != nb->end(); ++pair_iter) {
        int iatom = (*pair_iter)->first->getId();
        int jatom = (*pair_iter)->second->getId();
        double var = (*pair_iter)->dist();
        vec gradient = (*pair_iter)->r();
        gradient.normalize();

        CubicSpline &SP = sinfo->Spline;

        int &mpos = sinfo->matr_pos;

        // add iatom
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + iatom, mpos, gradient.x());
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + _nbeads + iatom, mpos, gradient.y());
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + 2 * _nbeads + iatom, mpos, gradient.z());

        // add jatom
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + jatom, mpos, -gradient.x());
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + _nbeads + jatom, mpos, -gradient.y());
        SP.AddToFitMatrix(_A, var,
                _least_sq_offset + 3 * _nbeads * _frame_counter + 2 * _nbeads + jatom, mpos, -gradient.z());
    }
    delete nb;
}


#ifndef __GUIDANCE_HH__
#define __GUIDANCE_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the GUIDANCE Module On Board)
LIBRARY DEPENDENCY:
      ((../src/guidance.cpp))
PROGRAMMERS:
      (((Chung-Fan Yang) (Chun-Hsu Lai) () () ))
*******************************************************************************/
#include <armadillo>
#include "aux.hh"
#include "cadac_constants.hh"
#include "cadac_util.hh"

class Guidance {
  TRICK_INTERFACE(Guidance);

 public:
  Guidance();
  void set_guidance_var(double in1, int in2, double in3, double in4, double in5,
                        double in6, double in7, double in8, double in9,
                        double in10, double in11, double in12, double in13,
                        double in14);
  void set_ltg_guidance();

  void default_data();
  void initialize();

  void guidance(double int_step);

  arma::vec3 guidance_ltg(double time_ltg_in);
  void guidance_ltg_tgo(double &tgop, arma::mat &BURNTN, arma::mat &L_IGRLN,
                        arma::mat &TGON, double &l_igrl, int &nstmax,
                        double &tgo, int &nst, arma::mat &TAUN, arma::mat VEXN,
                        arma::mat BOTN, double delay_ignition_in, double vgom,
                        double amag1, double amin_in, double time_ltg_in,
                        int num_stages_in);
  void guidance_ltg_igrl(double &s_igrl, double &j_igrl, double &q_igrl,
                         double &h_igrl, double &p_igrl, double &j_over_l,
                         double &tlam, double &qprime, int nst, int nstmax,
                         arma::mat BURNTN, arma::mat L_IGRLN, arma::mat TGON,
                         arma::mat TAUN, arma::mat VEXN, double l_igrl,
                         double time_ltg_in);
  void guidance_ltg_trate(arma::mat &ULAM_in, arma::mat &LAMD_in, arma::mat &RGO_in,
                          int &ipas2_flag_in, arma::mat VGO_in, double s_igrl,
                          double q_igrl, double j_over_l, double lamd_limit_in,
                          double vgom, double tgo, double tgop,
                          arma::mat SDII_in, arma::mat SBIIC, arma::mat VBIIC,
                          arma::mat RBIAS_in, arma::mat UD_in, arma::mat UY_in,
                          arma::mat UZ_in, arma::mat &RGRAV_in);
  void guidance_ltg_trate_rtgo(arma::mat &RGO_in, arma::mat &RGRAV_in, double tgo,
                               double tgop, arma::mat SDII_in, arma::mat SBIIC,
                               arma::mat VBIIC, arma::mat RBIAS_in, arma::mat ULAM_in,
                               arma::mat UD_in, arma::mat UY_in, arma::mat UZ_in,
                               double s_igrl);
  void guidance_ltg_pdct(arma::mat &SPII, arma::mat &VPII, arma::mat &RGRAV_in,
                         arma::mat &RBIAS_in, arma::mat LAMD_in, arma::mat ULAM_in,
                         double l_igrl, double s_igrl, double j_igrl,
                         double q_igrl, double h_igrl, double p_igrl,
                         double j_over_l, double qprime, arma::mat SBIIC,
                         arma::mat VBIIC, arma::mat RGO_in, double tgo);
  void guidance_ltg_crct(arma::mat &SDII_in, arma::mat &UD_in, arma::mat &UY_in,
                         arma::mat &UZ_in, arma::mat &VMISS, arma::mat &VGO_in,
                         double dbi_desired_in, double dvbi_desired_in,
                         double thtvdx_desired_in, arma::mat SPII, arma::mat VPII,
                         arma::mat SBIIC, arma::mat VBIIC);

  std::function<int()> grab_mprop;

  // std::function<double()> grab_dbi;
  // std::function<double()> grab_dvbi;
  std::function<double()> grab_thtvdx;
  // std::function<double()> grab_fmassr;
  std::function<arma::vec3()> grab_VBIIC;
  std::function<arma::vec3()> grab_SBIIC;
  std::function<arma::mat33()> grab_TBIC;
  std::function<arma::vec3()> grab_FSPCB;

  std::function<void()> set_no_thrust;
  std::function<void()> set_ltg_thrust;

  ///////////////////////////////////////////////////////////////////////////////
  // Definition of guidance module-variables
  // Member function of class 'Hyper'
  // Module-variable locations are assigned to hyper[400-499]
  // Overflow assignment hyper[850-899]
  //
  //       mguide =  0 no guidance
  //              =  5 linear tangent guidance law (LTG) for rocket ascent
  //
  // 030616 Created by Peter H Zipfel
  // 091214 Modified for ROCKET6, PZi
  ///////////////////////////////////////////////////////////////////////////////
  arma::vec3 get_UTBC();

  double get_alphacomx();
  double get_betacomx();

  void set_degree(double, double);
  int get_beco_flag();

 private:
  /* Propagative Stats */
  arma::vec
      UTBC; /* **  (--)        Commanded unit thrust vector in body coor */
  double
      _UTBC[3]; /* **  (--)        Commanded unit thrust vector in body coor */

  /* Diagnostic */
  arma::vec
      UTIC; /* **  (--)        Commanded unit thrust vector in inertial coor */
  double _UTIC[3]; /* **  (--)        Commanded unit thrust vector in inertial
                      coor */

  /* Set by input.py */
  double alphacomx; /* **  (d)         Alpha command */
  double betacomx;  /* **  (d)         Beta command */

  /* Internally set parameter */
  int init_flag;   /* **  (--)        Flag for initializing LTG flags */
  int inisw_flag;  /* **  (--)        Flag to initialize '..._intl()' */
  int skip_flag;   /* **  (--)        Flag to delay output */
  int ipas_flag;   /* **  (--)        Flag to initialize in '..._tgo()'  */
  int ipas2_flag;  /* **  (--)        Flag to initialize in '..._trat()'  */
  int print_flag;  /* **  (--)        Flag to cause print-out  */
  double time_ltg; /* **  (s)         Time since initiating LTG */

  int ltg_count; /* **  (--)        Counter of LTG guidance cycles  */

  /* Externally set parameter */
  int mguide;      /* **  (--)        Guidance modes, see table */
  double ltg_step; /* **  (s)         LTG guidance time step */

  arma::vec RBIAS;       /* **  (m)         Range-to-be-gained bias */
  double _RBIAS[3];      /* **  (m)         Range-to-be-gained bias */
  int beco_flag;         /* **  (--)        Boost engine cut-off flag */
  double dbi_desired;    /* **  (m)         Desired orbital end position */
  double dvbi_desired;   /* **  (m/s)       Desired orbital end velocity */
  double thtvdx_desired; /* **  (d)         Desired orbital flight path angle */
  int num_stages;        /* **  (s)         Number of stages in boost phase */
  double delay_ignition; /* **  (s)         Delay of motor ignition after
                            staging */
  double amin;           /* **  (m/s2)      Minimum longitudinal acceleration */
  double char_time1; /* **  (s)         Characteristic time 'tau' of stage 1 */
  double char_time2; /* **  (s)         Characteristic time 'tau' of stage 2 */
  double char_time3; /* **  (s)         Characteristic time 'tau' of stage 3 */
  double exhaust_vel1;   /* **  (m/s)       Exhaust velocity of stage 1 */
  double exhaust_vel2;   /* **  (m/s)       Exhaust velocity of stage 2 */
  double exhaust_vel3;   /* **  (m/s)       Exhaust velocity of stage 3 */
  double burnout_epoch1; /* **  (s)         Burn out of stage 1 at 'time_ltg' */
  double burnout_epoch2; /* **  (s)         Burn out of stage 2 at 'time_ltg' */
  double burnout_epoch3; /* **  (s)         Burn out of stage 3 at 'time_ltg' */
  double lamd_limit;     /* **  (1/s)       Limiter on 'lamd' */
  arma::vec RGRAV;       /* **  (m)         Postion loss due to gravity */
  double _RGRAV[3];      /* **  (m)         Postion loss due to gravity */
  arma::vec RGO;         /* **  (m)         Range-to-go vector */
  double _RGO[3];        /* **  (m)         Range-to-go vector */
  arma::vec VGO;         /* **  (m/s)       Velocity still to be gained */
  double _VGO[3];        /* **  (m/s)       Velocity still to be gained */
  arma::vec SDII;        /* **  (m)         Desired inertial position */
  double _SDII[3];       /* **  (m)         Desired inertial position */
  arma::vec UD;          /* **  (--)        Unit vector of SPII and SDII */
  double _UD[3];         /* **  (--)        Unit vector of SPII and SDII */
  arma::vec UY;          /* **  (--)        Unit vector normal to traj plane */
  double _UY[3];         /* **  (--)        Unit vector normal to traj plane */
  arma::vec
      UZ; /* **  (--)        Unit vector in traj plane, normal to SBBI_D */
  double
      _UZ[3];  /* **  (--)        Unit vector in traj plane, normal to SBBI_D */
//   double vgom; /* **  (m/s)       Velocity to be gained magnitude */
  double tgo_save;  /* **  (s)         Time to go to desired end state */
  int nst_save;     /* **  (--)        N-th stage number */
  arma::vec ULAM;  /* **  (--)        Unit thrust vector in VGO direction */
  double _ULAM[3]; /* **  (--)        Unit thrust vector in VGO direction */
  arma::vec LAMD;  /* **  (1/s)       Thrust vector turning rate */
  double _LAMD[3]; /* **  (1/s)       Thrust vector turning rate */
  double lamd;     /* **  (1/s)       Magnitude of LAMD */
  double dpd;      /* **  (m)         Distance of the predicted from the desired
                      end-point */
  double
      dbd; /* **  (m)         Distance of vehicle from the desired end-point */
//   double ddb; /* **  (m)         Position error at BECO */
//   double
//       dvdb; /* **  (m/s)       Distance of vehicle from the desired end-point */
//   double thtvddbx; /* **  (d)         Angle error at BECO */
};

#endif  // __GUIDANCE_HH__


/*
(c) 2015 Glen Joseph Fernandes
<glenjofe -at- gmail.com>

Distributed under the Boost Software
License, Version 1.0.
http://boost.org/LICENSE_1_0.txt
*/
#ifndef BOOST_ALIGN_ALIGN_UP_HPP
#define BOOST_ALIGN_ALIGN_UP_HPP

#include <boost/align/detail/align_up.hpp>
#include <boost/align/align_up_forward.hpp>

namespace boost {
namespace alignment {

BOOST_CONSTEXPR inline std::size_t align_up(std::size_t value,
    std::size_t alignment) BOOST_NOEXCEPT
{
    return (value + alignment - 1) & ~(alignment - 1);
}

} /* .alignment */
} /* .boost */

#endif


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/simd/detail/enforce_precision.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/two_split.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( two_prod_

                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE
      A0 operator() ( A0 const& a0,A0 const& a1,A0& a3) const BOOST_NOEXCEPT
    {
      A0 a2;
      boost::simd::two_prod(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( two_prod_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0,A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a0,A0 const& a1) const BOOST_NOEXCEPT
    {
      A0 first, second;
      boost::simd::two_prod( a0, a1, first, second );
      return  {first, second};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( two_prod_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE
      void operator() ( A0 const& a,A0 const& b, A0 & r0,A0 & r1) const BOOST_NOEXCEPT
    {
      detail::enforce_precision<A0> enforcer;
      A0 a1, a2, b1, b2;
      r0  = a*b;

      two_split(a, a1, a2);
      two_split(b, b1, b2);

#if defined(BOOST_SIMD_NO_INVALIDS)
      r1 = a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2);
#else
      r1 = if_zero_else(is_invalid(r0), a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2));
#endif

    }
  };
} } }

#endif


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "geometry.hpp"
#include <boost/functional/hash.hpp>

#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/num_geometries.hpp>
#include <boost/geometry/algorithms/num_points.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace miqp {
namespace common {
namespace geometry {

void PrintPolygon(BoostPolygon const& poly) {
  std::cout << "poly = [";
  std::vector<PointXY> points = poly.outer();
  for (std::vector<PointXY>::size_type i = 0; i < points.size(); ++i) {
    std::cout << bg::get<0>(points[i]) << ", " << bg::get<1>(points[i]) << "; ";
  }
  std::cout << "]" << std::endl;
}

bark::geometry::Polygon BoostPolygonToBarkPolygon(BoostPolygon const& poly) {
  using bark::geometry::Point2d;
  bark::geometry::Polygon barkpoly;
  std::vector<PointXY> points = poly.outer();
  for (std::vector<PointXY>::size_type i = 0; i < points.size(); ++i) {
    barkpoly.AddPoint(Point2d(bg::get<0>(points[i]), bg::get<1>(points[i])));
  }
  return barkpoly;
}

bark::geometry::Line BoostLinestringToBarkLine(BoostLinestring const& line) {
  using bark::geometry::Point2d;
  bark::geometry::Line barkline;
  for (auto& pt : line) {
    barkline.AddPoint(Point2d(bg::get<0>(pt), bg::get<1>(pt)));
  }
  return barkline;
}

BoostPolygon BarkPolygonToBoostPolygon(bark::geometry::Polygon const& poly) {
  return poly.obj_;
}

bool IsConvex(BoostPolygon const& poly) {
  BoostPolygon hull;
  boost::geometry::convex_hull(poly, hull);
  //! boost geometry equals sometimes returns a very small value, even if the
  //! polygon points are identical. Use area check
  const double diff = bg::area(hull) - bg::area(poly);
  const double numeric_tolerance = 1e-10;
  // std::cout << "diff is = " << diff << " and bg::equals is: "
  // << bg::equals(poly, hull) << std::endl;
  return (abs(diff) < numeric_tolerance);
  // return bg::equals(poly, hull);
}

void BufferPolygonCollection(BoostMultiPolygon& polygons_in,
                             const double buffer_radius) {
  for (auto it = polygons_in.begin(); it != polygons_in.end(); ++it) {
    bark::geometry::Polygon tmp;
    BufferPolygon(BoostPolygonToBarkPolygon(*it), buffer_radius, &tmp);
    *it = BarkPolygonToBoostPolygon(tmp);
  }
}

void MergeToConvexPolygons(BoostMultiPolygon& polygons_in,
                           const float tolerance,
                           bark::geometry::Polygon& buffered_env_for_merging) {
  BoostMultiPolygon merged_polygons_out;
  std::vector<int> merged_idxs;
  for (size_t i = 0; i < polygons_in.size(); ++i) {
    bool idx_i_already_merged =
        std::find(merged_idxs.begin(), merged_idxs.end(), i) !=
        merged_idxs.end();
    if (!idx_i_already_merged) {
      BoostPolygon merged_polygon_idx_i = polygons_in.at(i);

      for (size_t j = i + 1; j < polygons_in.size(); ++j) {
        bool idx_j_already_merged =
            std::find(merged_idxs.begin(), merged_idxs.end(), j) !=
            merged_idxs.end();
        if (!idx_j_already_merged &&
            boost::geometry::intersects(merged_polygon_idx_i,
                                        polygons_in.at(j))) {
          BoostMultiPolygon merged_polygon;
          boost::geometry::correct(merged_polygon_idx_i);
          boost::geometry::correct(polygons_in.at(j));
          boost::geometry::union_(merged_polygon_idx_i, polygons_in.at(j),
                                  merged_polygon);
          boost::geometry::correct(merged_polygon);
          if (merged_polygon.size() > 0) {
            BoostPolygon hull;
            boost::geometry::convex_hull(merged_polygon, hull);
            boost::geometry::correct(hull);
            double diff = bg::area(hull) - bg::area(merged_polygon);
            const double numeric_tol = 1e-3;
            if (fabs(diff) < tolerance + numeric_tol) {
              // we use some error measure, so that we can merge two polygons
              // even if they yield non-convex polygon.
              merged_polygon_idx_i = hull;
              merged_idxs.push_back(j);
            } else if (boost::geometry::within(hull,
                                               buffered_env_for_merging.obj_)) {
              LOG(INFO) << "Merged polygon is not convex, but hull can be used";
              merged_polygon_idx_i = hull;
              merged_idxs.push_back(j);
            }
          }
        }
      }
      merged_polygons_out.push_back(merged_polygon_idx_i);
    }
  }

  polygons_in = merged_polygons_out;
}

Eigen::MatrixXd Polygon2MiqpPolygonDefinition(
    const bark::geometry::Polygon& poly) {
  // Does two things: 1. reverse order of the points, 2. removes first=last
  // doubled point
  using bark::geometry::Point2d;
  std::vector<Point2d> points = poly.obj_.outer();
  const size_t nrPoints = points.size() - 1;
  Eigen::MatrixXd outpoly(nrPoints, 2);
  for (std::vector<Point2d>::size_type i = 0; i < nrPoints; ++i) {
    outpoly(nrPoints - 1 - i, 0) = bg::get<0>(points[i]);
    outpoly(nrPoints - 1 - i, 1) = bg::get<1>(points[i]);
  }
  return outpoly;
}

std::size_t GetHash(const bark::geometry::Polygon& polygon) {
  std::size_t polygon_hash = 0;
  std::size_t num_geometries = bg::num_geometries(polygon.obj_);
  std::size_t num_geom_hash = boost::hash_value(num_geometries);
  boost::hash_combine(polygon_hash, num_geom_hash);

  std::size_t num_pts = bg::num_points(polygon.obj_);
  std::size_t num_pts_hash = boost::hash_value(num_pts);
  boost::hash_combine(polygon_hash, num_pts_hash);

  double area = bg::area(polygon.obj_);
  std::size_t area_hash = boost::hash_value(area);
  boost::hash_combine(polygon_hash, area_hash);
  return polygon_hash;
}

void ConvertToBarkLine(double ref_in[], const int ref_size,
                       double simplification_distance,
                       bark::geometry::Line& line_out) {
  std::vector<bark::geometry::Point2d> pts;
  for (int i = 0; i < ref_size * 2; i += 2) {
    pts.emplace_back(ref_in[i], ref_in[i + 1]);
  }
  bark::geometry::Line ref;
  ref.AddPoints(pts);
  line_out = Simplify(ref, simplification_distance);
}

PolygonMap BoostMultiPolygonToBarkPolygonMap(BoostMultiPolygon& polygons_in) {
  PolygonMap map_out;
  PolygonId id = 0;
  for (auto it = polygons_in.begin(); it != polygons_in.end(); ++it) {
    bark::geometry::Polygon pconv =
        miqp::common::geometry::BoostPolygonToBarkPolygon(*it);
    map_out.insert(std::pair<PolygonId, bark::geometry::Polygon>(id, pconv));
    ++id;
  }
  return map_out;
}

}  // namespace geometry
}  // namespace common
}  // namespace miqp