/*
 * test_matord.cpp
 *
 *  Created on: 29 mai 2016
 *      Author: boubad
 */
#include <boost/test/unit_test.hpp>
/////////////////////////
#include <matrice.h>
/////////////////////////////
#include <mytestfixture.h>
#include <mytestvariablefixture.h>
////////////////////////////////////
#include <global_defs.h>
////////////////////
using namespace info;
using namespace std;
///////////////////////////////////
using MyIndivFixture = MyTestFixture<IDTYPE, INTTYPE, STRINGTYPE, WEIGHTYPE>;
using MyVariableFixture = MyTestVariableFixture<IDTYPE, INTTYPE, STRINGTYPE, WEIGHTYPE>;
/////////////////////////////////////
using MatOrdType = MatOrd<IDTYPE,DISTANCETYPE,STRINGTYPE>;
////////////////////////////////
using MatElemResultType = typename MatOrdType::MatElemResultType;
using MatElemResultPtr = typename MatOrdType::MatElemResultPtr;
using ConnectionType =typename MatOrdType::ConnectionType;
using SourceType = typename MatOrdType::SourceType;
using ints_vector = typename MatOrdType::ints_vector;
using IndivType = typename MatOrdType::IndivType;
using IndivTypePtr = typename MatOrdType::IndivTypePtr;
//////////////////////////////////////////
BOOST_AUTO_TEST_SUITE(MatOrdTestSuite)
BOOST_AUTO_TEST_CASE(testMortalMatOrd) {
	BOOST_TEST_MESSAGE("================= MORTAL =======================");
	//
	MyIndivFixture oIndFixture;
	SourceType *pIndivProvider = oIndFixture.mortal_source();
	BOOST_CHECK(pIndivProvider != nullptr);
	//
	MyVariableFixture oVarFixture;
	SourceType *pVarProvider = oVarFixture.mortal_source();
	BOOST_CHECK(pVarProvider != nullptr);
	//
	MatOrdType oMat;
	auto conn =
			oMat.connect(
					[&](MatElemResultPtr oIndCrit, MatElemResultPtr oVarCrit) {
						MatElemResultType *pVarCrit = oVarCrit.get();
						MatElemResultType *pIndCrit = oIndCrit.get();
						if ((pIndCrit != nullptr) && (pVarCrit != nullptr)) {
							DISTANCETYPE varCrit = pVarCrit->first;
							DISTANCETYPE indCrit = pIndCrit->first;
							STRINGTYPE s;
							STRINGTYPE ss;
							write_vector(pVarCrit->second, ss);
							write_vector(pIndCrit->second, s);
							BOOST_TEST_MESSAGE("Ind: " << indCrit << ",\t" << s << ",\tVar: " << varCrit << ", " << ss);
						} else if (pVarCrit != nullptr) {
							DISTANCETYPE varCrit = pVarCrit->first;
							STRINGTYPE s;
							write_vector(pVarCrit->second, s);
						BOOST_TEST_MESSAGE("Var: " << varCrit << ", \t" << s);
						} else if (pIndCrit != nullptr) {
							DISTANCETYPE indCrit = pIndCrit->first;
							STRINGTYPE s;
							write_vector(pIndCrit->second, s);
							BOOST_TEST_MESSAGE("Ind: " << indCrit << ", \t" << s);
						}
					});
	bool bRet = oMat.process(pIndivProvider, pVarProvider);
	BOOST_CHECK(bRet);
	ints_vector varids, indids;
	oMat.get_ids(indids, varids);
	BOOST_TEST_MESSAGE("========================================");
	BOOST_TEST_MESSAGE("VARIABLES");
	for (auto &aIndex : varids) {
		IndivTypePtr oInd = pVarProvider->find(aIndex);
		IndivType *p = oInd.get();
		if (p != nullptr) {
			STRINGTYPE s = p->sigle();
			BOOST_TEST_MESSAGE(s);
		} // p
	} // varIndex
	BOOST_TEST_MESSAGE("========================================");
	BOOST_TEST_MESSAGE("INDIVS");
	for (auto &aIndex : indids) {
		IndivTypePtr oInd = pIndivProvider->find(aIndex);
		IndivType *p = oInd.get();
		if (p != nullptr) {
			STRINGTYPE s = p->sigle();
			BOOST_TEST_MESSAGE(s);
		} // p
	} // varIndex
} //testMortalMatElem
BOOST_AUTO_TEST_CASE(testCONSOMatOrd) {
	//BOOST_TEST_MESSAGE("================= CONSO =======================");
	//
	MyIndivFixture oIndFixture;
	SourceType *pIndivProvider = oIndFixture.conso_source();
	BOOST_CHECK(pIndivProvider != nullptr);
	//
	MyVariableFixture oVarFixture;
	SourceType *pVarProvider = oVarFixture.conso_source();
	BOOST_CHECK(pVarProvider != nullptr);
	//
	MatOrdType oMat;
	auto conn =
			oMat.connect(
					[&](MatElemResultPtr oIndCrit, MatElemResultPtr oVarCrit) {
						MatElemResultType *pVarCrit = oVarCrit.get();
						MatElemResultType *pIndCrit = oIndCrit.get();
						if ((pIndCrit != nullptr) && (pVarCrit != nullptr)) {
						//	DISTANCETYPE varCrit = pVarCrit->first;
						//	DISTANCETYPE indCrit = pIndCrit->first;
							STRINGTYPE s;
							STRINGTYPE ss;
							write_vector(pVarCrit->second, ss);
							write_vector(pIndCrit->second, s);
							//BOOST_TEST_MESSAGE("Ind: " << indCrit << ",\t" << s << ",\tVar: " << varCrit << ", " << ss);
						} else if (pVarCrit != nullptr) {
							//DISTANCETYPE varCrit = pVarCrit->first;
							STRINGTYPE s;
							write_vector(pVarCrit->second, s);
							//BOOST_TEST_MESSAGE("Var: " << varCrit << ", \t" << s);
						} else if (pIndCrit != nullptr) {
							//DISTANCETYPE indCrit = pIndCrit->first;
							STRINGTYPE s;
							write_vector(pIndCrit->second, s);
							//BOOST_TEST_MESSAGE("Ind: " << indCrit << ", \t" << s);
						}
					});
	bool bRet = oMat.process(pIndivProvider, pVarProvider);
	BOOST_CHECK(bRet);
	ints_vector varids, indids;
	oMat.get_ids(indids, varids);
	//BOOST_TEST_MESSAGE("========================================");
	//BOOST_TEST_MESSAGE("VARIABLES");
	for (auto &aIndex : varids) {
		IndivTypePtr oInd = pVarProvider->find(aIndex);
		IndivType *p = oInd.get();
		if (p != nullptr) {
			STRINGTYPE s = p->sigle();
			//BOOST_TEST_MESSAGE(s);
		} // p
	} // varIndex
	//BOOST_TEST_MESSAGE("========================================");
	//BOOST_TEST_MESSAGE("INDIVS");
	for (auto &aIndex : indids) {
		IndivTypePtr oInd = pIndivProvider->find(aIndex);
		IndivType *p = oInd.get();
		if (p != nullptr) {
			STRINGTYPE s = p->sigle();
		//	BOOST_TEST_MESSAGE(s);
		} // p
	} // varIndex
} //testMortalMatElem
BOOST_AUTO_TEST_SUITE_END();


