#include "InverseProblem_Adjoint_InitialConfiguration.h"

#include <Eigen/Dense>
#include <tinyformat.h>
#include <cinder/Log.h>
#include <numeric>
#include <LBFGS.h>
#include <cinder/app/App.h>

#include "GradientDescent.h"
#include "Integration.h"

using namespace std;
using namespace Eigen;

#define DEBUG_SAVE_MATRICES 0

namespace ar {

    InverseProblem_Adjoint_InitialConfiguration::InverseProblem_Adjoint_InitialConfiguration()
        : meshPositionPrior(0)
		, gridPriors({0.01, 0.01, 5})
		, gridHyperEpsilon(0.5)
        , numIterations(10)
    {
    }

    InverseProblemOutput InverseProblem_Adjoint_InitialConfiguration::solveGrid(int deformedTimestep,
        BackgroundWorker* worker, IntermediateResultCallback_t callback)
    {
        CI_LOG_I("Solve for Initial Configuration at timestep " << deformedTimestep);
        // Create simulation
        worker->setStatus("Adjoint - Initial Configuration Grid: create simulation");
        SoftBodyGrid2D simulation;
        simulation.setGridResolution(input->gridResolution_);
        simulation.setSDF(input->gridReferenceSdf_);
        simulation.setExplicitDiffusion(true);
        simulation.setHardDirichletBoundaries(true);
        simulation.setAdvectionMode(SoftBodyGrid2D::AdvectionMode::DIRECT_FORWARD);
        simulation.resetBoundaries();
        for (const auto& b : input->gridDirichletBoundaries_)
            simulation.addDirichletBoundary(b.first.first, b.first.second, b.second);
        for (const auto& b : input->gridNeumannBoundaries_)
            simulation.addNeumannBoundary(b.first.first, b.first.second, b.second);
        if (worker->isInterrupted()) return InverseProblemOutput();

        // Set parameters with everything that can't be reconstructed here
        simulation.setGravity(input->settings_.gravity_);
        simulation.setMass(input->settings_.mass_);
        simulation.setDamping(input->settings_.dampingAlpha_, input->settings_.dampingBeta_);
        simulation.setMaterialParameters(input->settings_.youngsModulus_, input->settings_.poissonsRatio_);
        simulation.setRotationCorrection(SoftBodySimulation::RotationCorrection::None);
        if (worker->isInterrupted()) return InverseProblemOutput();

        GridUtils2D::grid_t targetSdf = input->gridResultsSdf_[deformedTimestep];

#if 1
		//Gradient Descent
		//define gradient of the cost function
        real finalCost = 0;
        const auto gradient =
            [&simulation, this, worker, targetSdf, &finalCost](const VectorX& x)
        {
            real cost;
			grid_t currentSdf = GridUtils2D::delinearize(x, simulation.getGridResolution(), simulation.getGridResolution());
			auto gradientGrid = gradientGrid_AdjointMethod(targetSdf, simulation,
				currentSdf, gridPriors, cost, worker);
			VectorX gradient = GridUtils2D::linearize(gradientGrid);
            finalCost = cost;
            return gradient;
        };
		//Optimize
        VectorX start = GridUtils2D::linearize(targetSdf);
        GradientDescent<VectorX> gd(start, gradient, 1e-10, 0.5);
		gd.setMinStepsize(0.0001);
        gd.setMaxStepsize(0.4);
        int oi;
        for (oi = 0; oi < numIterations; ++oi) {
            worker->setStatus(tfm::format("Adjoint - Initial Configuration Grid: optimization %d/%d", (oi + 1), numIterations));
            if (gd.step()) break;
			CI_LOG_I("Gradient descent, step size is " << gd.getLastStepSize());
            if (worker->isInterrupted()) return InverseProblemOutput();

            InverseProblemOutput output;
            output.initialGridSdf_ = GridUtils2D::delinearize(gd.getCurrentSolution(), input->gridResolution_, input->gridResolution_);
            output.finalCost_ = finalCost;
            callback(output);
        }
        VectorX finalValueV = gd.getCurrentSolution();
#else
        //LBFGS with Hyper-Optimization over the Gravity
        LBFGSpp::LBFGSParam<real> params;
        params.epsilon = 1e-10;
        params.max_iterations = numIterations;
        LBFGSpp::LBFGSSolver<real> lbfgs(params);
        //define gradient
        LBFGSpp::LBFGSSolver<real>::ObjectiveFunction_t fun([&simulation, this, worker, targetSdf](const VectorX& x, VectorX& gradient) -> real {
            real cost;
            grid_t currentSdf = GridUtils2D::delinearize(x, simulation.getGridResolution(), simulation.getGridResolution());
            auto gradientGrid = gradientGrid_AdjointMethod(targetSdf, simulation,
                currentSdf, gridPriors, cost, worker);
            gradient = GridUtils2D::linearize(gradientGrid);
            return cost;
        });
        LBFGSpp::LBFGSSolver<real>::CallbackFunction_t lbfgsCallback([worker, callback, this](const VectorX& x, const real& v, int k) -> bool {
            worker->setStatus(tfm::format("Adjoint - Initial Configuration Grid: optimization %d/%d", k, numIterations));

			InverseProblemOutput output;
			output.initialGridSdf_ = GridUtils2D::delinearize(x, input->gridResolution_, input->gridResolution_);
			output.finalCost_ = v;
			callback(output);

            return !worker->isInterrupted();
        });
        //optimize
        Vector2 gravity = input->settings_.gravity_;
        real hyperStepsize = 1;
        real hyperStep = 0;
        real finalCost = 0;
        VectorX finalValueV = GridUtils2D::linearize(targetSdf);
        int totalOI = 0;
        while (true)
        {
            CI_LOG_I("Try optimization with " << (hyperStep + hyperStepsize) * 100 << "% of the final gravity");
            simulation.setGravity((hyperStep + hyperStepsize) * gravity);
            VectorX currentValue = finalValueV;
            int oi = lbfgs.minimize(fun, currentValue, finalCost, lbfgsCallback);
            totalOI += oi;
            if (worker->isInterrupted()) break;
            if (finalCost > gridHyperEpsilon)
            {
                //not converged to global minimum, try smaller step
                hyperStepsize /= 2;
                CI_LOG_I("Not converged (cost " << finalCost << "), decrease hyper-stepsize to " << hyperStepsize);
            }
            else
            {
                //converged, increase gravity
                finalValueV = currentValue;
                hyperStep += hyperStepsize;
                hyperStepsize *= 1.2;
                CI_LOG_I("Converged, take " << hyperStep * 100 << "% of the final gravity as input to the next step and increase the step size to " << hyperStepsize);
                if (hyperStep >= 1 - 1e-5)
                    break; //finished
                if (hyperStep + hyperStepsize > 1)
                    hyperStepsize = 1 - hyperStep;
            }
        }
#endif

        InverseProblemOutput output;
        output.initialGridSdf_ = GridUtils2D::delinearize(finalValueV, input->gridResolution_, input->gridResolution_);
        output.finalCost_ = finalCost;

		CI_LOG_D("Ground truth sdf:\n" << input->gridReferenceSdf_);
		CI_LOG_D("Target sdf:\n" << targetSdf);
		CI_LOG_D("Reconstructed input sdf:\n" << output.initialGridSdf_.value());
		CI_LOG_D("Final cost: " << output.finalCost_);

        return output;
    }

    InverseProblemOutput InverseProblem_Adjoint_InitialConfiguration::solveMesh(int deformedTimestep,
        BackgroundWorker* worker, IntermediateResultCallback_t callback)
    {
        CI_LOG_I("Solve for Initial Configuration at timestep " << deformedTimestep);
        // Create simulation
        worker->setStatus("Adjoint - Initial Configuration Mesh: create simulation");
        SoftBodyMesh2D simulation;
        simulation.setMesh(input->meshReferencePositions_, input->meshReferenceIndices_);
        simulation.resetBoundaries();
        for (const auto& b : input->meshDirichletBoundaries_)
            simulation.addDirichletBoundary(b.first, b.second);
        for (const auto& b : input->meshNeumannBoundaries_)
            simulation.addNeumannBoundary(b.first, b.second);
        simulation.reorderNodes();
        if (worker->isInterrupted()) return InverseProblemOutput();

        // Set parameters with everything that can't be reconstructed here
        simulation.setGravity(input->settings_.gravity_);
        simulation.setMass(input->settings_.mass_);
        simulation.setDamping(input->settings_.dampingAlpha_, input->settings_.dampingBeta_);
		simulation.setMaterialParameters(input->settings_.youngsModulus_, input->settings_.poissonsRatio_);
        simulation.setRotationCorrection(SoftBodySimulation::RotationCorrection::None);
        if (worker->isInterrupted()) return InverseProblemOutput();

		SoftBodyMesh2D::Vector2List targetPositions = input->meshReferencePositions_;
		for (size_t i = 0; i < input->meshReferencePositions_.size(); ++i) targetPositions[i] += input->meshResultsDisplacement_[deformedTimestep][i];
#if 0
		//Gradient Descent
        //define gradient of the cost function
		real finalCost = 0;
        const auto gradient = [&simulation, &outputU, &finalCost, this, worker, deformedTimestep](const VectorX& x)
        {
            real cost;
			VectorX gradient = gradientMesh(deformedTimestep, simulation, x, outputU, cost, worker);
			finalCost = cost;
			return gradient;
        };

        //run optimization
		VectorX initialPositions = linearizePositions(input->meshReferencePositions_) + linearizePositions(input->meshResultsDisplacement_[deformedTimestep]);
        GradientDescent<VectorX> gd(initialPositions, gradient, 1e-10, 0.0001);
        int oi;
        for (oi = 0; oi < numIterations; ++oi) {
            worker->setStatus(tfm::format("Adjoint - Initial Configuration Mesh: optimization %d/%d", (oi + 1), numIterations));
            if (gd.step()) break;
            if (worker->isInterrupted()) return InverseProblemOutput();
        }
        VectorX finalValueV = gd.getCurrentSolution();
#else
		//LBFGS with Hyper-Optimization over the Gravity
		LBFGSpp::LBFGSParam<real> params;
		params.epsilon = 1e-10;
		params.max_iterations = numIterations;
		LBFGSpp::LBFGSSolver<real> lbfgs(params);
		//define gradient
		LBFGSpp::LBFGSSolver<real>::ObjectiveFunction_t fun([&targetPositions, &simulation, this, worker](const VectorX& x, VectorX& gradient) -> real {
			real cost;
			gradient = gradientMesh_AdjointMethod(targetPositions, simulation, x, meshPositionPrior, cost, worker);
			return cost;
		});
		LBFGSpp::LBFGSSolver<real>::CallbackFunction_t lbfgsCallback([worker, callback, this](const VectorX& x, const VectorX& g, const real& v, int k) -> bool {
			worker->setStatus(tfm::format("Adjoint - Initial Configuration Mesh: optimization %d/%d", k, numIterations));

			InverseProblemOutput output;
			output.initialMeshPositions_ = delinearizePositions(x);
			output.finalCost_ = v;
			callback(output);

			return !worker->isInterrupted();
		});
		//optimize
		Vector2 gravity = input->settings_.gravity_;
		real hyperStepsize = 1;
		real hyperStep = 0;
		real finalCost = 0;
		static const real hyperEpsilon = 1e-4;
        VectorX finalValueV = linearizePositions(input->meshReferencePositions_) + linearizePositions(input->meshResultsDisplacement_[deformedTimestep]);
		int totalOI = 0;
		while (true)
		{
			CI_LOG_I("Try optimization with " << (hyperStep + hyperStepsize) * 100 << "% of the final gravity");
			simulation.setGravity((hyperStep + hyperStepsize) * gravity);
			VectorX currentValue = finalValueV;
			int oi = lbfgs.minimize(fun, currentValue, finalCost, lbfgsCallback);
			totalOI += oi;
			if (worker->isInterrupted()) break;
			if (finalCost > hyperEpsilon)
			{
				//not converged to global minimum, try smaller step
				hyperStepsize /= 2;
				CI_LOG_I("Not converged (cost " << finalCost << "), decrease hyper-stepsize to " << hyperStepsize);
			} else
			{
				//converged, increase gravity
				finalValueV = currentValue;
				hyperStep += hyperStepsize;
				hyperStepsize *= 1.2;
				CI_LOG_I("Converged, take " << hyperStep * 100 << "% of the final gravity as input to the next step and increase the step size to " << hyperStepsize);
				if (hyperStep >= 1 - 1e-5)
					break; //finished
				if (hyperStep + hyperStepsize > 1)
					hyperStepsize = 1 - hyperStep;
			}
		}
#endif

		MatrixX positionLog(2, input->meshReferencePositions_.size() * 2);
		positionLog << finalValueV.transpose(), linearizePositions(input->meshReferencePositions_).transpose();
        CI_LOG_I("Optimization done after " << totalOI << " optimization steps.  Final positions | Reference positions:\n" << positionLog);

        InverseProblemOutput output;
        output.initialMeshPositions_ = delinearizePositions(finalValueV);
		output.finalCost_ = finalCost;

		simulation.setGravity(gravity);

        return output;
    }

