#include "ros/ros.h"
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Odometry.h>
#include <fstream>
#include <iostream>
#include <Eigen/Core>
#include <iomanip>
using namespace std;

void subCallback(const nav_msgs::Odometry &msg)
{
   Eigen::Vector3d way_points;
   way_points(0)  = msg.pose.pose.position.x;
   way_points(1)  = msg.pose.pose.position.y;
   way_points(2)  = msg.pose.pose.position.z;

//    cout<< "waypoint:" << way_points(0)<< endl;
   std::ofstream out("/home/rui/Desktop/t.txt",std::ios::app);
   if (out.fail())
    {
        cout<< "ERROR In Write Data"<<endl;
    }
   //<<fixed<<setprecision(2)的作用就是将输出结果转化为小数点后两位
   out<<fixed<<setprecision(2)<<way_points(0)<<"\t"<<way_points(1)<<"\t"<<way_points(2)<<std::endl;
//    out<<fixed<<setprecision(2)<<way_points(0)<<"\t\t"<<way_points(1)<<"\t\t"<<way_points(2)<<std::endl;
   // out<<fixed<<setprecision(2)<<x<<" "<<y<<" "<<z<<std::endl;
   out.close();
}

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

    ros::init(argc,argv,"subscrible_01"); //初始化节点
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/airsim_node_fast/drone_1/odom_local_ned", 1000, subCallback);

    ros::spin ();
    return 0;
}


#include "Day06-CustomCustoms.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>

#include <unordered_set>
#include <numeric>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2020
{
namespace Day06
{

using GroupAnswerTextSection = std::vector<std::string>;

struct GroupAnswer
{
    GroupAnswerTextSection answerSequences;

    int getUnionOfYesAnswers() const
    {
        std::unordered_set<char> answerUnion;
        auto allAnswers = boost::join(answerSequences, "");

        for (char answer : allAnswers)
        {
            answerUnion.insert(answer);
        }

        return answerUnion.size();
    }

    int getIntersectionOfYesAnswers() const
    {
        std::string firstAnswerSequenceSorted = answerSequences.front();
        std::sort(firstAnswerSequenceSorted.begin(), firstAnswerSequenceSorted.end());
        std::string answerIntersection{firstAnswerSequenceSorted.cbegin(), firstAnswerSequenceSorted.cend()};

        for (auto answerSequenceIter = std::next(answerSequences.cbegin()); answerSequenceIter != answerSequences.cend(); ++answerSequenceIter)
        {
            std::string currentAnswerSequenceSorted = *answerSequenceIter;
            std::sort(currentAnswerSequenceSorted.begin(), currentAnswerSequenceSorted.end());
            std::string newAnswerIntersection;
            std::set_intersection(answerIntersection.cbegin(), answerIntersection.cend(),
                                  currentAnswerSequenceSorted.cbegin(), currentAnswerSequenceSorted.cend(),
                                  std::back_inserter(newAnswerIntersection));

            answerIntersection = std::move(newAnswerIntersection);
        }

        return answerIntersection.size();
    }
};

std::vector<GroupAnswer> parseGroupAnswers(const std::vector<std::string>& lines)
{
    std::vector<GroupAnswerTextSection> groupAnswerTextSections;
    boost::split(groupAnswerTextSections, lines, [](const auto& elem) {return elem.empty(); });

    std::vector<GroupAnswer> groupAnswers;
    for (auto& groupAnswerTextSection : groupAnswerTextSections)
    {
        GroupAnswer groupAnswer{std::move(groupAnswerTextSection)};
        groupAnswers.push_back(std::move(groupAnswer));
    }

    return groupAnswers;
}

int sumAnyoneAnsweredYes(const std::vector<std::string>& lines)
{
    std::vector<GroupAnswer> groupAnswers = parseGroupAnswers(lines);
    return std::accumulate(groupAnswers.cbegin(), groupAnswers.cend(), 0, [](int acc, const auto& groupAnswer)
                           {
                               return acc + groupAnswer.getUnionOfYesAnswers();
                           });
}

int sumEveryoneAnsweredYes(const std::vector<std::string>& lines)
{
    std::vector<GroupAnswer> groupAnswers = parseGroupAnswers(lines);
    return std::accumulate(groupAnswers.cbegin(), groupAnswers.cend(), 0, [](int acc, const auto& groupAnswer)
                           {
                               return acc + groupAnswer.getIntersectionOfYesAnswers();
                           });
}

}
}
}


/*
 * 		ELABORAZIONE DI DATI TRIDIMENSIONALI - 2014/2015
 *
 * 		Department of Information Engineering (DEI)
 *
 * 						University of Padova
 *
 *
 * This source was developed for the final course project as well as for the
 * competition organized by Loccioni Group.
 *
 *
 *  Created on: Apr 22, 2015
 *      Authors:
 *      	Alessandro Beltramin
 *      	Matteo Sartori
 *
 */

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <algorithm>
#include <cstdio>
#include <map>
#include <memory>

#include "Calibration.hpp"
#include "Inspection.hpp"
#include "Parameters.hpp"
#include "Pipeline.hpp"
#include "ResultConnector.hpp"

using namespace pinspect;

namespace po = boost::program_options;

static Pipeline pipeline;

static CalibrationSet dataset;

/**
 * Map of connector objects created on input to pass to various ProcessingStages.
 */
static std::vector<boost::any> inputConnectorList;

/**
 * Bind a name to each ProcessingStage objects.
 * It's used to set up links during pipeline constructions.
 */
static std::map<std::string, ProcessingInterface*> stageMap;

/**
 * Allocate processing stages associated with names.
 */
static void initializeStageMap() {
	stageMap.insert(
	{
		{"findGridLeft", new FindGridPattern("findGridLeft")},
		{"findGridRight",new FindGridPattern("findGridRight")},
		{"leftCamera", new SingleCalibration("leftCamera")},
		{"rightCamera", new SingleCalibration("rightCamera")},
		{"stereoCamera", new StereoCalibration("stereoCamera")},
		{"rectifyMaps", new RectifyCalibration("rectifyMaps")},
		{"rectification", new Rectification("rectification")},
		{"cornerDetection", new CornerDetection("cornerDetection")},
		{"gridFitting", new GridFitting("gridFitting")},
		{"refinement", new Refinement("refinement")},
		{"triangulation", new Triangulation("triangulation")},
		{"registration", new Registration("registration")}
	});
}

/**
 * Free memory.
 */
static void freeStageMap() {
	for(auto x : stageMap)
		delete x.second;
}

/**
 * Retrieve of ProcessingStages. It has to be generics because they are of
 * different type.
 */
template<typename T> static T* getStage(std::string s) {
	return static_cast<T*>(stageMap[s]);
}

/**
 * Allocate a new InputConnector object and @return a pointer to it.
 */
template<typename Type> static InputConnector<Type>* newInputConnector(Type x) {

	InputConnector<Type> newObj;
	newObj.putValue(x);
	inputConnectorList.push_back(boost::any(newObj));

	// this automatically returns a pointer to the underlying object
	return boost::any_cast<InputConnector<Type>>(&inputConnectorList.back());
}

/**
 * This is for reading specific command arguments and to setup input
 * connections. Then the stage is added to the pipeline object.
 */
static void setCalibration(po::variables_map vm) {

	std::string leftFormat = "";
	std::string rightFormat= "";

	if(!vm.count("left-format") || !vm.count("right-format")) {
		std::cout << "\n\nOptions --left-format and --right-format mandatory" << std::endl;
		std::cout << "\nExample: --left-format=datasetLoc/Leftimg#.jpg" << std::endl;
		std::cout << "Where the '#' stand for the ID of the image" << std::endl;
		std::exit(1);
	}

	leftFormat = vm["left-format"].as<std::string>();
	rightFormat = vm["right-format"].as<std::string>();

	dataset = buildCalibrationSetFromRegex(leftFormat, rightFormat);

	FindGridPattern *findLeft = getStage<FindGridPattern>("findGridLeft");
	findLeft->setInput(newInputConnector(dataset.getLeftList()));
	findLeft->setDataset(&dataset);

	FindGridPattern *findRight = getStage<FindGridPattern>("findGridRight");
	findRight->setInput(newInputConnector(dataset.getRightList()));
	findRight->setDataset(&dataset);

	SingleCalibration *calibLeft = getStage<SingleCalibration>("leftCamera");
	calibLeft->setInput(findLeft->getResultConnector());
	calibLeft->setDataset(&dataset);

	SingleCalibration *calibRight = getStage<SingleCalibration>("rightCamera");
	calibRight->setInput(findRight->getResultConnector());
	calibRight->setDataset(&dataset);

	StereoCalibration *stereo = getStage<StereoCalibration>("stereoCamera");
	stereo->setInput(findLeft->getResultConnector(), calibLeft->getResultConnector(),
			findRight->getResultConnector(), calibRight->getResultConnector());

	pipeline.addStage(findLeft).addStage(calibLeft);
	pipeline.addStage(findRight).addStage(calibRight);
	pipeline.addStage(stereo);
}

/**
 * @see setCalibration
 */
