#include <cmath>
#include <chrono>
#include <thread>
#include <ctime>
#include <fstream>
#include <chrono>

#include <Eigen/Dense>

#include <cinder/app/App.h>
#include <cinder/app/RendererGl.h>
#include <cinder/gl/gl.h>
#include <cinder/CameraUi.h>
#include <cinder/params/Params.h>
#include <cinder/Log.h>
#include <cinder/ObjLoader.h>

#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/unique_ptr.hpp>

#include <tinyformat.h>

#include "../resources/Resources.h"
#include <Utils.h>
#include <BackgroundWorker.h>
#include <TransferFunctionEditor.h>
#include <Integration.h>
#include <GridUtils.h>
#include <TimeIntegrator.h>
#include <SoftBodyGrid2D.h>
#include <SoftBodyMesh2D.h>
#include <SoftBody2DResults.h>

#include <IInverseProblem.h>
#include <InverseProblem_HumanSoftTissue.h>
#include <InverseProblem_ProbabilisticElastography.h>
#include <InverseProblem_Adjoint_YoungsModulus.h>
#include <InverseProblem_Adjoint_InitialConfiguration.h>
#include <InverseProblem_Adjoint_Dynamic.h>
#include "PartialObservations.h"
#include <GridVisualization.h>

using namespace ci;
using namespace ci::app;
using namespace std;
using namespace ar;
using namespace Eigen;

class InverseSoftBodyApp : public App {
public:
	InverseSoftBodyApp();
	void setup() override;
	void keyDown(KeyEvent event) override;
    void keyUp(KeyEvent event) override;
	void mouseDown( MouseEvent event ) override;
	void update() override;
	void draw() override;

private:

    typedef double real;

	//AntTweakBar settings
	params::InterfaceGlRef	params;
    bool printMode;

    enum class ComputationMode
    {
        COMPUTATION_MODE_GRID = 1,
        COMPUTATION_MODE_MESH = 2,
        COMPUTATION_MODE_BOTH = 3
    };
    int computationMode;

    //input - loading
    std::unique_ptr<SoftBody2DResults> inputResults;
    int inputTimestep;

    //input - generate
    int gridResolution;
    bool fitObjectToGrid;
    enum class Scene
    {
        SCENE_BAR,
        SCENE_TORUS
    };
    Scene scene;
    double torusOuterRadius;
    double torusInnerRadius;
    Vector2f rectCenter;
    Vector2f rectHalfSize;
    bool spaceBarPressed = false;
    TimeIntegrator::Integrator timeIntegratorType;
    TimeIntegrator::DenseLinearSolver denseLinearSolverType;
    TimeIntegrator::SparseLinearSolver sparseLinearSolverType;
    bool useSparseMatrices;
    int sparseSolverIterations;
    real sparseSolverTolerance;
    SoftBodySimulation::RotationCorrection rotationCorrectionMode;
    Vector2f gravity;
    Vector2f neumannForce;
    float youngsModulus;
    float poissonsRatio;
    float mass;
    float dampingAlpha;
    float dampingBeta;
    float timestep;
    bool gridExplicitDiffusion;
    bool gridHardDirichletBoundaries;
    SoftBodyGrid2D::AdvectionMode gridAdvectionMode;
    bool solveForwardStatic;
    SoftBodyMesh2D meshSolver;
    SoftBodyGrid2D gridSolver;

	bool enableDirichletBoundaries_;
	bool enableCollision_;
	real groundPlaneHeight;
	real groundPlaneAngle;
	real groundStiffness_;
	real collisionSoftminAlpha_;

	//reconstruction
	enum class ReconstructionMode
	{
		HUMAN_SOFT_TISSUE, //human soft tissue algorithm
		PROBABILISTIC_ELASTOGRAPHY,
        ADJOINT_YOUNGS_MODULUS,
        ADJOINT_INITIAL_POSITIONS,
		ADJOINT_DYNAMIC
	};
	ReconstructionMode reconstructionMode;
	std::map<ReconstructionMode, std::unique_ptr<IInverseProblem>> reconstructionAlgorithms;
	InverseProblemOutput resultMesh;
	InverseProblemOutput resultGrid;
	std::optional<GridUtils2D::grid_t> cleanedSdf;
    bool usePartialObservations;
    PartialObservations partialObservations;

    //rendering
	bool showGrid;
	enum class GridVisualizationMode
	{
		GRID_VISUALIZATION_U,
		GRID_VISUALIZATION_SOLUTION,
		GRID_VISUALIZATION_BOUNDARY,
		_GRID_VISUALIZATION_COUNT
	};
	GridVisualizationMode showGridSolution;
	bool showReconstructedSolution = false;
	std::mutex invalidateRenderingMutex;
	std::condition_variable invalidateRenderingConditionVariable;
	//visualization helpers
	GridVisualization visualization;

    //processing
    ar::BackgroundWorkerPtr worker;
	double meshElapsedSeconds = 0;
	double gridElapsedSeconds = 0;

	int reconstructSdfIterations = 5;

private:
    void invalidateRendering(bool wait);
    void drawSimulation();
	void loadSimulation();

    void resetSimulation();
    void runSimulation();

	void resetReconstruction();
	void performReconstruction();

	void reconstructSDF(int mode, int iterations);
    void testPlot();
};

InverseSoftBodyApp::InverseSoftBodyApp()
{
	//initial config
    printMode = false;
    computationMode = (int)ComputationMode::COMPUTATION_MODE_GRID;
    gridResolution = 22;
    fitObjectToGrid = false;
    scene = Scene::SCENE_BAR;
    torusOuterRadius = 0.1;
    torusInnerRadius = 0.03;
    rectCenter << 0.5, 0.7;
    rectHalfSize << 0.24, 0.06;
    showGrid = true;
    timeIntegratorType = TimeIntegrator::Integrator::Newmark1;
    denseLinearSolverType = TimeIntegrator::DenseLinearSolver::PartialPivLU;
    sparseLinearSolverType = TimeIntegrator::SparseLinearSolver::BiCGSTAB;
    useSparseMatrices = false;
    sparseSolverIterations = 100;
    sparseSolverTolerance = 1e-5;
    rotationCorrectionMode = SoftBodySimulation::RotationCorrection::Corotation;
    inputTimestep = 0;

    gridExplicitDiffusion = true;
    gridHardDirichletBoundaries = false;
    gridAdvectionMode = SoftBodyGrid2D::AdvectionMode::DIRECT_FORWARD;
    gravity = Vector2f(0, -30);
    neumannForce = Vector2f(0, 0);
    youngsModulus = 200;
    poissonsRatio = 0.45;
    mass = 1.0;
    dampingAlpha = 0.01;
    dampingBeta = 0.01;
    timestep = 0.01;
	solveForwardStatic = false;

	enableDirichletBoundaries_ = true;
	enableCollision_ = false;
	groundPlaneHeight = 0.6;
	groundPlaneAngle = 0;
	groundStiffness_ = 1000;
	collisionSoftminAlpha_ = 100;

	//create reconstruction algorithms
	reconstructionAlgorithms.emplace(ReconstructionMode::HUMAN_SOFT_TISSUE, std::make_unique<InverseProblem_HumanSoftTissue>());
	reconstructionAlgorithms.emplace(ReconstructionMode::PROBABILISTIC_ELASTOGRAPHY, std::make_unique<InverseProblem_ProbabilisticElastography>());
    reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_YOUNGS_MODULUS, std::make_unique<InverseProblem_Adjoint_YoungsModulus>());
    reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_INITIAL_POSITIONS, std::make_unique<InverseProblem_Adjoint_InitialConfiguration>());
	reconstructionAlgorithms.emplace(ReconstructionMode::ADJOINT_DYNAMIC, std::make_unique<InverseProblem_Adjoint_Dynamic>());
	reconstructionMode = ReconstructionMode::ADJOINT_DYNAMIC;
    usePartialObservations = true;
	showGridSolution = GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY;

	resetSimulation();
}

void InverseSoftBodyApp::setup()
{
	//ANT TWEAK BAR
	//parameter ui, must happen before user-camera
	params = params::InterfaceGl::create(getWindow(), "Parameters", ivec2(350, 800));
    params->setOptions("", "refresh=0.05");
    params->addParam("PrintMode", &printMode).label("Print Mode");

	//input
    params->addButton("LoadSimulation", std::function<void()>([this]() {this->loadSimulation(); }), "label='Load Simulation'");
    params->addParam("InputStep", std::function<void(int)>([this](int v)
    {
        inputTimestep = v;
		showReconstructedSolution = false;
        invalidateRendering(false);
    }), std::function<int()>([this]()
    {
        return inputTimestep;
    })).label("Input Step").min(0);
    vector<string> computationModeEnums(4);
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_GRID] = "grid";
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_MESH] = "mesh";
    computationModeEnums[(int)ComputationMode::COMPUTATION_MODE_BOTH] = "overlay / both";
    params->addParam("Mode", computationModeEnums, (int*)&computationMode, "label='Mode'");

	//Soft Body properties - Generate Inputs
    params->addParam("InputResolution",
        std::function<void(int)>([this](int newValue) {this->gridResolution = newValue; this->resetSimulation(); }),
        std::function<int()>([this]() {return this->gridResolution; })
    ).min(4).group("Generate Input").label("Grid resolution").keyIncr("PGUP").keyDecr("PGDOWN");
    params->addParam("FitObjectToGrid",
        std::function<void(bool)>([this](bool newValue) {this->fitObjectToGrid = newValue; this->resetSimulation(); }),
        std::function<bool()>([this]() {return this->fitObjectToGrid; })
    ).group("Generate Input").label("Fit Object To Grid");
    vector<string> sceneEnums = { "Bar", "Torus" };
    params->addParam("Scene", sceneEnums, (int*)&scene).group("Generate Input").label("Scene")
        .accessors(std::function<void(int)>([this](int v)
    {
        scene = (Scene)v;
        resetSimulation();
        if (scene == Scene::SCENE_TORUS) {
            params->setOptions("InputTorusOuterRadius", "visible=true");
            params->setOptions("InputTorusInnerRadius", "visible=true");
            params->setOptions("InputRectCenterX", "visible=false");
            params->setOptions("InputRectHalfSizeX", "visible=false");
            params->setOptions("InputRectCenterY", "visible=false");
            params->setOptions("InputRectHalfSizeY", "visible=false");
        }
        else {
            params->setOptions("InputTorusOuterRadius", "visible=false");
            params->setOptions("InputTorusInnerRadius", "visible=false");
            params->setOptions("InputRectCenterX", "visible=true");
            params->setOptions("InputRectHalfSizeX", "visible=true");
            params->setOptions("InputRectCenterY", "visible=true");
            params->setOptions("InputRectHalfSizeY", "visible=true");
        }
    }), std::function<int()>([this]() {return (int)scene; }));
    params->addParam("InputTorusOuterRadius",
        std::function<void(double)>([this](double newValue) {this->torusOuterRadius = newValue; this->resetSimulation(); }),
        std::function<double()>([this]() {return this->torusOuterRadius; })
    ).min(0.01).max(0.5).step(0.01).group("Generate Input").label("Torus outer radius").visible(false);
    params->addParam("InputTorusInnerRadius",
        std::function<void(double)>([this](double newValue) {this->torusInnerRadius = newValue; this->resetSimulation(); }),
        std::function<double()>([this]() {return this->torusInnerRadius; })
    ).min(0.01).max(0.5).step(0.01).group("Generate Input").label("Torus inner radius").visible(false);
    params->addParam("InputRectCenterX",
        std::function<void(float)>([this](float newValue) {this->rectCenter.x() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectCenter.x(); })
    ).step(0.01).group("Generate Input").label("Rect center X").visible(true);
    params->addParam("InputRectCenterY",
        std::function<void(float)>([this](float newValue) {this->rectCenter.y() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectCenter.y(); })
    ).step(0.01).group("Generate Input").label("Rect center Y").visible(true);
    params->addParam("InputRectHalfSizeX",
        std::function<void(float)>([this](float newValue) {this->rectHalfSize.x() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectHalfSize.x(); })
    ).step(0.01).group("Generate Input").label("Rect half size X").visible(true);
    params->addParam("InputRectHalfSizeY",
        std::function<void(float)>([this](float newValue) {this->rectHalfSize.y() = newValue; this->resetSimulation(); }),
        std::function<float()>([this]() {return this->rectHalfSize.y(); })
    ).step(0.01).group("Generate Input").label("Rect half size Y").visible(true);
    params->addButton("InputReset", std::function<void()>([this]() {this->resetSimulation(); }), "label='Reset' group='Generate Input' key=r");

    params->addParam("SoftBodyGravity", &gravity.y()).step(0.001).group("Generate Input").label("Gravity");
    params->addParam("SoftBodyNeumannForce", &neumannForce.y()).step(0.001).group("Generate Input").label("Neumann Force")
        .accessors(std::function<void(float)>([this](float v)
    {
        neumannForce.y() = v;
        resetSimulation();
    }), std::function<float()>([this]() {
        return (float)neumannForce.y();
    }));
    params->addParam("SoftBodyYoungsModulus", &youngsModulus).min(0).step(0.01).group("Generate Input").label("Young's modulus");
    params->addParam("SoftBodyPoissonsRatio", &poissonsRatio).min(0.0001).max(0.4999).step(0.01).group("Generate Input").label("Poisson's ratio");
    params->addParam("SoftBodyMass", &mass).min(0.0001).step(0.01).group("Generate Input").label("Mass");
    params->addParam("SoftBodyDampingAlpha", &dampingAlpha).min(0).step(0.001).group("Generate Input").label("Damping on mass");
    params->addParam("SoftBodyDampingBeta", &dampingBeta).min(0).step(0.001).group("Generate Input").label("Damping on stiffness");
    vector<string> timeIntegratorTypeNames = { "Newmark 1", "Newmark 2", "Central Differences", "Linear Accelleration", "Newmark 3", "HHT-alpha" };
    params->addParam("SoftBodyTimeIntegration", timeIntegratorTypeNames, (int*)&timeIntegratorType).group("Generate Input").label("Time Integrator")
        .accessors(std::function<void(int)>([this](int v)
    {
        timeIntegratorType = (TimeIntegrator::Integrator)v;
        resetSimulation();
    }), std::function<int()>([this]() {return (int)timeIntegratorType; }));