    VectorX InverseProblem_Adjoint_InitialConfiguration::gradientMesh_AdjointMethod(
		const SoftBodyMesh2D::Vector2List& targetPositions, SoftBodyMesh2D& simulation,
        const VectorX& currentInitialPositions, real positionPrior, real& outputCost, BackgroundWorker* worker)
    {
        real poissonsRatio = simulation.getPoissonsRatio();
        real youngsModulus = simulation.getYoungsModulus();
		Vector2 gravity = simulation.getGravity();

        int n = static_cast<int>(currentInitialPositions.size() / 2);
		assert(n == simulation.getReferencePositions().size());
		for (int i = 0; i < n; ++i) {
			simulation.getReferencePositions()[i] = currentInitialPositions.segment<2>(2 * i);
		}

        //get winding order
        triangle_t referenceTriangle = {
			targetPositions[simulation.getTriangles()[0].x()],
			targetPositions[simulation.getTriangles()[0].y()],
			targetPositions[simulation.getTriangles()[0].z()]
        };
        int windingOrder = meshTriangleWindingOrder(referenceTriangle);

        //FORWARD

        MatrixX K = MatrixX::Zero(2 * simulation.getNumFreeNodes(), 2 * simulation.getNumFreeNodes());
        VectorX f = VectorX::Zero(2 * simulation.getNumFreeNodes());

        //assemble force vector f and stiffness matrix K
        simulation.assembleForceVector(f);
        if (worker->isInterrupted()) return VectorX::Zero(2*n);
        real materialMu, materialLambda;
        SoftBodySimulation::computeMaterialParameters(youngsModulus, poissonsRatio, materialMu, materialLambda);
        Matrix3 C = SoftBodySimulation::computeMaterialMatrix(materialMu, materialLambda);
        simulation.assembleStiffnessMatrix(C, K, &f, SoftBodyMesh2D::Vector2List(simulation.getNumNodes(), Vector2::Zero()));
        if (worker->isInterrupted()) return VectorX::Zero(2 * n);

        //solve for the current displacement
		assert(K.isApprox(K.transpose())); //Test if K is indeed symmetric
        PartialPivLU<MatrixX> Klu = K.partialPivLu();
        VectorX u = Klu.solve(f);

        // BACKWARD
		int p = simulation.getNumFreeNodes();

        //compute derivate of the cost function with respect to the output u (displacement)
        VectorX costDu(2 * p);
        for (int i = 0; i < simulation.getNumNodes(); ++i)
        {
			if (simulation.getNodeToFreeMap()[i] < 0) continue;
			costDu.segment<2>(2 * simulation.getNodeToFreeMap()[i])
				= currentInitialPositions.segment<2>(2 * i) + u.segment<2>(2 * simulation.getNodeToFreeMap()[i])
				- targetPositions[i];
        }
#if DEBUG_SAVE_MATRICES==1
        saveAsCSV(costDu, tfm::format("AdjointYoung-GradientMesh-costDu-k%f.csv", youngsModulus));
#endif

        //solve for the dual
        //In theory, I have to use K', but because I know that K is symmetric,
        //I can use K directly and not the transpose.
        VectorX lambda = Klu.solve(costDu); //rhs is called 'g'

		//compute yF, the dual of gX
		VectorX yF = VectorX::Zero(2 * p);
		for (int e = 0; e < simulation.getNumElements(); ++e)
		{
			const Vector3i& tri = simulation.getTriangles()[e];
			Vector6 ue = Vector6::Zero();
			for (int i2 = 0; i2 < 3; ++i2)
				if (simulation.getNodeStates()[tri[i2]] != SoftBodyMesh2D::DIRICHLET)
					ue.segment<2>(2 * i2) = u.segment<2>(2 * simulation.getNodeToFreeMap()[tri[i2]]);
			for (int i = 0; i < 3; ++i)
			{
				if (simulation.getNodeStates()[tri[i]] == SoftBodyMesh2D::DIRICHLET) continue;
				int col = simulation.getNodeToFreeMap()[tri[i]];
				for (int coord = 0; coord < 2; ++coord)
				{
					//current parameter: vertex 'col' with coordinate 'coord'
					//F(:,j) = -dK/dp*u + df/dp
					int j = 2 * col + coord;
					triangle_t triangle{
						currentInitialPositions.segment<2>(2 * tri[0]),
						currentInitialPositions.segment<2>(2 * tri[1]),
						currentInitialPositions.segment<2>(2 * tri[2])
					};
					Matrix6 KeDpos = meshStiffnessMatrixDerivative(triangle, 2 * i + coord, C, windingOrder);
					// dK/dp * u
					Vector6 y = KeDpos * ue;
					//F(:,j) -= dK/dp*u per triangle
					//F(:,j) += Df/dp per triangle
					Vector6 feDpos = meshForceDerivative(triangle, 2 * i + coord, gravity, windingOrder);
					// -dE/dp per triangle
					Vector6 Fe = -(y - feDpos);

					for (int i2 = 0; i2 < 3; ++i2)
						if (simulation.getNodeStates()[tri[i2]] != SoftBodyMesh2D::DIRICHLET)
							yF[j] += Fe.segment<2>(2 * i2).dot(lambda.segment<2>(2 * simulation.getNodeToFreeMap()[tri[i2]]));
				}
			}
		}

		//assemble gradient
		VectorX priorGradient = VectorX::Zero(2 * p); //TODO
		VectorX r = costDu; // dJ / dp
		VectorX gradient = yF + r + positionPrior * priorGradient;

		//map gradients back to the whole grid
		VectorX fullGradient = VectorX::Zero(2 * simulation.getNumNodes());
		for (int i = 0; i<p; ++i)
		{
			fullGradient.segment<2>(2 * simulation.getFreeToNodeMap()[i]) = gradient.segment<2>(2 * i);
		}

        //evaluate cost
        //cost on position difference
        real costMain = 0;
        for (int i = 0; i < simulation.getNumNodes(); ++i)
        {
            if (simulation.getNodeToFreeMap()[i] < 0) continue;
            costMain += (currentInitialPositions.segment<2>(2*i) + u.segment<2>(2 * simulation.getNodeToFreeMap()[i])
                - targetPositions[i]).squaredNorm() / 2;
        }
        //cost by the prior
        real costPrior = 0;
        for (int tri = 0; tri<simulation.getNumElements(); ++tri)
        {
            triangle_t triangle{
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][0]),
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][1]),
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][2])
            };
            costPrior += windingOrder * meshTriangleArea(triangle) / (
                (triangle[0] - triangle[1]).squaredNorm() + (triangle[0] - triangle[2]).squaredNorm() + (triangle[2] - triangle[1]).squaredNorm()
                );
        }
        //final cost
        outputCost = costMain + positionPrior * costPrior;
        //CI_LOG_I("Optimization: cost " << outputCost << ", gradient " << gradient.transpose());

        return fullGradient;
    }

    VectorX InverseProblem_Adjoint_InitialConfiguration::gradientMesh_Forward(
        const SoftBodyMesh2D::Vector2List & targetPositions, SoftBodyMesh2D & simulation,
        const VectorX & currentInitialPositions, real positionPrior, real & outputCost, BackgroundWorker * worker)
    {
        real poissonsRatio = simulation.getPoissonsRatio();
        real youngsModulus = simulation.getYoungsModulus();
		Vector2 gravity = simulation.getGravity();

        int n = static_cast<int>(currentInitialPositions.size() / 2);
        assert(n == simulation.getReferencePositions().size());
        for (int i = 0; i < n; ++i) {
            simulation.getReferencePositions()[i] = currentInitialPositions.segment<2>(2 * i);
        }

        //get winding order
        triangle_t referenceTriangle = {
            targetPositions[simulation.getTriangles()[0].x()],
            targetPositions[simulation.getTriangles()[0].y()],
            targetPositions[simulation.getTriangles()[0].z()]
        };
        int windingOrder = meshTriangleWindingOrder(referenceTriangle);

        //FORWARD

        MatrixX K = MatrixX::Zero(2 * simulation.getNumFreeNodes(), 2 * simulation.getNumFreeNodes());
        VectorX f = VectorX::Zero(2 * simulation.getNumFreeNodes());

        //assemble force vector F and stiffness matrix K
        simulation.assembleForceVector(f);
        if (worker->isInterrupted()) return VectorX::Zero(2 * n);
        real materialMu, materialLambda;
        SoftBodySimulation::computeMaterialParameters(youngsModulus, poissonsRatio, materialMu, materialLambda);
        Matrix3 C = SoftBodySimulation::computeMaterialMatrix(materialMu, materialLambda);
        simulation.assembleStiffnessMatrix(C, K, &f, SoftBodyMesh2D::Vector2List(simulation.getNumNodes(), Vector2::Zero()));
        if (worker->isInterrupted()) return VectorX::Zero(2 * n);

        //solve for the current displacement
        PartialPivLU<MatrixX> Klu = K.partialPivLu();
        VectorX u = Klu.solve(f);

        // DERIVATIVES
        int p = simulation.getNumFreeNodes();

        //compute derivate of the cost function with respect to the output u (displacement)
        VectorX costDu(2 * p);
        for (int i = 0; i < simulation.getNumNodes(); ++i)
        {
            if (simulation.getNodeToFreeMap()[i] < 0) continue;
            costDu.segment<2>(2 * simulation.getNodeToFreeMap()[i])
                = currentInitialPositions.segment<2>(2 * i) + u.segment<2>(2 * simulation.getNodeToFreeMap()[i])
                - targetPositions[i];
        }

        MatrixX F = MatrixX::Zero(2 * p, 2 * p);

        //current parameter: vertex 'col' with coordinate 'coord' -> j=2*col+coord
        //F(:,j) = -dK/dp*u + df/dp

        for (int e=0; e<simulation.getNumElements(); ++e)
        {
            const Vector3i& tri = simulation.getTriangles()[e];
            for (int i=0; i<3; ++i)
            {
                if (simulation.getNodeStates()[tri[i]] == SoftBodyMesh2D::DIRICHLET) continue;
                int col = simulation.getNodeToFreeMap()[tri[i]];
                for (int coord = 0; coord < 2; ++coord)
                {
                    //current parameter: vertex 'col' with coordinate 'coord'
                    //F(:,j) = -dK/dp*u + df/dp
                    int j = 2 * col + coord;
                    triangle_t triangle{
                        currentInitialPositions.segment<2>(2 * tri[0]),
                        currentInitialPositions.segment<2>(2 * tri[1]),
                        currentInitialPositions.segment<2>(2 * tri[2])
                    };
                    Matrix6 KeDpos = meshStiffnessMatrixDerivative(triangle, 2 * i + coord, C, windingOrder);
                    Vector6 ue = Vector6::Zero();
                    for (int i2 = 0; i2 < 3; ++i2)
                        if (simulation.getNodeStates()[tri[i2]] != SoftBodyMesh2D::DIRICHLET)
                            ue.segment<2>(2 * i2) = u.segment<2>(2 * simulation.getNodeToFreeMap()[tri[i2]]);
                    Vector6 y = KeDpos * ue;
                    //F(:,j) -= dK/dp*u per triangle
                    for (int i2 = 0; i2 < 3; ++i2)
                        if (simulation.getNodeStates()[tri[i2]] != SoftBodyMesh2D::DIRICHLET)
                            F.block<2, 1>(2 * simulation.getNodeToFreeMap()[tri[i2]], j) -= y.segment<2>(2 * i2);
					//F(:,j) += Df/dp per triangle
					Vector6 FeDpos = meshForceDerivative(triangle, 2 * i + coord, gravity, windingOrder);
					for (int i2 = 0; i2 < 3; ++i2)
						if (simulation.getNodeStates()[tri[i2]] != SoftBodyMesh2D::DIRICHLET)
							F.block<2, 1>(2 * simulation.getNodeToFreeMap()[tri[i2]], j) += FeDpos.segment<2>(2 * i2);
                }
            }
        }

        //assemble final gradient
        VectorX priorGradient = VectorX::Zero(2 * p); //TODO
        MatrixX X = Klu.solve(F);
        VectorX g = costDu; // dJ / du
        VectorX gX = g.transpose() * X;
		VectorX r = costDu; // dJ / dp
        VectorX gradient = gX + r + positionPrior * priorGradient;

        //map gradients back to the whole grid
        VectorX fullGradient = VectorX::Zero(2 * simulation.getNumNodes());
        for (int i=0; i<p; ++i)
        {
            fullGradient.segment<2>(2 * simulation.getFreeToNodeMap()[i]) = gradient.segment<2>(2 * i);
        }

        //evaluate cost
        //cost on position difference
        real costMain = 0;
        for (int i = 0; i < simulation.getNumNodes(); ++i)
        {
            if (simulation.getNodeToFreeMap()[i] < 0) continue;
            costMain += (currentInitialPositions.segment<2>(2 * i) + u.segment<2>(2 * simulation.getNodeToFreeMap()[i])
                - targetPositions[i]).squaredNorm() / 2;
        }
        //cost by the prior
        real costPrior = 0;
        for (int tri = 0; tri<simulation.getNumElements(); ++tri)
        {
            triangle_t triangle{
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][0]),
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][1]),
                currentInitialPositions.segment<2>(2 * simulation.getTriangles()[tri][2])
            };
            costPrior += windingOrder * meshTriangleArea(triangle) / (
                (triangle[0] - triangle[1]).squaredNorm() + (triangle[0] - triangle[2]).squaredNorm() + (triangle[2] - triangle[1]).squaredNorm()
                );
        }
        //final cost
        outputCost = costMain + positionPrior * costPrior;
        //CI_LOG_I("Optimization: cost " << outputCost << ", gradient " << gradient.transpose());

        return fullGradient;
    }

    VectorX InverseProblem_Adjoint_InitialConfiguration::gradientMesh_Numerical(
        const SoftBodyMesh2D::Vector2List & targetPositions, SoftBodyMesh2D & simulation,
        const VectorX & currentInitialPositions, real positionPrior, real & outputCost, BackgroundWorker * worker)
    {
        real poissonsRatio = simulation.getPoissonsRatio();
        real youngsModulus = simulation.getYoungsModulus();
        int n = simulation.getNumNodes();
        for (int i = 0; i < n; ++i) {
            simulation.getReferencePositions()[i] = currentInitialPositions.segment<2>(2 * i);
        }

        int p = simulation.getNumFreeNodes();
        VectorX gradient = VectorX::Zero(2 * n);

        // Reference forward problem
        real materialMu, materialLambda;
        SoftBodySimulation::computeMaterialParameters(youngsModulus, poissonsRatio, materialMu, materialLambda);
        Matrix3 C = SoftBodySimulation::computeMaterialMatrix(materialMu, materialLambda);

        MatrixX K = MatrixX::Zero(2 * p, 2 * p);
        VectorX F = VectorX::Zero(2 * p);
        simulation.assembleForceVector(F);
        simulation.assembleStiffnessMatrix(C, K, &F, SoftBodyMesh2D::Vector2List(simulation.getNumNodes(), Vector2::Zero()));
        VectorX u = K.partialPivLu().solve(F);
        if (worker->isInterrupted()) return gradient;

        real costReference = 0;
        for (int i=0; i<p; ++i)
        {
            costReference += (currentInitialPositions.segment<2>(2 * simulation.getFreeToNodeMap()[i])
                + u.segment<2>(2 * i)
                - targetPositions[simulation.getFreeToNodeMap()[i]])
                .squaredNorm() / 2;
        }

        //numerical differentiation
        real epsilon = 1e-7;
        for (int j=0; j<p; ++j)
        {
            for (int c=0; c<2; ++c)
            {
                VectorX positions = currentInitialPositions;
                positions[2 * simulation.getFreeToNodeMap()[j] + c] += epsilon;
                for (int i = 0; i < n; ++i) {
                    simulation.getReferencePositions()[i] = positions.segment<2>(2 * i);
                }
                K.setZero();
                F.setZero();
                simulation.assembleForceVector(F);
                simulation.assembleStiffnessMatrix(C, K, &F, SoftBodyMesh2D::Vector2List(simulation.getNumNodes(), Vector2::Zero()));
                VectorX u = K.partialPivLu().solve(F);
				if (worker->isInterrupted()) return gradient;

                real cost = 0;
                for (int i = 0; i<p; ++i)
                {
                    cost += (positions.segment<2>(2 * simulation.getFreeToNodeMap()[i])
                        + u.segment<2>(2 * i)
                        - targetPositions[simulation.getFreeToNodeMap()[i]])
                        .squaredNorm() / 2;
                }
                gradient[2 * simulation.getFreeToNodeMap()[j] + c] = (cost - costReference) / epsilon;
            }
        }

        outputCost = costReference;
        return gradient;
    }

	InverseProblem_Adjoint_InitialConfiguration::grid_t InverseProblem_Adjoint_InitialConfiguration::gradientGrid_AdjointMethod(
		const grid_t & targetSdf, SoftBodyGrid2D & simulation,
		const grid_t & initialSdf, const GridPriors& priors,
		real & outputCost, BackgroundWorker * worker, bool onlyCost)
	{
		//only these modes are supported
		assert(simulation.isExplicitDiffusion());
		assert(simulation.isHardDirichletBoundaries());
		assert(simulation.getRotationCorrection() == SoftBodySimulation::RotationCorrection::None);
        assert(simulation.getAdvectionMode() == SoftBodyGrid2D::AdvectionMode::DIRECT_FORWARD);

		//get settings from the simulation
		real poissonsRatio = simulation.getPoissonsRatio();
		real youngsModulus = simulation.getYoungsModulus();
		Vector2 gravity = simulation.getGravity();
		int resolution = simulation.getGridResolution();
		Vector2 size = simulation.getCellSize();
		real h = size.x();

		// allocate output
		outputCost = 0;
		grid_t outputGradient = grid_t::Zero(resolution, resolution);

		//FORWARD

		//set initial / current SDF as the reference SDF in the simulation
		//and the degrees of freedom are computed as well
		simulation.setSDF(initialSdf);
		//collect degrees of freedom in the stifness solve
		const Eigen::MatrixXi& posToIndex = simulation.getPosToIndex();
		const SoftBodyGrid2D::indexToPos_t& indexToPos = simulation.getIndexToPos();
		const int dof = simulation.getDoF();

		MatrixX K = MatrixX::Zero(dof * 2, dof * 2);
		VectorX f = VectorX::Zero(dof * 2);
		const VectorX prevU = VectorX::Zero(dof * 2); //previous displacements for rotation correction. Not needed here

		//assemble force vector f and stiffness matrix K
		VectorX collisionForces = VectorX::Zero(2 * dof);
		simulation.assembleForceVector(f, posToIndex, collisionForces);
		if (worker->isInterrupted()) return outputGradient;
		real materialMu, materialLambda;
		SoftBodySimulation::computeMaterialParameters(youngsModulus, poissonsRatio, materialMu, materialLambda);
		Matrix3 C = SoftBodySimulation::computeMaterialMatrix(materialMu, materialLambda);
		simulation.assembleStiffnessMatrix(C, materialMu, materialLambda, K, &f, posToIndex, prevU);
		if (worker->isInterrupted()) return outputGradient;

		//solve for the current displacement
		PartialPivLU<MatrixX> Klu = K.partialPivLu();
		VectorX u = Klu.solve(f);
		if (worker->isInterrupted()) return outputGradient;

		//map back to a grid
		grid_t uGridX = grid_t::Zero(resolution, resolution);
		grid_t uGridY = grid_t::Zero(resolution, resolution);
		for (int i = 0; i < dof; ++i) {
			Vector2i p = indexToPos.at(i);
			uGridX(p.x(), p.y()) = u[2 * i];
			uGridY(p.x(), p.y()) = u[2 * i + 1];
		}
		if (worker->isInterrupted()) return outputGradient;

		//perform diffusion step
		GridUtils2D::bgrid_t validCells = simulation.computeValidCells(uGridX, uGridY, posToIndex);
		uGridX = GridUtils2D::fillGridDiffusion(uGridX, validCells);
		uGridY = GridUtils2D::fillGridDiffusion(uGridY, validCells);
		if (worker->isInterrupted()) return outputGradient;

		//advect levelset
		grid_t advectionWeights;
		grid_t forwardSdf = GridUtils2D::advectGridDirectForward(initialSdf, uGridX, uGridY, -1 / h, &advectionWeights);
		if (worker->isInterrupted()) return outputGradient;

		//reconstruct SDF
        GridUtils2D::RecoverSDFSussmannAdjointStorage recoveryStorage;
        grid_t forwardSdf2 = simulation.getSdfRecoveryIterations() > 0
            ? GridUtils2D::recoverSDFSussmann(forwardSdf, real(0.01), simulation.getSdfRecoveryIterations(), &recoveryStorage)
			: forwardSdf;

		// COST
		grid_t costWeighting = advectionWeights.min(1);
		outputCost += gridCostSimilarity(forwardSdf, targetSdf, &costWeighting);
		// prior on the SDF
		real costPriorSdf = 0;
		grid_t priorSdfGrad = GridUtils2D::recoverSDFSussmannGradient(initialSdf, initialSdf, GridUtils2D::bgrid_t::Constant(resolution, resolution, true), costPriorSdf, priors.sdfEpsilon);
		outputCost += priors.sdfPrior * costPriorSdf;

        if (onlyCost)
            return outputGradient;

		// BACKWARD

		//The derivative of the cost with respect to the forward (output) SDF
		grid_t costDforwardSdf1 = gridCostSimilarityDerivative(forwardSdf2, targetSdf, &costWeighting);

		// propagate back, get the adjoint solutions

        // Adjoint: recover SDF
        grid_t adjOutputSdf = grid_t::Zero(resolution, resolution);
        GridUtils2D::recoverSDFSussmannAdjoint(forwardSdf, forwardSdf2, costDforwardSdf1, adjOutputSdf, recoveryStorage);

		//Adjoint: advect levelset
		grid_t adjInputSdf = grid_t::Zero(resolution, resolution);
		grid_t adjUGridX = grid_t::Zero(resolution, resolution);
		grid_t adjUGridY = grid_t::Zero(resolution, resolution);
		GridUtils2D::advectGridDirectForwardAdjoint(
			adjInputSdf, adjUGridX, adjUGridY, adjOutputSdf,
			uGridX, uGridY, initialSdf, forwardSdf, -1 / h, advectionWeights);
		if (worker->isInterrupted()) return outputGradient;

		//Adjoint: perform diffusion step
		GridUtils2D::fillGridDiffusionAdjoint(adjUGridX, adjUGridX, uGridX, validCells); //note that the adjoint is both input and output
		GridUtils2D::fillGridDiffusionAdjoint(adjUGridY, adjUGridY, uGridY, validCells); //it is modified in-place
		if (worker->isInterrupted()) return outputGradient;

		//Adjoint: map back to a grid
		VectorX adjU(2 * dof);
		for (int i = 0; i < dof; ++i) {
			Vector2i p = indexToPos.at(i);
			adjU[2 * i] = adjUGridX(p.x(), p.y());
			adjU[2 * i + 1] = adjUGridY(p.x(), p.y());
		}
		if (worker->isInterrupted()) return outputGradient;

		//Adjoint: solve for the current displacement
		//since K is symmetric, I can use K directly instead of K' as normally required by the Adjoint Method.
		VectorX lambdaU = Klu.solve(adjU);
		if (worker->isInterrupted()) return outputGradient;

		// DERIVATIVES OF THE UNKNOWNS
		// Matrix-Free assembly of y'F, y = (lambdaU, adjInputSdf)

		// advection / currentSdf
		grid_t gradientAdvection
    		= GridUtils2D::advectGridDirectForwardAdjOpMult(adjInputSdf, uGridX, uGridY, -1 / h, advectionWeights);

		// elasticity / lambdaU -> compute element-wise
        grid_t gradientElasticity =
            gridElasticityAdjOpMult(lambdaU, u, initialSdf,
                gravity, C, simulation, posToIndex);

		// ASSEMBLE FINAL GRADIENT
		grid_t gradientPriors = grid_t::Zero(resolution, resolution); //(r in the notes)
		gradientPriors += priorSdfGrad.unaryExpr([&priors](real v)
		{
			return priors.sdfPrior * std::clamp(v, -priors.sdfMaxDistance, priors.sdfMaxDistance);
		});
		outputGradient = gradientAdvection + gradientElasticity + gradientPriors;

        CI_LOG_I("Optimize: cost " << outputCost);

		return outputGradient;
	}

	void InverseProblem_Adjoint_InitialConfiguration::setupParams(cinder::params::InterfaceGlRef params,
		const std::string& group)
	{
		params->addParam("InverseProblem_Adjoint_InitialConfiguration_PositionPrior", &meshPositionPrior)
			.group(group).label("Mesh Prior - Initial Positions").min(0).step(0.01);
        params->addParam("InverseProblem_Adjoint_InitialConfiguration_SDFPrior1", &gridPriors.sdfPrior)
            .group(group).label("Grid Prior - Weight on SDF").min(0).step(0.01);
		params->addParam("InverseProblem_Adjoint_InitialConfiguration_SDFPrior2", &gridPriors.sdfEpsilon)
			.group(group).label("Grid Prior - SDF Epsilon").min(0).step(0.01);
		params->addParam("InverseProblem_Adjoint_InitialConfiguration_SDFPrior3", &gridPriors.sdfMaxDistance)
			.group(group).label("Grid Prior - SDF Max distance").min(0).step(0.1);
		params->addParam("InverseProblem_Adjoint_InitialConfiguration_GridHyperEpsilon", &gridHyperEpsilon)
			.group(group).label("Grid Hyper Epsilon").min(0).step(0.01);
		params->addParam("InverseProblem_Adjoint_InitialConfiguration_NumIterations", &numIterations)
			.group(group).label("Iterations").min(1);
	}

	void InverseProblem_Adjoint_InitialConfiguration::setParamsVisibility(cinder::params::InterfaceGlRef params,
		bool visible) const
	{
		std::string option = visible ? "visible=true" : "visible=false";
		params->setOptions("InverseProblem_Adjoint_InitialConfiguration_PositionPrior", option);
        params->setOptions("InverseProblem_Adjoint_InitialConfiguration_SDFPrior1", option);
		params->setOptions("InverseProblem_Adjoint_InitialConfiguration_SDFPrior2", option);
		params->setOptions("InverseProblem_Adjoint_InitialConfiguration_SDFPrior3", option);
		params->setOptions("InverseProblem_Adjoint_InitialConfiguration_GridHyperEpsilon", option);
		params->setOptions("InverseProblem_Adjoint_InitialConfiguration_NumIterations", option);
	}

	VectorX InverseProblem_Adjoint_InitialConfiguration::linearizePositions(const SoftBodyMesh2D::Vector2List & positions)
    {
        size_t n = positions.size();
        VectorX x(n*2);
        for (size_t i=0; i<n; ++i)
        {
            x.segment<2>(2 * i) = positions[i];
        }
        return x;
    }

    SoftBodyMesh2D::Vector2List InverseProblem_Adjoint_InitialConfiguration::delinearizePositions(const VectorX & linearizedPositions)
    {
        size_t n = linearizedPositions.size() / 2;
        SoftBodyMesh2D::Vector2List l(n);
        for (size_t i=0; i<n; ++i)
        {
            l[i] = linearizedPositions.segment<2>(2 * i);
        }
        return l;
    }

    real InverseProblem_Adjoint_InitialConfiguration::meshTriangleArea(const triangle_t & triangle)
    {
        return ((triangle[1].x() - triangle[0].x())*(triangle[2].y() - triangle[0].y()) - (triangle[2].x() - triangle[0].x())*(triangle[1].y() - triangle[0].y())) / 2;
    }

	int InverseProblem_Adjoint_InitialConfiguration::meshTriangleWindingOrder(const triangle_t & triangle)
	{
		return meshTriangleArea(triangle) > 0 ? 1 : -1;
	}

    std::array<real, 6> InverseProblem_Adjoint_InitialConfiguration::meshTriangleAreaDerivative(const triangle_t & triangle)
    {
        return std::array<real, 6>({
            (triangle[1].y() - triangle[2].y()) / 2,
            (triangle[2].x() - triangle[1].x()) / 2,
            (triangle[2].y() - triangle[0].y()) / 2,
            (triangle[0].x() - triangle[2].x()) / 2,
            (triangle[0].y() - triangle[1].y()) / 2,
            (triangle[1].x() - triangle[0].x()) / 2
        });
    }

    Matrix36 InverseProblem_Adjoint_InitialConfiguration::meshDerivativeMatrix(const triangle_t & triangle)
    {
        Matrix36 Be;
        Be << (triangle[1].y() - triangle[2].y()), 0, (triangle[2].y() - triangle[0].y()), 0, (triangle[0].y() - triangle[1].y()), 0,
            0, (triangle[2].x() - triangle[1].x()), 0, (triangle[0].x() - triangle[2].x()), 0, (triangle[1].x() - triangle[0].x()),
            (triangle[2].x() - triangle[1].x()), (triangle[1].y() - triangle[2].y()), (triangle[0].x() - triangle[2].x()), (triangle[2].y() - triangle[0].y()), (triangle[1].x() - triangle[0].x()), (triangle[0].y() - triangle[1].y());
        Be *= (1 / meshTriangleArea(triangle));
        return Be;
    }

    Matrix36 InverseProblem_Adjoint_InitialConfiguration::meshDerivativeMatrixDerivative(const triangle_t & triangle, int i)
    {
        static const std::array<Matrix36, 6> Bprime = {
            (Matrix36() << 0,0,0,0,0,0, 0,0,0,1,0,-1, 0,0,1,0,-1,0).finished(), //d x1
            (Matrix36() << 0,0,-1,0,1,0, 0,0,0,0,0,0, 0,0,0,-1,0,1).finished(), //d y1
            (Matrix36() << 0,0,0,0,0,0, 0,-1,0,0,0,1, -1,0,0,0,1,0).finished(), //d x2
            (Matrix36() << 1,0,0,0,-1,0, 0,0,0,0,0,0, 0,1,0,0,0,-1).finished(), //d y2
            (Matrix36() << 0,0,0,0,0,0, 0,1,0,-1,0,0, 1,0,-1,0,0,0).finished(), //d x3
            (Matrix36() << -1,0,1,0,0,0, 0,0,0,0,0,0, 0,-1,0,1,0,0).finished()  //d y3
        };
        //return (1 / meshTriangleArea(triangle)) * (Bprime[i] - meshDerivativeMatrix(triangle) * meshTriangleAreaDerivative(triangle)[i]);
        real area = meshTriangleArea(triangle);
        return (Bprime[i] / area - meshDerivativeMatrix(triangle)*meshTriangleAreaDerivative(triangle)[i] / area);
    }

    Matrix6 InverseProblem_Adjoint_InitialConfiguration::meshStiffnessMatrix(const triangle_t & triangle, const Matrix3 & C)
    {
        Matrix36 Be = meshDerivativeMatrix(triangle);
        return abs(meshTriangleArea(triangle)) * Be.transpose() * C * Be;
    }

    Matrix6 InverseProblem_Adjoint_InitialConfiguration::meshStiffnessMatrixDerivative(const triangle_t & triangle, int i, const Matrix3& C, int windingOrder)
    {
        Matrix36 Be = meshDerivativeMatrix(triangle);
        Matrix36 BePrime = meshDerivativeMatrixDerivative(triangle, i);
        return windingOrder * (meshTriangleAreaDerivative(triangle)[i] * (Be.transpose() * C * Be)
             + meshTriangleArea(triangle) * (BePrime.transpose() * C * Be + Be.transpose() * C * BePrime));
    }

	Vector6 InverseProblem_Adjoint_InitialConfiguration::meshForce(const triangle_t & triangle, const Vector2 & gravity)
	{
		return abs(meshTriangleArea(triangle)) * (Vector6() << gravity, gravity, gravity).finished();
	}

	Vector6 InverseProblem_Adjoint_InitialConfiguration::meshForceDerivative(const triangle_t & triangle, int i, const Vector2 & gravity, int windingOrder)
	{
		return meshTriangleAreaDerivative(triangle)[i] * windingOrder * (Vector6() << gravity, gravity, gravity).finished();
	}

	real InverseProblem_Adjoint_InitialConfiguration::gridCostSimilarity(
		const grid_t & currentPhi, const grid_t & targetPhi,
		const grid_t * weighting)
	{
		if (weighting)
			return ((*weighting) * (currentPhi.unaryExpr(&sdfTransformFun) - targetPhi.unaryExpr(&sdfTransformFun))).matrix().squaredNorm() / 2;
		else
			return (currentPhi.unaryExpr(&sdfTransformFun) - targetPhi.unaryExpr(&sdfTransformFun)).matrix().squaredNorm() / 2;
	}

	InverseProblem_Adjoint_InitialConfiguration::grid_t InverseProblem_Adjoint_InitialConfiguration::gridCostSimilarityDerivative(
		const grid_t& currentPhi, const grid_t& targetPhi,
		const grid_t* weighting)
	{
		if (weighting)
			return currentPhi.unaryExpr(&sdfTransformFunDerivative) * (*weighting) * (*weighting) * (currentPhi.unaryExpr(&sdfTransformFun) - targetPhi.unaryExpr(&sdfTransformFun));
		else
			return currentPhi.unaryExpr(&sdfTransformFunDerivative) * (currentPhi.unaryExpr(&sdfTransformFun) - targetPhi.unaryExpr(&sdfTransformFun));
	}

	std::array<Vector8, 4> InverseProblem_Adjoint_InitialConfiguration::gridGravityForceDerivative(
		const std::array<real, 4>& sdfs, const Vector2& gravity, const SoftBodyGrid2D& simulation)
	{
		Vector8 bodyForces;
		bodyForces << gravity, gravity, gravity, gravity;
		std::array<real, 4> derivatives = Integration2D<real>::integrateQuadDsdf<real>(simulation.getCellSize(), sdfs, { 1,1,1,1 });
		return {
			bodyForces * derivatives[0],
			bodyForces * derivatives[1],
			bodyForces * derivatives[2],
			bodyForces * derivatives[3]
		};
	}

	Vector8 InverseProblem_Adjoint_InitialConfiguration::gridGravityForce(const std::array<real, 4>& sdfs,
		const Vector2& gravity, const SoftBodyGrid2D& simulation)
	{
		Vector8 bodyForces;
		bodyForces << gravity, gravity, gravity, gravity;
		return bodyForces * Integration2D<real>::integrateQuad<real>(simulation.getCellSize(), sdfs, { 1,1,1,1 });
	}

	std::array<Matrix8, 4> InverseProblem_Adjoint_InitialConfiguration::gridStiffnessMatrixDerivative(
		const std::array<real, 4>& sdfs, const Matrix3& C, const SoftBodyGrid2D& simulation)
	{
		array<Matrix8, 4> Kex = {
			simulation.getB1().transpose() * C * simulation.getB1(),
			simulation.getB2().transpose() * C * simulation.getB2(),
			simulation.getB3().transpose() * C * simulation.getB3(),
			simulation.getB4().transpose() * C * simulation.getB4(),
		};
		return Integration2D<real>::integrateQuadDsdf(simulation.getCellSize(), sdfs, Kex);
	}

	Matrix8 InverseProblem_Adjoint_InitialConfiguration::gridStiffnessMatrix(const std::array<real, 4>& sdfs,
		const Matrix3& C, const SoftBodyGrid2D& simulation)
	{
		array<Matrix8, 4> Kex = {
			simulation.getB1().transpose() * C * simulation.getB1(),
			simulation.getB2().transpose() * C * simulation.getB2(),
			simulation.getB3().transpose() * C * simulation.getB3(),
			simulation.getB4().transpose() * C * simulation.getB4(),
		};
		return Integration2D<real>::integrateQuad(simulation.getCellSize(), sdfs, Kex);
	}

	InverseProblem_Adjoint_InitialConfiguration::grid_t InverseProblem_Adjoint_InitialConfiguration::gridElasticityAdjOpMult(
        const VectorX & lambdaU, const VectorX& u, const grid_t & sdf,
        const Vector2 & gravity, const Matrix3 & C, const SoftBodyGrid2D & simulation,
        const Eigen::MatrixXi & posToIndex)
    {
		////Test: not matrix-free
		//int dof = lambdaU.size() / 2;
		//MatrixX F = MatrixX::Zero(2 * dof, dof);

        int resolution = simulation.getGridResolution();
        grid_t gradientElasticity = grid_t::Zero(resolution, resolution);
        for (int xy = 0; xy < (resolution - 1)*(resolution - 1); ++xy) {
            int x = xy / (resolution - 1);
            int y = xy % (resolution - 1);

            array<real, 4> sdfs = { sdf(x, y), sdf(x + 1,y), sdf(x, y + 1), sdf(x + 1, y + 1) };
            if (utils::outside(sdfs[0]) && utils::outside(sdfs[1]) && utils::outside(sdfs[2]) && utils::outside(sdfs[3])) continue;

            std::array<Vector8, 4> FeDphi = gridGravityForceDerivative(sdfs, gravity, simulation);
            std::array<Matrix8, 4> KeDphi = gridStiffnessMatrixDerivative(sdfs, C, simulation);

            int mapping[4] = {
                posToIndex(x, y),
                posToIndex(x + 1, y),
                posToIndex(x, y + 1),
                posToIndex(x + 1, y + 1)
            };
            bool dirichlet[4] = {
                simulation.getGridDirichlet()(x, y),
                simulation.getGridDirichlet()(x + 1, y),
                simulation.getGridDirichlet()(x, y + 1),
                simulation.getGridDirichlet()(x + 1, y + 1)
            };
            Vector8 ue;
            Vector8 lambdaUE;
            for (int i = 0; i < 4; ++i) {
                ue.segment<2>(2 * i) = u.segment<2>(2 * mapping[i]);
                lambdaUE.segment<2>(2 * i) = lambdaU.segment<2>(2 * mapping[i]);
            }
            for (int i = 0; i < 4; ++i) {
                if (dirichlet[i]) continue;
                Vector8 EeDphi = -(KeDphi[i] * ue - FeDphi[i]);
                gradientElasticity(x + (i % 2), y + (i / 2)) += lambdaUE.dot(EeDphi);
				//for (int j=0; j<4; ++j)
				//{
				//	F.block<2, 1>(2 * mapping[j], mapping[i]) += EeDphi.segment<2>(2 * j);
				//}
            }
        }

		//VectorX result = lambdaU.transpose() * F;
		//for (int x=0; x<resolution; ++x) for (int y=0; y<resolution; ++y)
		//{
		//	int i = posToIndex(x, y);
		//	if (i >= 0)
		//		gradientElasticity(x, y) = result[i];
		//}

        return gradientElasticity;
    }
}