#include <vector>

//#include <Qt3DIncludes.h>
#include <GaussIncludes.h>
#include <ForceSpring.h>
#include <FEMIncludes.h>
#include <PhysicalSystemParticles.h>
//Any extra things I need such as constraints
#include <ConstraintFixedPoint.h>
#include <TimeStepperEulerImplicitLinear.h>
#include <TimeStepperEulerImplicit.h>
#include <AssemblerParallel.h>

#include <igl/get_seconds.h>
#include <igl/writeDMAT.h>
#include <igl/readDMAT.h>
#include <igl/viewer/Viewer.h>
#include <igl/readMESH.h>
#include <igl/unproject_onto_mesh.h>

#include <stdlib.h>
#include <iostream>
#include <string>
#include <sstream>
#include <string.h>
#include <json.hpp>
#include <boost/filesystem.hpp>

using Eigen::VectorXd;
using Eigen::Vector3d;
using Eigen::VectorXi;
using Eigen::MatrixXd;
using Eigen::MatrixXi;
using Eigen::SparseMatrix;
using Eigen::SparseVector;

namespace fs = boost::filesystem;
using json = nlohmann::json;
using namespace Gauss;
using namespace FEM;
using namespace ParticleSystem; //For Force Spring

/* Tetrahedral finite elements */

//typedef physical entities I need

typedef PhysicalSystemFEM<double, NeohookeanTet> NeohookeanTets;

typedef World<double,
                        std::tuple<PhysicalSystemParticleSingle<double> *, NeohookeanTets *>,
                        std::tuple<ForceSpringFEMParticle<double> *>,
                        std::tuple<ConstraintFixedPoint<double> *> > MyWorld;
typedef TimeStepperEulerImplicit<double, AssemblerEigenSparseMatrix<double>,
 AssemblerEigenVector<double> > MyTimeStepper;

// Mesh
Eigen::MatrixXd V; // Verts
Eigen::MatrixXi T; // Tet indices
Eigen::MatrixXi F; // Face indices

// Mouse/Viewer state
Eigen::RowVector3f last_mouse;
Eigen::RowVector3d dragged_pos;
bool is_dragging = false;
int dragged_vert = 0;
int current_frame = 0;

// Parameters
bool saving_training_data = true;
std::string output_dir = "output_data/";
json mouse_json;

void save_displacements_DMAT_and_energy(int current_frame, MyWorld &world, NeohookeanTets *tets, json mouse_json) { // TODO: Add mouse position data to ouput
    auto q = mapDOFEigen(tets->getQ(), world);
    Eigen::Map<Eigen::MatrixXd> dV(q.data(), V.cols(), V.rows()); // Get displacements only
    Eigen::MatrixXd displacements = dV.transpose();

    std::stringstream displacements_filename;
    displacements_filename << output_dir << "displacements_" << current_frame << ".dmat";
    igl::writeDMAT(displacements_filename.str(), displacements, false); // Don't use ascii

    AssemblerEigenVector<double> internal_force; //maybe?
    getInternalForceVector(internal_force, *tets, world);
    std::stringstream force_filename;
    force_filename<<output_dir<<"internalForces_"<<current_frame<<".dmat";
    std::cout << (*internal_force).size() << " " << q.size() << std::endl;
    igl::writeDMAT(force_filename.str(), *internal_force, false);

    std::stringstream energy_filename;
    energy_filename << output_dir << "energy_" << current_frame << ".dmat";
    Eigen::MatrixXd energy_vec = tets->getStrainEnergyPerElement(world.getState());
    std::cout<<"here"<<std::endl;
    igl::writeDMAT(energy_filename.str(), energy_vec, false); // Don't use ascii
    // std::cout<<energy_vec<<std::endl;
    std::ofstream fout(output_dir + "mouse.json");
    fout << mouse_json;
    fout.close();

    std::cout << "Saved " << displacements_filename.str() << std::endl;
    std::cout << "Saved " << energy_filename.str() << std::endl;
    // std::cout << "Saved " << force_filename.str() << std::endl;
}

void save_base_configurations_DMAT(Eigen::MatrixXd &V, Eigen::MatrixXi &F) {
    std::stringstream verts_filename, faces_filename;
    verts_filename << output_dir << "base_verts.dmat";
    faces_filename << output_dir << "base_faces.dmat";

    igl::writeDMAT(verts_filename.str(), V, false); // Don't use ascii
    igl::writeDMAT(faces_filename.str(), F, false); // Don't use ascii
}

// Todo put this in utilities
Eigen::MatrixXd getCurrentVertPositions(MyWorld &world, NeohookeanTets *tets) {
    // Eigen::Map<Eigen::MatrixXd> q(mapStateEigen<0>(world).data(), V.cols(), V.rows()); // Get displacements only
    auto q = mapDOFEigen(tets->getQ(), world);
    Eigen::Map<Eigen::MatrixXd> dV(q.data(), V.cols(), V.rows()); // Get displacements only

    return V + dV.transpose();
}

void reset_world (MyWorld &world) {
        auto q = mapStateEigen(world); // TODO is this necessary?
        q.setZero();
}