static void setRectifyMaps(po::variables_map vm) {

	RectifyCalibration *stage = getStage<RectifyCalibration>("rectifyMaps");
	stage->setInput(getStage<SingleCalibration>("leftCamera")->getResultConnector(),
			getStage<SingleCalibration>("rightCamera")->getResultConnector(),
			getStage<StereoCalibration>("stereoCamera")->getResultConnector());

	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setRectification(po::variables_map vm) {

	std::string leftImage = "";
	std::string rightImage= "";

	if (!vm.count("left-image") || !vm.count("right-image")) {
		std::cout << "\n\nOptions --left-image and --right-image mandatory" << std::endl;
		std::exit(1);
	}

	leftImage = vm["left-image"].as<std::string>();
	rightImage = vm["right-image"].as<std::string>();

	Rectification *stage = getStage<Rectification>("rectification");

	stage->setInput(newInputConnector(buildImagePair(leftImage, rightImage)),
			getStage<SingleCalibration>("leftCamera")->getResultConnector(),
			getStage<SingleCalibration>("rightCamera")->getResultConnector(),
			getStage<RectifyCalibration>("rectifyMaps")->getResultConnector());

	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setCornerDetection(po::variables_map vm) {

	CornerDetection *stage = getStage<CornerDetection>("cornerDetection");
	stage->setInput(getStage<Rectification>("rectification")->getResultConnector());
	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setGridFitting(po::variables_map vm) {

	GridFitting *stage = getStage<GridFitting>("gridFitting");
	stage->setInput(getStage<CornerDetection>("cornerDetection")->getResultConnector(),
			getStage<Rectification>("rectification")->getResultConnector());
	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setRefinement(po::variables_map vm) {

	Refinement *stage = getStage<Refinement>("refinement");
	stage->setInput(getStage<GridFitting>("gridFitting")->getResultConnector(),
			getStage<Rectification>("rectification")->getResultConnector());
	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setTriangulation(po::variables_map vm) {

	Triangulation *stage = getStage<Triangulation>("triangulation");
	stage->setInput(getStage<Refinement>("refinement")->getResultConnector(),
			getStage<RectifyCalibration>("rectifyMaps")->getResultConnector());
	pipeline.addStage(stage);
}

/**
 * @see setCalibration
 */
static void setRegistration(po::variables_map vm) {

	Registration *stage = getStage<Registration>("registration");
	stage->setInput(getStage<Triangulation>("triangulation")->getResultConnector());
	pipeline.addStage(stage);
}

typedef void (*commandHandler)(po::variables_map vm);

typedef std::pair<std::string, commandHandler> CommandPair;

typedef std::vector<CommandPair> CommandList;

/**
 * Represent commands with a description of which operation to perform.
 */
static std::vector<std::pair<std::string,std::string>> commandDescription = {
		{"calib", "perform calibration"},
		{"rectmap", "compute rectification maps"},
		{"rectify", "rectify input images"},
		{"cornerdet", "corner detection"},
		{"gridfit", "fitting the grid model"},
		{"refine", "pin position refinement"},
		{"triang", "triangulate and project in 3D"},
		{"regist", "model registration, compute errors"}
};

/**
 * Associate to each command name the static function to call to
 * create the ProcessingStage object specific for that command.
 */
static CommandList commandList = {
		{"calib", setCalibration},
		{"rectmap", setRectifyMaps},
		{"rectify", setRectification},
		{"cornerdet", setCornerDetection},
		{"gridfit", setGridFitting},
		{"refine", setRefinement},
		{"triang", setTriangulation},
		{"regist", setRegistration}
};

static void printCommands() {
	for(auto e : commandDescription)
		std::printf("   %s\t%s\n", e.first.c_str(), e.second.c_str());
}

/**
 * Decide how to build the pipeline while parsing command line arguments.
 * @param from decide where to start executing stage
 * @param to what stage to stop at.
 *
 * Every stage between from and to are added in the order decided statically by
 * @see commandList object.
 */
static void parseCommands(std::string from, std::string to, po::variables_map vm) {

	auto fromLambda = [from](CommandPair p){ return p.first == from; };
	auto toLambda   = [to](CommandPair p){ return p.first == to; };

	auto fromIt = std::find_if(commandList.begin(), commandList.end(), fromLambda);
	auto toIt = std::find_if(commandList.begin(), commandList.end(), toLambda);

	if(from == "start")
		fromIt = commandList.begin();
	if(to == "end")
		toIt = commandList.end() - 1;

	//boundary check
	if(fromIt == commandList.end())
		throw std::runtime_error("Invalid command '" + from + "'");

	if(fromIt > toIt)
		throw std::runtime_error("Invalid commands '" + from + "' '" + to + "'");

	if(toIt == commandList.end() && to.front() != '-')
		throw std::runtime_error("Invalid command '" + to + "'");
	else if(toIt == commandList.end())
		//we assume the user wants only one stage
		toIt = fromIt;

	std::cout << "Commands to execute: ";
	std::for_each(fromIt, toIt+1, [vm](CommandPair p) {
		std::cout << "'" << p.first << "' ";
		p.second(vm);
	});

	std::cout << std::endl;
}

std::string optionDescription = "\nUsage: pinspect [OPTIONS] FROM-CMD [TO-CMD]\n\nPrimary options";

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

	std::string paramsFilename = "parameters.xml";
	std::string resultsDirname = "results";
	std::string fromCommand = "";
	std::string toCommand = "";

	//boost object for describe program options.
	po::options_description desc(optionDescription);
	desc.add_options()
	    ("help,h", "produce help message")
			("print-params,p", "print set parameters")
	    ("parameters", po::value<std::string>(&paramsFilename), "set parameters file")
			("results", po::value<std::string>(&resultsDirname), "set results directory")
			("left-format", po::value<std::string>(),"left images form of the dataset")
			("right-format", po::value<std::string>(),"right images form of the dataset")
			("left-image,l", po::value<std::string>(), "left image to inspect")
			("right-image,r", po::value<std::string>(), "right image to inspect")
			("commands", po::value<std::vector<std::string>>(), "from command to command");

	po::positional_options_description p;
	p.add("commands", 2);

	//this actually parses argv
	po::variables_map vm;
	po::store(po::command_line_parser(argc, argv).positional(p).options(desc).run(),vm);
	po::notify(vm);

	if(vm.count("help")) {
		std::cout << desc << std::endl;
		std::cout << "Commands available:" << std::endl;
		printCommands();
		std::exit(1);
	}

	if(!vm.count("commands")) {
		std::cout << "Need to specify commands to execute" << std::endl;
		std::exit(1);
	}

	std::vector<std::string> commands = vm["commands"].as<std::vector<std::string>>();
	fromCommand = commands[0];
	toCommand   = commands[1];
	if(toCommand == "")
		toCommand = fromCommand;

	//initialize static members of ProcessignInterface
	//which are configurations common to all stages
	Parameters parameters = populateDefaultParameters(paramsFilename);
	if(vm.count("print-params"))
		parameters.print();
	ProcessingInterface::setParameters(parameters);
	ProcessingInterface::setOutputDirectory(resultsDirname + "/");
	//create if not existing
	boost::filesystem::create_directory(resultsDirname);

	initializeStageMap();

	try {
		parseCommands(fromCommand, toCommand, vm);
	}
	catch(std::runtime_error& e) {
		std::cout << e.what() << std::endl;
		std::cout << "Commands available:" << std::endl;
		printCommands();
		std::exit(1);
	}

	//this checks for missing inputs
	pipeline.checkStagesInput();

	auto start = std::chrono::steady_clock::now();
	pipeline.execute();

	auto end = std::chrono::steady_clock::now();
	auto diff = end - start;
	std::cout << "Total execution time: ";
	std::cout << std::chrono::duration<double, std::milli>(diff).count() << " ms"
			<< std::endl;

	freeStageMap();
	return 0;
}


#include "adafParameters.h"
#include "adafFunctions.h"
#include <math.h>
#include <iostream>
#include <fstream>
#include <fmath/physics.h>
#include <fparameters/parameters.h>
#include <boost/property_tree/ptree.hpp>
#include <fparameters/SpaceIterator.h>
#include "State.h"

using namespace std;

///////////////////////

void adafParameters()
{
	ifstream adafFile, adafParams;

	adafFile.open("adafFile.txt"); adafParams.open("adafParameters.txt");
	adafFile >> nRaux;
	adafParams >> blackHoleMass >> accRateOut >> s >> magFieldPar >> alpha >> jAngMom >> delta >> rTr >> powerIndex;
	adafParams.close();

	logr.resize(nRaux,0.0);
	logTi.resize(nRaux,0.0);
	logTe.resize(nRaux,0.0);
	logv.resize(nRaux,0.0);

	for (size_t i=0;i<nRaux;i++) {
		adafFile >> logr[i] >> logTi[i] >> logTe[i] >> logv[i];
	}
	adafFile.close();

	blackHoleMass *= solarMass;
	schwRadius = 2.0*gravitationalConstant*blackHoleMass / cLight2;
	accRateOut = accRateOut * 1.39e18 * blackHoleMass/solarMass;
	cout << "Total outer accretion power = " << accRateOut*cLight2 << " erg s^{-1}" << endl;
	cout << "Total inner accretion power = " << accRateOut*pow(1.0/exp(logr.back()), s)*cLight2
		 << " erg s^{-1}" << endl;

	rTr = rTr * schwRadius;
	rOutCD = GlobalConfig.get<double>("rOutCD") * schwRadius;
	eMeanMolecularWeight = GlobalConfig.get<double>("mu_e");
	iMeanMolecularWeight = GlobalConfig.get<double>("mu_i");

	nR = GlobalConfig.get<int>("model.particle.default.dim.radius.samples");
	nE = GlobalConfig.get<int>("model.particle.photon.dim.energy.samples");
	nRcd = GlobalConfig.get<int>("model.particle.default.dim.radius_cd.samples");

    paso_r = pow(exp(logr.back())/exp(logr.front()),1.0/(nR));
	paso_rCD = pow(rOutCD/rTr,1.0/(nRcd));

	logMinEnergy = GlobalConfig.get<double>("model.particle.photon.dim.energy.min");
	logMaxEnergy = GlobalConfig.get<double>("model.particle.photon.dim.energy.max");

	inclination = GlobalConfig.get<double>("inclination");

    calculateComptonScatt = GlobalConfig.get<int>("calculateComptonScatt");
	height_method = GlobalConfig.get<int>("height_method");

    calculateThermal = GlobalConfig.get<int>("calculateThermal");
	numProcesses = GlobalConfig.get<int>("thermal.numProcesses");
    if (calculateThermal) {
        calculateComptonRedMatrix = GlobalConfig.get<int>("thermal.compton.calculateRedMatrix");
        comptonMethod = GlobalConfig.get<int>("thermal.compton.method");
        if (1) {

            nGammaCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nGammaCompton");
            nTempCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nTempCompton");
            nNuPrimCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nNuPrimCompton");
            nNuCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nNuCompton");
            gammaMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.gammaMinCompton");
            gammaMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.gammaMaxCompton");
            tempMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.tempMinCompton");
            tempMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.tempMaxCompton");
            nuPrimMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuPrimMinCompton");
            nuPrimMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuPrimMaxCompton");
            nuMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuMinCompton");
            nuMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuMaxCompton");

            ofstream fileSizes;
            fileSizes.open("comptonRedMatrix/sizesVecCompton.dat",ios::out);
            fileSizes << nGammaCompton << "\t" << gammaMinCompton << "\t" << gammaMaxCompton << endl
                      << nTempCompton << "\t" << tempMinCompton << "\t" << tempMaxCompton << endl
                      << nNuPrimCompton << "\t" << nuPrimMinCompton << "\t" << nuPrimMaxCompton << endl
                      << nNuCompton << "\t" << nuMinCompton << "\t" << nuMaxCompton << endl;
            fileSizes.close();
        } else {
            ifstream fileSizes;
            fileSizes.open("comptonRedMatrix/sizesVecCompton.dat",ios::in);
            fileSizes >> nGammaCompton >> gammaMinCompton >> gammaMaxCompton
                      >> nTempCompton >> tempMinCompton >> tempMaxCompton
                      >> nNuPrimCompton >> nuPrimMinCompton >> nuPrimMaxCompton
                      >> nNuCompton >> nuMinCompton >> nuMaxCompton;
            fileSizes.close();
        }
        calculatePhotonDensityGap = GlobalConfig.get<int>("thermal.calculatePhotonDensityGap");
    }
	calculateJetEmission = GlobalConfig.get<int>("nonThermal.calculateJetEmission");

    calculateNonThermal = GlobalConfig.get<int>("calculateNonThermal");
    if (calculateNonThermal) {
		accMethod = GlobalConfig.get<double>("nonThermal.acc_method");
		calculateNTprotons = GlobalConfig.get<int>("nonThermal.protons");
		calculateNTelectrons = GlobalConfig.get<int>("nonThermal.electrons");
        calculateLosses = GlobalConfig.get<int>("nonThermal.calculateLosses");
		calculateFlare = GlobalConfig.get<int>("nonThermal.calculateFlare");
        calculateNTdistributions = GlobalConfig.get<int>("nonThermal.calculateDistributions");
        calculateNonThermalLum = GlobalConfig.get<int>("nonThermal.calculateLuminosities");
		calculateNonThermalHE = GlobalConfig.get<int>("nonThermal.calculateHighEnergyProcesses");
        calculateNeutronInj = GlobalConfig.get<int>("nonThermal.neutrons.calculateInjection");
		calculateNeutronDis = GlobalConfig.get<int>("nonThermal.neutrons.calculatePropagation");
		calculateJetDecay = GlobalConfig.get<int>("nonThermal.neutrons.calculateJetDecay");
		calculateSecondaries = GlobalConfig.get<int>("nonThermal.calculateSecondaries");
		calculateNeutrinos = GlobalConfig.get<int>("nonThermal.calculateNeutrinos");
	}
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef CPCMSOLVER_HPP
#define CPCMSOLVER_HPP

#include <iosfwd>
#include <string>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

class Cavity;
class IGreensFunction;

#include "PCMSolver.hpp"

/*! \file CPCMSolver.hpp
 *  \class CPCMSolver
 *  \brief Solver for conductor-like approximation: C-PCM (COSMO)
 *  \author Roberto Di Remigio
 *  \date 2013, 2016
 *
 *  \note We store the scaled, Hermitian, symmetrized S matrix and use a robust
 *  Cholesky decomposition to solve for the ASC.
 *  This avoids computing and storing the inverse explicitly.
 *  The S matrix is already scaled by the dielectric factor entering the
 *  definition of the conductor model!
 */

class CPCMSolver : public PCMSolver
{
public:
    CPCMSolver() {}
    /*! \brief Construct solver
     *  \param[in] symm whether the system matrix has to be symmetrized
     *  \param[in] corr factor to correct the conductor results
     */
    CPCMSolver(bool symm, double corr)
        : PCMSolver(), hermitivitize_(symm), correction_(corr) {}
    virtual ~CPCMSolver() {}
    friend std::ostream & operator<<(std::ostream & os, CPCMSolver & solver) {
        return solver.printSolver(os);
    }
private:
    /*! Whether the system matrix has to be symmetrized */
    bool hermitivitize_;
    /*! Correction for the conductor results */
    double correction_;
    /*! S matrix, not symmetry blocked */
    Eigen::MatrixXd S_;
    /*! S matrix, symmetry blocked form */
    std::vector<Eigen::MatrixXd> blockS_;

    /*! \brief Calculation of the PCM matrix
     *  \param[in] cavity the cavity to be used
     *  \param[in] gf_i Green's function inside the cavity
     *  \param[in] gf_o Green's function outside the cavity
     */
    virtual void buildSystemMatrix_impl(const Cavity & cavity, const IGreensFunction & gf_i, const IGreensFunction & gf_o) __override;
    /*! \brief Returns the ASC given the MEP and the desired irreducible representation
     *  \param[in] potential the vector containing the MEP at cavity points
     *  \param[in] irrep the irreducible representation of the MEP and ASC
     */
    virtual Eigen::VectorXd computeCharge_impl(const Eigen::VectorXd & potential,
            int irrep = 0) const __override;
    virtual std::ostream & printSolver(std::ostream & os) __override;
};

#endif // CPCMSOLVER_HPP


/*
 *  Copyright (c) 2018 Julian Soeren Lorenz, Carnegie Mellon University, All rights reserved.
 *
 *	Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *	following conditions are met:
 *
 *   	1. Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *   	   following disclaimer.
 *   	2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *   	   following disclaimer in the documentation and/or other materials provided with the distribution.
 *   	3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *   	   products derived from this software without specific prior written permission.
 *
 *	THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *	INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *	ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *	INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *	GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 *	LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 *	OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *	END OF LICENSE
 *
 *	Author: Julian Soeren Lorenz
 *	Email:  JulianLorenz@live.de
 *
*/

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
//Messages
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/Imu.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/VFR_HUD.h>
#include <mavros_msgs/ParamGet.h>

//Standart libs
#include <tgmath.h>
#include <mutex>
#include <sstream>
#include <iostream>
#include <fstream>
#include <ctime>

//Boost libs
#include <boost/bind.hpp>
#include <boost/ref.hpp>

//Eigen
#include <Eigen/Dense>

//Own headers
#include "filter/cekfilter.h"
#include "filter/simplewindkf.h"
#include "filter/WindTurbulenceIntensityFilter.h"
#include "util/EigenRosConversions.h"

using namespace std;

typedef Matrix<double,5,1> Vector5d;
typedef Matrix<double,6,1> Vector6d;
typedef Matrix<double,7,1> Vector7d;

typedef struct AircraftDataset{
	std::mutex mutex;
	double airspeed;//m/s
	double heading;	//degrees
	Eigen::Vector3d gps_vel_ned; //m/s
	Eigen::Vector3d gps_vel_enu; //m/s
	Eigen::Vector3d angular_velocity;
	Eigen::Vector3d acceleration;
	Eigen::Vector3d rpy; // Euler angles in deg
	Eigen::Quaterniond q_ned2body; //attitute quaternion
	Eigen::Quaterniond q_enu2body;
	Eigen::Vector3d Va_f;
	Eigen::Vector3d Va_g;
	Eigen::Matrix3d R_f2g;
	Eigen::Matrix3d R_g2f;
	double differentialPressure; //Pa
}AircraftDataset;

AircraftDataset data;

CEKFilter cekf;
SimpleWindKF simpleWindKF;

bool received_vfr_hud = false;
bool received_imu_data = false;
bool received_local_position = false;

/*===========================================================================================================
	C A L L B A C K S
============================================================================================================*/
void Wind_Estimation_Callback(const geometry_msgs::TwistStamped::ConstPtr& msg){
	ROS_INFO("Windspeed: [%3.3f %3.3f %3.3f]", msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z);
}

void VFR_HUD_Callback(const mavros_msgs::VFR_HUD::ConstPtr& msg){
	//ROS_INFO("Airspeed: %f m/s", msg->airspeed);
	data.mutex.lock();
	data.airspeed = msg->airspeed;
	data.Va_f << msg->airspeed,0,0;
	data.heading = msg->heading;
	//data.gps_vel(2) = (-1)*(msg->climb);

	data.mutex.unlock();
	received_vfr_hud = true;
}

void IMU_Data_Callback(const sensor_msgs::Imu::ConstPtr& msg){
	data.mutex.lock();
	data.angular_velocity = Util::toVector3d(msg->angular_velocity);
	data.angular_velocity(1) *= -1;
	data.angular_velocity(2) *= -1;
	data.acceleration = Util::toVector3d(msg->linear_acceleration);
	data.acceleration(1) *= -1;
	data.acceleration(2) *= -1;

	//Convert quaternion from ENU to NED
	tf::Quaternion enu2ned = tf::createQuaternionFromRPY(0, Util::PI, -0.5*Util::PI);
	tf::Quaternion att;
	tf::quaternionMsgToTF(msg->orientation,att);
	tf::quaternionTFToEigen(att,data.q_enu2body);
	att = enu2ned*att;
	tf::quaternionTFToEigen(att,data.q_ned2body);

	data.mutex.unlock();
	received_imu_data = true;
}

void Local_Position_Velocity_Callback(const geometry_msgs::TwistStamped::ConstPtr& msg){
	data.mutex.lock();

	data.gps_vel_enu = Util::toVector3d(msg->twist.linear); //Data comes in North, East, Up.

	data.gps_vel_ned(0) = data.gps_vel_enu(1);
	data.gps_vel_ned(1) = data.gps_vel_enu(0);
	data.gps_vel_ned(2) = (-1)*data.gps_vel_enu(2);

	data.mutex.unlock();
	received_local_position = true;
}

/*===========================================================================================================
	M A I N   F U N C T I O N
============================================================================================================*/

int main(int argc, char **argv)
{
	ros::init(argc, argv, "wind_estimator");

	ros::NodeHandle node;

	double filterFreq;

	//==================
	// Setup publishers
	ros::Publisher filter_freq_pub = node.advertise<std_msgs::Float64>("wind_estimator/filter_frequency",100);

	ros::Publisher cekf_wind_pub = node.advertise<geometry_msgs::TwistWithCovarianceStamped>("wind_estimator/cekf/wind",100);
	ros::Publisher cekf_wind_sigma_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/cekf/wind/sigma",100);
	ros::Publisher cekf_wind_3sig_upper_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/cekf/wind/3sig/upper",100);
	ros::Publisher cekf_wind_3sig_lower_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/cekf/wind/3sig/lower",100);
	ros::Publisher cekf_airspeed_pub = node.advertise<geometry_msgs::TwistWithCovarianceStamped>("wind_estimator/cekf/airspeed",100);
	ros::Publisher cekf_eta_pub = node.advertise<std_msgs::Float64>("wind_estimator/cekf/calibrationFactor",100);
	ros::Publisher cekf_ti_pub = node.advertise<std_msgs::Float64>("wind_estimator/cekf/turbulenceIntensity",100);

	ros::Publisher swkf_wind_pub = node.advertise<geometry_msgs::TwistWithCovarianceStamped>("wind_estimator/swkf/wind",100);
	ros::Publisher swkf_wind_sigma_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/swkf/wind/sigma",100);
	ros::Publisher swkf_wind_3sig_upper_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/swkf/wind/3sig/upper",100);
	ros::Publisher swkf_wind_3sig_lower_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/swkf/wind/3sig/lower",100);
	ros::Publisher swkf_ti_pub = node.advertise<std_msgs::Float64>("wind_estimator/swkf/turbulenceIntensity",100);

	ros::Publisher unfiltered_wind_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/unfiltered/wind",100);
	ros::Publisher moving_average_wind_pub = node.advertise<geometry_msgs::TwistStamped>("wind_estimator/movingAverage/wind",100);

	//==================
	// Setup subscribers
	mavros_msgs::State state;

	ros::Subscriber imu_data_sub = node.subscribe("mavros/imu/data",10,IMU_Data_Callback);
	ros::Subscriber local_position_velocity_sub = node.subscribe("mavros/local_position/velocity",10,Local_Position_Velocity_Callback);
	ros::Subscriber vfr_hud_sub = node.subscribe("mavros/vfr_hud",10,VFR_HUD_Callback);
	//ros::Subscriber wind_estimation_sub = node.subscribe("mavros/wind_estimation", 10,Wind_Estimation_Callback);

	//=====================
	// Setup service server
	//ros::ServiceServer imuSetter_service = node.advertiseService("wind_estimator/useImu",setUseImu);

	//=====================
	// Setup service client
	ros::ServiceClient param_get_client = node.serviceClient<mavros_msgs::ParamGet>("mavros/param/get");

	//=====================
	// Start spinner
	ros::AsyncSpinner spinner(4);
	spinner.start();

	//=====================
	// Declare filter
	// Calibrating EKF
	cekf.setUseIMUInput(false);
	CEKFilter::MeasurementType cekf_measurement;
	CEKFilter::InputType cekf_input;
	CEKFilter::StateType cekf_estimation;
	CEKFilter::StateCovarianceType cekf_covariance;
	bool isInitialized = false;

	// No filter
	Vector3d unfiltered_wind;

	// Moving average filter
	MovingAverageFilter<Vector3d> moving_average_filter(10*60*3); //10min average -> 10 Samples/s * 60s/min * 10min = 6000 Samples
	Vector3d moving_average_wind;

	//Simple wind kalmanfilter
	SimpleWindKF::MeasurementType swkf_measurement;
	SimpleWindKF::StateType swkf_estimation;
	SimpleWindKF::StateCovarianceType swkf_covariance;

	ElapsedTimer elapsedTime;

	//=====================
	// Prepare log file in .csv format
	ofstream logfile;
	ElapsedTimer logTimer;
	logTimer.reset();
    time_t t = time(0);   // get time now
    struct tm * now = localtime( & t );
    char logfilename[100];
    sprintf(logfilename,"wind_estimator_%04d_%02d_%02d_%02d_%02d_%02d.log",now->tm_year+1900,now->tm_mon+1,now->tm_mday,now->tm_hour,now->tm_min,now->tm_sec);
	logfile.open(logfilename, std::ofstream::out);
	if(logfile.is_open()){
		ROS_INFO("Logfile successfully opened!");
	}else{
		ROS_WARN("Failed to open logfile!");
	}
	logfile << "# Log file of wind_estimator node" << endl;
	logfile << "time \t"

			<< "unfiltered_wind_x \t"
			<< "unfiltered_wind_y \t"
			<< "unfiltered_wind_z \t"

			<< "cekf_wind_x \t"
			<< "cekf_wind_y \t"
			<< "cekf_wind_z \t"
			<< "cekf_wind_x_sigma \t"
			<< "cekf_wind_y_sigma \t"
			<< "cekf_wind_z_sigma \t"
			<< "cekf_eta \t"
			<< "cekf_eta_sigma \t"
			<< "cekf_airspeed_x \t"
			<< "cekf_airspeed_y \t"
			<< "cekf_airspeed_z \t"
			<< "cekf_airspeed_x_sigma \t"
			<< "cekf_airspeed_y_sigma \t"
			<< "cekf_airspeed_z_sigma \t"
			<< "cekf_turbulence_intensity \t"

			<< "swkf_wind_x \t"
			<< "swkf_wind_y \t"
			<< "swkf_wind_z \t"
			<< "swkf_wind_x_sigma \t"
			<< "swkf_wind_y_sigma \t"
			<< "swkf_wind_z_sigma \t"
			<< "swkf_turbulence_intensity \t"

			<< "moving_average_wind_x \t"
			<< "moving_average_wind_y \t"
			<< "moving_average_wind_z \t"

			<< "loop_calc_time \t"
			<< "frequency"

			<< endl;

	/*==============================================
		Main loop
	================================================*/
	while (ros::ok())
	{
		while(!(received_vfr_hud && received_imu_data && received_local_position) && ros::ok());
		data.mutex.lock();
			elapsedTime.reset();
			//Request AIRSPEED_RATIO = 1/eta
			mavros_msgs::ParamGet srv;
			srv.request.param_id = "ARSPD_RATIO";
			double ARSPD_RATIO = 0;
			if(param_get_client.call(srv) && srv.response.success){
				ARSPD_RATIO = srv.response.value.real;
				data.differentialPressure = (1/ARSPD_RATIO) * pow(data.airspeed,2);
				ROS_INFO("Eta: %f", 1/ARSPD_RATIO);
			}else{
				ROS_ERROR("Failed to get parameter! %d",srv.response.success);
			}

			received_vfr_hud = false;
			received_imu_data = false;
			received_local_position = false;

			/*==============================================
				Calculate common variables
			================================================*/
			data.R_g2f = Util::calcRotationMatrix(data.q_enu2body);
			data.R_f2g = data.R_g2f.transpose();

			data.Va_g = data.R_f2g * data.Va_f;

			/*=============================================
			    Calculate wind without any filter
			================================================*/

			unfiltered_wind = data.gps_vel_enu - data.Va_g;
			cout << "Unfiltered wind: " << endl << unfiltered_wind << endl;

			/*==============================================
				Update CEKF
			================================================*/
			if(!isInitialized){
				isInitialized = true;
				//Init CEKF
				cekf_estimation << 0.1, 0.1, 0.1, 0.6125, data.R_g2f * data.gps_vel_enu;
				cekf.initialize(cekf_estimation);
				cekf.setModelVarWind(1e-4,1e-4);
				cekf.setModelVarEta(1e-7);
				cekf.setModelVarAirspeed(1e-2,0.1);
				cekf.setMeasurementVarGroundspeedV(1);
				cekf.setMeasurementVarGroundspeedH(1);
				cekf.setMeasurementVarPressure(10);
				cout << "Initialized filter with state: " << endl << cekf_estimation << endl;
				//Init SWKF
				simpleWindKF.setModelVarWindspeed(0.0001);
				simpleWindKF.setMeasurementVarAirspeed(1);
				simpleWindKF.setMeasurementVarGroundspeed(2);
				//Init MovingAverageFilter
				moving_average_filter.initialize(unfiltered_wind);
			}
			//Fill measurement vector z
			cekf_measurement << data.gps_vel_enu, data.differentialPressure;
			//Fill input vector u
			cekf_input.head(3) = data.angular_velocity;
			cekf_input.tail(3) = data.acceleration;
			//Run filter
			cekf_estimation = cekf.getValue(cekf_measurement,
													cekf_input,
													data.q_enu2body);
			cekf_covariance = cekf.getStateCovariance();

			filterFreq = cekf.getFrequency();

			ROS_INFO("Ti: %f",cekf.getWindTurbulenceIntensity());

			cout << "CEKF State" << endl << cekf_estimation << endl;

			/*==============================================
				Update Lorenz's filter - SWKF
			================================================*/
			swkf_measurement << data.gps_vel_enu, data.Va_g;
			swkf_estimation = simpleWindKF.getValue(swkf_measurement);
			swkf_covariance = simpleWindKF.getStateCovariance();

			cout << "SWKF State:" << endl << swkf_estimation << endl;

			/*=============================================
			    Update MovingAverageFilter
			================================================*/

			moving_average_wind = moving_average_filter.getValue(unfiltered_wind);
			cout << "10min average:" << endl << moving_average_wind << endl;

			/*==============================================
				publish results
			================================================*/
			//General
			std_msgs::Float64 filter_freq_msg;
			filter_freq_msg.data = filterFreq;
			filter_freq_pub.publish(filter_freq_msg);

			//CEKF
			geometry_msgs::TwistWithCovarianceStamped cekf_wind_msg;
			geometry_msgs::TwistStamped cekf_wind_sigma_msg;
			geometry_msgs::TwistStamped cekf_wind_3sig_upper_msg;
			geometry_msgs::TwistStamped cekf_wind_3sig_lower_msg;
			geometry_msgs::TwistWithCovarianceStamped cekf_airspeed_msg;
			std_msgs::Float64 cekf_eta_msg;
			std_msgs::Float64 cekf_ti_msg;

			cekf_wind_msg.header.stamp = ros::Time::now();
			cekf_wind_sigma_msg.header.stamp = ros::Time::now();
			cekf_wind_3sig_upper_msg.header.stamp = ros::Time::now();
			cekf_wind_3sig_upper_msg.header.stamp = ros::Time::now();
			cekf_airspeed_msg.header.stamp = ros::Time::now();
			tf::vectorEigenToMsg(cekf_estimation.tail(3),cekf_airspeed_msg.twist.twist.linear);
			tf::vectorEigenToMsg(cekf.getWindSigmaVector(), cekf_wind_sigma_msg.twist.linear);
			tf::vectorEigenToMsg(cekf.getWind3SigBorderUpper(), cekf_wind_3sig_upper_msg.twist.linear);
			tf::vectorEigenToMsg(cekf.getWind3SigBorderLower(), cekf_wind_3sig_lower_msg.twist.linear);
			tf::vectorEigenToMsg(cekf_estimation.head(3),cekf_wind_msg.twist.twist.linear);

			Eigen::Matrix<double,6,6> cov = Eigen::Matrix<double,6,6>::Zero(6,6);
			//Assign airspeed covariances
			cov.topLeftCorner(3,3) = cekf_covariance.bottomRightCorner(3,3);
			cekf_airspeed_msg.twist.covariance = Util::toBoostArray(cov);
			//Assign wind covariances
			cov.topLeftCorner(3,3) = cekf_covariance.topLeftCorner(3,3);
			cekf_wind_msg.twist.covariance = Util::toBoostArray(cov);

			cekf_eta_msg.data = cekf_estimation(3);

			cekf_ti_msg.data = cekf.getWindTurbulenceIntensity();

			cekf_wind_pub.publish(cekf_wind_msg);
			cekf_wind_sigma_pub.publish(cekf_wind_sigma_msg);
			cekf_wind_3sig_upper_pub.publish(cekf_wind_3sig_upper_msg);
			cekf_wind_3sig_lower_pub.publish(cekf_wind_3sig_lower_msg);
			cekf_airspeed_pub.publish(cekf_airspeed_msg);
			cekf_eta_pub.publish(cekf_eta_msg);
			cekf_ti_pub.publish(cekf_ti_msg);

			ROS_INFO("CEKF magnitude: %f m/s | CEKF wind dir: %03.1f degrees", cekf.getWindMagnitude(), cekf.getWindDirFROM());

			//SWKF
			geometry_msgs::TwistWithCovarianceStamped swkf_wind_msg;
			geometry_msgs::TwistStamped swkf_wind_sigma_msg;
			geometry_msgs::TwistStamped swkf_wind_3sig_upper_msg;
			geometry_msgs::TwistStamped swkf_wind_3sig_lower_msg;
			std_msgs::Float64 swkf_ti_msg;

			swkf_wind_msg.header.stamp = ros::Time::now();
			tf::vectorEigenToMsg(swkf_estimation.tail(3),swkf_wind_msg.twist.twist.linear);

			cov = Eigen::Matrix<double,6,6>::Zero(6,6);
			cov.topLeftCorner(3,3) = swkf_covariance.bottomRightCorner(3,3);
			swkf_wind_msg.twist.covariance = Util::toBoostArray(cov);
			tf::vectorEigenToMsg(simpleWindKF.getWindSigmaVector(), swkf_wind_sigma_msg.twist.linear);
			tf::vectorEigenToMsg(simpleWindKF.getWind3SigBorderUpper(), swkf_wind_3sig_upper_msg.twist.linear);
			tf::vectorEigenToMsg(simpleWindKF.getWind3SigBorderLower(), swkf_wind_3sig_lower_msg.twist.linear);

			swkf_ti_msg.data = simpleWindKF.getWindTurbulenceIntensity();

			swkf_wind_pub.publish(swkf_wind_msg);
			swkf_wind_sigma_pub.publish(swkf_wind_sigma_msg);
			swkf_wind_3sig_upper_pub.publish(swkf_wind_3sig_upper_msg);
			swkf_wind_3sig_lower_pub.publish(swkf_wind_3sig_lower_msg);
			swkf_ti_pub.publish(swkf_ti_msg);

			//Unfiltered wind
			geometry_msgs::TwistStamped unfiltered_wind_msg;
			unfiltered_wind_msg.header.stamp = ros::Time::now();
			tf::vectorEigenToMsg(unfiltered_wind, unfiltered_wind_msg.twist.linear);
			unfiltered_wind_pub.publish(unfiltered_wind_msg);

			//Moving average wind
			geometry_msgs::TwistStamped moving_average_wind_msg;
			moving_average_wind_msg.header.stamp = ros::Time::now();
			tf::vectorEigenToMsg(moving_average_wind, moving_average_wind_msg.twist.linear);
			moving_average_wind_pub.publish(moving_average_wind_msg);

			ROS_INFO("Frequency: %f hz",filterFreq);
			ROS_INFO("Total calculation time: %f ms",elapsedTime.elapsed()*1000);

			/*==============================================
				log data
			================================================*/
			logfile << logTimer.elapsed() << "\t"

					<< unfiltered_wind(0) << "\t"
					<< unfiltered_wind(1) << "\t"
					<< unfiltered_wind(2) << "\t"

					<< cekf.getWindVector()(0) << "\t"
					<< cekf.getWindVector()(1) << "\t"
					<< cekf.getWindVector()(2) << "\t"
					<< cekf.getWindSigmaVector()(0) << "\t"
					<< cekf.getWindSigmaVector()(1) << "\t"
					<< cekf.getWindSigmaVector()(2) << "\t"
					<< cekf_estimation(3) << "\t"
					<< sqrt(cekf_covariance(3,3)) << "\t"
					<< cekf_estimation(4) << "\t"
					<< cekf_estimation(5) << "\t"
					<< cekf_estimation(6) << "\t"
					<< sqrt(cekf_covariance(4,4)) << "\t"
					<< sqrt(cekf_covariance(5,5)) << "\t"
					<< sqrt(cekf_covariance(6,6)) << "\t"
					<< cekf.getWindTurbulenceIntensity() << "\t"

					<< simpleWindKF.getWindVector()(0) << "\t"
					<< simpleWindKF.getWindVector()(1) << "\t"
					<< simpleWindKF.getWindVector()(2) << "\t"
					<< simpleWindKF.getWindSigmaVector()(0) << "\t"
					<< simpleWindKF.getWindSigmaVector()(1) << "\t"
					<< simpleWindKF.getWindSigmaVector()(2) << "\t"
					<< simpleWindKF.getWindTurbulenceIntensity() << "\t"

					<< moving_average_wind(0) << "\t"
					<< moving_average_wind(1) << "\t"
					<< moving_average_wind(2) << "\t"

					<< elapsedTime.elapsed() << "\t"
					<< filterFreq

					<< endl;

		data.mutex.unlock();
	}// end of while loop

	logfile.close();

	return 0;
}
// %EndTag(FULLTEXT)%


#define PY_SSIZE_T_CLEAN
#include <Python.h>
#include <vector>
#include <boost/timer/timer.hpp>
#include <GeomLib.hpp>
#include <TwoDLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/report/handler/InactiveReportHandler.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/utilities/ProgressBar.hpp>
#include <MPILib/include/BasicDefinitions.hpp>
#include <MPILib/include/MiindTvbModelAbstract.hpp>

/** This class is generically named MiindModel so no code change is required in TVB.
 * We can implement any simulation we desire and once the shared library is generated,
 * it can be copied to the TVB working directory and referenced when
 * instantiating the tvb.simulator.models.Miind class.
 *
 * e.g tvb.simulator.models.Miind('libmiindlif.so',76, 1**2, 0.001)
 */
class MiindModel : public MPILib::MiindTvbModelAbstract<DelayedConnection, MPILib::utilities::CircularDistribution> {
public:

	MiindModel(int num_nodes, double simulation_length) :
		MiindTvbModelAbstract(num_nodes, simulation_length){}

	void init()
	{
		_time_step = 0.001;
		for(int i=0; i<_num_nodes; i++) {
			std::vector<std::string> vec_mat_0{"lif_0.01_0_0_0_.mat"};
    	TwoDLib::MeshAlgorithm<DelayedConnection> alg_mesh_0("lif.model",vec_mat_0,_time_step);

      DelayedConnection con_external(2000,0.01,0);
			// As a basic example, we just implement a single lif node for each region
			// in TVB's connectivity.
			MPILib::NodeId id_0 = network.addNode(alg_mesh_0, MPILib::EXCITATORY_DIRECT);
			// Each node must have an incoming and outgoing connection to TVB
      network.setNodeExternalSuccessor(id_0);
			network.setNodeExternalPrecursor(id_0, con_external);
		}

		/* Any handler is permitted but as TVB provides it's own output, we get a speed
		 * increase by doing no IO in MIIND.
		 * If ROOT handlers are used, remember that the sim_name must include a
		 * directory and that directory must exist in the working directory.
		 * e.g sim_name = miind_output/miind_lif
		 */
		std::string sim_name = "miind_lif";
		report::handler::InactiveReportHandler handler = report::handler::InactiveReportHandler();

		SimulationRunParameter par_run( handler,(_simulation_length/_time_step)+1,0,
										_simulation_length,_time_step,_time_step,sim_name,_time_step);

		network.configureSimulation(par_run);
	}
};

static MiindModel *model;

static PyObject *miind_init(PyObject *self, PyObject *args)
{
    int nodes;
    double sim_time;

    if (!PyArg_ParseTuple(args, "ii", &nodes, &sim_time))
	return NULL;

    model = new MiindModel(nodes, sim_time);

    model->init();

    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject *miind_getTimeStep(PyObject *self, PyObject *args)
{
    return Py_BuildValue("d", model->getTimeStep());
}

static PyObject *miind_getSimulationLength(PyObject *self, PyObject *args)
{
    return Py_BuildValue("d", model->getSimulationLength());
}

static PyObject *miind_startSimulation(PyObject *self, PyObject *args)
{
    model->startSimulation();
    Py_INCREF(Py_None);
    return Py_None;
}

static PyObject *miind_evolveSingleStep(PyObject *self, PyObject *args)
{
    PyObject *float_list;
    int pr_length;

    if (!PyArg_ParseTuple(args, "O", &float_list))
        return NULL;
    pr_length = PyObject_Length(float_list);
    if (pr_length < 0)
        return NULL;

    std::vector<double> activities(pr_length);

    for (int index = 0; index < pr_length; index++) {
        PyObject *item;
        item = PyList_GetItem(float_list, index);
        if (!PyFloat_Check(item))
            activities[index] = 0.0;
        activities[index] = PyFloat_AsDouble(item);
    }

    std::vector<double> out_activities = model->evolveSingleStep(activities);

    PyObject* tuple = PyTuple_New(pr_length);

    for (int index = 0; index < pr_length; index++) {
        PyTuple_SetItem(tuple, index, Py_BuildValue("d", out_activities[index]));
    }

    return tuple;
}

static PyObject *miind_endSimulation(PyObject *self, PyObject *args)
{
    model->endSimulation();
    Py_INCREF(Py_None);
    return Py_None;
}

static PyMethodDef MiindModelMethods[] = {
    {"init",  miind_init, METH_VARARGS, "Init Miind Model."},
    {"getTimeStep",  miind_getTimeStep, METH_VARARGS, "Get time step."},
    {"getSimulationLength",  miind_getSimulationLength, METH_VARARGS, "Get sim time."},
    {"startSimulation",  miind_startSimulation, METH_VARARGS, "Start simulation."},
    {"evolveSingleStep",  miind_evolveSingleStep, METH_VARARGS, "Evolve one time step."},
    {"endSimulation",  miind_endSimulation, METH_VARARGS, "Clean up."},
    {NULL, NULL, 0, NULL}        /* Sentinel */
};

static struct PyModuleDef miindmodule = {
    PyModuleDef_HEAD_INIT,
    "libmiindlif",   /* name of module */
    NULL, /* module documentation, may be NULL */
    -1,       /* size of per-interpreter state of the module,
                 or -1 if the module keeps state in global variables. */
    MiindModelMethods
};

PyMODINIT_FUNC
PyInit_libmiindlif(void)
{
    return PyModule_Create(&miindmodule);
}


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_rbdyn/configuration_io.h>
#include <mc_tasks/MetaTaskLoader.h>
#include <mc_tasks/PositionBasedVisServoTask.h>

#include <Eigen/Geometry>

namespace mc_tasks
{

PositionBasedVisServoTask::PositionBasedVisServoTask(const std::string & bodyName,
                                                     const sva::PTransformd & X_t_s,
                                                     const sva::PTransformd & X_b_s,
                                                     const mc_rbdyn::Robots & robots,
                                                     unsigned int robotIndex,
                                                     double stiffness,
                                                     double weight)
: TrajectoryTaskGeneric<tasks::qp::PositionBasedVisServoTask>(robots, robotIndex, stiffness, weight)
{
  finalize(robots.mbs(), static_cast<int>(rIndex), bodyName, X_t_s, X_b_s);
  type_ = "pbvs";
  name_ = "pbvs_" + robots.robot(robotIndex).name() + "_" + bodyName;
}

PositionBasedVisServoTask::PositionBasedVisServoTask(const std::string & surfaceName,
                                                     const sva::PTransformd & X_t_s,
                                                     const mc_rbdyn::Robots & robots,
                                                     unsigned int robotIndex,
                                                     double stiffness,
                                                     double weight)
: PositionBasedVisServoTask(robots.robot(robotIndex).surface(surfaceName).bodyName(),
                            X_t_s,
                            robots.robot(robotIndex).surface(surfaceName).X_b_s(),
                            robots,
                            robotIndex,
                            stiffness,
                            weight)
{
}

void PositionBasedVisServoTask::reset()
{
  TrajectoryTaskGeneric::reset();
  X_t_s_ = sva::PTransformd::Identity();
  errorT->error(X_t_s_);
}

void PositionBasedVisServoTask::error(const sva::PTransformd & X_t_s)
{
  X_t_s_ = X_t_s;
  errorT->error(X_t_s_);
}

void PositionBasedVisServoTask::addToLogger(mc_rtc::Logger & logger)
{
  TrajectoryBase::addToLogger(logger);
  logger.addLogEntry(name_ + "_error", [this]() -> const sva::PTransformd & { return X_t_s_; });
  logger.addLogEntry(name_ + "_eval", [this]() -> sva::PTransformd {
    Eigen::Vector6d eval = errorT->eval();
    Eigen::Vector3d angleAxis = eval.head(3);
    Eigen::Vector3d axis = angleAxis / angleAxis.norm();
    double angle = angleAxis.dot(axis);
    Eigen::Quaterniond quat(Eigen::AngleAxisd(angle, axis));
    return sva::PTransformd(quat, eval.tail(3));
  });
}

void PositionBasedVisServoTask::removeFromLogger(mc_rtc::Logger & logger)
{
  TrajectoryBase::removeFromLogger(logger);
  logger.removeLogEntry(name_ + "_error");
  logger.removeLogEntry(name_ + "_eval");
}

} // namespace mc_tasks

namespace
{

static auto registered = mc_tasks::MetaTaskLoader::register_load_function(
    "pbvs",
    [](mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) {
      std::shared_ptr<mc_tasks::PositionBasedVisServoTask> t;
      auto robotIndex = robotIndexFromConfig(config, solver.robots(), "pbvs");
      if(config.has("surface"))
      {
        t = std::make_shared<mc_tasks::PositionBasedVisServoTask>(config("surface"), sva::PTransformd::Identity(),
                                                                  solver.robots(), robotIndex);
      }
      else if(config.has("body"))
      {
        t = std::make_shared<mc_tasks::PositionBasedVisServoTask>(config("body"), sva::PTransformd::Identity(),
                                                                  config("X_b_s", sva::PTransformd::Identity()),
                                                                  solver.robots(), robotIndex);
      }
      t->load(solver, config);
      return t;
    });
}


#include <alien/kernels/mtl/linear_solver/MTLInternalLinearSolver.h>

#include <alien/AlienExternalPackagesPrecomp.h>

#include <alien/expression/solver/SolverStater.h>
#include <alien/core/backend/LinearSolverT.h>

#include <alien/core/backend/SolverFabricRegisterer.h>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

#include <alien/kernels/mtl/data_structure/MTLVector.h>
#include <alien/kernels/mtl/data_structure/MTLMatrix.h>
#include <alien/kernels/mtl/MTLBackEnd.h>

#include <alien/kernels/mtl/algebra/MTLLinearAlgebra.h>
#include <alien/data/Space.h>

#include <alien/kernels/mtl/linear_solver/MTLOptionTypes.h>
#include <ALIEN/axl/MTLLinearSolver_IOptions.h>
#include <alien/kernels/mtl/linear_solver/arcane/MTLLinearSolverService.h>
#include <ALIEN/axl/MTLLinearSolver_IOptions.h>
#include <ALIEN/axl/MTLLinearSolver_StrongOptions.h>

/*---------------------------------------------------------------------------*/

#ifdef USE_PMTL4
mtl::par::environment* m_global_environment = NULL;
#endif /* USE_PMTL4 */

/*---------------------------------------------------------------------------*/
namespace Alien {

template class ALIEN_EXTERNAL_PACKAGES_EXPORT LinearSolver<BackEnd::tag::mtl>;

MTLInternalLinearSolver::MTLInternalLinearSolver(
    Arccore::MessagePassing::IMessagePassingMng* parallel_mng,
    IOptionsMTLLinearSolver* options)
: m_initialized(false)
, m_solver_option(MTLOptionTypes::BiCGStab)
, m_preconditioner_option(MTLOptionTypes::ILU0PC)
, m_max_iteration(0)
, m_precision(0)
, m_output_level(0)
, m_parallel_mng(parallel_mng)
, m_options(options)
{
  ;
}

/*---------------------------------------------------------------------------*/

MTLInternalLinearSolver::~MTLInternalLinearSolver()
{
  ;
}

/*---------------------------------------------------------------------------*/

void
MTLInternalLinearSolver::init()
{
  m_stater.reset();
  m_stater.startInitializationMeasure();

#ifdef USE_PMTL4
  int argc = 0;
  char** argv = NULL;
  if (m_global_environment == NULL)
// m_global_environment = new mtl::par::environment(argc,argv) ;
#endif /* USE_PMTL4 */

    m_max_iteration = m_options->maxIterationNum();
  m_precision = m_options->stopCriteriaValue();
  m_solver_option = m_options->solver();
  m_preconditioner_option = m_options->preconditioner();

  m_initialized = true;
}

/*---------------------------------------------------------------------------*/

void
MTLInternalLinearSolver::updateParallelMng(
    Arccore::MessagePassing::IMessagePassingMng* pm)
{
  m_parallel_mng = pm;
}

/*---------------------------------------------------------------------------*/

const Alien::SolverStatus&
MTLInternalLinearSolver::getStatus() const
{
  return m_status;
}

/*---------------------------------------------------------------------------*/

bool
MTLInternalLinearSolver::solve(
    MTLMatrix const& matrix, MTLVector const& rhs, MTLVector& sol)
{
  using namespace Alien;

  const MatrixInternal::MTLMatrixType& A = matrix.internal()->m_internal;
  const VectorInternal::MTLVectorType& b = rhs.internal()->m_internal;
  VectorInternal::MTLVectorType& x = sol.internal()->m_internal;

  return _solve(A, b, x);
}

/*---------------------------------------------------------------------------*/

std::shared_ptr<ILinearAlgebra>
MTLInternalLinearSolver::algebra() const
{
  return std::shared_ptr<ILinearAlgebra>(new Alien::MTLLinearAlgebra());
}

/*---------------------------------------------------------------------------*/

bool
MTLInternalLinearSolver::_solve(MatrixInternal::MTLMatrixType const& matrix,
    VectorInternal::MTLVectorType const& rhs, VectorInternal::MTLVectorType& x)
{
#ifdef EXPORT
  mtl::io::matrix_market_ostream ifile("matrix.txt");
  ifile << matrix;
  ifile.close();
  ofstream rhsfile("rhs.txt");
  rhsfile << rhs;
  ofstream xfile("x.txt");
  xfile << x;
// exit(0) ;
#endif /* EXPORT */

  try {

    switch (m_solver_option) {
    case MTLOptionTypes::BiCGStab: {
      itl::basic_iteration<double> iter(rhs, m_max_iteration, m_precision);
      // itl::noisy_iteration<double> iter(rhs,m_max_iteration,m_precision);
      switch (m_preconditioner_option) {
      case MTLOptionTypes::NonePC: {
        itl::pc::identity<MatrixInternal::MTLMatrixType> P(matrix);
        itl::bicgstab(matrix, x, rhs, P, iter);
      } break;
      case MTLOptionTypes::DiagPC: {
        itl::pc::diagonal<MatrixInternal::MTLMatrixType> P(matrix);
        itl::bicgstab(matrix, x, rhs, P, iter);
      } break;
      case MTLOptionTypes::ILU0PC: {
        itl::pc::ilu_0<MatrixInternal::MTLMatrixType, float> P(matrix);
        itl::bicgstab(matrix, x, rhs, P, iter);
      } break;
      case MTLOptionTypes::ILUTPC:
      case MTLOptionTypes::SSORPC: {
        alien_fatal([&] { cout() << "Preconditioner not available"; });
      } break;
      }
      m_status.iteration_count = iter.iterations();
      m_status.residual = iter.resid();
      if (m_status.iteration_count >= m_max_iteration)
        m_status.succeeded = false;
      else
        m_status.succeeded = true;
    } break;
    case MTLOptionTypes::CG: {
      std::cout<<"MTL4 CG"<<std::endl ;
      //itl::basic_iteration<double> iter(rhs, m_max_iteration, m_precision);
      itl::noisy_iteration<double> iter(rhs,m_max_iteration,m_precision);
      switch (m_preconditioner_option) {
      case MTLOptionTypes::NonePC: {
        itl::pc::identity<MatrixInternal::MTLMatrixType> P(matrix);
        itl::cg(matrix, x, rhs, P, iter);
      } break;
      case MTLOptionTypes::DiagPC: {
        itl::pc::diagonal<MatrixInternal::MTLMatrixType> P(matrix);
        itl::cg(matrix, x, rhs, P, iter);
      } break;
      case MTLOptionTypes::SSORPC: {
        alien_fatal([&] { cout() << "Preconditioner not available"; });
      } break;
      }
      m_status.iteration_count = iter.iterations();
      m_status.residual = iter.resid();
      std::cout<<"NB ITER : "<<m_status.iteration_count<<" residual="<<m_status.residual<<std::endl;
      if (m_status.iteration_count >= m_max_iteration)
        m_status.succeeded = false;
      else
        m_status.succeeded = true;
    } break;
#ifdef MTL_HAS_UMFPACK
    case MTLOptionTypes::LU: {
      mtl::matrix::umfpack::solver<MatrixInternal::MTLMatrixType> solver(matrix);
      solver(x, rhs);
      m_status.succeeded = true;
    } break;
#endif /* MTL_HAS_UMFPACK */
    default: {
      alien_fatal([&] { cout() << "Solver option not available"; });
    } break;
    }

  } catch (mtl::logic_error& e) {
    throw Arccore::FatalErrorException(A_FUNCINFO,
        Arccore::String::format(
            "MTL Login Error Exception catched while solving linear system : {0}",
            e.what()));
  } catch (mtl::runtime_error& e) {
    throw Arccore::FatalErrorException(A_FUNCINFO,
        Arccore::String::format(
            "MTL Runtime Error Exception catched while solving linear system : {0}",
            e.what()));
  } catch (mtl::index_out_of_range& e) {
    throw Arccore::FatalErrorException(A_FUNCINFO,
        Arccore::String::format(
            "MTL Out of range Exception catched while solving linear system : {0}",
            e.what()));
  }

#ifdef EXPORT
  mtl::io::matrix_market_ostream ifile("matrix.txt");
  ifile << matrix;
  ifile.close();
  ofstream rhsfile("rhs.txt");
  rhsfile << rhs;
  rhsfile.close();
  ofstream solfile("sol.txt");
  solfile << x;
  solfile.close();
// exit(0) ;
#endif

  return m_status.succeeded;
}

/*---------------------------------------------------------------------------*/

void
MTLInternalLinearSolver::end()
{
  if (m_output_level > 0)
    internalPrintInfo();
}

/*---------------------------------------------------------------------------*/

bool
MTLInternalLinearSolver::hasParallelSupport() const
{
#ifdef USE_PMTL4
  return true;
#else /* USE_PMTL4 */
  return false;
#endif /* USE_PMTL4 */
}

/*---------------------------------------------------------------------------*/

void
MTLInternalLinearSolver::internalPrintInfo() const
{
  m_stater.print(const_cast<Arccore::ITraceMng*>(traceMng()), m_status,
      Arccore::String::format("Linear Solver : {0}", "MTLLinearSolver"));
}

IInternalLinearSolver<MTLMatrix, MTLVector>*
MTLInternalLinearSolverFactory(
    Arccore::MessagePassing::IMessagePassingMng* p_mng, IOptionsMTLLinearSolver* options)
{
  return new MTLInternalLinearSolver(p_mng, options);
}

template<>
class SolverFabric<Alien::BackEnd::tag::mtl>
: public ISolverFabric
{
public :
   BackEndId backend() const {
     return "mtl4" ;
  }

  void
  add_options(CmdLineOptionDescType& cmdline_options) const
  {
    using namespace boost::program_options;
    options_description desc("MTL4 options");
    desc.add_options()("mtl4-solver", value<std::string>()->default_value("bicgs"),"solver algo name : bicgstab")
                      ("mtl4-precond", value<std::string>()->default_value("none"),"preconditioner ilu diag none");

    cmdline_options.add(desc) ;
  }

  template<typename OptionT>
  Alien::ILinearSolver* _create(OptionT const& options,Alien::IMessagePassingMng* pm) const
  {
    double tol = get<double>(options,"tol");
    int max_iter = get<int>(options,"max-iter");

    std::string solver_type_s = get<std::string>(options,"mtl4-solver");
    MTLOptionTypes::eSolver solver_type =
        OptionsMTLLinearSolverUtils::stringToSolverEnum(solver_type_s);
    std::string precond_type_s = get<std::string>(options,"mtl4-precond");
    MTLOptionTypes::ePreconditioner precond_type =
        OptionsMTLLinearSolverUtils::stringToPreconditionerEnum(precond_type_s);
    // options
    auto solver_options = std::make_shared<StrongOptionsMTLLinearSolver>(
        MTLLinearSolverOptionsNames::_outputLevel = get<int>(options,"output-level"),
        MTLLinearSolverOptionsNames::_maxIterationNum = max_iter,
        MTLLinearSolverOptionsNames::_stopCriteriaValue = tol,
        MTLLinearSolverOptionsNames::_preconditioner = precond_type,
        MTLLinearSolverOptionsNames::_solver = solver_type);
    // service
    return new Alien::MTLLinearSolverService(pm, solver_options);

  }

  Alien::ILinearSolver* create(CmdLineOptionType const& options,Alien::IMessagePassingMng* pm) const
  {
    return _create(options,pm) ;
  }

  Alien::ILinearSolver* create(JsonOptionType const& options,Alien::IMessagePassingMng* pm) const
  {
    return _create(options,pm) ;
  }
};

typedef SolverFabric<Alien::BackEnd::tag::mtl> MTL4SolverFabric ;
REGISTER_SOLVER_FABRIC(MTL4SolverFabric);

} // namespace Alien


#define BOOST_TEST_MODULE "test_read_position_restraint_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/input/read_external_interaction.hpp>

BOOST_AUTO_TEST_CASE(read_position_restraint_harmonic)
{
    mjolnir::LoggerManager::set_default_logger("test_read_position_restraint_interaction.log");

    using traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type   = traits_type::real_type;
    {
        using namespace toml::literals;
        const toml::value v = u8R"(
            interaction = "PositionRestraint"
            potential   = "Harmonic"
            parameters  = [
                {position = [0.0, 0.0, 0.0], index =   0, k = 10.0, v0 =  0.0},
                {position = [1.0,-1.0,10.0], index = 100, k = 3.14, v0 = 10.0},
            ]
            )"_toml;

        const auto base = mjolnir::read_external_interaction<traits_type>(v);
        BOOST_TEST(static_cast<bool>(base));

        const auto derv = dynamic_cast<mjolnir::PositionRestraintInteraction<
            traits_type, mjolnir::HarmonicPotential<real_type>>*
            >(base.get()); // check the expected type is contained
        BOOST_TEST(static_cast<bool>(derv));

        const auto& interaction = *derv;

        BOOST_TEST(interaction.potentials().size() == 2u);

        BOOST_TEST(std::get<0>(interaction.potentials().at(0)) ==   0u);
        BOOST_TEST(std::get<0>(interaction.potentials().at(1)) == 100u);

        BOOST_TEST(std::get<1>(interaction.potentials().at(0)).at(0) == 0.0);
        BOOST_TEST(std::get<1>(interaction.potentials().at(0)).at(1) == 0.0);
        BOOST_TEST(std::get<1>(interaction.potentials().at(0)).at(2) == 0.0);
        BOOST_TEST(std::get<1>(interaction.potentials().at(1)).at(0) == 1.0);
        BOOST_TEST(std::get<1>(interaction.potentials().at(1)).at(1) ==-1.0);
        BOOST_TEST(std::get<1>(interaction.potentials().at(1)).at(2) ==10.0);

        BOOST_TEST(std::get<2>(interaction.potentials().at(0)).k()  == 10.0);
        BOOST_TEST(std::get<2>(interaction.potentials().at(1)).k()  == 3.14);
        BOOST_TEST(std::get<2>(interaction.potentials().at(0)).v0() ==  0.0);
        BOOST_TEST(std::get<2>(interaction.potentials().at(1)).v0() == 10.0);

    }
}


/**
 * @file kde_model_impl.hpp
 * @author Roberto Hueso
 *
 * Implementation of KDE Model.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_KDE_MODEL_IMPL_HPP
#define MLPACK_METHODS_KDE_MODEL_IMPL_HPP

// In case it hasn't been included yet.
#include "kde_model.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace kde {

//! Initialize the KDEModel with the given parameters.
inline KDEModel::KDEModel(const double bandwidth,
                          const double relError,
                          const double absError,
                          const KernelTypes kernelType,
                          const TreeTypes treeType) :
  bandwidth(bandwidth),
  relError(relError),
  absError(absError),
  kernelType(kernelType),
  treeType(treeType)
{
  // Nothing to do.
}

// Copy constructor.
inline KDEModel::KDEModel(const KDEModel& other) :
  bandwidth(other.bandwidth),
  relError(other.relError),
  absError(other.absError),
  kernelType(other.kernelType),
  treeType(other.treeType)
{
  // Nothing to do.
}

// Move constructor.
inline KDEModel::KDEModel(KDEModel&& other) :
  bandwidth(other.bandwidth),
  relError(other.relError),
  absError(other.absError),
  kernelType(other.kernelType),
  treeType(other.treeType),
  kdeModel(std::move(other.kdeModel))
{
  // Reset other model.
  other.bandwidth = 1.0;
  other.relError = 0.05;
  other.absError = 0;
  other.kernelType = KernelTypes::GAUSSIAN_KERNEL;
  other.treeType = TreeTypes::KD_TREE;
  other.kdeModel = decltype(other.kdeModel)();
}

inline KDEModel& KDEModel::operator=(KDEModel other)
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
  bandwidth = other.bandwidth;
  relError = other.relError;
  absError = other.absError;
  kernelType = other.kernelType;
  treeType = other.treeType;
  kdeModel = std::move(other.kdeModel);
  return *this;
}

// Clean memory.
inline KDEModel::~KDEModel()
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
}

inline void KDEModel::BuildModel(arma::mat&& referenceSet)
{
  // Clean memory, if necessary.
  boost::apply_visitor(DeleteVisitor(), kdeModel);

  // Build the actual model.
  if (kernelType == GAUSSIAN_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::KDTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::BallTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::StandardCoverTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::Octree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::RTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::KDTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::BallTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::StandardCoverTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::Octree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::RTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::KDTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::BallTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::StandardCoverTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::Octree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::RTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::KDTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::BallTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::StandardCoverTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::Octree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::RTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::KDTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::BallTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::StandardCoverTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::Octree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::RTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }

  // Train the model.
  TrainVisitor train(std::move(referenceSet));
  boost::apply_visitor(train, kdeModel);
}

// Perform bichromatic evaluation.
inline void KDEModel::Evaluate(arma::mat&& querySet, arma::vec& estimations)
{
  Log::Info << "Evaluating KDE..." << std::endl;
  DualBiKDE eval(std::move(querySet), estimations);
  boost::apply_visitor(eval, kdeModel);
}

// Perform monochromatic evaluation.
inline void KDEModel::Evaluate(arma::vec& estimations)
{
  Log::Info << "Evaluating KDE..." << std::endl;
  DualMonoKDE eval(estimations);
  boost::apply_visitor(eval, kdeModel);
}

// Clean memory.
inline void KDEModel::CleanMemory()
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
}

// Parameters for KDE evaluation.
DualMonoKDE::DualMonoKDE(arma::vec& estimations):
    estimations(estimations)
{}

// Default KDE evaluation.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void DualMonoKDE::operator()(KDETypeT<KernelType, TreeType>* kde) const
{
  if (kde)
  {
    kde->Evaluate(estimations);
    const size_t dimension = (kde->ReferenceTree())->Dataset().n_rows;
    KernelNormalizer::ApplyNormalizer<KernelType>(kde->Kernel(),
                                                  dimension,
                                                  estimations);
  }
  else
  {
    throw std::runtime_error("no KDE model initialized");
  }
}

// Parameters for KDE evaluation.
DualBiKDE::DualBiKDE(arma::mat&& querySet, arma::vec& estimations):
    dimension(querySet.n_rows),
    querySet(std::move(querySet)),
    estimations(estimations)
{}

// Default KDE evaluation.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void DualBiKDE::operator()(KDETypeT<KernelType, TreeType>* kde) const
{
  if (kde)
  {
    kde->Evaluate(std::move(querySet), estimations);
    KernelNormalizer::ApplyNormalizer<KernelType>(kde->Kernel(),
                                                  dimension,
                                                  estimations);
  }
  else
  {
    throw std::runtime_error("no KDE model initialized");
  }
}

// Parameters for Train.
TrainVisitor::TrainVisitor(arma::mat&& referenceSet) :
    referenceSet(std::move(referenceSet))
{}

// Default Train.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void TrainVisitor::operator()(KDEType<KernelType, TreeType>* kde) const
{
  Log::Info << "Training KDE model..." << std::endl;
  if (kde)
    kde->Train(std::move(referenceSet));
  else
    throw std::runtime_error("no KDE model initialized");
}

// Delete model.
template<typename KDEType>
void DeleteVisitor::operator()(KDEType* kde) const
{
  if (kde)
    delete kde;
}

// Mode of model.
template<typename KDEType>
KDEMode& ModeVisitor::operator()(KDEType* kde) const
{
  if (kde)
    return kde->Mode();
  else
    throw std::runtime_error("no KDE model initialized");
}

// Get mode of model.
KDEMode KDEModel::Mode() const
{
  return boost::apply_visitor(ModeVisitor(), kdeModel);
}

// Modify mode of model.
KDEMode& KDEModel::Mode()
{
  return boost::apply_visitor(ModeVisitor(), kdeModel);
}

// Serialize the model.
template<typename Archive>
void KDEModel::serialize(Archive& ar, const unsigned int /* version */)
{
  ar & BOOST_SERIALIZATION_NVP(bandwidth);
  ar & BOOST_SERIALIZATION_NVP(relError);
  ar & BOOST_SERIALIZATION_NVP(absError);
  ar & BOOST_SERIALIZATION_NVP(kernelType);
  ar & BOOST_SERIALIZATION_NVP(treeType);

  if (Archive::is_loading::value)
    boost::apply_visitor(DeleteVisitor(), kdeModel);

  ar & BOOST_SERIALIZATION_NVP(kdeModel);
}

} // namespace kde
} // namespace mlpack

#endif


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Main
// Link program modules (pre, solver, post)
//
// Variables accessed throughout the code
//
// *Geometry*
// - symY: defines symmetry about Y axis
// - sRef: reference surface of the full wing
//
// *Networks and Field*
// - bPan: (network of) body panels (structure)
// - wPan: (network of) wake panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panels (structure)
//
// *Physics*
// - Minf: freestream Mach number
// - alpha: freestream angle of attack
// - vInf: freestream velocity vector
// - cL: lift coefficient
// - cD: drag coefficient
//
// *Numerics*
// - numC: numerical parameters (structure)

#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include "pre.h"
#include "solver.h"
#include "post.h"

#define ANSI_COLOR_BLUE    "\x1b[1;34m"
#define ANSI_COLOR_RESET   "\x1b[0m"

using namespace std;
using namespace Eigen;

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

    //// Variable definition
	// Geometry
    bool symY = 0;
    double sRef = 1;
    // Constants
    Numerical_CST numC = {};
	// Freestream
    double Minf = 0;
    double alpha = 0;
    Vector3d vInf(1.0, 0.0, 0.0);
	// Surface and wake panels
    // TODO: if several Networks are considered, use std::vector <network>
	Network bPan = {};
	Network wPan = {};
	// Field cells
	Field fPan = {};
    // Sub-panels
    Subpanel sp = {};
	// Forces
    double cL = 0, cD = 0;

    //// Begin flow
    // Hello World
    time_t now = time(0); // get time now
    char* localNow = ctime(&now);
    cout << ANSI_COLOR_BLUE;
    cout << "***********************************" << endl;
    cout << "**              \\_/              **" << endl;
    cout << "**     \\_______O(_)O_______/     **" << endl;
    cout << "**         _                     **" << endl;
    cout << "**        / \\   __  __  __       **" << endl;
    cout << "**       / _ \\ |__||__||  |      **" << endl;
    cout << "**      /_/ \\_\\|__ |  \\|__|      **" << endl;
    cout << "***********************************";
    cout << ANSI_COLOR_RESET << endl;
    cout << "Hi! My name is Aero v1.0-1809" << endl;
    cout << "Solver started on " << localNow << endl;

    // Check parameters
    if (argc != 3) {
        cout << "Incorrect number of parameters provided!" << endl;
        cout << "Usage: ./aero <pathToConfigFile(.cfg)> <pathToGridgridFile(.pts)>." << endl;
        return 1;
    }

    // Set time counter
    clock_t start = clock();
    clock_t startS = clock();

    // Pre-processing
    pre(argv, numC, symY, sRef, Minf, alpha, vInf, bPan, wPan, fPan, sp);
    clock_t endS = clock();
    cout << "Preprocessing time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Solver
    startS = clock();
    solver(numC, symY, sRef, alpha, vInf, Minf, bPan, wPan, fPan, sp, cL, cD);
    endS = clock();
    cout << "Solver time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Post-processing
    startS = clock();
    post(sRef, alpha, Minf, bPan, fPan, cL, cD);
    endS = clock();
    cout << "Postprocessing time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Total time
    clock_t end = clock();
    cout << "***** Run summary *****" << endl;
    cout << "Total time: " << (end - start) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    return 0;
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_trajectories_cubic_hermite_spline_hpp__
#define __multicontact_api_trajectories_cubic_hermite_spline_hpp__

#include "multicontact-api/trajectories/fwd.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/archive.hpp"

#include <Eigen/Dense>

namespace multicontact_api
{
  namespace trajectories
  {
    namespace detail
    {
//      template<typename _Scalar, int _dim>
    }

    template<typename _Scalar, int _dim>
    struct CubicHermiteSplineTpl
    : public serialization::Serializable< CubicHermiteSplineTpl<_Scalar,_dim> >
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      typedef _Scalar Scalar;
      enum { dim = _dim };
      typedef Eigen::Matrix<Scalar,dim,Eigen::Dynamic> MatrixDx;
      typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorX;
      typedef Eigen::Matrix<Scalar,dim,1> VectorD;
      typedef Eigen::DenseIndex Index;

      CubicHermiteSplineTpl()
      : m_absicca()
      , m_points(_dim,0)
      , m_derivatives(_dim,0)
      , m_ds()
      {}

      CubicHermiteSplineTpl(const Index size)
      : m_absicca(size)
      , m_points(_dim,size)
      , m_derivatives(_dim,size)
      , m_ds((size-1>0)?size-1:0)
      {}

      template<typename VectorDerived, typename MatrixDerived>
      CubicHermiteSplineTpl(const Eigen::MatrixBase<VectorDerived> & absicca,
                            const Eigen::MatrixBase<MatrixDerived> & points,
                            const Eigen::MatrixBase<MatrixDerived> & derivatives
                            )
      : m_absicca(absicca)
      , m_points(points)
      , m_derivatives(derivatives)
      , m_ds(absicca.size()-1)
      {
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(MatrixDerived,MatrixDx);
        //EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(VectorDerived,VectorX);

        assert(points.cols() == derivatives.cols() && "Points and derivatives must have the same number of columns.");
        assert(points.cols() == absicca.size() && "Points and times must have the same dimension.");

        compute();
        assert(check());
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      : m_absicca(other.m_absicca)
      , m_points(other.m_points)
      , m_derivatives(other.m_derivatives)
      {
        compute();
      }

      static CubicHermiteSplineTpl Constant(const VectorD & point)
      {
        MatrixDx points(point.size(),2), derivatives(MatrixDx::Zero(point.size(),2));
        points << point, point;

        VectorX absicca(2); absicca << 0., 1.;

        return CubicHermiteSplineTpl(absicca,points,derivatives);
      }

      template<typename OtherScalar>
      bool operator==(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        return
        m_absicca == other.m_absicca
        && m_points == other.m_points
        && m_derivatives == other.m_derivatives
        && m_ds == other.m_ds
        ;
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl operator+(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        return CubicHermiteSplineTpl(m_absicca, m_points+other.m_points,
                                     m_derivatives+other.m_derivatives);
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl operator-(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        return CubicHermiteSplineTpl(m_absicca, m_points-other.m_points,
                                     m_derivatives-other.m_derivatives);
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl& operator+=(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        m_points += other.m_points;
        m_derivatives += other.m_derivatives;
        return *this;
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl& operator-=(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        m_points-=other.m_points;
        m_derivatives-=other.m_derivatives;
        return *this;
      }

      template<typename OtherScalar>
      bool operator!=(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      { return !(*this != other); }

      const VectorX & absicca() const { return m_absicca; }
      void setAbsicca(const VectorX & absicca)
      {
        assert(absicca.size() == size());
        m_absicca = absicca;
        compute();
        assert(check());
      }

      const MatrixDx & points() const { return m_points; }
      MatrixDx & points() { return m_points; }

      const MatrixDx & derivatives() const { return m_derivatives; }
      MatrixDx & derivatives() { return m_derivatives; }

      ///
      /// \brief Returns the number of points contained in the trajectory.
      ///
      Index size() const { return m_points.cols(); }

      ///
      /// \brief Resize the trajectory with the given input size.
      ///
      void resize(const Index size)
      {
        assert(size >= 0);
        m_points.conservativeResize(dimension(),size);
        m_derivatives.conservativeResize(dimension(),size);
        m_absicca.conservativeResize(size);
        m_ds.conservativeResize(size-1);
      }

      ///
      /// \brief Returns the dimension of the trajectory.
      ///
      Index dimension() const { return m_points.rows(); }

      ///
      /// \brief Returns the number of intervals contained in the trajectory.
      ///
      Index numIntervals() const { return size()-1; }

      ///
      /// \brief Eval the spline at value s.
      ///
      template<typename Derived1, typename Derived2>
      void eval(const Scalar t,
                const Eigen::MatrixBase<Derived1> & p,
                const Eigen::MatrixBase<Derived2> & m) const
      {
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived1,VectorD)
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived2,VectorD)

        const Index id = findInterval(t);
        Eigen::MatrixBase<Derived1> & p_ = const_cast<Eigen::MatrixBase<Derived1> &>(p);
        Eigen::MatrixBase<Derived2> & m_ = const_cast<Eigen::MatrixBase<Derived2> &>(m);
        if(id == size()-1)
        {
          p_ = m_points.template rightCols<1>();
          m_ = m_derivatives.template rightCols<1>();
          return;
        }

        const typename MatrixDx::ConstColXpr p0 = m_points.col(id);
        const typename MatrixDx::ConstColXpr p1 = m_points.col(id+1);

        const typename MatrixDx::ConstColXpr m0 = m_derivatives.col(id);
        const typename MatrixDx::ConstColXpr m1 = m_derivatives.col(id+1);

        const Scalar & t0 = m_absicca[id];
        const Scalar & t1 = m_absicca[id+1];

        const Scalar dt = (t1-t0);
        const Scalar alpha = (t - t0)/dt;

        assert(0. <= alpha <= 1. && "alpha must be in [0,1]");

        Scalar h00, h10, h01, h11;
        evalCoeffs(alpha,h00,h10,h01,h11);
        h10 *= dt; h11 *= dt;

        p_ = (h00 * p0 + h10 * m0 + h01 * p1 + h11 * m1);

        m_ = (1.-alpha) * m0 + alpha * m1;
      }

    protected:

      static void evalCoeffs(const Scalar t,
                             Scalar & h00, Scalar & h10, Scalar & h01, Scalar & h11)
      {
        h00 = 1.; h10 = t; h01 = 0.; h11 = 0.;

        Scalar t_pow = t;

        // t^2
        t_pow = t*t;
        h00 -= 3*t_pow;
        h10 -= 2*t_pow;
        h01 += 3*t_pow;
        h11 -= t_pow;

        // t^3
        t_pow *= t;
        h00 += 2*t_pow;
        h10 += t_pow;
        h01 -= 2*t_pow;
        h11 += t_pow;
      }

      ///
      /// \brief Internal check. It checks if the abissica is monotone.
      ///
      bool check() const
      {
        return (m_ds.array() > 0.).all();
      }

      ///
      /// \brief Internal computation
      ///
      void compute()
      {
        assert(numIntervals() >= 1 && "The number of intervals must be greater or equal to 1.");
        m_ds = m_absicca.tail(numIntervals()) - m_absicca.head(numIntervals());
      }

      ///
      /// \brief Returns the index of the interval for the interpolation.
      ///
      Index findInterval(const Scalar t) const
      {
        if(t < m_absicca[0]) return 0;
        if(t > m_absicca[size()-1]) return numIntervals()-1;

        Index left_id = 0;
        Index right_id = size()-1;

        while(left_id <= right_id)
        {
          const Index middle_id = left_id + (right_id - left_id)/2;
          if(m_absicca[middle_id] < t)
            left_id = middle_id+1;
          else if(m_absicca[middle_id] > t)
            right_id = middle_id-1;
          else
            return middle_id;
        }

        return left_id-1;
      }

      VectorX m_absicca;
      MatrixDx m_points;
      MatrixDx m_derivatives;
      VectorX m_ds;

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void save(Archive & ar, const unsigned int /*version*/) const
      {
        const Index m_size = size();
        ar & boost::serialization::make_nvp("size",m_size);
        ar & boost::serialization::make_nvp("absicca",m_absicca);
        ar & boost::serialization::make_nvp("points",m_points);
        ar & boost::serialization::make_nvp("derivatives",m_derivatives);
      }

      template<class Archive>
      void load(Archive & ar, const unsigned int /*version*/)
      {
        Index m_size;
        ar & boost::serialization::make_nvp("size",m_size);
        resize(m_size);

        ar & boost::serialization::make_nvp("absicca",m_absicca);
        ar & boost::serialization::make_nvp("points",m_points);
        ar & boost::serialization::make_nvp("derivatives",m_derivatives);

        compute();
      }

      BOOST_SERIALIZATION_SPLIT_MEMBER()

    };

    template<typename Scalar, int dim> CubicHermiteSplineTpl<Scalar,dim>
    createHermiteSplineAtAbsicca(const CubicHermiteSplineTpl<Scalar,dim>& spline,
                                    const typename
                                    CubicHermiteSplineTpl<Scalar,dim>::VectorX& absicca)
    {
      typedef CubicHermiteSplineTpl<Scalar,dim> SplineType;
      typedef typename SplineType::VectorX VectorX;
      typedef typename SplineType::MatrixDx MatrixDx;
      const VectorX& t0 = spline.absicca();

      //Assert that the final and initial point is the same
      assert(absicca[0] == t0[0]);
      assert(absicca[absicca.size()-1] == t0[t0.size()-1]);
      //Assert that the new resolution is higher.
      //Otherwise, there might be loss of information.
      assert(absicca.size() >= spline.size());

      MatrixDx p_new(dim, absicca.size());
      MatrixDx m_new(dim, absicca.size());

      for (int k=0;k<absicca.size();k++)
      {
        typename MatrixDx::ColXpr p = p_new.col(k);
        typename MatrixDx::ColXpr m = m_new.col(k);
        spline.eval(absicca[k], p, m);
      }

      return SplineType(absicca, p_new, m_new);
    }
  }
}

#endif // ifndef __multicontact_api_trajectories_cubic_hermite_spline_hpp__


#include <iostream>
#include <CGAL/basic.h>
#include <cassert>
#include <CGAL/Exponent_vector.h>
#include <boost/concept_check.hpp>

int main() {
  typedef CGAL::Exponent_vector T;

  boost::function_requires< boost::DefaultConstructibleConcept<T> >();
  boost::function_requires< boost::AssignableConcept<T> >();
  boost::function_requires< boost::EqualityComparableConcept<T> >();
  boost::function_requires< boost::ComparableConcept<T> >();
  boost::function_requires< boost::RandomAccessContainerConcept<T> >();

  CGAL::Exponent_vector ev1,ev2;
  assert(ev1.size()==0);
  assert(ev2.size()==0);
  assert( (ev1==ev2));
  assert(!(ev1!=ev2));
  assert(!(ev1<ev2));
  assert(!(ev1>ev2));

  ev1.push_back(1);
  ev1.push_back(0);
  ev1.push_back(0);

  ev2.push_back(0);
  ev2.push_back(1);
  ev2.push_back(0);

  assert(!(ev2 == ev1)) ;
  assert( (ev2 != ev1)) ;
  assert(!(ev2 < ev1)) ;
  assert( (ev2 > ev1)) ;

  assert(CGAL::is_valid(ev1));

  std::vector<int> vec;
  vec.push_back(0); vec.push_back(1); vec.push_back(5);

  ev1 = CGAL::Exponent_vector(vec.begin(),vec.end());
  assert(ev1[0] == 0);
  assert(ev1[1] == 1);
  assert(ev1[2] == 5);

  // new constructors
  // univariate
  {
    CGAL::Exponent_vector ev(-1);
    assert(!CGAL::is_valid(ev));
  }
  {
    CGAL::Exponent_vector ev(0);
    assert(CGAL::is_valid(ev));
    assert(ev[0] == 0);
  }
  {
    CGAL::Exponent_vector ev(1);
    assert(CGAL::is_valid(ev));
    assert(ev[0] == 1);
  }

  // bivariate
  {
    CGAL::Exponent_vector ev(0,-1);
    assert(!CGAL::is_valid(ev));
  }
  {
    CGAL::Exponent_vector ev(2,1);
    assert(CGAL::is_valid(ev));
    assert(ev[0] == 2);
    assert(ev[1] == 1);

  }

  // trivariate
  {
    CGAL::Exponent_vector ev(0,0,-1);
    assert(!CGAL::is_valid(ev));
  }
  {
    CGAL::Exponent_vector ev(3,2,1);
    assert(CGAL::is_valid(ev));
    assert(ev[0] == 3);
    assert(ev[1] == 2);
    assert(ev[2] == 1);
  }

  // four-variate
  {
    CGAL::Exponent_vector ev(0,0,0,-1);
    assert(!CGAL::is_valid(ev));
  }
  {
    CGAL::Exponent_vector ev(4,3,2,1);
    assert(CGAL::is_valid(ev));
    assert(ev[0] == 4);
    assert(ev[1] == 3);
    assert(ev[2] == 2);
    assert(ev[3] == 1);
  }

  using std::swap; swap(ev1,ev2);

  return 0;
}