/**
 * \file TimeVaryingSecondOrderSVFFilter.cpp
 */

#include "TimeVaryingSecondOrderSVFFilter.h"

#include <boost/math/constants/constants.hpp>

#include <cassert>

namespace ATK
{
  template<typename SVFCoefficients>
  class TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
  public:
    typename SVFCoefficients::DataType iceq1{0};
    typename SVFCoefficients::DataType iceq2{0};
  };

  template<typename SVFCoefficients>
  TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::TimeVaryingSecondOrderSVFFilter(gsl::index nb_channels)
  :SVFCoefficients(nb_channels), state(std::make_unique<SVFState[]>(nb_channels))
  {
  }

  template<typename SVFCoefficients>
  TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::~TimeVaryingSecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void TimeVaryingSecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state = std::make_unique<SVFState[]>(nb_input_ports - 1);
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFFilter<DataType>::process_impl(gsl::index size) const
  {
    assert(nb_input_ports - 1 == nb_output_ports);

    for(gsl::index i = 0; i < size; ++i)
    {
      update_coeffs(converted_inputs[0][i]);

      for(gsl::index j = 0; j < nb_input_ports - 1; ++j)
      {
        const DataType* ATK_RESTRICT input = converted_inputs[j+1];
        DataType* ATK_RESTRICT output = outputs[j];

        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = 2 * v1 - state[j].iceq1;
        state[j].iceq2 = 2 * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  TimeVaryingSecondOrderSVFBaseCoefficients<DataType>::TimeVaryingSecondOrderSVFBaseCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(1 + nb_channels, nb_channels)
  {
  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFBaseCoefficients<DataType_>::set_Q(DataType_ Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be strictly positive");
    }
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFLowPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFLowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFLowPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFBandPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFBandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFBandPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFHighPassCoefficients<DataType_>::TimeVaryingSecondOrderSVFHighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFHighPassCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFNotchCoefficients<DataType_>::TimeVaryingSecondOrderSVFNotchCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFNotchCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFPeakCoefficients<DataType_>::TimeVaryingSecondOrderSVFPeakCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFPeakCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFBellCoefficients<DataType_>::TimeVaryingSecondOrderSVFBellCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {

  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFBellCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be strictly positive");
    }
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFBellCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / (Q * gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType_>::TimeVaryingSecondOrderSVFLowShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {

  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFLowShelfCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType_>::TimeVaryingSecondOrderSVFHighShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType_>
  void TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(DataType_ gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  DataType TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void TimeVaryingSecondOrderSVFHighShelfCoefficients<DataType>::update_coeffs(DataType g) const
  {
    auto k = 1 / (Q* gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

#if ATK_ENABLE_INSTANTIATION
  template class TimeVaryingSecondOrderSVFBaseCoefficients<float>;

  template class TimeVaryingSecondOrderSVFLowPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFBandPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFHighPassCoefficients<float>;
  template class TimeVaryingSecondOrderSVFNotchCoefficients<float>;
  template class TimeVaryingSecondOrderSVFPeakCoefficients<float>;
  template class TimeVaryingSecondOrderSVFBellCoefficients<float>;
  template class TimeVaryingSecondOrderSVFLowShelfCoefficients<float>;
  template class TimeVaryingSecondOrderSVFHighShelfCoefficients<float>;

  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBandPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighPassCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFNotchCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFPeakCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBellCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowShelfCoefficients<float> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighShelfCoefficients<float> >;
#endif
  template class TimeVaryingSecondOrderSVFBaseCoefficients<double>;

  template class TimeVaryingSecondOrderSVFLowPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFBandPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFHighPassCoefficients<double>;
  template class TimeVaryingSecondOrderSVFNotchCoefficients<double>;
  template class TimeVaryingSecondOrderSVFPeakCoefficients<double>;
  template class TimeVaryingSecondOrderSVFBellCoefficients<double>;
  template class TimeVaryingSecondOrderSVFLowShelfCoefficients<double>;
  template class TimeVaryingSecondOrderSVFHighShelfCoefficients<double>;

  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBandPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighPassCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFNotchCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFPeakCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFBellCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFLowShelfCoefficients<double> >;
  template class TimeVaryingSecondOrderSVFFilter<TimeVaryingSecondOrderSVFHighShelfCoefficients<double> >;
}


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

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

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/strategies/buffer.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_most_precise.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
#include <boost/geometry/io/wkt/wkt.hpp>
#endif

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace buffer
{

class join_round_by_divide
{
public :

    inline join_round_by_divide(std::size_t max_level = 4)
        : m_max_level(max_level)
    {}

    template
    <
        typename PromotedType,
        typename Point,
        typename DistanceType,
        typename RangeOut
    >
    inline void mid_points(Point const& vertex,
                Point const& p1, Point const& p2,
                DistanceType const& buffer_distance,
                RangeOut& range_out,
                std::size_t level = 1) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;

        // Generate 'vectors'
        coordinate_type const vp1_x = get<0>(p1) - get<0>(vertex);
        coordinate_type const vp1_y = get<1>(p1) - get<1>(vertex);

        coordinate_type const vp2_x = (get<0>(p2) - get<0>(vertex));
        coordinate_type const vp2_y = (get<1>(p2) - get<1>(vertex));

        // Average them to generate vector in between
        coordinate_type const two = 2;
        coordinate_type const v_x = (vp1_x + vp2_x) / two;
        coordinate_type const v_y = (vp1_y + vp2_y) / two;

        PromotedType const length2 = geometry::math::sqrt(v_x * v_x + v_y * v_y);

        PromotedType prop = buffer_distance / length2;

        Point mid_point;
        set<0>(mid_point, get<0>(vertex) + v_x * prop);
        set<1>(mid_point, get<1>(vertex) + v_y * prop);

        if (level < m_max_level)
        {
            mid_points<PromotedType>(vertex, p1, mid_point, buffer_distance, range_out, level + 1);
        }
        range_out.push_back(mid_point);
        if (level < m_max_level)
        {
            mid_points<PromotedType>(vertex, mid_point, p2, buffer_distance, range_out, level + 1);
        }
    }

    template <typename Point, typename DistanceType, typename RangeOut>
    inline bool apply(Point const& ip, Point const& vertex,
                Point const& perp1, Point const& perp2,
                DistanceType const& buffer_distance,
                RangeOut& range_out) const
    {
        typedef typename coordinate_type<Point>::type coordinate_type;

        typedef typename geometry::select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        geometry::equal_to<Point> equals;

        if (equals(perp1, perp2))
        {
#ifdef BOOST_GEOMETRY_DEBUG_BUFFER_WARN
            std::cout << "Corner for equal points " << geometry::wkt(ip) << " " << geometry::wkt(perp1) << std::endl;
#endif
            return false;
        }

        // Generate 'vectors'
        coordinate_type const vix = (get<0>(ip) - get<0>(vertex));
        coordinate_type const viy = (get<1>(ip) - get<1>(vertex));

        promoted_type const length_i = geometry::math::sqrt(vix * vix + viy * viy);

        promoted_type const bd = geometry::math::abs(buffer_distance);
        promoted_type prop = bd / length_i;

        Point bp;
        set<0>(bp, get<0>(vertex) + vix * prop);
        set<1>(bp, get<1>(vertex) + viy * prop);

        range_out.push_back(perp1);

        if (m_max_level > 1)
        {
            mid_points<promoted_type>(vertex, perp1, bp, bd, range_out);
            range_out.push_back(bp);
            mid_points<promoted_type>(vertex, bp, perp2, bd, range_out);
        }
        else if (m_max_level == 1)
        {
            range_out.push_back(bp);
        }

        range_out.push_back(perp2);
        return true;
    }

    template <typename NumericType>
    static inline NumericType max_distance(NumericType const& distance)
    {
        return distance;
    }

private :
    std::size_t m_max_level;
};

}} // namespace strategy::buffer

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BUFFER_JOIN_ROUND_BY_DIVIDE_HPP


#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <ceres/gradient_checker.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/gravity-provider.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "ceres-error-terms/inertial-error-term-eigen.h"
#include "ceres-error-terms/parameterization/quaternion-param-eigen.h"

using ceres_error_terms::InertialErrorTermEigen;

class PosegraphErrorTermsEigen : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    rot0_.coeffs() << 0, 0, 0, 1;
    rot1_.coeffs() << 0, 0, 0, 1;
    pos0_ << 0, 0, 0;
    pos1_ << 1.5, 0, 0;

    accel_bias0_ << 0, 0, 0;
    accel_bias1_ << 0, 0, 0;
    gyro_bias0_ << 0, 0, 0;
    gyro_bias1_ << 0, 0, 0;

    velocity0_ << 1, 0, 0;
    velocity1_ << 2, 0, 0;

    common::GravityProvider gravity_provider(
        common::locations::kAltitudeZurichMeters,
        common::locations::kLatitudeZurichDegrees);
    gravity_magnitude_ = gravity_provider.getGravityMagnitude();

    imu_timestamps_ns_ << 0, 0.5 * 1e9, 1.0 * 1e9;
    imu_data_ << 1, 1, 1, 0, 0, 0, gravity_magnitude_, gravity_magnitude_,
        gravity_magnitude_, 0, 0, 0, 0, 0, 0, 0, 0, 0;
  }

  void addResidual();
  void solve();
  void checkGradient();

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Matrix<int64_t, 1, 3> imu_timestamps_ns_;
  Eigen::Matrix<double, 6, 3> imu_data_;

  Eigen::Quaterniond rot0_;
  Eigen::Quaterniond rot1_;
  Eigen::Vector3d pos0_;
  Eigen::Vector3d pos1_;

  Eigen::Vector3d accel_bias0_;
  Eigen::Vector3d accel_bias1_;
  Eigen::Vector3d gyro_bias0_;
  Eigen::Vector3d gyro_bias1_;

  Eigen::Vector3d velocity0_;
  Eigen::Vector3d velocity1_;

  Eigen::Matrix<double, 6, 1> imu_bias0_;
  Eigen::Matrix<double, 6, 1> imu_bias1_;

  double gravity_magnitude_;
};

void PosegraphErrorTermsEigen::addResidual() {
  rot0_.normalize();
  rot1_.normalize();
  imu_bias0_ << gyro_bias0_, accel_bias0_;
  imu_bias1_ << gyro_bias1_, accel_bias1_;

  ceres::CostFunction* inertial_term_cost =
      new ceres_error_terms::InertialErrorTermEigen(
          imu_data_, imu_timestamps_ns_, 1, 1, 1, 1, gravity_magnitude_);

  problem_.AddResidualBlock(
      inertial_term_cost, NULL, rot0_.coeffs().data(), pos0_.data(),
      velocity0_.data(), imu_bias0_.data(), rot1_.coeffs().data(), pos1_.data(),
      velocity1_.data(), imu_bias1_.data());

  ceres::LocalParameterization* quaternion_parameterization =
      new ceres_error_terms::EigenQuaternionParameterization;
  problem_.SetParameterization(
      rot0_.coeffs().data(), quaternion_parameterization);
  problem_.SetParameterization(
      rot1_.coeffs().data(), quaternion_parameterization);
}

void PosegraphErrorTermsEigen::checkGradient() {
  rot0_.normalize();
  rot1_.normalize();
  imu_bias0_ << gyro_bias0_, accel_bias0_;
  imu_bias1_ << gyro_bias1_, accel_bias1_;

  ceres::CostFunction* inertial_term_cost =
      new ceres_error_terms::InertialErrorTermEigen(
          imu_data_, imu_timestamps_ns_, 1, 1, 1, 1, gravity_magnitude_);

  std::vector<double*> parameter_blocks;
  parameter_blocks.push_back(rot0_.coeffs().data());
  parameter_blocks.push_back(pos0_.data());
  parameter_blocks.push_back(velocity0_.data());
  parameter_blocks.push_back(imu_bias0_.data());
  parameter_blocks.push_back(rot1_.coeffs().data());
  parameter_blocks.push_back(pos1_.data());
  parameter_blocks.push_back(velocity1_.data());
  parameter_blocks.push_back(imu_bias1_.data());

  ceres::LocalParameterization* orientation_parameterization =
      new ceres_error_terms::EigenQuaternionParameterization;

  ceres::NumericDiffOptions numeric_diff_options;
  numeric_diff_options.ridders_relative_initial_step_size = 1e-3;

  std::vector<const ceres::LocalParameterization*> local_parameterizations;
  local_parameterizations.push_back(orientation_parameterization);
  local_parameterizations.push_back(NULL);
  local_parameterizations.push_back(NULL);
  local_parameterizations.push_back(NULL);
  local_parameterizations.push_back(orientation_parameterization);
  local_parameterizations.push_back(NULL);
  local_parameterizations.push_back(NULL);
  local_parameterizations.push_back(NULL);

  ceres::GradientChecker gradient_checker(
      inertial_term_cost, &local_parameterizations, numeric_diff_options);
  ceres::GradientChecker::ProbeResults results;

  if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results)) {
    std::cout << "An error has occurred:\n" << results.error_log;
  }
}

void PosegraphErrorTermsEigen::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 500;
  options.gradient_tolerance = 1e-50;
  options.function_tolerance = 1e-50;
  options.parameter_tolerance = 1e-50;
  options.num_threads = 8;
  options.num_linear_solver_threads = 8;

  ceres::Solve(options, &problem_, &summary_);

  LOG(INFO) << summary_.message;
  LOG(INFO) << summary_.BriefReport();
}

TEST_F(PosegraphErrorTermsEigen, InertialTermZeroCost) {
  addResidual();
  solve();

  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermFinalPositionOptimization) {
  pos1_ << 1.43, -0.2, 0.175;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());

  solve();

  EXPECT_NEAR_EIGEN(pos1_, Eigen::Vector3d(1.5, 0, 0), 1e-15);
  EXPECT_NEAR_EIGEN(rot1_.coeffs(), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(
    PosegraphErrorTermsEigen, InertialTermFinalPositionVelocityOptimization) {
  pos1_ << 1.43, -0.2, 0.175;
  velocity1_ << 2.1, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());
  solve();
  EXPECT_NEAR_EIGEN(pos1_, Eigen::Vector3d(1.5, 0, 0), 1e-15);
  EXPECT_NEAR_EIGEN(rot1_.coeffs(), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermFinalRotationOptimization) {
  rot1_.coeffs() << 0.024225143749034013, 0.04470367401201076,
      0.04242220263937102, 0.9978051316080664;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());

  solve();
  //  EXPECT_NEAR_EIGEN(pos1_, pos1_, 1e-15);
  EXPECT_NEAR_EIGEN(rot1_.coeffs(), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermFinalVelocityOptimization) {
  velocity1_ << 2.1, 0, 0;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());

  solve();
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermStartPositionOptimization) {
  pos0_ << 0.13, -0.2, 0.175;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());
  solve();

  //  checkGradient();

  EXPECT_ZERO_EIGEN(pos0_, 1e-15);
  EXPECT_NEAR_EIGEN(rot0_.coeffs(), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermStartRotationOptimization) {
  rot0_.coeffs() << 0.024225143749034013, 0.04470367401201076,
      0.04242220263937102, 0.9978051316080664;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());
  solve();
  EXPECT_ZERO_EIGEN(pos0_, 1e-15);
  EXPECT_NEAR_EIGEN(rot0_.coeffs(), Eigen::Vector4d(0, 0, 0, 1), 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermStartVelocityOptimization) {
  velocity0_ << 2.1, -0.2, 0.1;
  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());

  solve();
  EXPECT_NEAR_EIGEN(velocity0_, Eigen::Vector3d(1, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermStartImuBiasOptimization) {
  gyro_bias0_ << 0.3, -0.2, 0.1;
  accel_bias0_ << 0.1, -0.2, 0.1;

  addResidual();

  problem_.SetParameterBlockConstant(imu_bias1_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(velocity1_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());

  solve();
  EXPECT_ZERO_EIGEN(imu_bias0_, 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

TEST_F(PosegraphErrorTermsEigen, InertialTermFinalImuBiasOptimization) {
  gyro_bias1_ << 0.3, -0.2, 0.1;
  accel_bias1_ << -0.3, -0.2, 0.1;

  addResidual();

  problem_.SetParameterBlockConstant(imu_bias0_.data());
  problem_.SetParameterBlockConstant(velocity0_.data());
  problem_.SetParameterBlockConstant(pos0_.data());
  problem_.SetParameterBlockConstant(rot0_.coeffs().data());
  problem_.SetParameterBlockConstant(pos1_.data());
  problem_.SetParameterBlockConstant(rot1_.coeffs().data());

  solve();
  EXPECT_ZERO_EIGEN(imu_bias1_, 1e-15);
  EXPECT_NEAR_EIGEN(velocity1_, Eigen::Vector3d(2, 0, 0), 1e-15);
  EXPECT_LT(summary_.final_cost, 1e-15);
}

MAPLAB_UNITTEST_ENTRYPOINT


/*
 * Copyright 2009-2015 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 <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "mkl.h"
#include "mkl_lapacke.h"

namespace votca { namespace tools {

using namespace std;

/**
 * ublas binding to MKL  Singular Value Decomposition
 *
 * A = U S V^T
 *
 * @param A MxN matrix do decompose. Becomes an MxN orthogonal matrix U
 * @param V NxN orthogonal square matrix
 * @param S N vector of non-negative numbers forming a non-increasing sequence
 * @return succeeded or not
 */

bool linalg_singular_value_decomposition(ub::matrix<double> &A, ub::matrix<double> &VT, ub::vector<double> &S ){
        // matrix inversion using MKL

    // define LAPACK variables
    MKL_INT m = A.size1();
    MKL_INT n = A.size2();

    if (m>n){
        throw runtime_error("Matrix for svd has the wrong shape first dimension must be equal or larger than second.");
    }
    //MKL_INT info;
    //MKL_INT ipiv[n];
    ub::matrix<double>work=ub::zero_matrix<double>(m,n);
    // initialize V
    S.resize(n, false);
    VT.resize(n, n, false);

    // pointers for LAPACK
    double * a = const_cast<double*>(&A.data().begin()[0]);
    double * s = const_cast<double*>(&S.data().begin()[0]);
    double * vt = const_cast<double*>(&VT.data().begin()[0]);
    double * superb = const_cast<double*>(&work.data().begin()[0]);
    // solve
    int status= LAPACKE_dgesvd( LAPACK_ROW_MAJOR, 'O', 'A',  m,  n,  a, n,  s,  NULL,m,  vt, n, superb );
    return (status != 0);

}

}}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_ASINH_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_ASINH_HPP_INCLUDED
#include <nt2/hyperbolic/functions/asinh.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/log_2.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/oneosqrteps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/average.hpp>
#include <nt2/include/functions/simd/bitofsign.hpp>
#include <nt2/include/functions/simd/bitwise_xor.hpp>
#include <nt2/include/functions/simd/bitwise_xor.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/hypot.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/log.hpp>
#include <nt2/include/functions/simd/minusone.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( asinh_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<double_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      A0 x =  nt2::abs(a0);
      bA0 test = gt(x,Oneosqrteps<A0>());
      A0 z = if_else(test,minusone(x), x+sqr(x)/nt2::oneplus(hypot(One<A0>(), x)));
      #ifndef BOOST_SIMD_NO_INFINITIES
      z = if_else(is_equal(x, Inf<A0>()),x, z);
      #endif
      z =  seladd(test, log1p(z), Log_2<A0>());
      return bitwise_xor(bitofsign(a0), z);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( asinh_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<single_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      // Exhaustive test for: boost::dispatch::functor<nt2::tag::asinh_, boost::simd::tag::sse4_2_>
      //              versus: float(boost::math::asinh(double)
      //              With T: boost::simd::native<float, boost::simd::tag::sse_, void>
      //            in range: [-3.40282e+38, 3.40282e+38]
      // 4278190076 values computed.
      // 3619320676 values (84.60%)  within 0.0 ULPs
      //  658843138 values (15.40%)  within 0.5 ULPs
      //      26262 values ( 0.00%)  within 1.0 ULPs
      typedef typename meta::as_logical<A0>::type bA0;
      typedef typename meta::scalar_of<A0>::type sA0;
      A0 x = nt2::abs(a0);
      bA0 lthalf = lt(x,Half<A0>());
      A0 x2 = nt2::sqr(x);
      A0 z = Zero<A0>();
      std::size_t nb = inbtrue(lthalf);
      A0 bts = bitofsign(a0);
      if(nb > 0)
      {
        z = horner < NT2_HORNER_COEFF_T(sA0, 5,
                                        ( 0x3ca4d6e6
                                        , 0xbd2ee581
                                        , 0x3d9949b1
                                        , 0xbe2aa9ad
                                        , 0x3f800000
                                        )
                                       )> (x2)*x;
        if(nb >= meta::cardinal_of<A0>::value) return  b_xor(z, bts);
      }
      A0 tmp =  if_else(gt(x, Oneosqrteps<A0>()),
                       x, average(x, hypot(One<A0>(), x)));
     return b_xor(if_else(lthalf, z, log(tmp)+Log_2<A0>()), bts);
    }
  };
} }
#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::log_likelihood_evaluator.hpp               //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under 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_STATISTICS_MODEL_FUNCTIONAL_LOG_LIKELIHOOD_EVALUATOR_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_LIKELIHOOD_EVALUATOR_HPP_ER_2009
#include <boost/range.hpp>
#include <boost/statistics/model/wrap/aggregate/model_parameter.hpp>
#include <boost/statistics/model/wrap/aggregate/model_dataset.hpp>
#include <boost/statistics/model/algorithm/log_likelihood.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Evaluates the log-likelihood at a parameter values given a model and
    // a dataset.
    //
    // T is the result_type
    template<typename T,typename M,typename Rx,typename Ry>
    class log_likelihood_evaluator{
        public:
        typedef T result_type;
        typedef model_dataset_<M,Rx,Ry> model_dataset_type;

        // Constructor
        log_likelihood_evaluator();
        log_likelihood_evaluator(const model_dataset_type&);
        log_likelihood_evaluator(const log_likelihood_evaluator&);
        log_likelihood_evaluator& operator=(const log_likelihood_evaluator&);

        // Evaluate
        template<typename P> result_type operator()(const P& p)const;

        // Access
        const model_dataset_type& model_dataset()const;

        private:
        model_dataset_type md_;
    };

    // Implementation //

    // Construction
    template<typename T,typename M,typename Rx,typename Ry>
    log_likelihood_evaluator<T,M,Rx,Ry>::log_likelihood_evaluator(){}

    template<typename T,typename M,typename Rx,typename Ry>
    log_likelihood_evaluator<T,M,Rx,Ry>::log_likelihood_evaluator(
        const model_dataset_type& md
    ):md_(md){}

    template<typename T,typename M,typename Rx,typename Ry>
    log_likelihood_evaluator<T,M,Rx,Ry>::log_likelihood_evaluator(
        const log_likelihood_evaluator& that
    ):md_(that.md_){}

    template<typename T,typename M,typename Rx,typename Ry>
    log_likelihood_evaluator<T,M,Rx,Ry>&
    log_likelihood_evaluator<T,M,Rx,Ry>::operator=(
        const log_likelihood_evaluator& that
    ){
        if(&that!=this){
            md_ = (that.md_);
        }
        return (*this);
    }

    // Evaluate
    template<typename T,typename M,typename Rx,typename Ry>
    template<typename P>
    typename log_likelihood_evaluator<T,M,Rx,Ry>::result_type
    log_likelihood_evaluator<T,M,Rx,Ry>::operator()(const P& p)const{
        return log_likelihood<T>(
            make_model_parameter(
                model_dataset().model(),
                p
            ),
            boost::begin( model_dataset().covariates() ),
            boost::end( model_dataset().covariates() ),
            boost::begin( model_dataset().responses() )
        );
    }

    // Access
    template<typename T,typename M,typename Rx,typename Ry>
    const typename log_likelihood_evaluator<T,M,Rx,Ry>::model_dataset_type&
    log_likelihood_evaluator<T,M,Rx,Ry>::model_dataset()const{
        return (this->md_);
    }

}// model
}// statistics
}// boost