#if SOFT_BODY_SUPPORT_SPARSE_MATRICES==1
    params->addParam("SoftBodyUseSparseMatrices", std::function<void(bool)>([this](bool v)
    {
        useSparseMatrices = v;
        if (v)
        {
            params->setOptions("SoftBodyDenseLinearSolver", "visible=false");
            params->setOptions("SoftBodySparseLinearSolver", "visible=true");
            params->setOptions("SoftBodySparseSolverIterations", "visible=true");
            params->setOptions("SoftBodySparseSolverTolerance", "visible=true");
        }
        else
        {
            params->setOptions("SoftBodyDenseLinearSolver", "visible=true");
            params->setOptions("SoftBodySparseLinearSolver", "visible=false");
            params->setOptions("SoftBodySparseSolverIterations", "visible=false");
            params->setOptions("SoftBodySparseSolverTolerance", "visible=false");
        }
    }), std::function<bool()>([this]()
    {
        return useSparseMatrices;
    })).group("Generate Input").label("Sparse matrices");
    vector<string> denseLinearSolverTypeNames = { "PartialPivLU", "FullPivLU", "HouseholderQR", "ColPivHousholderQR", "FullPivHouseholderQR", "CompleteOrthogonalDecomposition", "LLT", "LDLT" };
    params->addParam("SoftBodyDenseLinearSolver", denseLinearSolverTypeNames, (int*)&denseLinearSolverType).group("Generate Input").label("Dense Linear Solver");
    vector<string> sparseLinearSolverTypeNames = { "Conjugate Gradient", "BiCGSTAB ", "Sparse-LU" };
    params->addParam("SoftBodySparseLinearSolver", sparseLinearSolverTypeNames, (int*)&sparseLinearSolverType).group("Generate Input").label("Sparse Linear Solver");
    params->addParam("SoftBodySparseSolverIterations", &sparseSolverIterations).group("Generate Input").label("Sparese Solver iterations").min(0);
    params->addParam("SoftBodySparseSolverTolerance", &sparseSolverTolerance).group("Generate Input").label("Sparese Solver tolerance").min(0).step(0.00001);
#else
    vector<string> denseLinearSolverTypeNames = { "PartialPivLU", "FullPivLU", "HouseholderQR", "ColPivHousholderQR", "FullPivHouseholderQR", "CompleteOrthogonalDecomposition", "LLT", "LDLT" };
    params->addParam("SoftBodyDenseLinearSolver", denseLinearSolverTypeNames, (int*)&denseLinearSolverType).group("Generate Input").label("Dense Linear Solver");