int main(int argc, char **argv) {
    fs::path model_root(argv[1]);
    fs::path training_data_root = model_root / "training_data/training/";
    fs::path mesh_path = model_root / "tets.mesh";
    fs::path sim_params_path = training_data_root / "parameters.json";

    //Remove everything in oputput dir
    fs::path output_dir_path(output_dir);
    fs::remove_all(output_dir_path);
    fs::create_directory(output_dir_path);

    std::ifstream fin(sim_params_path.string());
    json sim_params;
    fin >> sim_params;

    // Load displacements
    std::cout << "Finding recorded displacements..." << std::endl;
    std::vector<MatrixXd> recorded_displacements;
    std::vector<int> displacement_nums;
    for (auto i = fs::directory_iterator(training_data_root); i != fs::directory_iterator(); i++)
    {
        fs::path cand_path = i->path();

        std::string name = cand_path.filename().string();
        std::string substr = "displacements";
        if(strncmp(name.c_str(), substr.c_str(), substr.size()) == 0) { // Is displacement
            std::string num_str = name.substr(substr.size()+1);
            displacement_nums.push_back(std::stoi(num_str, nullptr));
        }
    }
    std::cout << "Loading recorded displacements..." << std::endl;
    std::sort(displacement_nums.begin(), displacement_nums.end());
    for (std::vector<int>::iterator i = displacement_nums.begin(); i != displacement_nums.end(); ++i)
    {
        fs::path dis_path = training_data_root / ("displacements_" + std::to_string(*i) + ".dmat");
        MatrixXd U;
        igl::readDMAT(dis_path.string(), U);
        recorded_displacements.push_back(U);
    }
    std::cout << "Done." << std::endl;
    //Setup Physics
    MyWorld world;

    igl::readMESH(mesh_path.string(), V, T, F);
    NeohookeanTets *tets = new NeohookeanTets(V,T);
    for(auto element: tets->getImpl().getElements()) {
        element->setDensity(sim_params["density"]);//1000.0);
        element->setParameters(sim_params["YM"], sim_params["Poisson"]);
    }

    mouse_json["mouse_state_per_frame"] = json::array();
    // // Pinned particle to attach spring for dragging
    PhysicalSystemParticleSingle<double> *pinned_point = new PhysicalSystemParticleSingle<double>();
    pinned_point->getImpl().setMass(100000000); //10000000
    auto fem_attached_pos = PosFEM<double>(&tets->getQ()[0],0, &tets->getImpl().getV());

    double spring_stiffness = 150.0;
    double spring_rest_length = 0.1;
    ForceSpringFEMParticle<double> *forceSpring = new ForceSpringFEMParticle<double>(fem_attached_pos, // TODO compare getV to V. Get rid of double use of index
                                                                                PosParticle<double>(&pinned_point->getQ()),
                                                                                spring_rest_length, 0.0);

    world.addSystem(pinned_point);
    world.addForce(forceSpring);
    world.addSystem(tets);
    // fixDisplacementMin(world, tets, sim_params["fixed_axis"], sim_params["displacement_tol"]);
    world.finalize(); //After this all we're ready to go (clean up the interface a bit later)

    reset_world(world);

    MyTimeStepper stepper(sim_params["time_step"], (int)sim_params["implicit_its"]);
    // stepper.step(world);

    if(saving_training_data) {
        save_base_configurations_DMAT(V, F);
    }

    /** libigl display stuff **/
    igl::viewer::Viewer viewer;
    double tot_time = 0.0;
    viewer.callback_pre_draw = [&](igl::viewer::Viewer & viewer)
    {
        // Update with saved displacements
        MatrixXd new_U = recorded_displacements[current_frame].transpose();
        auto q = mapDOFEigen(tets->getQ(), world);
        q = Eigen::Map<VectorXd>(new_U.data(), new_U.rows() * new_U.cols());

        // predefined colors
        const Eigen::RowVector3d orange(1.0,0.7,0.2);
        const Eigen::RowVector3d yellow(1.0,0.9,0.2);
        const Eigen::RowVector3d blue(0.2,0.3,0.8);
        const Eigen::RowVector3d green(0.2,0.6,0.3);

        if(is_dragging) {
            auto q_part = mapDOFEigen(pinned_point->getQ(), world);
            Eigen::MatrixXd part_pos = q_part;

            viewer.data.set_points(part_pos.transpose(), orange);
        } else {
            Eigen::MatrixXd part_pos;
            viewer.data.set_points(part_pos, orange);
        }

        if(viewer.core.is_animating)
        {
            // Save Current configuration

            json mouse_state;
            mouse_state["dragged_pos"] = {dragged_pos[0], dragged_pos[1], dragged_pos[2]};
            mouse_state["dragged_vert"] = dragged_vert;
            mouse_state["is_dragging"] = is_dragging;
            mouse_json["mouse_state_per_frame"].push_back(mouse_state);
            // if(mouse_json["potential_energy_per_frame"].size() == current_frame) {
            //     std::cout << "Index mismatch!" << std::endl;
            // }
            if(saving_training_data) {
                save_displacements_DMAT_and_energy(current_frame, world, tets, mouse_json);
            }
            double start = igl::get_seconds();
            // stepper.step(world);
            tot_time += igl::get_seconds() - start;

            Eigen::MatrixXd newV = getCurrentVertPositions(world, tets);
            // std::cout<< newV.block(0,0,10,3) << std::endl;
            viewer.data.set_vertices(newV);
            viewer.data.compute_normals();

            current_frame++;
            std::cout << "Average timestep: " << (tot_time / current_frame) << std::endl;
        }
        return false;
    };

    viewer.callback_key_pressed = [&](igl::viewer::Viewer &, unsigned int key, int mod)
    {
        switch(key)
        {
            case 'P':
            case 'p':
            {
                viewer.core.is_animating = !viewer.core.is_animating;
                break;
            }
            case 'r':
            case 'R':
            {
                reset_world(world);
                break;
            }
            default:
            return false;
        }
        return true;
    };

    viewer.callback_mouse_down = [&](igl::viewer::Viewer&, int, int)->bool
    {
        Eigen::MatrixXd curV = getCurrentVertPositions(world, tets);
        last_mouse = Eigen::RowVector3f(viewer.current_mouse_x,viewer.core.viewport(3)-viewer.current_mouse_y,0);
        std::cout << last_mouse << std::endl;
        // Find closest point on mesh to mouse position
        int fid;
        Eigen::Vector3f bary;
        if(igl::unproject_onto_mesh(
            last_mouse.head(2),
            viewer.core.view * viewer.core.model,
            viewer.core.proj,
            viewer.core.viewport,
            curV, F,
            fid, bary))
        {
            long c;
            bary.maxCoeff(&c);
            dragged_pos = curV.row(F(fid,c)) + Eigen::RowVector3d(0.001,0.0,0.0); //Epsilon offset so we don't div by 0
            dragged_vert = F(fid,c);
            std::cout << "Dragging vert: " << dragged_vert << std::endl;

            // Update the system
            is_dragging = true;
            forceSpring->getImpl().setStiffness(spring_stiffness);
            auto pinned_q = mapDOFEigen(pinned_point->getQ(), world);
            pinned_q = dragged_pos;//(dragged_pos).cast<double>(); // necessary?

            fem_attached_pos = PosFEM<double>(&tets->getQ()[dragged_vert],dragged_vert, &tets->getImpl().getV());
            forceSpring->getImpl().setPosition0(fem_attached_pos);

            return true;
        }

        return false; // TODO false vs true??
    };

    viewer.callback_mouse_up = [&](igl::viewer::Viewer&, int, int)->bool
    {
        is_dragging = false;
        forceSpring->getImpl().setStiffness(0.0);
        // auto pinned_q = mapDOFEigen(pinned_point->getQ(), world);
        // pinned_q = Eigen::RowVector3d(-10000.0,0.0,0.0); // Move the point out of view

        return false;
    };

    viewer.callback_mouse_move = [&](igl::viewer::Viewer &, int,int)->bool
    {
        if(is_dragging) {
            Eigen::RowVector3f drag_mouse(
                viewer.current_mouse_x,
                viewer.core.viewport(3) - viewer.current_mouse_y,
                last_mouse(2));

            Eigen::RowVector3f drag_scene,last_scene;

            igl::unproject(
                drag_mouse,
                viewer.core.view*viewer.core.model,
                viewer.core.proj,
                viewer.core.viewport,
                drag_scene);
            igl::unproject(
                last_mouse,
                viewer.core.view*viewer.core.model,
                viewer.core.proj,
                viewer.core.viewport,
                last_scene);

            dragged_pos += ((drag_scene-last_scene)*4.5).cast<double>(); //TODO why do I need to fine tune this
            // dragged_pos += (drag_mouse-last_mouse).cast<double>();
            last_mouse = drag_mouse;

            // Update the system
            // TODO dedupe this
            auto pinned_q = mapDOFEigen(pinned_point->getQ(), world);
            pinned_q = dragged_pos; //(dragged_pos).cast<double>(); // necessary?
        }

        return false;
    };

    viewer.data.set_mesh(V,F);
    viewer.core.show_lines = true;
    viewer.core.invert_normals = true;
    viewer.core.is_animating = false;
    viewer.data.face_based = true;

    viewer.launch();
    return EXIT_SUCCESS;
}