#endif


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <bits/stdc++.h>
#include <boost/geometry.hpp>

using namespace std;
namespace bg = boost::geometry;

typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
typedef bg::model::polygon<point_t> poly_t;

const double EPS = 1e-9; // used in various places to solve precision issues

// This program takes input integer n and k, followed by n points and outputs concave hull using KNN algorithm.
// The concave hull of a geometry represents a possibly concave geometry that encloses all geometries within the set.

struct pnt{
    double x, y;

    bool operator<(pnt const& oth) {
        if (x == oth.x) {
            return y < oth.y;
        }
        return x < oth.x;
    }

    bool operator==(const pnt& a) const {return a.x == x && a.y == y;}
};

struct line{
    double a, b, c;
};

double dist(pnt a, pnt b) {
    // returns distance square between two points
    return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
}

int cross_product(pnt a, pnt b, pnt c) {
    // returns 1 if points are oriented in counter-clockwise manner
    // returns 0 if points are oriented in clockwise manner
    double ans = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y);
    if (!ans) {
        // returns -1 if collinear
        return -1;
    }
    return (ans < 0 ? 1 : 0);
}

void CleanList(vector<pnt> &pt) {
    // this function removes duplicates from input vector
    // and then sorts the vector with increasing x value
    int n = pt.size();
    set<pair<double,double>> st;
    for (int i = 0; i < n; ++i) {
        st.insert({pt[i].x, pt[i].y});
    }
    for (int i = 0; i < n; ++i) {
        pt.pop_back();
    }
    for (auto &p: st) {
        pt.push_back({p.first, p.second});
    }
    sort(pt.begin(), pt.end());
}