#endif
    params->addParam("SoftBodyTimeStep", &timestep).min(0.001).step(0.001).group("Generate Input").label("Time step");
    params->addParam("SoftBodyGridExplicitDiffusion", &gridExplicitDiffusion, "group='Generate Input' label='Grid Displacement Diffusion' true='explicit (post-process)' false='implicit (matrix)'");
    params->addParam("SoftBodyGridHardDirichletBoundaries", &gridHardDirichletBoundaries).group("Generate Input").label("Grid Hard Dirichlet Boundaries");
    vector<string> softBodyAdvectionNames;
    for (int i = 0; i < static_cast<int>(SoftBodyGrid2D::AdvectionMode::_COUNT_); ++i)
        softBodyAdvectionNames.push_back(SoftBodyGrid2D::advectionModeName(static_cast<SoftBodyGrid2D::AdvectionMode>(i)));
    params->addParam("SoftBodyAdvectionMode", softBodyAdvectionNames, (int*)&gridAdvectionMode).group("Generate Input").label("Grid Advection");

    vector<string> rotationCorrectionNames = { "None", "Corotation" };
    params->addParam("SoftBodyRotationCorrection", rotationCorrectionNames, (int*)&rotationCorrectionMode).group("Generate Input").label("Rotation correction");

	params->addParam("SoftBodyDirichlet",
		std::function<void(bool)>([this](bool newValue) {this->enableDirichletBoundaries_ = newValue; this->resetSimulation(); }),
		std::function<bool()>([this]() {return this->enableDirichletBoundaries_; }))
		.group("Generate Input").label("Enable Dirichlet Boundaries");
	params->addParam("SoftBodyCollision", &enableCollision_).group("Generate Input").label("Enable Collision");
	params->addParam("InputGroundPlaneHeight", &groundPlaneHeight).min(0).max(1).step(0.001).group("Generate Input").label("Ground Height");
	params->addParam("InputGroundPlaneAngle", &groundPlaneAngle).min(-1).max(1).step(0.001).group("Generate Input").label("Ground Angle");
	params->addParam("SoftBodyCollisionGroundStiffness", &groundStiffness_).group("Generate Input").label("Ground Stiffness").min(0).step(0.001);
	params->addParam("SoftBodyCollisionSoftmaxAlpha", &collisionSoftminAlpha_).group("Generate Input").label("Collision Softmax-Alpha").min(1).max(1000).step(0.001);

	params->addParam("SoftBodyStaticDynamic", &solveForwardStatic).group("Generate Input").label("Solution Mode").optionsStr("true='Static' false='Dynamic'");

	//reconstruction
	vector<string> reconstructionModeNames = {
	    "Human Soft Tissue",
	    "Probabilistic Elastography",
        "Adjoint - Youngs Modulus",
        "Adjoint - Initial Positions",
		"Adjoint - Dynamic"
	};
	params->addParam("ReconstructionAlgorithm", reconstructionModeNames, (int*)&reconstructionMode)
		.group("Reconstruction").label("Algorithm")
		.accessors(std::function<void(int)>([this](int v) {
			reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, false);
			reconstructionMode = static_cast<ReconstructionMode>(v);
			reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, true);
			resetReconstruction();
		}), std::function<int()>([this]() {
			return static_cast<int>(reconstructionMode);
		}));
	for (auto& p : reconstructionAlgorithms) {
		p.second->setupParams(params, "Reconstruction");
		p.second->setParamsVisibility(params, false);
	}
	reconstructionAlgorithms[reconstructionMode]->setParamsVisibility(params, true);
    params->addParam("ReconstructionUsePartialObservation", &usePartialObservations).label("Partial Observation").group("Reconstruction");
	params->addButton("ReconstructionRun", std::function<void()>([this]() {this->performReconstruction(); }), "group='Reconstruction' label='Solve' key=RETURN");
	params->addButton("ReconstructionCancel", std::function<void()>([this]() {
		if (worker) worker->interrupt();
	}), "group='Reconstruction' label='Cancel'");

    //partial observations
    partialObservations.initParams(params);

	//rendering
    params->addParam("RenderingShowGrid", &showGrid).group("Rendering").label("Show grid");
	vector<string> gridVisualizationModeEnums((int)GridVisualizationMode::_GRID_VISUALIZATION_COUNT);
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_U] = "u";
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_SOLUTION] = "solution";
	gridVisualizationModeEnums[(int)GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY] = "boundary";
	params->addParam("RenderingShowGridSolutionMode", gridVisualizationModeEnums, (int*)&showGridSolution, "label='Grid solution' group=Rendering");
	params->addParam("RenderingShowSolution", std::function<void(bool)>([this](bool v)
	{
		showReconstructedSolution = v;
		invalidateRendering(false);
	}), std::function<bool()>([this]()
	{
		return showReconstructedSolution;
	})).label("Show Reconstruction");

	params->addParam("RecontructSdfIterations", &reconstructSdfIterations).label("Reconstuct Sdf - Iterations").min(0).group("Test");
	params->addButton("ReconstructSdfViscosity", std::function<void()>([this]() {this->reconstructSDF(1, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Viscosity' group=Test ");
	params->addButton("ReconstructSdfUpwind", std::function<void()>([this]() {this->reconstructSDF(2, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Upwind' group=Test ");
	params->addButton("ReconstructSdfSussmann", std::function<void()>([this]() {this->reconstructSDF(3, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Sussmann' group=Test ");
    params->addButton("ReconstructSdfFastMarching", std::function<void()>([this]() {this->reconstructSDF(4, this->reconstructSdfIterations); }), "label='Reconstruct SDF - Fast Marching' group=Test ");
    params->addButton("TestPlot", std::function<void()>([this]() {this->testPlot(); }), "label='Test Plot' group=Test ");
	params->setOptions("Test", "opened=false");

	visualization.setup();
}

void InverseSoftBodyApp::keyDown(KeyEvent event)
{
	App::keyDown(event);
    if (event.isHandled()) return;
	if (event.getChar() == 'f') {
		// Toggle full screen when the user presses the 'f' key.
		setFullScreen(!isFullScreen());
	}
	else if (event.getCode() == KeyEvent::KEY_ESCAPE) {
		// Exit full screen, or quit the application, when the user presses the ESC key.
		if (isFullScreen())
			setFullScreen(false);
		else
			quit();
	}
	else if (event.getChar() == 'p') {
		//Screenshot
		Surface surface = copyWindowSurface();
        //if (printMode) {
        //    //only save the grid
        //    int windowWidth = getWindow()->getWidth();
        //    int windowHeight = getWindow()->getHeight();
        //    int gridBoundary = 50;
        //    int gridSize = std::min(windowWidth, windowHeight) - 2 * gridBoundary;
        //    int gridOffsetX = windowWidth / 2;
        //    int gridOffsetY = windowHeight / 2;
        //    Surface surface2(gridSize + 10, gridSize + 10, false);
        //    surface2.copyFrom(surface, Area(gridOffsetX - 5, gridOffsetY - 5, gridOffsetX + gridSize + 10, gridOffsetY + gridSize + 10));
        //    surface = surface2;
        //}
		//construct filename
		time_t now = time(NULL);
		struct tm tstruct;
		char buf[100];
		tstruct = *localtime(&now);
		strftime(buf, sizeof(buf), "%d-%m-%Y_%H-%M-%S", &tstruct);
		string fileName = string("../screenshots/InverseSoftBodyApp-") + string(buf) + ".png";
		//write out
		writeImage(fileName, surface);
        CI_LOG_I("Screenshot saved to " << fileName);
	}
    else if (event.getCode() == KeyEvent::KEY_SPACE)
    {
        spaceBarPressed = true;
    }
}

void InverseSoftBodyApp::keyUp(KeyEvent event)
{
    App::keyUp(event);
    if (event.isHandled()) return;
    if (event.getCode() == KeyEvent::KEY_SPACE)
    {
        spaceBarPressed = false;
    }
}

void InverseSoftBodyApp::mouseDown( MouseEvent event )
{
	App::mouseDown(event);
}

void InverseSoftBodyApp::update()
{
    //perform time stepping
    if (spaceBarPressed) {
		runSimulation();
    }

	//update transfer function editor
	visualization.update();
}

void InverseSoftBodyApp::drawSimulation()
{
	// Draw sdf
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID)
	{
		visualization.gridDrawSdf();
		if (showGrid)
			visualization.gridDrawGridLines();
		if (showGridSolution == GridVisualizationMode::GRID_VISUALIZATION_BOUNDARY)
			visualization.gridDrawObjectBoundary();
		if (!gridSolver.hasSolution())
			visualization.gridDrawBoundaryConditions(gridSolver.getGridDirichlet(), gridSolver.getGridNeumannX(), gridSolver.getGridNeumannY());
		if (gridSolver.hasSolution() && showGridSolution == GridVisualizationMode::GRID_VISUALIZATION_U) {
			gl::ScopedColor c;
			if (printMode)
				gl::color(0, 0, 0.5);
			else
				gl::color(1, 1, 1);
			visualization.gridDrawDisplacements();
		}
	}

	// Draw partial observations
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID
		&& usePartialObservations) {
		gl::ScopedMatrices m;
		visualization.applyTransformation();
		gl::ScopedColor c;
		partialObservations.draw();
	}

	// Draw mesh
	if (computationMode & (int)ComputationMode::COMPUTATION_MODE_MESH) {
		visualization.meshDraw();
	}

	// Draw ground
	if (enableCollision_)
	{
		visualization.drawGround(groundPlaneHeight, groundPlaneAngle);
	}
}

void InverseSoftBodyApp::draw()
{
    using namespace ar::utils;
    if (printMode)
        gl::clear(Color(1, 1, 1));
    else
        gl::clear(Color(0, 0, 0));

    // WINDOW SPACE
    gl::disableDepthRead();
    gl::disableDepthWrite();
    gl::setMatricesWindow(getWindowSize(), true);

    //draw the simulation
    if (inputResults != nullptr)
    {
        drawSimulation();
    }

    // Draw the background worker's status
    if (worker && !worker->isDone()) {
        //draw waiting animation
        {
            gl::ScopedModelMatrix scopedMatrix;
            gl::ScopedColor scopedColor;
            gl::translate(25, getWindowHeight() - 50);
            int step; double dummy; step = static_cast<int>(std::modf(getElapsedSeconds(), &dummy) * 8);
            for (int i = 0; i < 8; ++i) {
                float c = ((i + step)%8) / 7.0f;
                gl::color(c, c, c);
                gl::drawSolidRoundedRect(Rectf(5, -2, 15, 2), 2);
                gl::rotate(-2.0f * M_PI / 8.0f);
            }
        }
        //draw status
        gl::ScopedColor scopedColor;
        if (printMode)
            gl::color(0, 0, 0);
        else
            gl::color(1, 1, 1);
        gl::drawString(worker->getStatus(), vec2(50, getWindowHeight() - 50));
    }

	//draw result
	{
		static Font font("Arial", 20);
		static ColorA color(1, 1, 1, 1);
		gl::ScopedColor scopedColor;
		if (printMode)
			gl::color(0, 0, 0);
		else
			gl::color(1, 1, 1);
		int gridSize = std::min(getWindowWidth(), getWindowHeight()) - 100;
		int x = (getWindowWidth() + gridSize) / 2 + 50;
		int y = 50;
		int stepY = 15;

		gl::drawString("Results Mesh", vec2(x, y), color, font); y += stepY;
		gl::drawString("<Prop>: <Computed> - <Truth> (<StdDev>)", vec2(x, y), color, font); y += stepY;
		if (resultMesh.youngsModulus_.has_value()) {
			gl::drawString(
				tfm::format("Young's Modulus: %.2f - %.2f (%.2f)", resultMesh.youngsModulus_.value(), inputResults->settings_.youngsModulus_, resultMesh.youngsModulusStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.poissonsRatio_.has_value()) {
			gl::drawString(
				tfm::format("Poisson Ratio: %.2f - %.2f", resultMesh.poissonsRatio_.value(), inputResults->settings_.poissonsRatio_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.mass_.has_value()) {
			gl::drawString(
				tfm::format("Mass: %.2f - %.2f (%.2f)", resultMesh.mass_.value(), inputResults->settings_.mass_, resultMesh.massStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.dampingAlpha_.has_value()) {
			gl::drawString(
				tfm::format("Mass Damping: %.2f - %.2f (%.2f)", resultMesh.dampingAlpha_.value(), inputResults->settings_.dampingAlpha_, resultMesh.dampingAlphaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.dampingBeta_.has_value()) {
			gl::drawString(
				tfm::format("Stiffness Damping: %.2f - %.2f (%.2f)", resultMesh.dampingBeta_.value(), inputResults->settings_.dampingBeta_, resultMesh.dampingBetaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.groundHeight.has_value())
		{
			gl::drawString(
				tfm::format("Ground Height: %.3f - %.3f", resultMesh.groundHeight.value(), inputResults->settings_.groundPlaneHeight_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultMesh.groundAngle.has_value())
		{
			gl::drawString(
				tfm::format("Ground Angle: %.3f - %.3f", resultMesh.groundAngle.value(), inputResults->settings_.groundPlaneAngle_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		gl::drawString(tfm::format("Final Cost: %.3f", resultMesh.finalCost_), vec2(x, y), color, font); y += stepY;
		if (meshElapsedSeconds > 0)
			gl::drawString(tfm::format("Time: %.2f sec", meshElapsedSeconds), vec2(x, y), color, font); y += stepY;

		y += stepY;
		gl::drawString("Results Grid", vec2(x, y), color, font); y += stepY;
		gl::drawString("<Prop>: <Computed> - <Truth> (<StdDev>)", vec2(x, y), color, font); y += stepY;
		if (resultGrid.youngsModulus_.has_value()) {
			gl::drawString(
				tfm::format("Young's Modulus: %.2f - %.2f (%.2f)", resultGrid.youngsModulus_.value(), inputResults->settings_.youngsModulus_, resultGrid.youngsModulusStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.poissonsRatio_.has_value()) {
			gl::drawString(
				tfm::format("Poisson Ratio: %.2f - %.2f", resultMesh.poissonsRatio_.value(), inputResults->settings_.poissonsRatio_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.mass_.has_value()) {
			gl::drawString(
				tfm::format("Mass: %.2f - %.2f (%.2f)", resultGrid.mass_.value(), inputResults->settings_.mass_, resultGrid.massStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.dampingAlpha_.has_value()) {
			gl::drawString(
				tfm::format("Mass Damping: %.2f - %.2f (%.2f)", resultGrid.dampingAlpha_.value(), inputResults->settings_.dampingAlpha_, resultGrid.dampingAlphaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.dampingBeta_.has_value()) {
			gl::drawString(
				tfm::format("Stiffness Damping: %.2f - %.2f (%.2f)", resultGrid.dampingBeta_.value(), inputResults->settings_.dampingBeta_, resultGrid.dampingBetaStdDev_.value_or(0))
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.groundHeight.has_value())
		{
			gl::drawString(
				tfm::format("Ground Height: %.3f - %.3f", resultGrid.groundHeight.value(), inputResults->settings_.groundPlaneHeight_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		if (resultGrid.groundAngle.has_value())
		{
			gl::drawString(
				tfm::format("Ground Angle: %.3f - %.3f", resultGrid.groundAngle.value(), inputResults->settings_.groundPlaneAngle_)
				, vec2(x, y), color, font);
			y += stepY;
		}
		gl::drawString(tfm::format("Final Cost: %.3f", resultGrid.finalCost_), vec2(x, y), color, font); y += stepY;
		if (gridElapsedSeconds > 0)
			gl::drawString(tfm::format("Time: %.2f sec", gridElapsedSeconds), vec2(x, y), color, font); y += stepY;
	}

	// Draw the interface
	visualization.setTfeVisible(!printMode);
	params->draw();
	visualization.drawTransferFunctionEditor();

	//notify waiting threads
	invalidateRenderingConditionVariable.notify_all();
}

void InverseSoftBodyApp::invalidateRendering(bool wait)
{
	//GRID
	SoftBodyGrid2D::grid_t referenceSdf = inputResults->gridReferenceSdf_;
	SoftBodyGrid2D::grid_t currentSdf;
	SoftBodyGrid2D::grid_t currentUx = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
	SoftBodyGrid2D::grid_t currentUy = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
	if (inputTimestep > 0) {
		if (cleanedSdf.has_value()) {
			currentSdf = cleanedSdf.value();
		}
		else if (showReconstructedSolution && resultGrid.resultGridSdf_.has_value()) {
			currentSdf = resultGrid.resultGridSdf_.value();
		}
		else if (showReconstructedSolution && resultGrid.initialGridSdf_.has_value()) {
			currentSdf = resultGrid.initialGridSdf_.value();
		}
		else {
			if (inputResults->gridResultsSdf_.size() > inputTimestep - 1) {
				currentSdf = inputResults->gridResultsSdf_[inputTimestep - 1];
				currentUx = inputResults->gridResultsUx_[inputTimestep - 1];
				currentUy = inputResults->gridResultsUy_[inputTimestep - 1];
			}
			else
				currentSdf = inputResults->gridReferenceSdf_;
		}
	}
	else
	{
		currentSdf = inputResults->gridReferenceSdf_;
	}
	if (gridSolver.hasSolution())
	{
		visualization.setGrid(gridSolver.getSdfReference(), gridSolver.getSdfSolution(), gridSolver.getUGridX(), gridSolver.getUGridY());
	}
	else
	{
		GridUtils2D::grid_t z = GridUtils2D::grid_t::Zero(gridSolver.getGridResolution(), gridSolver.getGridResolution());
		visualization.setGrid(gridSolver.getSdfReference(), gridSolver.getSdfReference(), z, z);
	}

	//MESH
	auto pos = inputResults->meshReferencePositions_;
	if (inputTimestep > 0) {
		if (showReconstructedSolution && resultMesh.resultMeshDisp_.has_value()) {
			for (size_t i = 0; i < pos.size(); ++i)
				pos[i] += resultMesh.resultMeshDisp_.value()[i];
		}
		else if (showReconstructedSolution && resultMesh.initialMeshPositions_.has_value()) {
			pos = resultMesh.initialMeshPositions_.value();
		}
		else {
			if (inputResults->meshResultsDisplacement_.size() > inputTimestep - 1)
				for (size_t i = 0; i < pos.size(); ++i)
					pos[i] += inputResults->meshResultsDisplacement_[inputTimestep - 1][i];
		}
	}
	visualization.setMesh(pos, inputResults->meshReferenceIndices_, meshSolver.getNodeStates());

	//wait for the rendering to happen
	if (wait)
	{
		std::unique_lock<std::mutex> lock(invalidateRenderingMutex);
		invalidateRenderingConditionVariable.wait_for(lock, 50ms);
	}
}

void InverseSoftBodyApp::loadSimulation()
{
	//get save path
	fs::path path = getOpenFilePath(fs::path("../saves/"));
	if (path.empty()) {
		CI_LOG_W("Saving cancelled by the user");
		return;
	}
	std::string pathS = path.string();
	CI_LOG_I("Load results from " << pathS);

	//load it
	std::ifstream ifs(pathS, std::ifstream::binary);
	boost::archive::binary_iarchive ia(ifs);
	ia >> inputResults;
	CI_LOG_I("Results loaded");
    params->setOptions("InputStep", "max=" + std::to_string(inputResults->numSteps_));
	inputTimestep = 0;

	resetReconstruction();
}

void InverseSoftBodyApp::resetSimulation()
{
	//reset solution
	inputResults = std::make_unique<SoftBody2DResults>();

	//create scene
	switch (scene)
	{
	case Scene::SCENE_BAR:
		gridSolver = SoftBodyGrid2D::CreateBar(
			rectCenter.cast<real>(), rectHalfSize.cast<real>(), gridResolution,
			fitObjectToGrid, enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initGridReference(gridSolver);
		meshSolver = SoftBodyMesh2D::CreateBar(
			rectCenter.cast<real>(), rectHalfSize.cast<real>(),
			gridResolution, fitObjectToGrid,
			enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initMeshReference(meshSolver);
		break;
	case Scene::SCENE_TORUS:
		gridSolver = SoftBodyGrid2D::CreateTorus(
			torusOuterRadius, torusInnerRadius, gridResolution,
			enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>());
		inputResults->initGridReference(gridSolver);
		meshSolver = SoftBodyMesh2D::CreateTorus(
			torusOuterRadius, torusInnerRadius,
			gridResolution, enableDirichletBoundaries_, Vector2(0, 0), neumannForce.cast<real>().eval());
		inputResults->initMeshReference(meshSolver);
	}

	resetReconstruction();
	//invalidate texture and mesh
	showReconstructedSolution = false;
	inputTimestep = 0;
	invalidateRendering(false);
}

void InverseSoftBodyApp::runSimulation()
{
	if (worker != nullptr && !worker->isDone()) {
		//still running
		return;
	}

	if (solveForwardStatic && !enableDirichletBoundaries_)
	{
		CI_LOG_E("Can't solve for static solution if dirichlet boundaries are disabled");
		return;
	}

	//declare background task
	std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//pass arguments
		meshSolver.setGravity(gravity.cast<real>());
		meshSolver.setMaterialParameters(youngsModulus, poissonsRatio);
		meshSolver.setMass(mass);
		meshSolver.setDamping(dampingAlpha, dampingBeta);
		meshSolver.setDenseLinearSolver(denseLinearSolverType);
		meshSolver.setSparseLinearSolver(sparseLinearSolverType);
		meshSolver.setTimeIntegrator(timeIntegratorType);
		meshSolver.setUseSparseMatrices(useSparseMatrices);
		meshSolver.setSparseSolveIterations(sparseSolverIterations);
		meshSolver.setSparseSolveTolerance(sparseSolverTolerance);
		meshSolver.setRotationCorrection(rotationCorrectionMode);
		meshSolver.setTimestep(timestep);
		meshSolver.setGroundPlane(groundPlaneHeight, groundPlaneAngle);
		meshSolver.setEnableCollision(enableCollision_);
		meshSolver.setCollisionResolution(SoftBodySimulation::CollisionResolution::SPRING_IMPLICIT);
		meshSolver.setCollisionVelocityDamping(0);
		meshSolver.setGroundStiffness(groundStiffness_);
		meshSolver.setCollisionSoftmaxAlpha(collisionSoftminAlpha_);
		gridSolver.setGravity(gravity.cast<real>());
		gridSolver.setMaterialParameters(youngsModulus, poissonsRatio);
		gridSolver.setMass(mass);
		gridSolver.setDamping(dampingAlpha, dampingBeta);
		gridSolver.setDenseLinearSolver(denseLinearSolverType);
		gridSolver.setSparseLinearSolver(sparseLinearSolverType);
		gridSolver.setTimeIntegrator(timeIntegratorType);
		gridSolver.setUseSparseMatrices(useSparseMatrices);
		gridSolver.setSparseSolveIterations(sparseSolverIterations);
		gridSolver.setSparseSolveTolerance(sparseSolverTolerance);
		gridSolver.setExplicitDiffusion(gridExplicitDiffusion);
		gridSolver.setHardDirichletBoundaries(gridHardDirichletBoundaries);
		gridSolver.setAdvectionMode(gridAdvectionMode);
		gridSolver.setRotationCorrection(rotationCorrectionMode);
		gridSolver.setTimestep(timestep);
		gridSolver.setGroundPlane(groundPlaneHeight, groundPlaneAngle);
		gridSolver.setEnableCollision(enableCollision_);
		gridSolver.setCollisionResolution(SoftBodySimulation::CollisionResolution::SPRING_IMPLICIT);
		gridSolver.setCollisionVelocityDamping(0);
		gridSolver.setGroundStiffness(groundStiffness_);
		gridSolver.setCollisionSoftmaxAlpha(collisionSoftminAlpha_);

		inputResults->settings_ = gridSolver.getSettings();

		//solve it
		if (int(computationMode) & int(ComputationMode::COMPUTATION_MODE_MESH)) {
			auto start1 = std::chrono::steady_clock::now();
			if (solveForwardStatic) {
				meshSolver.solveStaticSolution(worker);
				if (worker->isInterrupted()) return;
			}
			else {
				meshSolver.solveDynamicSolution(worker);
				if (worker->isInterrupted()) return;
			}
			auto duration1 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
			meshElapsedSeconds = duration1.count() / 1000.0;
			inputResults->meshResultsDisplacement_.push_back(meshSolver.getCurrentDisplacements());
			inputResults->meshResultsVelocities_.push_back(meshSolver.getCurrentVelocities());
		}

		if (int(computationMode) & int(ComputationMode::COMPUTATION_MODE_GRID)) {
			auto start2 = std::chrono::steady_clock::now();
			if (solveForwardStatic) {
				gridSolver.solveStaticSolution(worker);
				if (worker->isInterrupted()) return;
			}
			else {
				gridSolver.solveDynamicSolution(worker);
				if (worker->isInterrupted()) return;
			}
			auto duration2 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
            CI_LOG_D("Reference simulation result:\n" << gridSolver.getSdfSolution());
            CI_LOG_D("Reference uX:\n" << gridSolver.getUGridX());
            CI_LOG_D("Reference uY:\n" << gridSolver.getUGridY());
            CI_LOG_D("Settings: " << gridSolver.getSettings() << " " << gridSolver.getGridSettings());
			gridElapsedSeconds = duration2.count() / 1000.0;
			inputResults->gridResultsSdf_.push_back(gridSolver.getSdfSolution());
			inputResults->gridResultsUx_.push_back(gridSolver.getUGridX());
			inputResults->gridResultsUy_.push_back(gridSolver.getUGridY());
			inputResults->gridResultsUxy_.push_back(gridSolver.getUSolution());
            partialObservations.setSdf(gridSolver.getSdfReference(), gridSolver.getSdfSolution(), gridSolver.getUGridX(), gridSolver.getUGridY());
            inputResults->gridPartialObservations_.push_back(partialObservations.getObservations());
		}

		inputResults->numSteps_++;
		inputTimestep = inputResults->numSteps_;
		showReconstructedSolution = false;
		params->setOptions("InputStep", "max=" + std::to_string(inputResults->numSteps_));
		invalidateRendering(false);
	};

	//start background worker
	worker = make_shared<BackgroundWorker>(task);
	CI_LOG_I("Background worker started");
}

void InverseSoftBodyApp::resetReconstruction()
{
	CI_LOG_D("reset");
	worker = nullptr;

	cleanedSdf.reset();
	resultMesh = InverseProblemOutput();
	resultGrid = InverseProblemOutput();
	meshElapsedSeconds = 0;
	gridElapsedSeconds = 0;
	invalidateRendering(false);
}

void InverseSoftBodyApp::performReconstruction()
{
	if (worker != nullptr && !worker->isDone()) {
		//still running
		return;
	}

	if (this->inputTimestep == 0) {
		CI_LOG_I("no timestep selected");
		return;
	}

	resetReconstruction();

	//declare background task
	std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//get algorithm
		IInverseProblem* alg = reconstructionAlgorithms[reconstructionMode].get();
		alg->setInput(inputResults.get());
        if (InverseProblem_Adjoint_Dynamic* algD = dynamic_cast<InverseProblem_Adjoint_Dynamic*>(alg))
        {
            algD->setGridUsePartialObservation(usePartialObservations);
        }
		//run them
		if (computationMode & (int)ComputationMode::COMPUTATION_MODE_MESH) {
			auto start1 = std::chrono::steady_clock::now();
			resultMesh = alg->solveMesh(this->inputTimestep-1, worker, [this](const InverseProblemOutput& intermediateSolution)
			{
				showReconstructedSolution = true;
				resultMesh = intermediateSolution;
				invalidateRendering(true);
			});
			auto duration1 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start1);
			meshElapsedSeconds = duration1.count() / 1000.0;
			if (worker->isInterrupted()) return;
		}
		if (computationMode & (int)ComputationMode::COMPUTATION_MODE_GRID) {
			auto start2 = std::chrono::steady_clock::now();
			resultGrid = alg->solveGrid(this->inputTimestep-1, worker, [this](const InverseProblemOutput& intermediateSolution)
			{
				showReconstructedSolution = true;
				resultGrid = intermediateSolution;
				invalidateRendering(true);
			});
			if (worker->isInterrupted()) return;
			auto duration2 = std::chrono::duration_cast<chrono::milliseconds>(std::chrono::steady_clock::now() - start2);
			gridElapsedSeconds = duration2.count() / 1000.0;
		}
		showReconstructedSolution = true;
		invalidateRendering(true);
	};

	//start background worker
	worker = make_shared<BackgroundWorker>(task);
	CI_LOG_I("Background worker started");
}

void InverseSoftBodyApp::reconstructSDF(int mode, int iterations)
{
	GridUtils2D::grid_t sdf;
	if (inputTimestep > 0) {
		if (showReconstructedSolution && resultGrid.resultGridSdf_.has_value()) {
			sdf = resultGrid.resultGridSdf_.value();
		}
		else if (showReconstructedSolution && resultGrid.initialGridSdf_.has_value()) {
			sdf = resultGrid.initialGridSdf_.value();
		}
		else {
			sdf = inputResults->gridResultsSdf_[inputTimestep - 1];
		}
	}
	else
	{
		sdf = inputResults->gridReferenceSdf_;
	}

	real viscosity = 0.01;
	if (iterations > 0) {
        if (mode == 1)
            sdf = GridUtils2D::recoverSDFViscosity(sdf, viscosity, iterations);
        else if (mode == 2)
            sdf = GridUtils2D::recoverSDFUpwind(sdf, iterations);
        else if (mode == 3)
            sdf = GridUtils2D::recoverSDFSussmann(sdf, viscosity, iterations);
        else if (mode == 4)
            sdf = GridUtils2D::recoverSDFFastMarching(sdf);
	}

	cleanedSdf = sdf;
	invalidateRendering(false);

	CI_LOG_I("SDF cleaned up");
}

void InverseSoftBodyApp::testPlot()
{
    std::function<void(BackgroundWorker*)> task = [this](BackgroundWorker* worker) {
		//get algorithm
		IInverseProblem* alg = reconstructionAlgorithms[reconstructionMode].get();
		if (InverseProblem_Adjoint_Dynamic* algD = dynamic_cast<InverseProblem_Adjoint_Dynamic*>(alg))
        {
            algD->setGridUsePartialObservation(usePartialObservations);
        }
        alg->setInput(inputResults.get());
		//run test plot
		alg->testPlot(this->inputTimestep, worker);
    };
    worker = make_shared<BackgroundWorker>(task);
    CI_LOG_I("Background worker started");
}

#if 1
CINDER_APP( InverseSoftBodyApp, RendererGl(RendererGl::Options().msaa(8)), [&](App::Settings *settings)
{
	settings->setWindowSize(1600, 900);
} )
#endif


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

#ifndef MTL_CHOLESKY_INCLUDE
#define MTL_CHOLESKY_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>
#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/operation/dmat_dmat_mult.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/recursion/base_case_cast.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

namespace with_bracket {

    // ============================================================================
    // Generic Cholesky factorization and operands for Cholesky on with submatrices
    // ============================================================================

    template < typename Matrix >
    void cholesky_base (Matrix & matrix)
    {
	vampir_trace<5001> tracer;
	typedef typename  Collection<Matrix>::size_type    size_type;
	for (size_type k = 0; k < matrix.num_cols(); k++) {
	    matrix[k][k] = sqrt (matrix[k][k]);

		for (size_type i = k + 1; i < matrix.num_rows(); i++) {
		    matrix[i][k] /= matrix[k][k];
		    typename Collection<Matrix>::value_type d = matrix[i][k];

		    for (size_type j = k + 1; j <= i; j++)
			matrix[i][j] -= d * matrix[j][k];
		}
	}
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW)
    {
	vampir_trace<5002> tracer;
	typedef typename  Collection<MatrixSW>::size_type    size_type;
	for (size_type k = 0; k < NW.num_rows (); k++) {

	    for (size_type i = 0; i < SW.num_rows (); i++) {
		SW[i][k] /= NW[k][k];
		typename MatrixSW::value_type d = SW[i][k];

		for (size_type j = k + 1; j < SW.num_cols (); j++)
		    SW[i][j] -= d * NW[j][k];
	    }
	}
    }

    // Lower(SE) -= SW * SW^T
    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW)
    {
	vampir_trace<5003> tracer;
	typedef typename  Collection<MatrixSE>::size_type    size_type;
	for (size_type k = 0; k < SW.num_cols (); k++)

	    for (size_type i = 0; i < SE.num_rows (); i++) {
		    typename MatrixSW::value_type d = SW[i][k];
		    for (size_type j = 0; j <= i; j++)
			SE[i][j] -= d * SW[j][k];
	    }
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
    {
	vampir_trace<5004> tracer;
	typedef typename  Collection<MatrixNE>::size_type    size_type;
	for (size_type k = 0; k < NW.num_cols (); k++)
	    for (size_type i = 0; i < NE.num_rows (); i++) {
		typename MatrixNW::value_type d = NW[i][k];
		for (size_type j = 0; j < NE.num_cols (); j++)
		    NE[i][j] -= d * SW[j][k];
	    }
    }

    // ======================
    // Corresponding functors
    // ======================

    struct cholesky_base_t
    {
	template < typename Matrix >
	void operator() (Matrix & matrix)
	{
	    cholesky_base(matrix);
	}
    };

    struct tri_solve_base_t
    {
	template < typename MatrixSW, typename MatrixNW >
	void operator() (MatrixSW & SW, const MatrixNW & NW)
	{
	    tri_solve_base(SW, NW);
	}
    };

    struct tri_schur_base_t
    {
	template < typename MatrixSE, typename MatrixSW >
	void operator() (MatrixSE & SE, const MatrixSW & SW)
	{
	    tri_schur_base(SE, SW);
	}
    };

    struct schur_update_base_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator() (MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    schur_update_base(NE, NW, SW);
	}
    };

} // namespace with_bracket

#if 0
namespace with_iterator {

    // ============================================================================
    // Generic Cholesky factorization and operands for Cholesky on with submatrices
    // ============================================================================
    // CURRENTLY NOT SUPPORTED -- CAUSES SEGFAULT, e.g. with icc 11.0 in r8536 !!!!
    // ============================================================================

    template < typename Matrix >
    void cholesky_base (Matrix& matrix)
    {
	vampir_trace<5001> tracer;
	typedef typename  Collection<Matrix>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all    all_it;

        typedef typename Collection<Matrix>::value_type                         value_type;
        typedef typename range_generator<col, Matrix>::type       cur_type;
        typedef typename range_generator<all_it, cur_type>::type    iter_type;

	typedef typename range_generator<row, Matrix>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type   riter_type;

	size_type k= 0;
	for (cur_type kb= begin<col>(matrix), kend= end<col>(matrix); kb != kend; ++kb, ++k) {

	    iter_type ib= begin<all_it>(kb), iend= end<all_it>(kb);
	    ib+= k; // points now to matrix[k][k]

	    value_type root= sqrt (*ib);
	    *ib= root;

	    ++ib; // points now to matrix[k+1][k]
	    rcur_type rb= begin<row>(matrix); rb+= k+1; // to row k+1
	    for (size_type i= k + 1; ib != iend; ++ib, ++rb, ++i) {
		*ib = *ib / root;
		typename Collection<Matrix>::value_type d = *ib;
		riter_type it1= begin<all_it>(rb);    it1+= k+1;      // matrix[i][k+1]
		riter_type it1end= begin<all_it>(rb); it1end+= i+1;   // matrix[i][i+1]
		iter_type it2= begin<all_it>(kb);     it2+= k+1;      // matrix[k+1][k]
		for (; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
	}
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW)
    {
	vampir_trace<5002> tracer;
	typedef typename  Collection<MatrixSW>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixNW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixSW>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < NW.num_rows (); k++)
	    for (size_type i = 0; i < SW.num_rows (); i++) {

		typename MatrixSW::value_type d = SW[i][k] /= NW[k][k];

		rcur_type sw_i= begin<row>(SW);     sw_i+= i;  // row i
		riter_type it1= begin<all_it>(sw_i);  it1+= k+1; // SW[i][k+1]
		riter_type it1end= end<all_it>(sw_i);

		ccur_type nw_k= begin<col>(NW);     nw_k+= k;  // column k
		citer_type it2= begin<all_cit>(nw_k); it2+= k+1; // NW[k+1][k]

		for(; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
    }

    // Lower(SE) -= SW * SW^T
    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW)
    {
	vampir_trace<5003> tracer;
	typedef typename  Collection<MatrixSE>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixSW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixSE>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < SW.num_cols (); k++)
	    for (size_type i = 0; i < SE.num_rows (); i++) {
		typename MatrixSW::value_type d = SW[i][k];

		rcur_type se_i= begin<row>(SE);       se_i+= i;      // row i
		riter_type it1= begin<all_it>(se_i);                   // SE[i][0]
		riter_type it1end= begin<all_it>(se_i); it1end+= i+1;  // SE[i][i+i]

		ccur_type sw_k= begin<col>(SW);     sw_k+= k;        // column k
		citer_type it2= begin<all_cit>(sw_k);                  // SW[0][k]

		for(; it1 != it1end; ++it1, ++it2)
		    *it1 = *it1 - d * *it2;
	    }
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
    {
	vampir_trace<5004> tracer;
	typedef typename  Collection<MatrixNE>::size_type    size_type;

	using namespace glas::tag; using mtl::traits::range_generator;
	typedef tag::iter::all        all_it;
	typedef tag::const_iter::all  all_cit;

        typedef typename range_generator<col, MatrixSW>::type       ccur_type;
        typedef typename range_generator<all_cit, ccur_type>::type    citer_type;

	typedef typename range_generator<row, MatrixNE>::type       rcur_type;
	typedef typename range_generator<all_it, rcur_type>::type     riter_type;

	for (size_type k = 0; k < NW.num_cols (); k++)
	    for (size_type i = 0; i < NE.num_rows (); i++) {
		typename MatrixNW::value_type d = NW[i][k];
#if 0
		rcur_type ne_i= begin<row>(NE);       ne_i+= i;      // row i
		riter_type it1= begin<all_it>(ne_i);                   // NE[i][0]
		riter_type it1end= end<all_it>(ne_i);                  // NE[i][num_col]

		ccur_type sw_k= begin<col>(SW);     sw_k+= k;        // column k
		citer_type it2= begin<all_cit>(sw_k);                  // SW[0][k]
#endif
		for (size_type j = 0; j < NE.num_cols (); j++)
		    NE[i][j] -= d * SW[j][k];
	    }
    }

    // ======================
    // Corresponding functors
    // ======================

    struct cholesky_base_t
    {
	template < typename Matrix >
	void operator() (Matrix & matrix)
	{
	    cholesky_base(matrix);
	}
    };

    struct tri_solve_base_t
    {
	template < typename MatrixSW, typename MatrixNW >
	void operator() (MatrixSW & SW, const MatrixNW & NW)
	{
	    tri_solve_base(SW, NW);
	}
    };

    struct tri_schur_base_t
    {
	template < typename MatrixSE, typename MatrixSW >
	void operator() (MatrixSE & SE, const MatrixSW & SW)
	{
	    tri_schur_base(SE, SW);
	}
    };

    struct schur_update_base_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator() (MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    schur_update_base(NE, NW, SW);
	}
    };

} // namespace with_iterator
#endif

// ==================================
// Functor types for Cholesky visitor
// ==================================

template <typename BaseTest, typename CholeskyBase, typename TriSolveBase, typename TriSchur, typename SchurUpdate>
struct recursive_cholesky_visitor_t
{
    typedef  BaseTest                   base_test;

    template < typename Recursator >
    bool is_base(const Recursator& recursator) const
    {
	return base_test()(recursator);
    }

    template < typename Matrix >
    void cholesky_base(Matrix & matrix) const
    {
	CholeskyBase()(matrix);
    }

    template < typename MatrixSW, typename MatrixNW >
    void tri_solve_base(MatrixSW & SW, const MatrixNW & NW) const
    {
	TriSolveBase()(SW, NW);
    }

    template < typename MatrixSE, typename MatrixSW >
    void tri_schur_base(MatrixSE & SE, const MatrixSW & SW) const
    {
	TriSchur()(SE, SW);
    }

    template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
    void schur_update_base(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW) const
    {
	SchurUpdate()(NE, NW, SW);
    }
};

namespace detail {

    // Compute schur update with external multiplication; must have Assign == minus_mult_assign_t !!!
    template <typename MatrixMult>
    struct mult_schur_update_t
    {
	template < typename MatrixNE, typename MatrixNW, typename MatrixSW >
	void operator()(MatrixNE & NE, const MatrixNW & NW, const MatrixSW & SW)
	{
	    transposed_view<MatrixSW> trans_sw(const_cast<MatrixSW&>(SW));
	    MatrixMult()(NW, trans_sw, NE);
	}
    };

} // detail

namespace with_bracket {
    typedef recursive_cholesky_visitor_t<recursion::bound_test_static<64>, cholesky_base_t, tri_solve_base_t,
					 tri_schur_base_t, schur_update_base_t >
               recursive_cholesky_base_visitor_t;
}

#if 0
namespace with_iterator {
    typedef recursive_cholesky_visitor_t<recursion::bound_test_static<64>,
					 cholesky_base_t, tri_solve_base_t, tri_schur_base_t, schur_update_base_t>
               recursive_cholesky_base_visitor_t;
}
#endif

typedef with_bracket::recursive_cholesky_base_visitor_t                    recursive_cholesky_default_visitor_t;

namespace with_recursator {

    template <typename Recursator, typename Visitor>
    void schur_update(Recursator E, Recursator W, Recursator N, Visitor vis)
    {
	vampir_trace<5005> tracer;
	using namespace recursion;

	if (E.is_empty() || W.is_empty() || N.is_empty())
	    return;

	if (vis.is_base(E)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_E(base_case_cast<base_test>(E.get_value())),
		         base_W(base_case_cast<base_test>(W.get_value())),
		         base_N(base_case_cast<base_test>(N.get_value()));
	    vis.schur_update_base(base_E, base_W, base_N);
	} else{
	    schur_update(     E.north_east(),W.north_west()     ,N.south_west()     , vis);
	    schur_update(     E.north_east(),     W.north_east(),     N.south_east(), vis);

	    schur_update(E.north_west()     ,     W.north_east(),     N.north_east(), vis);
	    schur_update(E.north_west()     ,W.north_west()     ,N.north_west()     , vis);

	    schur_update(E.south_west()     ,W.south_west()     ,N.north_west()     , vis);
	    schur_update(E.south_west()     ,     W.south_east(),     N.north_east(), vis);

	    schur_update(     E.south_east(),     W.south_east(),     N.south_east(), vis);
	    schur_update(     E.south_east(),W.south_west()     ,N.south_west()     , vis);
	}
    }

    template <typename Recursator, typename Visitor>
    void tri_solve(Recursator S, Recursator N, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5006> tracer;

        if (S.is_empty())
	    return;

        if (vis.is_base(S)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_S(base_case_cast<base_test>(S.get_value())),
		         base_N(base_case_cast<base_test>(N.get_value()));

	    vis.tri_solve_base(base_S, base_N);
        } else{

	    tri_solve(S.north_west()     ,N.north_west(), vis);
	    schur_update(  S.north_east(),S.north_west()     ,N.south_west(), vis);
	    tri_solve(     S.north_east(),     N.south_east(), vis);
	    tri_solve(S.south_west()     ,N.north_west()     , vis);
	    schur_update(  S.south_east(),S.south_west()     ,N.south_west(), vis);
	    tri_solve(     S.south_east(),     N.south_east(), vis);
	}
    }

    template <typename Recursator, typename Visitor>
    void tri_schur(Recursator E, Recursator W, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5007> tracer;

        if (E.is_empty() || W.is_empty())
	    return;

        if (vis.is_base(W)) {
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_E(base_case_cast<base_test>(E.get_value())),
               		 base_W(base_case_cast<base_test>(W.get_value()));
	    vis.tri_schur_base(base_E, base_W);
        } else{

	    schur_update(E.south_west(),     W.south_west(),    W.north_west(), vis);
	    schur_update(E.south_west(),     W.south_east(),    W.north_east(), vis);
	    tri_schur(   E.south_east()     ,     W.south_east(), vis);
	    tri_schur(   E.south_east()     ,W.south_west()     , vis);
	    tri_schur(        E.north_west(),     W.north_east(), vis);
	    tri_schur(        E.north_west(),W.north_west()     , vis);
        }
    }

    template <typename Recursator, typename Visitor>
    void cholesky(Recursator recursator, Visitor vis)
    {
	using namespace recursion;
	vampir_trace<5008> tracer;

        if (recursator.is_empty())
	    return;

        if (vis.is_base (recursator)){
	    typedef typename Visitor::base_test  base_test;
	    typedef typename base_case_matrix<typename Recursator::matrix_type, base_test>::type matrix_type;

	    matrix_type  base_matrix(base_case_cast<base_test>(recursator.get_value()));
	    vis.cholesky_base (base_matrix);
        } else {
	    cholesky(recursator.north_west(), vis);
	    tri_solve(    recursator.south_west(),       recursator.north_west(), vis);
	    tri_schur(    recursator.south_east(), recursator.south_west(), vis);
	    cholesky(     recursator.south_east(), vis);
        }
    }

} // namespace with_recursator

template <typename Backup= with_bracket::cholesky_base_t>
struct recursive_cholesky_t
{
    template <typename Matrix>
    void operator()(Matrix& matrix)
    {
	(*this)(matrix, recursive_cholesky_default_visitor_t());
    }

    template <typename Matrix, typename Visitor>
    void operator()(Matrix& matrix, Visitor vis)
    {
	apply(matrix, vis, typename mtl::traits::category<Matrix>::type());
    }

private:
    // If the matrix is not sub-divisible then take backup function
    template <typename Matrix, typename Visitor>
    void apply(Matrix& matrix, Visitor, tag::universe)
    {
	Backup()(matrix);
    }

    // Only if matrix is sub-divisible, otherwise backup
    template <typename Matrix, typename Visitor>
    void apply(Matrix& matrix, Visitor vis, tag::qsub_divisible)
    {
	matrix::recursator<Matrix>  recursator(matrix);
	with_recursator::cholesky(recursator, vis);
    }
};

template <typename Matrix, typename Visitor>
inline void recursive_cholesky(Matrix& matrix, Visitor vis)
{
    recursive_cholesky_t<>()(matrix, vis);
}

template <typename Matrix>
inline void recursive_cholesky(Matrix& matrix)
{
    recursive_cholesky(matrix, recursive_cholesky_default_visitor_t());
}

template <typename Matrix>
void fill_matrix_for_cholesky(Matrix& matrix)
{
    vampir_trace<5008> tracer;
    typedef typename Collection<Matrix>::size_type   size_type;
    typedef typename Collection<Matrix>::value_type  value_type;

    value_type   x= 1.0;
    for (size_type i= 0; i < num_rows(matrix); i++)
       for (size_type j= 0; j <= i; j++)
	   if (i != j) {
	       matrix[i][j]= x; matrix[j][i]= x;
	       x+= 1.0;
	   }

    for (size_type i= 0; i < num_rows(matrix); i++) {
	value_type rowsum= 0.0;
	for (size_type j=0; j<matrix.num_cols(); j++)
	    if (i != j)
		rowsum += matrix[i][j];
	matrix[i][i]= rowsum * 2;
    }
}

}} // namespace mtl::matrix

#endif // MTL_CHOLESKY_INCLUDE


#define DEBUG 1
/**
 * File    : C.cpp
 * Author  : Kazune Takahashi
 * Created : 6/17/2020, 5:57:05 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Solve -----

class Solve
{

public:
  Solve()
  {
  }

  void flush()
  {
  }

private:
};

// ----- main() -----

/*
int main()
{
  Solve solve;
  solve.flush();
}
*/

int main()
{
  int N;
  cin >> N;
  map<string, int> M;
  for (auto i{0}; i < N; ++i)
  {
    string S;
    cin >> S;
    M[S];
    M[S]++;
  }
  int maxi{0};
  for (auto const &e : M)
  {
    ch_max(maxi, e.second);
  }
  for (auto const &e : M)
  {
    if (e.second == maxi)
    {
      cout << e.first << endl;
    }
  }
}


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core
*/

#ifndef openOR_core_math_matrixfunctions_hpp
#define openOR_core_math_matrixfunctions_hpp

#include <boost/numeric/ublas/lu.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Utility/conceptcheck.hpp>
#include <openOR/Math/constants.hpp>
#include <openOR/Math/utilities.hpp>
#include <openOR/Math/matrix.hpp>
#include <openOR/Math/matrixsetaxis.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/vectorfunctions.hpp>
#include <openOR/Math/create.hpp>

#include <openOR/Math/determinant.hpp>
#include <openOR/Math/detail/inverse_impl.hpp>

namespace openOR {
	namespace Math {

		/**
		* @brief Returns the x-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			xAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 0>(mat), get<1, 0>(mat), get<2, 0>(mat));
		}

		/**
		* @brief Returns the y-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			yAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 1>(mat), get<1, 1>(mat), get<2, 1>(mat));
		}

		/**
		* @brief Returns the z-axis vector of a 3x3 or 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			zAxis(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 2>(mat), get<1, 2>(mat), get<2, 2>(mat));
		}

		/**
		* @brief Returns the translation vector of a 4x4 matrix.
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			translation(const Type& mat) {
				return create<typename MatrixTraits<Type>::Vector3Type>(get<0, 3>(mat), get<1, 3>(mat), get<2, 3>(mat));
		}

		namespace Impl {
			template <class Type, class Vec, int I, int Count>
			struct SetScaleHelp {
				void operator()(Type& mat, const Vec& vec) const {
					get<I, I>(mat) = get<I>(vec);
					SetScaleHelp < Type, Vec, I + 1, Count > ()(mat, vec);
				}
			};

			template <class Type, class Vec, int Count>
			struct SetScaleHelp<Type, Vec, Count, Count> {
				void operator()(Type& mat, const Vec& vec) const {}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstVector<Vec>)),
			(void))
			setScale(Type& mat, const Vec& vec) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				Impl::SetScaleHelp<Type, Vec, 0, MatrixTraits<Type>::RowDimension::value>()(mat, vec);
		}

		namespace Impl {
			template <class Type, class Vec, int J, int Count, int Row>
			struct GetRowHelp {
				void operator()(Vec& vec, const Type& mat) const {
					get<J>(vec) = get<Row, J>(mat);
					GetRowHelp < Type, Vec, J + 1, Count, Row > ()(vec, mat);
				}
			};

			template <class Type, class Vec, int Count, int Row>
			struct GetRowHelp<Type, Vec, Count, Count, Row> {
				void operator()(Vec& vec, const Type& mat) const {}
			};

			template <class Type, class Vec, int I, int Count, int Col>
			struct GetColHelp {
				void operator()(Vec& vec, const Type& mat) const {
					get<I>(vec) = get<I, Col>(mat);
					GetColHelp < Type, Vec, I + 1, Count, Col > ()(vec, mat);
				}
			};

			template <class Type, class Vec, int Count, int Col>
			struct GetColHelp<Type, Vec, Count, Count, Col> {
				void operator()(Vec& vec, const Type& mat) const {}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <int I, class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::Vector<typename MatrixTraits<Type>::RowVectorType>)),
			(typename MatrixTraits<Type>::RowVectorType))
			row(const Type& mat) {
				typedef typename MatrixTraits<Type>::RowVectorType VectorType;
				BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<I>, typename MatrixTraits<Type>::RowDimension>));
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename VectorTraits<VectorType>::Dimension, typename MatrixTraits<Type>::RowDimension>));

				VectorType vec;
				Impl::GetRowHelp<Type, VectorType, 0, MatrixTraits<Type>::ColDimension::value, I>()(vec, mat);
				return vec;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <int J, class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::Vector<typename MatrixTraits<Type>::ColVectorType>)),
			(typename MatrixTraits<Type>::ColVectorType))
			col(const Type& mat) {
				typedef typename MatrixTraits<Type>::ColVectorType VectorType;
				BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<J>, typename MatrixTraits<Type>::ColDimension>));
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename VectorTraits<VectorType>::Dimension, typename MatrixTraits<Type>::ColDimension>));

				VectorType vec;
				Impl::GetColHelp<Type, VectorType, 0, MatrixTraits<Type>::RowDimension::value, J>()(vec, mat);
				return vec;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationX(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = 1;
				get<0, 1>(mat) = 0;
				get<0, 2>(mat) = 0;
				get<1, 0>(mat) = 0;
				get<1, 1>(mat) = cos(angle);
				get<1, 2>(mat) = -sin(angle);
				get<2, 0>(mat) = 0;
				get<2, 1>(mat) = sin(angle);
				get<2, 2>(mat) = cos(angle);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationY(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = cos(angle);
				get<0, 1>(mat) = 0;
				get<0, 2>(mat) = sin(angle);
				get<1, 0>(mat) = 0;
				get<1, 1>(mat) = 1;
				get<1, 2>(mat) = 0;
				get<2, 0>(mat) = -sin(angle);
				get<2, 1>(mat) = 0;
				get<2, 2>(mat) = cos(angle);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationZ(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = cos(angle);
				get<0, 1>(mat) = -sin(angle);
				get<0, 2>(mat) = 0;
				get<1, 0>(mat) = sin(angle);
				get<1, 1>(mat) = cos(angle);
				get<1, 2>(mat) = 0;
				get<2, 0>(mat) = 0;
				get<2, 1>(mat) = 0;
				get<2, 2>(mat) = 1;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateX(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationX(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateY(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationY(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			rotateZ(Type& mat, typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType angle) {
				Type rotMat = MatrixTraits<Type>::IDENTITY;
				setRotationZ(rotMat, angle);
				mat = Math::prod(rotMat, mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(typename MatrixTraits<Type>::Matrix33Type))
			rotationAsMatrix(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::ColDimension, boost::mpl::int_<3> >));

				typename MatrixTraits<Type>::Matrix33Type matResult;
				get<0, 0>(matResult) = get<0, 0>(mat);
				get<0, 1>(matResult) = get<0, 1>(mat);
				get<0, 2>(matResult) = get<0, 2>(mat);
				get<1, 0>(matResult) = get<1, 0>(mat);
				get<1, 1>(matResult) = get<1, 1>(mat);
				get<1, 2>(matResult) = get<1, 2>(mat);
				get<2, 0>(matResult) = get<2, 0>(mat);
				get<2, 1>(matResult) = get<2, 1>(mat);
				get<2, 2>(matResult) = get<2, 2>(mat);
				return matResult;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type, class Type2>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>))
			((Concept::ConstMatrix<Type2>)),
			(void))
			setRotation(Type& mat, const Type2& matRotation) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Type>::ColDimension, boost::mpl::int_<3> >));
				get<0, 0>(mat) = get<0, 0>(matRotation);
				get<0, 1>(mat) = get<0, 1>(matRotation);
				get<0, 2>(mat) = get<0, 2>(matRotation);
				get<1, 0>(mat) = get<1, 0>(matRotation);
				get<1, 1>(mat) = get<1, 1>(matRotation);
				get<1, 2>(mat) = get<1, 2>(matRotation);
				get<2, 0>(mat) = get<2, 0>(matRotation);
				get<2, 1>(mat) = get<2, 1>(matRotation);
				get<2, 2>(mat) = get<2, 2>(matRotation);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Mat, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Mat>))
			((Concept::ConstVector<Vec>)),
			(void))
			setRotation(Mat& mat, const Vec& vecAxis, typename ScalarTraits<typename MatrixTraits<Mat>::ValueType>::RealType angle) {
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Mat>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename MatrixTraits<Mat>::RowDimension, boost::mpl::int_<3> >));
				BOOST_MPL_ASSERT((boost::mpl::greater_equal<typename VectorTraits<Vec>::Dimension, boost::mpl::int_<3> >));

				//assert(std::abs(norm(vecAxis) - 1.0) <= 0.001);

				setXAxis(mat, rotate3(create<Vec>(1, 0, 0), vecAxis, angle));
				setYAxis(mat, rotate3(create<Vec>(0, 1, 0), vecAxis, angle));
				setZAxis(mat, rotate3(create<Vec>(0, 0, 1), vecAxis, angle));
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(boost::math::quaternion<typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType>))
			rotationAsQuaternions(const Type& mat) {

				typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
				typedef boost::math::quaternion<RealType> Quaternions;
				Quaternions q;

				RealType tr = get<0, 0>(mat) + get<1, 1>(mat) + get<2, 2>(mat);
				RealType s;
				RealType w, x, y, z;

				if (tr > 0.0)
				{
					s = sqrt(tr + static_cast<RealType>(1));
					w = s * static_cast<RealType>(0.5);
					s = static_cast<RealType>(0.5) / s;
					x = static_cast<RealType>((get<2, 1>(mat) - get<1, 2>(mat)) * s);
					y = static_cast<RealType>((get<0, 2>(mat) - get<2, 0>(mat)) * s);
					z = static_cast<RealType>((get<1, 0>(mat) - get<0, 1>(mat)) * s);
				}
				else
				{
					if (get<0, 0>(mat) > get<1, 1>(mat) && get<0, 0>(mat) > get<2, 2>(mat))
					{
						RealType s = 2.0 * sqrt(1.0 + get<0, 0>(mat) - get<1, 1>(mat) - get<2, 2>(mat));
						w = (get<2, 1>(mat) - get<1, 2>(mat)) / s;
						x = 0.25 * s;
						y = (get<1, 0>(mat) + get<0, 1>(mat)) / s;
						z = (get<0, 2>(mat) + get<2, 0>(mat)) / s;

					} else if (get<1, 1>(mat) > get<2, 2>(mat)) {
						RealType s = 2.0 * sqrt(1.0 + get<1, 1>(mat) - get<0, 0>(mat) - get<2, 2>(mat));
						w = (get<0, 2>(mat) - get<2, 0>(mat)) / s;
						x = (get<1, 0>(mat) + get<0, 1>(mat)) / s;
						y = 0.25 * s;
						z = (get<2, 1>(mat) + get<1, 2>(mat)) / s;
					} else {
						RealType s = 2.0 * sqrt(1.0 + get<2, 2>(mat) - get<0, 0>(mat) - get<1, 1>(mat));
						w = (get<1, 0>(mat) - get<0, 1>(mat)) / s;
						x = (get<0, 2>(mat) + get<2, 0>(mat)) / s;
						y = (get<2, 1>(mat) + get<1, 2>(mat)) / s;
						z = 0.25 * s;
					}
				}
				return Quaternions(w, x, y, z);
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsEulerAngles(const Type& mat, double& heading, double& attitude, double& bank)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
			// Assuming the angles are in radians.
			if (get<1, 0>(mat) > static_cast<RealType>(0.998)) { // singularity at north pole
				heading = atan2(get<0, 2>(mat), get<2, 2>(mat));
				attitude = PI / 2;
				bank = 0;
				return;
			}
			if (get<1, 0>(mat) < static_cast<RealType>(-0.998)) { // singularity at south pole
				heading = atan2(get<0, 2>(mat), get<2, 2>(mat));
				attitude = -PI / 2;
				bank = 0;
				return;
			}
			heading = atan2(-get<2, 0>(mat), get<0, 0>(mat));
			bank = atan2(-get<1, 2>(mat), get<1, 1>(mat));
			attitude = asin(get<1, 0>(mat));
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotation(Type& mat, const boost::math::quaternion<typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType>& quaternion) {

				typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;
				typedef boost::math::quaternion<RealType> Quaternions;

				RealType s = static_cast<RealType>(2) / static_cast<RealType>(square(quaternion.R_component_1()) +
					square(quaternion.R_component_2()) +
					square(quaternion.R_component_3()) +
					square(quaternion.R_component_4()));

				RealType xs = quaternion.R_component_2() * s;
				RealType ys = quaternion.R_component_3() * s;
				RealType zs = quaternion.R_component_4() * s;

				RealType wx = quaternion.R_component_1() * xs;
				RealType wy = quaternion.R_component_1() * ys;
				RealType wz = quaternion.R_component_1() * zs;

				RealType xx = quaternion.R_component_2() * xs;
				RealType xy = quaternion.R_component_2() * ys;
				RealType xz = quaternion.R_component_2() * zs;

				RealType yy = quaternion.R_component_3() * ys;
				RealType yz = quaternion.R_component_3() * zs;

				RealType zz = quaternion.R_component_4() * zs;

				get<0, 0>(mat) = static_cast<RealType>(1) - (yy + zz);
				get<1, 0>(mat) = xy + wz;
				get<2, 0>(mat) = xz - wy;

				get<0, 1>(mat) = xy - wz;
				get<1, 1>(mat) = static_cast<RealType>(1) - (xx + zz);
				get<2, 1>(mat) = yz + wx;

				get<0, 2>(mat) = xz + wy;
				get<1, 2>(mat) = yz - wx;
				get<2, 2>(mat) = static_cast<RealType>(1) - (xx + yy);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationAxis(Type& mat, const Math::Vector3d& axis, const double& angle) {
				typename MatrixTraits<Type>::ValueType c = cos(angle), s = sin(angle);
				typename MatrixTraits<Type>::ValueType cn = 1.0 - cos(angle);
				double nx = axis(0), ny = axis(1), nz = axis(2);
				double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz;
				double nxs = nx * s, nys = ny * s, nzs = nz * s;

				get<0, 0>(mat) = nx * nx * cn + c;
				get<0, 1>(mat) = nxny * cn - nzs;
				get<0, 2>(mat) = nxnz * cn + nys;

				get<1, 0>(mat) = nxny * cn + nzs;
				get<1, 1>(mat) = ny * ny * cn + c;
				get<1, 2>(mat) = nynz * cn - nxs;

				get<2, 0>(mat) = nxnz * cn - nys;
				get<2, 1>(mat) = nynz * cn + nxs;
				get<2, 2>(mat) = nz * nz * cn + c;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationStationaryZYX(Type& mat,
			const typename MatrixTraits<Type>::ValueType& radZ,
			const typename MatrixTraits<Type>::ValueType& radY,
			const typename MatrixTraits<Type>::ValueType& radX)
		{
			// angles must be in radians
			double cx = cos(radX);
			double sx = sin(radX);
			double cy = cos(radY);
			double sy = sin(radY);
			double cz = cos(radZ);
			double sz = sin(radZ);

			// describes the rotation with euler angles around stationary axes
			// is equal to R = Rx * Ry * Rz

			get<0, 0>(mat) = cy * cz;
			get<0, 1>(mat) = -cy * sz;
			get<0, 2>(mat) = -sy;
			get<1, 0>(mat) = -sx * sy * cz + cx * sz;
			get<1, 1>(mat) = sx * sy * sz + cx * cz;
			get<1, 2>(mat) = -sx * cy;
			get<2, 0>(mat) = cx * sy * cz + sx * sz;
			get<2, 1>(mat) = -cx * sy * sz + sx * cz;
			get<2, 2>(mat) = cx * cy;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsStationaryZYX(const Type& mat, double& radZ, double& radY, double& radX)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;

			double sx, sy, sz, cx, cy, cz, rx, ry, rz;

			ry = -asin(get<0, 2>(mat));
			cy = cos(ry);

			if (abs(cy) > 0.00005) {
				sx = -get<1, 2>(mat) / cy;
				cx = get<2, 2>(mat) / cy;

				sz = -get<0, 1>(mat) / cy;
				cz = get<0, 0>(mat) / cy;

				rx = atan2(sx, cx);
				rz = atan2(sz, cz);
			} else {
				rx = 0;

				sz = get<1, 0>(mat);
				cz = get<2, 0>(mat);
				rz = atan2(sz, cz);
			}

			radZ = rz; radY = ry; radX = rx;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotation(Type& mat,
			const typename MatrixTraits<Type>::ValueType& heading,
			const typename MatrixTraits<Type>::ValueType& attitude,
			const typename MatrixTraits<Type>::ValueType& bank)
		{
			// angles must be in radians
			double ch = cos(heading);
			double sh = sin(heading);
			double ca = cos(attitude);
			double sa = sin(attitude);
			double cb = cos(bank);
			double sb = sin(bank);

			get<0, 0>(mat) = ch * ca;
			get<0, 1>(mat) = sh * sb - ch * sa * cb;
			get<0, 2>(mat) = ch * sa * sb + sh * cb;
			get<1, 0>(mat) = sa;
			get<1, 1>(mat) = ca * cb;
			get<1, 2>(mat) = -ca * sb;
			get<2, 0>(mat) = -sh * ca;
			get<2, 1>(mat) = sh * sa * cb + ch * sb;
			get<2, 2>(mat) = -sh * sa * sb + ch * cb;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			setRotationYprDeg(Type& mat,
			const typename MatrixTraits<Type>::ValueType& degYaw,
			const typename MatrixTraits<Type>::ValueType& degPitch,
			const typename MatrixTraits<Type>::ValueType& degRoll)
		{
			typename MatrixTraits<Type>::ValueType cy, sy;
			typename MatrixTraits<Type>::ValueType cp = cos(degPitch * M_PI / 180.0), sp = sin(degPitch * M_PI / 180.0);
			typename MatrixTraits<Type>::ValueType cr = cos(degRoll * M_PI / 180.0), sr = sin(degRoll * M_PI / 180.0);

			cy = cos(degYaw * M_PI / 180.0); sy = sin(degYaw * M_PI / 180.0);

			// correct cos
			if (degYaw == 90.0 || degYaw == 270.0) cy = 0.0;
			if (degPitch == 90.0 || degPitch == 270.0) cp = 0.0;
			if (degRoll == 90.0 || degRoll == 270.0) cr = 0.0;

			// correct sin
			if (degYaw == 0.0 || degYaw == 180.0) sy = 0.0;
			if (degPitch == 0.0 || degPitch == 180.0) sp = 0.0;
			if (degRoll == 0.0 || degRoll == 180.0) sr = 0.0;

			get<0, 0>(mat) = cr * cp;
			get<0, 1>(mat) = cr * sp * sy - sr * cy;
			get<0, 2>(mat) = cr * sp * cy + sr * sy;
			get<1, 0>(mat) = sr * cp;
			get<1, 1>(mat) = sr * sp * sy + cr * cy;
			get<1, 2>(mat) = sr * sp * cy - cr * sy;
			get<2, 0>(mat) = -sp;
			get<2, 1>(mat) = cp * sy;
			get<2, 2>(mat) = cp * cy;

			//get<0, 0>(mat) = cp * cy;
			//get<0, 1>(mat) = cp * sy;
			//get<0, 2>(mat) = -sp;
			//get<1, 0>(mat) = sr * sp * cy - sr * cy;
			//get<1, 1>(mat) = sr * sp * sy + cr * cy;
			//get<1, 2>(mat) = sr * cp;
			//get<2, 0>(mat) = cr * sp * cy + sr * sy;
			//get<2, 1>(mat) = cr * sp * sy - sr * cy;
			//get<2, 2>(mat) = cr * cp;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>)),
			(void))
			rotationAsYpr(const Type& mat, double& radYaw, double& radPitch, double& radRoll)
		{
			typedef typename ScalarTraits<typename MatrixTraits<Type>::ValueType>::RealType RealType;

			double sy, sp, sr, cy, cp, cr, ry, rp, rr;

			rp = -asin(get<2, 0>(mat));
			cp = cos(rp);
			if (abs(cp) > 0.0005) {
				sy = get<2, 1>(mat) / cp;
				cy = get<2, 2>(mat) / cp;

				sr = get<1, 0>(mat) / cp;
				cr = get<0, 0>(mat) / cp;

				ry = atan2(sy, cy);
				rr = atan2(sr, cr);
			} else {
				cy = get<0, 1>(mat);
				sy = get<1, 1>(mat);

				rr = M_PI * -0.5;
				ry = atan2(sy, cy);
			}

			radYaw = ry; radPitch = rp; radRoll = rr;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			orthoInverse(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				//assert(isHomogeneous(mat));
				Type matResult;
				get<0, 0>(matResult) = get<0, 0>(mat);
				get<0, 1>(matResult) = get<1, 0>(mat);
				get<0, 2>(matResult) = get<2, 0>(mat);
				get<1, 0>(matResult) = get<0, 1>(mat);
				get<1, 1>(matResult) = get<1, 1>(mat);
				get<1, 2>(matResult) = get<2, 1>(mat);
				get<2, 0>(matResult) = get<0, 2>(mat);
				get<2, 1>(matResult) = get<1, 2>(mat);
				get<2, 2>(matResult) = get<2, 2>(mat);
				get<3, 0>(matResult) = 0;
				get<3, 1>(matResult) = 0;
				get<3, 2>(matResult) = 0;
				get<3, 3>(matResult) = 1;
				get<0, 3>(matResult) = -(get<0, 0>(mat) * get<0, 3>(mat) + get<1, 0>(mat) * get<1, 3>(mat) + get<2, 0>(mat) * get<2, 3>(mat));
				get<1, 3>(matResult) = -(get<0, 1>(mat) * get<0, 3>(mat) + get<1, 1>(mat) * get<1, 3>(mat) + get<2, 1>(mat) * get<2, 3>(mat));
				get<2, 3>(matResult) = -(get<0, 2>(mat) * get<0, 3>(mat) + get<1, 2>(mat) * get<1, 3>(mat) + get<2, 2>(mat) * get<2, 3>(mat));
				return matResult;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type&))
			orthoInvert(Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				mat = orthoInverse(mat);
				return mat;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			inverse(const Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				return Impl::Inverse<Type, MatrixTraits<Type>::RowDimension::value>::get(mat);
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			invert(Type& mat) {
				mat = inverse(mat);
		}

		namespace Impl {
			template <class Mat, class MatSrc, int I, int J>
			struct TransposeOp {
				inline void operator()(Mat& mat, const MatSrc& matSrc) const {
					Math::get<I, J>(mat) = Math::get<J, I>(matSrc);
				}
			};
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type))
			transposed(const Type& mat) {
				typedef typename MatrixTraits<Type>::RowDimension RowDimension;
				typedef typename MatrixTraits<Type>::ColDimension ColDimension;
				BOOST_MPL_ASSERT((boost::mpl::equal_to<RowDimension, ColDimension>));
				Type matTransposed;
				Impl::MatrixCompileTimeIterator<Type, 0, RowDimension::value, 0, ColDimension::value>().template apply<Impl::TransposeOp, Type>(matTransposed, mat);
				return matTransposed;
		}

		/**
		* @brief
		* @ingroup openOR_core
		*/
		template<class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(Type&))
			transpose(Type& mat) {
				BOOST_MPL_ASSERT((boost::mpl::equal_to<typename MatrixTraits<Type>::RowDimension, typename MatrixTraits<Type>::ColDimension>));
				mat = transposed(mat);
				return mat;
		}

		/**
		* @brief
		* @ingroup openOR_core_math
		*/
		template<class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::ConstVector<Vec>))
			((Concept::Vector<typename MatrixTraits<Type>::Vector3Type>)),
			(typename MatrixTraits<Type>::Vector3Type))
			prod44x3(const Type& mat, const Vec& vec) {
				typename MatrixTraits<Type>::Vector3Type result;
				get<0>(result) = get<0, 0>(mat) * get<0>(vec) + get<0, 1>(mat) * get<1>(vec) + get<0, 2>(mat) * get<2>(vec) + get<0, 3>(mat);
				get<1>(result) = get<1, 0>(mat) * get<0>(vec) + get<1, 1>(mat) * get<1>(vec) + get<1, 2>(mat) * get<2>(vec) + get<1, 3>(mat);
				get<2>(result) = get<2, 0>(mat) * get<0>(vec) + get<2, 1>(mat) * get<1>(vec) + get<2, 2>(mat) * get<2>(vec) + get<2, 3>(mat);
				return result;
		}

		/**
		* @brief
		* @ingroup openOR_core_math
		*/
		template<class Type, class Vec>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::ConstMatrix<Type>))
			((Concept::ConstVector<Vec>))
			((Concept::Vector<typename MatrixTraits<Type>::Vector2Type>)),
			(typename MatrixTraits<Type>::Vector2Type))
			prod33x2(const Type& mat, const Vec& vec) {
				typename MatrixTraits<Type>::Vector2Type result;
				get<0>(result) = get<0, 0>(mat) * get<0>(vec) + get<0, 1>(mat) * get<1>(vec) + get<0, 2>(mat);
				get<1>(result) = get<1, 0>(mat) * get<0>(vec) + get<1, 1>(mat) * get<1>(vec) + get<1, 2>(mat);
				return result;
		}

		template <class Type>
		inline
			OPENOR_CONCEPT_REQUIRES(
			((Concept::Matrix<Type>)),
			(void))
			makeHomogeneous(Type& mat, int unmodifiedAxis = 2) // 0: xAxis; 1: yAxis; 2: zAxis
		{
			get<3, 0>(mat) = 0;
			get<3, 1>(mat) = 0;
			get<3, 2>(mat) = 0;
			get<3, 3>(mat) = 1;
			typename MatrixTraits<Type>::Vector3Type vecXAxis = xAxis(mat);
			typename MatrixTraits<Type>::Vector3Type vecYAxis = yAxis(mat);
			typename MatrixTraits<Type>::Vector3Type vecZAxis = zAxis(mat);

			switch (unmodifiedAxis)
			{
			case 0:
				normalize(vecXAxis);
				vecZAxis = cross(vecXAxis, vecYAxis);
				normalize(vecZAxis);
				vecYAxis = cross(vecZAxis, vecXAxis);
				break;
			case 1:
				normalize(vecYAxis);
				vecZAxis = cross(vecXAxis, vecYAxis);
				normalize(vecZAxis);
				vecXAxis = cross(vecYAxis, vecZAxis);
				break;
			default:
				normalize(vecZAxis);
				vecXAxis = cross(vecYAxis, vecZAxis);
				normalize(vecXAxis);
				vecYAxis = cross(vecZAxis, vecXAxis);
				break;
			}
			setXAxis(mat, vecXAxis);
			setYAxis(mat, vecYAxis);
			setZAxis(mat, vecZAxis);
		}

	}
}

#endif