/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/ZZ_p.h>
#include <json/json.h>
#include "HE1Encipher.h"

NTL::ZZ HE1Encipher::ONE = NTL::ZZ(1);

HE1Encipher::HE1Encipher()
{
	pmod = nullptr;
};

HE1Encipher::~HE1Encipher()
{
	delete pmod;
};

void HE1Encipher::generateParameters(long lambda, long eta){
	generateModulus(lambda,eta);
	/* Set local modulus to pq */
	NTL::ZZ_p::init(modulus);
	NTL::ZZ_p tmp = NTL::to_ZZ_p(p);
	pmod = new NTL::ZZ_p(tmp);
}

std::string HE1Encipher::writeParametersToJSON()
{
	Json::Value root;
	std::stringstream modStr;
	modStr << modulus;
	root["modulus"] = modStr.str();
	Json::FastWriter writer;
	std::string json = writer.write(root);
	return json;
};


#include <boost/geometry/formulas/vertex_latitude.hpp>


/*
 * @file SURF_FlannMatcher
 * @brief SURF detector + descriptor + FLANN Matcher
 * @author A. Huaman
 */

#include <fstream>
#include <iostream>
#include <stdio.h>
#include <iomanip>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Dense>
#include <opengv/amm.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
#include <opengv/optimization_tools/objective_function_tools/GlobalPnPFunctionInfo.hpp>
#include <opengv/optimization_tools/objective_function_tools/SquaredFunctionNoIterationsInfo.hpp>
#include <opengv/optimization_tools/solver_tools/SolverToolsNoncentralRelativePose.hpp>

#include "opencv2/core.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/xfeatures2d.hpp"

#include <opengv/amm.hpp>

#include <opengv/statistic/iterations_info.hpp>
#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace cv;
using namespace cv::xfeatures2d;
void readme();
/*
 * @function main
 * @brief Main function
 */
int main( int argc, char** argv )
{
  if( argc != 3 )
  { readme(); return -1; }
  Mat img_1 = imread( argv[1], IMREAD_GRAYSCALE );
  Mat img_2 = imread( argv[2], IMREAD_GRAYSCALE );
  if( !img_1.data || !img_2.data )
  { std::cout<< " --(!) Error reading images " << std::endl; return -1; }
  //-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
  int minHessian = 400;
  Ptr<SURF> detector = SURF::create();
  detector->setHessianThreshold(minHessian);
  std::vector<KeyPoint> keypoints_1, keypoints_2;
  Mat descriptors_1, descriptors_2;
  detector->detectAndCompute( img_1, Mat(), keypoints_1, descriptors_1 );
  detector->detectAndCompute( img_2, Mat(), keypoints_2, descriptors_2 );
  //-- Step 2: Matching descriptor vectors using FLANN matcher
  FlannBasedMatcher matcher;
  std::vector< DMatch > matches;
  matcher.match( descriptors_1, descriptors_2, matches );
  double max_dist = 0; double min_dist = 100;
  //-- Quick calculation of max and min distances between keypoints
  for( int i = 0; i < descriptors_1.rows; i++ )
  { double dist = matches[i].distance;
    if( dist < min_dist ) min_dist = dist;
    if( dist > max_dist ) max_dist = dist;
  }
  printf("-- Max dist : %f \n", max_dist );
  printf("-- Min dist : %f \n", min_dist );
  //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist,
  //-- or a small arbitary value ( 0.02 ) in the event that min_dist is very
  //-- small)
  //-- PS.- radiusMatch can also be used here.
  std::vector< DMatch > good_matches;
  for( int i = 0; i < descriptors_1.rows; i++ )
  { if( matches[i].distance <= max(2*min_dist, 0.02) )
    { good_matches.push_back( matches[i]); }
  }
  //-- Draw only "good" matches
  /*Mat img_matches;
  drawMatches( img_1, keypoints_1, img_2, keypoints_2,
               good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
               vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
  //-- Show detected matches
  imshow( "Good Matches", img_matches );*/
  std::vector<KeyPoint> k1;
  std::vector<KeyPoint> k2;
  for( int i = 0; i < (int)good_matches.size(); i++ )
  {
    //printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx );
    std::cout << ( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ) << std::endl;
    k1.push_back(keypoints_1[good_matches[i].queryIdx]);
    k2.push_back(keypoints_2[good_matches[i].trainIdx]);
  }
  for( int i = 0; i < (int)good_matches.size(); i++ )
    {
      std::cout << "Point1: " << k1[i].pt << std::endl;
      std::cout << "Point2: " << k2[i].pt << std::endl;
    }

  std::cout << "Cal matrices: " << std::endl;
  Eigen::Matrix3d K_left = Eigen::Matrix3d::Identity(3,3);
  K_left(0,0) = 9.597910e+02; K_left(0,1) =  0.000000e+00; K_left(0,2) = 6.960217e+02;
  K_left(1,0) = 0.000000e+00; K_left(1,1) =  9.569251e+02; K_left(1,2) = 2.241806e+02;
  K_left(2,0) = 0.000000e+00; K_left(2,1) =  0.000000e+00; K_left(2,2) = 1.000000e+00;

  Eigen::Matrix3d K_right = Eigen::Matrix3d::Identity(3,3);
  K_right(0,0) = 9.037596e+02; K_right(0,1) =  0.000000e+00; K_right(0,2) = 6.957519e+02;
  K_right(1,0) = 0.000000e+00; K_right(1,1) =  9.019653e+02; K_right(1,2) = 2.242509e+02;
  K_right(2,0) = 0.000000e+00; K_right(2,1) =  0.000000e+00; K_right(2,2) = 1.000000e+00;

  //To use the methods it will be necessary to build the central adapter
  opengv::bearingVectors_t v1;
  opengv::bearingVectors_t v2;
  int cols = (int)good_matches.size();
  for( int i = 0; i < cols; i++ )
    {
      std::cout << "Start cycle: " << i << std::endl;
      Eigen::MatrixXd v_left = Eigen::MatrixXd::Zero(3,1);
      v_left(0,0) = k1[i].pt.x;
      v_left(1,0) = k1[i].pt.y;
      v_left(2,0) = 1;
      //std::cout << "v1: " << std::endl << v1 << std::endl;

      Eigen::MatrixXd v_right = Eigen::MatrixXd::Zero(3,1);
      v_right(0,0) = k2[i].pt.x;
      v_right(1,0) = k2[i].pt.y;
      v_right(2,0) = 1;
      //std::cout << "v2: " << std::endl << v2 <<std::endl;

      v_left = K_left.inverse() * v_left;
      double norm_left = v_left.norm();
      v_left = v_left / norm_left;

      v_right = K_right.inverse() * v_right;
      double norm_right = v_right.norm();
      v_right = v_right /norm_right;

      v1.push_back(v_left);
      v2.push_back(v_right);
    }
  opengv::relative_pose::CentralRelativeAdapter adapter(v1, v2);
  //timer
  struct timeval tic;
  struct timeval toc;
  opengv::rotation_t rot = Eigen::MatrixXd::Identity(3,3);
  opengv::translation_t trans = Eigen::MatrixXd::Zero(3,1);
  trans(0,0) = 0.0;trans(1,0) = 0.5;trans(2,0) = 0.0;
  ObjectiveFunctionInfo * obj_func = new SquaredFunctionNoIterationsInfo(adapter);

  SolverTools * solver_func = new SolverToolsNoncentralRelativePose();
  double tol = 1e-6;
  double step = 0.01;
  amm optimizer_func;
  std::vector<iterations_info> stat_iter;
  opengv::transformation_t result = optimizer_func.amm_solver(tol, rot, trans, obj_func, solver_func, step, stat_iter);
  std::cout << "Rotation: " << std::endl << result.block<3,3>(0,0) << std::endl;
  std::cout << "Translation: " << std::endl << result.block<3,1>(0,3) << std::endl;
  delete solver_func;
  delete obj_func;

  std::cout << "running fivept_nister" << std::endl;
  opengv::essentials_t fivept_nister_essentials;
  gettimeofday( &tic, 0 );
  fivept_nister_essentials = opengv::relative_pose::fivept_nister(adapter);
  gettimeofday( &toc, 0 );
  double fivept_nister_time = TIMETODOUBLE(timeval_minus(toc,tic));

   std::cout << "running fivept_stewenius" << std::endl;
   opengv::complexEssentials_t fivept_stewenius_essentials;
   gettimeofday( &tic, 0 );
   fivept_stewenius_essentials = opengv::relative_pose::fivept_stewenius(adapter);
   gettimeofday( &toc, 0 );
   double fivept_stewenius_time = TIMETODOUBLE(timeval_minus(toc,tic));
   std::cout << "results from stewenius' five-point algorithm:" << std::endl;
   for(int i = 0; i < (int) fivept_stewenius_essentials.size(); ++i){
     std::cout << fivept_stewenius_essentials.at(i) << std::endl << std::endl;
   }
   std::cout << "results from nisters' five-point algorithm:" << std::endl;
   for( size_t i = 0; i < fivept_nister_essentials.size(); i++ )
     std::cout << fivept_nister_essentials.at(i) << std::endl << std::endl;
   //waitKey(0);
   opengv::translation_t twopt_translation;
   gettimeofday( &tic, 0 );
   twopt_translation = opengv::relative_pose::twopt(adapter,true);
   gettimeofday( &toc, 0 );
   double twopt_time = TIMETODOUBLE(timeval_minus(toc,tic));
   std::cout << "results from two-points algorithm:" << std::endl;
   std::cout << twopt_translation << std::endl << std::endl;

   std::cout << "running eightpt" << std::endl;
   opengv::essential_t eightpt_essential;
   gettimeofday( &tic, 0 );
   eightpt_essential = opengv::relative_pose::eightpt(adapter);
   gettimeofday( &toc, 0 );
   double eightpt_time = TIMETODOUBLE(timeval_minus(toc,tic));
   std::cout << "results from eight-point algorithm:" << std::endl;
   std::cout << eightpt_essential << std::endl << std::endl;

   /*NON LINEAR*/
   std::cout << "setting perturbed rotation and ";
   std::cout << "running eigensolver" << std::endl;
   opengv::translation_t t_perturbed;
   opengv::rotation_t R_perturbed;
   Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity(3,3);
   Eigen::MatrixXd position = Eigen::MatrixXd::Zero(3,1);
   position(0,0) = 0.5;position(1,0) = 0.5;position(2,0) = 0.5;
   opengv::getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.01);
   opengv::rotation_t eigensolver_rotation;
   gettimeofday( &tic, 0 );
   adapter.setR12(R_perturbed);
   eigensolver_rotation = opengv::relative_pose::eigensolver(adapter);
   gettimeofday( &toc, 0 );
   double eigensolver_time = TIMETODOUBLE(timeval_minus(toc,tic));

   std::cout << "setting perturbed pose and ";
   std::cout << "performing nonlinear optimization" << std::endl;
   opengv::getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
   opengv::transformation_t nonlinear_transformation;
   gettimeofday( &tic, 0 );
   adapter.sett12(t_perturbed);
   adapter.setR12(R_perturbed);
   nonlinear_transformation = opengv::relative_pose::optimize_nonlinear(adapter);
   gettimeofday( &toc, 0 );
   double nonlinear_time = TIMETODOUBLE(timeval_minus(toc,tic));

   for(int i=5; i < good_matches.size(); ++i){
     std::cout << "setting perturbed pose and ";
     std::cout << "performing nonlinear optimization with " << i << " indices" << std::endl;
     std::vector<int> indices10 = opengv::getNindices(i);
     opengv::getPerturbedPose( position, rotation, t_perturbed, R_perturbed, 0.1);
     adapter.sett12(t_perturbed);
     adapter.setR12(R_perturbed);
     opengv::transformation_t nonlinear_transformation_10 =
       opengv::relative_pose::optimize_nonlinear(adapter,indices10);
     std::cout << "results from nonlinear algorithm with only few correspondences:";
     std::cout << std::endl;
     std::cout << std::endl << nonlinear_transformation_10 << std::endl;
   }
   /************/
   std::cout << "results from eigensystem based rotation solver:" << std::endl;
   std::cout << eigensolver_rotation << std::endl << std::endl << std::endl;
   std::cout << "results from nonlinear algorithm:" << std::endl;
   std::cout << nonlinear_transformation << std::endl << std::endl;
   return 0;
}
/*
 * @function readme
 */