void RemovePoint(vector<pnt> &pt, pnt cur) {
    // removes the cur point from vector pt
    int n = pt.size();
    bool active = false;
    for (int i = 0; i < n; ++i) {
        if (pt[i].x == cur.x and pt[i].y == cur.y) {
            for (int j = i; j < n - 1; ++j) {
                pt[j] = pt[j + 1];
            }
            break;
        }
    }
    pt.pop_back();
}

vector<pnt> NearestPoints(vector<pnt> pt, pnt cur, int cur_k) {
    // this function returns vector with cur_k nearest points to point cur
    sort(pt.begin(), pt.end(), [&](pnt a, pnt b) {
        return (dist(a, cur) < dist(b, cur));
    });
    vector<pnt> ans;
    for (int i = 0; i < min(cur_k, int(pt.size())); ++i) {
        ans.push_back(pt[i]);
    }
    sort(ans.begin(), ans.end());
    return ans;
}

bool IsInside(vector<pnt> pt, pnt cur) {
    // returns true if cur is inside or in the border of the polygon formed by points in vector pt
    poly_t poly;
    for (int i = 0; i < int(pt.size()); ++i) {
        bg::append(poly, point_t(pt[i].x, pt[i].y));
    }
    point_t now = point_t(cur.x, cur.y);
    return bg::covered_by(now, poly);
}

bool PointOnSegment(pnt a, pnt b, pnt p) {
    // returns true if point p lies on the line segment formed by point a and b (exclusive)
    if (p == a or p == b) {
        return false;
    }
    double len = sqrt(dist(a, b));
    double tot = sqrt(dist(a, p)) + sqrt(dist(b, p));
    return (len + EPS > tot);
}

bool Intersect(pnt a, pnt b, pnt c, pnt d) {
    // returns true if line formed by point a and b intersects line formed by point c and d
    // if segments only touches each other than return false
    line l1 = {a.y - b.y, b.x - a.x, a.x * (b.y - a.y) + a.y * (a.x - b.x)};
    line l2 = {c.y - d.y, d.x - c.x, c.x * (d.y - c.y) + c.y * (c.x - d.x)};
    double D = l1.a * l2.b - l1.b * l2.a;
    double Ny = l1.c * l2.a - l1.a * l2.c;
    double Nx = l1.b * l2.c - l1.c * l2.b;
    if (abs(D) < EPS) {
        if (abs(Nx) < EPS and abs(Ny) < EPS) {
            double Sum = sqrt(dist(a, b)) + sqrt(dist(c, d));
            double Max = sqrt(max({dist(a, c), dist(a, d), dist(b, c), dist(b, d)}));
            if (Sum > Max) {
                return true;
            }
            return false;
        }
        return false;
    }
    pnt Mid = {Nx / D, Ny / D};
    if (PointOnSegment(a, b, Mid) and PointOnSegment(c, d, Mid)) {
        return true;
    }
    return false;
}