void readme()
{ std::cout << " Usage: ./SURF_FlannMatcher <img1> <img2>" << std::endl; }


#ifndef STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP
#define STAN_MATH_PRIM_FUN_MODIFIED_BESSEL_SECOND_KIND_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/functor/apply_scalar_binary.hpp>
#include <boost/math/special_functions/bessel.hpp>

namespace stan {
namespace math {

/**
 *
   \f[
   \mbox{modified\_bessel\_second\_kind}(v, z) =
   \begin{cases}
     \textrm{error} & \mbox{if } z \leq 0 \\
     K_v(z) & \mbox{if } z > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   \frac{\partial\, \mbox{modified\_bessel\_second\_kind}(v, z)}{\partial z} =
   \begin{cases}
     \textrm{error} & \mbox{if } z \leq 0 \\
     \frac{\partial\, K_v(z)}{\partial z} & \mbox{if } z > 0 \\[6pt]
     \textrm{NaN} & \mbox{if } z = \textrm{NaN}
   \end{cases}
   \f]

   \f[
   {K_v}(z)
   =
   \frac{\pi}{2}\cdot\frac{I_{-v}(z) - I_{v}(z)}{\sin(v\pi)}
   \f]

   \f[
   \frac{\partial \, K_v(z)}{\partial z} = -\frac{v}{z}K_v(z)-K_{v-1}(z)
   \f]
 *
 */
template <typename T2, require_arithmetic_t<T2>* = nullptr>
inline T2 modified_bessel_second_kind(int v, const T2 z) {
  return boost::math::cyl_bessel_k(v, z);
}

/**
 * Enables the vectorised application of the modified_bessel_second_kind
 * function, when the first and/or second arguments are containers.
 *
 * @tparam T1 type of first input
 * @tparam T2 type of second input
 * @param a First input
 * @param b Second input
 * @return modified_bessel_second_kind function applied to the two inputs.
 */
template <typename T1, typename T2, require_any_container_t<T1, T2>* = nullptr>
inline auto modified_bessel_second_kind(const T1& a, const T2& b) {
  return apply_scalar_binary(a, b, [&](const auto& c, const auto& d) {
    return modified_bessel_second_kind(c, d);
  });
}

}  // namespace math
}  // namespace stan