vector<pnt> KNN(vector<pnt> input, int k) {
    vector<pnt> pt = input;

    // k must be atleast 2
    k = max(k, 2);
    if (pt.size() < 3 or k > int(pt.size())) {
        return {};
    }
    if (pt.size() == 3) {
        return pt;
    }

    int cur_k = min(k, int(pt.size()) - 1);
    vector<pnt> hull = {pt[0]};
    pnt cur_pt = pt[0];
    int step = 0;
    RemovePoint(pt, cur_pt);

    while (pt.size()) {
        if (step == 3) {
            // once a polygon with more than 3 edges is formed add the first point to the pt vector again
            pt.push_back(hull[0]);
        }
        vector<pnt> nearest = NearestPoints(pt, cur_pt, cur_k);
        vector<pnt> good;
        for (int i = 0; i < int(nearest.size()); ++i) {
            bool ok = true;
            for (int j = int(hull.size()) - 1; j > 0; --j) {
                if (Intersect(hull[j], hull[j - 1], nearest[i], cur_pt)) {
                    ok = false;
                    break;
                }
            }
            if (ok) {
                // non intersecting edge is good
                good.push_back(nearest[i]);
            }
        }
        if (good.empty()) {
            // if no good points remaining, increase k
            return KNN(input, k + 1);
        }

        pnt best = good[0];
        for (int i = 0; i < int(good.size()); ++i) {
            int val = cross_product(cur_pt, good[i], best);
            if (val) {
                // update best if we have counter-clockwise orientation
                if (val == -1) {
                    // in case of collinear points take the nearest one as best
                    if (dist(good[i], cur_pt) < dist(best, cur_pt)) {
                        best = good[i];
                    }
                } else {
                    best = good[i];
                }
            }
        }
        cur_pt = best;
        hull.push_back(cur_pt);
        RemovePoint(pt, cur_pt);

        if (cur_pt == hull[0]) {
            // cycle completed
            break;
        }
        cur_k = min(cur_k, int(pt.size()));
        ++step;
    }
    for (auto &p: pt) {
        if (!IsInside(hull, p)) {
            // if any point is outside the concave hull, increase k
            return KNN(input, k + 1);
        }
    }
    cout << "k => " << k << "\n";
    return hull;
}

int main() {
    cout << fixed << setprecision(0);
    // take user input
    int n, k;
    cin >> n >> k;
    vector<pnt> pt;
    for (int i = 0; i < n; ++i) {
        double a, b;
        cin >> a >> b;
        pt.push_back({a, b});
    }

    CleanList(pt);
    vector<pnt> hull = KNN(pt, k);

    if (hull.empty()) {
        cout << "Invalid input\n";
    } else {
        cout << "Hull size: " << int(hull.size()) << "\n";
        for (int i = 0; i < int(hull.size()); ++i) {
            cout << hull[i].x << " " << hull[i].y << "\n";
        }
    }

    // To calculate the time taken in executing the code
    cout << "Time: " << (int)(clock() * 1000. / CLOCKS_PER_SEC) << "ms";
    return 0;
}


//=======================================================================
// Copyright (c)
//
// Distributed under 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)
//=======================================================================
/**
 * @file fast_exp_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-06-20
 */
#include "test_utils/logger.hpp"

#include "paal/utils/fast_exp.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(FastExpTest) {
    double biggestErr = 0;
    for (int i : paal::irange(-80, 80)) {
        paal::assign_max(biggestErr, std::abs(exp(i) - paal::fast_exp(i)) / exp(i));
    }
    LOGLN("biggest error " << biggestErr);
    BOOST_CHECK(biggestErr < 0.04);
}