#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/constant/eps.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/constant/mone.hpp>
#include <eve/constant/mzero.hpp>
#include <eve/function/is_positive.hpp>
#include <eve/function/lambert.hpp>
#include <eve/function/diff/lambert.hpp>
#include <boost/math/special_functions/lambert_w.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of lambert"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  using kT  = kumi::tuple<T,T>;
  using kv_t= kumi::tuple<v_t, v_t>;
  TTS_EXPR_IS( eve::lambert(T())  ,kT);
  TTS_EXPR_IS( eve::lambert(v_t()), kv_t);
};

//==================================================================================================
// lambert  tests
//==================================================================================================
EVE_TEST( "Check behavior of lambert on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-0.367879, 0.367879)
                             , eve::test::randoms(-0.367879,0.0)
                             , eve::test::randoms(0, 10.0)
                             , eve::test::randoms(-0.367879, -0.367879+1.0e-6)
                             )
        )
  <typename T>(T a0, T a1, T a2,  T a3)
{
  using elt_t = eve::element_type_t<T>;
  if constexpr( eve::platform::supports_invalids )
  {
    {
      auto[w0, wm1] = eve::lambert(eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
    {
      auto[w0, wm1] = eve::lambert(eve::inf(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::inf(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::inf(eve::as<T>()));
    }
    {
      auto[w0, wm1] = eve::lambert(eve::mone(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
  }
  {
    auto[w0, wm1] = eve::lambert(eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::zero(eve::as<T>()));
  }
  {
    auto[w0, wm1] = eve::lambert(eve::mzero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::minf(eve::as<T>()));
  }
  {
    using v_t =  eve::element_type_t<T>;
    auto std_w0  = [](auto v)->v_t{return boost::math::lambert_w0(v); };
    auto std_wm1 = [](auto v)->v_t{return eve::is_positive(v) ? boost::math::lambert_w0(v) : boost::math::lambert_wm1(v); };
    {
      auto[w0, wm1] = eve::lambert(a0);
      TTS_ULP_EQUAL(w0, map(std_w0, a0), 10.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a0), 10.0);
    }
    {
      auto[w0, wm1] = eve::lambert(a1);
      TTS_ULP_EQUAL(w0, map(std_w0, a1), 300.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a1), 3.0);
    }
    {
      auto[w0, wm1] = eve::lambert(a2);
      TTS_ULP_EQUAL(w0, map(std_w0, a2), 3.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a2), 3.0);
    }
    {
      elt_t tol = 5000*eve::eps(eve::as<elt_t>());
      auto[w0, wm1] = eve::lambert(a3);
      TTS_ABSOLUTE_EQUAL(w0, map(std_w0, a3), tol);
      TTS_ABSOLUTE_EQUAL(wm1,map(std_wm1, a3), tol);
    }
 }
};

EVE_TEST( "Check behavior of lambert on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-0.367879, 0.367879)
                             , eve::test::randoms(-0.367879,0.0)
                             , eve::test::randoms(0, 10.0)
                             , eve::test::randoms(-0.367879, -0.367879+1.0e-6)
                             )
        )
  <typename T>(T a0, T a1, T a2,  T a3)
{
  using elt_t = eve::element_type_t<T>;
  if constexpr( eve::platform::supports_invalids )
  {
    {
      auto[w0, wm1] = eve::lambert(eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
    {
      auto[w0, wm1] = eve::lambert(eve::inf(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::inf(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::inf(eve::as<T>()));
    }
    {
      auto[w0, wm1] = eve::lambert(eve::mone(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
  }
  {
    auto[w0, wm1] = eve::lambert(eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::zero(eve::as<T>()));
  }
  {
    auto[w0, wm1] = eve::lambert(eve::mzero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::minf(eve::as<T>()));
  }
  {
    using v_t =  eve::element_type_t<T>;
    auto std_w0  = [](auto v)->v_t{return boost::math::lambert_w0(v); };
    auto std_wm1 = [](auto v)->v_t{return eve::is_positive(v) ? boost::math::lambert_w0(v) : boost::math::lambert_wm1(v); };
    {
      auto[w0, wm1] = eve::lambert(a0);
      TTS_ULP_EQUAL(w0, map(std_w0, a0), 10.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a0), 10.0);
    }
    {
      auto[w0, wm1] = eve::lambert(a1);
      TTS_ULP_EQUAL(w0, map(std_w0, a1), 300.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a1), 3.0);
    }
    {
      auto[w0, wm1] = eve::lambert(a2);
      TTS_ULP_EQUAL(w0, map(std_w0, a2), 3.0);
      TTS_ULP_EQUAL(wm1,map(std_wm1, a2), 3.0);
    }
    {
      elt_t tol = 5000*eve::eps(eve::as<elt_t>());
      auto[w0, wm1] = eve::lambert(a3);
      TTS_ABSOLUTE_EQUAL(w0, map(std_w0, a3), tol);
      TTS_ABSOLUTE_EQUAL(wm1,map(std_wm1, a3), tol);
    }
 }
};

EVE_TEST( "Check behavior of lambert diff on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(-0.367879, 0.367879)
                             , eve::test::randoms(-0.367879,0.0)
                             , eve::test::randoms(0, 10.0)
                             )
        )
  <typename T>(T a0, T a1, T a2)
{
  using eve::diff;
  if constexpr( eve::platform::supports_invalids )
  {
    {
      auto[w0, wm1] = diff(eve::lambert)(eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
    {
      auto[w0, wm1] = diff(eve::lambert)(eve::inf(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::zero(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::zero(eve::as<T>()));
    }
    {
      auto[w0, wm1] = diff(eve::lambert)(eve::mone(eve::as<T>()));
      TTS_IEEE_EQUAL(w0, eve::nan(eve::as<T>()));
      TTS_IEEE_EQUAL(wm1, eve::nan(eve::as<T>()));
    }
  }
  {
    auto[w0, wm1] = diff(eve::lambert)(eve::zero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::one(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::one(eve::as<T>()));
  }
  {
    auto[w0, wm1] = diff(eve::lambert)(eve::mzero(eve::as<T>()));
    TTS_IEEE_EQUAL(w0, eve::one(eve::as<T>()));
    TTS_IEEE_EQUAL(wm1, eve::minf(eve::as<T>()));
  }
  {
    using v_t =  eve::element_type_t<T>;
    auto dstd_w0  = [](auto v)->v_t{return boost::math::lambert_w0_prime(v); };
    auto dstd_wm1 = [](auto v)->v_t{return eve::is_positive(v) ? boost::math::lambert_w0_prime(v) : boost::math::lambert_wm1_prime(v); };
    {
      auto[w0, wm1] = diff(eve::lambert)(a0);
      TTS_RELATIVE_EQUAL(w0, map(dstd_w0, a0), 0.05);
      TTS_RELATIVE_EQUAL(wm1,map(dstd_wm1, a0), 0.05);
    }
    {
      auto[w0, wm1] = diff(eve::lambert)(a1);
      TTS_ULP_EQUAL(w0, map(dstd_w0, a1), 50.0);
      TTS_ULP_EQUAL(wm1,map(dstd_wm1, a1),50.0);
    }
    {
      auto[w0, wm1] = diff(eve::lambert)(a2);
      TTS_ULP_EQUAL(w0, map(dstd_w0, a2), 3.0);
      TTS_ULP_EQUAL(wm1,map(dstd_wm1, a2), 3.0);
    }
 }
};


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-predicates

    This function object returns @ref True if x is not greater than @ref Zero else returns @ref False.

    @par Header <boost/simd/function/is_ngtz.hpp>

    @par Note

     Using `is_ngtz(x)` is similar to: `!(x > 0)`

    @par Example:

      @snippet is_ngtz.cpp is_ngtz

    @par Possible output:

      @snippet is_ngtz.txt is_ngtz

  **/
  as_logical_t<Value> is_ngtz(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_ngtz.hpp>
#include <boost/simd/function/simd/is_ngtz.hpp>

#endif


//
// Copyright (c) 2015-2020 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/urdf/model.hxx"

#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>

#include <sstream>
#include <boost/foreach.hpp>
#include <limits>

namespace pinocchio
{
  namespace urdf
  {
    namespace details
    {
      ///
      /// \brief Convert URDF Inertial quantity to Spatial Inertia.
      ///
      /// \param[in] Y The input URDF Inertia.
      ///
      /// \return The converted Spatial Inertia pinocchio::Inertia.
      ///
      static Inertia convertFromUrdf(const ::urdf::Inertial & Y)
      {
        const ::urdf::Vector3 & p = Y.origin.position;
        const ::urdf::Rotation & q = Y.origin.rotation;

        const Inertia::Vector3 com(p.x,p.y,p.z);
        const Inertia::Matrix3 & R = Eigen::Quaterniond(q.w,q.x,q.y,q.z).matrix();

        Inertia::Matrix3 I;
        I << Y.ixx,Y.ixy,Y.ixz,
             Y.ixy,Y.iyy,Y.iyz,
             Y.ixz,Y.iyz,Y.izz;
        return Inertia(Y.mass,com,R*I*R.transpose());
      }

      static Inertia convertFromUrdf(const ::urdf::InertialSharedPtr & Y)
      {
        if (Y) return convertFromUrdf(*Y);
        return Inertia::Zero();
      }

      static FrameIndex getParentLinkFrame(const ::urdf::LinkConstSharedPtr link,
                                           UrdfVisitorBase & model)
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(link && link->getParent());
        FrameIndex id = model.getBodyId(link->getParent()->name);
        return id;
      }

      ///
      /// \brief Recursive procedure for reading the URDF tree.
      ///        The function returns an exception as soon as a necessary Inertia or Joint information are missing.
      ///
      /// \param[in] link The current URDF link.
      /// \param[in] model The model where the link must be added.
      ///
      void parseTree(::urdf::LinkConstSharedPtr link,
                     UrdfVisitorBase & model)
      {
        typedef UrdfVisitorBase::Scalar Scalar;
        typedef UrdfVisitorBase::SE3 SE3;
        typedef UrdfVisitorBase::Vector Vector;
        typedef UrdfVisitorBase::Vector3 Vector3;
        typedef Model::FrameIndex FrameIndex;

        // Parent joint of the current body
        const ::urdf::JointConstSharedPtr joint =
        ::urdf::const_pointer_cast< ::urdf::Joint>(link->parent_joint);

        if(joint) // if the link is not the root of the tree
        {
          PINOCCHIO_CHECK_INPUT_ARGUMENT(link->getParent());

          const std::string & joint_name = joint->name;
          const std::string & link_name = link->name;
          const std::string & parent_link_name = link->getParent()->name;
          std::ostringstream joint_info;

          FrameIndex parentFrameId = getParentLinkFrame(link, model);

          // Transformation from the parent link to the joint origin
          const SE3 jointPlacement
          = convertFromUrdf(joint->parent_to_joint_origin_transform);

          const Inertia Y = convertFromUrdf(link->inertial);

          Vector max_effort(1), max_velocity(1), min_config(1), max_config(1);
          Vector friction(Vector::Constant(1,0.)), damping(Vector::Constant(1,0.));
          Vector3 axis (joint->axis.x, joint->axis.y, joint->axis.z);

          const Scalar infty = std::numeric_limits<Scalar>::infinity();

          switch(joint->type)
          {
            case ::urdf::Joint::FLOATING:
              joint_info << "joint FreeFlyer";

              max_effort   = Vector::Constant(6, infty);
              max_velocity = Vector::Constant(6, infty);
              min_config   = Vector::Constant(7,-infty);
              max_config   = Vector::Constant(7, infty);
              min_config.tail<4>().setConstant(-1.01);
              max_config.tail<4>().setConstant( 1.01);

              friction = Vector::Constant(6, 0.);
              damping = Vector::Constant(6, 0.);

              model.addJointAndBody(UrdfVisitorBase::FLOATING, axis,
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
              break;

            case ::urdf::Joint::REVOLUTE:
              joint_info << "joint REVOLUTE with axis";

              // TODO I think the URDF standard forbids REVOLUTE with no limits.
              assert(joint->limits);
              if(joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                min_config << joint->limits->lower;
                max_config << joint->limits->upper;
              }

              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }

              model.addJointAndBody(UrdfVisitorBase::REVOLUTE, axis,
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
              break;

            case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits
              joint_info << "joint CONTINUOUS with axis";

              min_config.resize(2);
              max_config.resize(2);
              min_config << -1.01, -1.01;
              max_config <<  1.01,  1.01;

              if(joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
              }
              else
              {
                max_effort << infty;
                max_velocity << infty;
              }

              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }

              model.addJointAndBody(UrdfVisitorBase::CONTINUOUS, axis,
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
              break;

            case ::urdf::Joint::PRISMATIC:
              joint_info << "joint PRISMATIC with axis";

              // TODO I think the URDF standard forbids REVOLUTE with no limits.
              assert(joint->limits);
              if (joint->limits)
              {
                max_effort << joint->limits->effort;
                max_velocity << joint->limits->velocity;
                min_config << joint->limits->lower;
                max_config << joint->limits->upper;
              }

              if(joint->dynamics)
              {
                friction << joint->dynamics->friction;
                damping << joint->dynamics->damping;
              }

              model.addJointAndBody(UrdfVisitorBase::PRISMATIC, axis,
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
              break;

            case ::urdf::Joint::PLANAR:
              joint_info << "joint PLANAR with normal axis along Z";

              max_effort   = Vector::Constant(3, infty);
              max_velocity = Vector::Constant(3, infty);
              min_config   = Vector::Constant(4,-infty);
              max_config   = Vector::Constant(4, infty);
              min_config.tail<2>().setConstant(-1.01);
              max_config.tail<2>().setConstant( 1.01);

              friction = Vector::Constant(3, 0.);
              damping = Vector::Constant(3, 0.);

              model.addJointAndBody(UrdfVisitorBase::PLANAR, axis,
                                    parentFrameId,jointPlacement,joint->name,
                                    Y,link->name,
                                    max_effort,max_velocity,min_config,max_config,
                                    friction,damping);
              break;

            case ::urdf::Joint::FIXED:
              // In case of fixed joint, if link has inertial tag:
              //    -add the inertia of the link to his parent in the model
              // Otherwise do nothing.
              // In all cases:
              //    -let all the children become children of parent
              //    -inform the parser of the offset to apply
              //    -add fixed body in model to display it in gepetto-viewer

              joint_info << "fixed joint";
              model.addFixedJointAndBody(parentFrameId, jointPlacement,
                  joint_name, Y, link_name);
              break;

            default:
              throw std::invalid_argument("The type of joint " + joint_name + " is not supported.");
              break;
          }

          model
            << "Adding Body" << '\n'
            << '\"' << link_name << "\" connected to \"" << parent_link_name << "\" through joint \"" << joint_name << "\"\n"
            << "joint type: " << joint_info.str() << '\n'
            << "joint placement:\n" << jointPlacement << '\n'
            << "body info: " << '\n'
            << "  mass: " << Y.mass() << '\n'
            << "  lever: " << Y.lever().transpose() << '\n'
            << "  inertia elements (Ixx,Iyx,Iyy,Izx,Izy,Izz): " << Y.inertia().data().transpose() << '\n' << '\n';
        }
        else if (link->getParent())
          throw std::invalid_argument(link->name + " - joint information missing.");

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child, link->child_links)
        {
          parseTree(child, model);
        }
      }

      ///
      /// \brief Parse a tree with a specific root joint linking the model to the environment.
      ///        The function returns an exception as soon as a necessary Inertia or Joint information are missing.
      ///
      /// \param[in] link The current URDF link.
      /// \param[in] model The model where the link must be added.
      ///
      void parseRootTree(const ::urdf::ModelInterface * urdfTree,
                         UrdfVisitorBase& model)
      {
        model.setName(urdfTree->getName());

        ::urdf::LinkConstSharedPtr root_link = urdfTree->getRoot();
        model.addRootJoint(convertFromUrdf(root_link->inertial), root_link->name);

        BOOST_FOREACH(::urdf::LinkConstSharedPtr child, root_link->child_links)
        {
          parseTree(child, model);
        }
      }

      void parseRootTree(const std::string & filename,
                         UrdfVisitorBase& model)
      {
        ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile (filename);
        if (urdfTree)
          return parseRootTree (urdfTree.get(), model);
        else
          throw std::invalid_argument("The file " + filename + " does not "
              "contain a valid URDF model.");
      }

      void parseRootTreeFromXML(const std::string & xmlString,
                                UrdfVisitorBase& model)
      {
        ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString);
        if (urdfTree)
          return parseRootTree (urdfTree.get(), model);
        else
          throw std::invalid_argument("The XML stream does not contain a valid "
              "URDF model.");
      }
    } // namespace details
  } // namespace urdf
} // namespace pinocchio


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_MODF_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_MODF_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/trunc.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/vector.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::modf_, tag::cpu_,
                       (A0)(X),
                       ((simd_<arithmetic_<A0>,X>))
                      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::modf_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>
    {
      typedef typename meta::strip<A0>::type        A00;
      typedef boost::fusion::vector<A00, A00>       type;
    };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename NT2_RETURN_TYPE(1)::type type;
      type res;
      eval(a0,boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  private:

    template<class A0,class R0,class R1> inline void
    eval(A0 const& a0, R0& r0, R1& r1)const
    {
      r1 = nt2::trunc(a0);
      r0 = a0-r1;
    }
  };
} }

#endif
// modified by jt the 04/01/2011


//  (C) Copyright 2005 Daniel Egloff, Eric Niebler
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_covariance.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    std::vector<double> dummy;
    dummy.push_back(0);
    dummy.push_back(0);

    accumulator_set<double, stats<tag::weighted_covariance<double, tag::covariate1> >, double > acc;
    accumulator_set<std::vector<double>, stats<tag::weighted_covariance<double, tag::covariate1> >, double > acc2(sample = dummy);
    accumulator_set<double, stats<tag::weighted_covariance<std::vector<double>, tag::covariate1> >, double > acc3(covariate1 = dummy);
    accumulator_set<std::vector<double>, stats<tag::weighted_covariance<std::vector<double>, tag::covariate1> >, double > acc4(sample = dummy, covariate1 = dummy);

    std::vector<double> a;
    a.push_back(1.);
    a.push_back(2.);
    std::vector<double> b;
    b.push_back(3.);
    b.push_back(4.);
    std::vector<double> c;
    c.push_back(2.);
    c.push_back(5.);
    std::vector<double> d;
    d.push_back(4.);
    d.push_back(2.);

    // double - double
    {
        acc(1., weight = 1.1, covariate1 = 2.);
        acc(1., weight = 2.2, covariate1 = 4.);
        acc(2., weight = 3.3, covariate1 = 3.);
        acc(6., weight = 4.4, covariate1 = 1.);
    }

    // vector - double
    {
        acc2(a, weight = 1.1, covariate1 = 1.);
        acc2(b, weight = 2.2, covariate1 = 1.);
        acc2(c, weight = 3.3, covariate1 = 2.);
        acc2(d, weight = 4.4, covariate1 = 6.);
    }

    // double - vector
    {
        acc3(1., weight = 1.1, covariate1 = a);
        acc3(1., weight = 2.2, covariate1 = b);
        acc3(2., weight = 3.3, covariate1 = c);
        acc3(6., weight = 4.4, covariate1 = d);
    }

    // vector - vector
    {
        acc4(a, weight = 1.1, covariate1 = b);
        acc4(b, weight = 2.2, covariate1 = c);
        acc4(a, weight = 3.3, covariate1 = c);
        acc4(d, weight = 4.4, covariate1 = b);
    }

    double epsilon = 1e-6;

    BOOST_CHECK_CLOSE((weighted_covariance(acc)), -2.39, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc2))[0], 1.93, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc2))[1], -2.09, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc3))[0], 1.93, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc3))[1], -2.09, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc4))(0,0), 0.4, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc4))(0,1), -0.2, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc4))(1,0), -0.4, epsilon);
    BOOST_CHECK_CLOSE((weighted_covariance(acc4))(1,1), 0.2, epsilon);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("weighted_covariance test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#pragma once

#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>

namespace ipc {

template <typename DerivedLocalGrad, typename DerivedGrad>
void local_gradient_to_global_gradient(
    const Eigen::MatrixBase<DerivedLocalGrad>& local_grad,
    const std::vector<long>& ids,
    int dim,
    Eigen::PlainObjectBase<DerivedGrad>& grad)
{
    for (int i = 0; i < ids.size(); i++) {
        grad.segment(dim * ids[i], dim) += local_grad.segment(dim * i, dim);
    }
}

template <typename Derived>
void local_hessian_to_global_triplets(
    const Eigen::MatrixBase<Derived>& local_hessian,
    const std::vector<long>& ids,
    int dim,
    std::vector<Eigen::Triplet<double>>& triplets)
{
    for (int i = 0; i < ids.size(); i++) {
        for (int j = 0; j < ids.size(); j++) {
            for (int k = 0; k < dim; k++) {
                for (int l = 0; l < dim; l++) {
                    triplets.emplace_back(
                        dim * ids[i] + k, dim * ids[j] + l,
                        local_hessian(dim * i + k, dim * j + l));
                }
            }
        }
    }
}

} // namespace ipc