/**
 * NDTime (Natural Deep Time) class for Discrete Event Simulation purposes
 * Copyright (C) 2016  Laouen Mayal Louan Belloli
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef NDTIME_H
#define NDTIME_H

#include <iostream>
#include <string>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <boost/algorithm/string.hpp>

class NDTime {
private:
    bool _inf;
    bool _possitive;
    int _hours; // 3600 seconds
    int _minutes; // 60 seconds
    int _seconds; // 10^0 seconds
    int _milliseconds; 	 // 10^-3 seconds
    int _microseconds; // 10^-6 seconds
    int _nanoseconds; // 10^-9 seconds
    int _picoseconds; // 10^-12 seconds
    int _femtoseconds; // 10^-15 seconds

    void add_hours(int a) {
        this->_hours += a;
    }

    void add_minutes(int a) {
        this->_minutes += a;

        while (this->_minutes >= 60) {
            this->add_hours(1);
            this->_minutes -= 60;
        }

        while (this->_minutes < 0) {
            this->add_hours(-1);
            this->_minutes += 60;
        }
    }

    void add_seconds(int a) {
        this->_seconds += a;

        while (this->_seconds >= 60) {
            this->add_minutes(1);
            this->_seconds -= 60;
        }

        while (this->_seconds < 0) {
            this->add_minutes(-1);
            this->_seconds += 60;
        }
    }

    void add_milliseconds(int a) {
        this->_milliseconds += a;

        while (this->_milliseconds >= 1000) {
            this->add_seconds(1);
            this->_milliseconds -= 1000;
        }

        while (this->_milliseconds < 0) {
            this->add_seconds(-1);
            this->_milliseconds += 1000;
        }
    }

    void add_microseconds(int a) {
        this->_microseconds += a;

        while (this->_microseconds >= 1000) {
            this->add_milliseconds(1);
            this->_microseconds -= 1000;
        }

        while (this->_microseconds < 0) {
            this->add_milliseconds(-1);
            this->_microseconds += 1000;
        }
    }

    void add_nanoseconds(int a) {
        this->_nanoseconds += a;

        while (this->_nanoseconds >= 1000) {
            this->add_microseconds(1);
            this->_nanoseconds -= 1000;
        }

        while (this->_nanoseconds < 0) {
            this->add_microseconds(-1);
            this->_nanoseconds += 1000;
        }
    }

    void add_picoseconds(int a) {
        this->_picoseconds += a;

        while (this->_picoseconds >= 1000) {
            this->add_nanoseconds(1);
            this->_picoseconds -= 1000;
        }

        while (this->_picoseconds < 0) {
            this->add_nanoseconds(-1);
            this->_picoseconds += 1000;
        }
    }

    void add_femtoseconds(int a) {
        this->_femtoseconds += a;

        while (this->_femtoseconds >= 1000) {
            this->add_picoseconds(1);
            this->_femtoseconds -= 1000;
        }

        while (this->_femtoseconds < 0) {
            this->add_picoseconds(-1);
            this->_femtoseconds += 1000;
        }
    }

    static bool deepView(bool togle) {
        static bool deep_precision = false;
        if (togle)
            deep_precision = !deep_precision;
        return deep_precision;
    }

public:

    static bool deep_precision;

    /******************************************/
    /************** CONSTRUCTORS **************/
    /******************************************/

    NDTime() { // default constructor

        this->resetToZero();
    }

    NDTime(const NDTime& o) { // copy constructor
        _inf = o._inf;
        _possitive = o._possitive;
        _hours = o._hours;
        _minutes = o._minutes;
        _seconds = o._seconds;
        _milliseconds = o._milliseconds;
        _microseconds = o._microseconds;
        _nanoseconds = o._nanoseconds;
        _picoseconds = o._picoseconds;
        _femtoseconds = o._femtoseconds;
    }

    NDTime(std::initializer_list<int> a_args) { // variable unit amount specification constructor

        this->resetToZero();
        int a = 0;

        for (auto i: a_args) {

            switch(a) {
                case 0:
                    this->_hours = abs(i);
                    this->_possitive = (i >= 0);
                    break;
                case 1:
                    if (this->_hours == 0) assert(abs(i) < 60);
                    else assert(0 <= i && i < 60);
                    this->_minutes = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 2:
                    if (this->_minutes == 0) assert(abs(i) < 60);
                    else assert(0 <= i && i < 60);
                    this->_seconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 3:
                    if (this->_seconds == 0) assert(abs(i) < 1000);
                    else assert(0 <= i && i < 1000);
                    this->_milliseconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 4:
                    if (this->_milliseconds == 0) assert(abs(i) < 1000);
                    else assert(0 <= i && i < 1000);
                    this->_microseconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 5:
                    if (this->_microseconds == 0) assert(abs(i) < 1000);
                    else assert(0 <= i && i < 1000);
                    this->_nanoseconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 6:
                    if (this->_nanoseconds == 0) assert(abs(i) < 1000);
                    else assert(0 <= i && i < 1000);
                    this->_picoseconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
                case 7:
                    if (this->_picoseconds == 0) assert(abs(i) < 1000);
                    else assert(0 <= i && i < 1000);
                    this->_femtoseconds = abs(i);
                    this->_possitive = (i >= 0) && this->_possitive;
                    break;
            }
            a++;
        }
    }

    // valid options are "inf", "infinity", "-inf" "-infinity" and hh:mm:ss:mmss:mcs:nnss:ppss:ffss
    // NOTE: it is not necessary to specify all the units.
    NDTime(const std::string a) {
        int v;
        unsigned i;
        std::vector<std::string> strs;

        this->resetToZero();

        if (a == "inf" || a == "infinity") {
            this->_inf = true;
            this->_possitive = true;
        } else if (a == "-inf" || a == "-infinity") {
            this->_inf = true;
            this->_possitive = false;
        } else {
            boost::split(strs,a,boost::is_any_of(":"));
            for(i=0; i<strs.size(); ++i) {
                v = std::stoi(strs[i]);
                switch(i) {
                    case 0:
                        this->_possitive = v >= 0;
                        this->_hours = abs(v);
                        break;
                    case 1: assert(v >= 0 && v < 60); this->_minutes = v; break;
                    case 2: assert(v >= 0 && v < 60); this->_seconds = v; break;
                    case 3: assert(v >= 0 && v < 1000); this->_milliseconds = v; break;
                    case 4: assert(v >= 0 && v < 1000); this->_microseconds = v; break;
                    case 5: assert(v >= 0 && v < 1000); this->_nanoseconds = v; break;
                    case 6: assert(v >= 0 && v < 1000); this->_picoseconds = v; break;
                    case 8: assert(v >= 0 && v < 1000); this->_femtoseconds = v; break;
                }
            }
        }
    }

    static void startDeepView() {
        if (!NDTime::deepView(false))
            NDTime::deepView(true);
    }

    static void stopDeepView() {
        if (NDTime::deepView(false))
            NDTime::deepView(true);
    }

    static NDTime infinity() noexcept {
        return NDTime("inf");
    }

    static NDTime minus_infinity() noexcept {
        return NDTime("-inf");
    }

    static NDTime zero() noexcept {
        return NDTime({0});
    }

    void resetToZero() {
        this->_inf = false;
        this->_possitive = true;
        this->_hours = 0;
        this->_minutes = 0;
        this->_seconds = 0;
        this->_milliseconds = 0;
        this->_microseconds = 0;
        this->_nanoseconds = 0;
        this->_picoseconds = 0;
        this->_femtoseconds = 0;
    }

    NDTime& operator+=(const NDTime& o) noexcept {
        bool this_positive, o_positive, greater_positive;
        NDTime greater, lower;
        NDTime other = o;

        if (this->_inf && other._inf) {
            if (this->_possitive != other._possitive) {
                this->resetToZero();
            }

        } else if (other._inf) {
            this->resetToZero();
            this->_inf = true;
            this->_possitive = other._possitive;

        } else if (this->_possitive != other._possitive) {

            this_positive = this->_possitive;
            o_positive = other._possitive;

            this->_possitive = other._possitive = true;

            if (*this == other) {
                this->resetToZero();
                return *this;

            } else if (*this > other) {
                greater_positive = this_positive;
                greater = *this;
                lower = other;
            } else {
                greater_positive = o_positive;
                greater = other;
                lower = *this;
            }

            greater.add_hours(-1 * lower._hours);
            greater.add_minutes(-1 * lower._minutes);
            greater.add_seconds(-1 * lower._seconds);
            greater.add_milliseconds(-1 * lower._milliseconds);
            greater.add_microseconds(-1 * lower._microseconds);
            greater.add_nanoseconds(-1 * lower._nanoseconds);
            greater.add_picoseconds(-1 * lower._picoseconds);
            greater.add_femtoseconds(-1 * lower._femtoseconds);

            greater._possitive = greater_positive;
            *this = greater;
        } else {

            this->add_hours(other._hours);
            this->add_minutes(other._minutes);
            this->add_seconds(other._seconds);
            this->add_milliseconds(other._milliseconds);
            this->add_microseconds(other._microseconds);
            this->add_nanoseconds(other._nanoseconds);
            this->add_picoseconds(other._picoseconds);
            this->add_femtoseconds(other._femtoseconds);

        }

        return *this;
    }

    NDTime& operator-=(const NDTime& o) noexcept {
        NDTime minus_o = o;
        minus_o._possitive = !o._possitive;
        *this += minus_o;
        return *this;
    }

    NDTime operator+(const NDTime& rhs) const {
        NDTime res = *this;
        res += rhs;
        return res;
    }

    NDTime operator-(const NDTime& rhs) const {
        NDTime res = *this;
        res -= rhs;
        return res;
    }

    bool operator==(const NDTime& rhs) const {
        if (this->_possitive != rhs._possitive) return false;
        else if (this->_inf && rhs._inf) return true;
        else if (this->_inf || rhs._inf) return false;
        else {
            bool res = true;
            res = res && (this->_hours == rhs._hours);
            res = res && (this->_minutes == rhs._minutes);
            res = res && (this->_seconds == rhs._seconds);
            res = res && (this->_milliseconds == rhs._milliseconds);
            res = res && (this->_microseconds == rhs._microseconds);
            res = res && (this->_nanoseconds == rhs._nanoseconds);
            res = res && (this->_picoseconds == rhs._picoseconds);
            res = res && (this->_femtoseconds == rhs._femtoseconds);
            return res;
        }
    }

    bool operator!=(const NDTime& rhs) const {
        return !(*this == rhs);
    }

    bool operator<(const NDTime& rhs) const {
        if (*this == rhs) return false;
        else if (*this == NDTime::infinity() || rhs == NDTime::minus_infinity()) return false;
        else if (rhs == NDTime::infinity() || *this == NDTime::minus_infinity()) return true;
        else if (this->_possitive != rhs._possitive) return !this->_possitive;
        else if (this->_possitive) {

            if (this->_hours < rhs._hours) return true;
            else if (this->_hours > rhs._hours) return false;

            if (this->_minutes < rhs._minutes) return true;
            else if (this->_minutes > rhs._minutes) return false;

            if (this->_seconds < rhs._seconds) return true;
            else if (this->_seconds > rhs._seconds) return false;

            if (this->_milliseconds < rhs._milliseconds) return true;
            else if (this->_milliseconds > rhs._milliseconds) return false;

            if (this->_microseconds < rhs._microseconds) return true;
            else if (this->_microseconds > rhs._microseconds) return false;

            if (this->_nanoseconds < rhs._nanoseconds) return true;
            else if (this->_nanoseconds > rhs._nanoseconds) return false;

            if (this->_picoseconds < rhs._picoseconds) return true;
            else if (this->_picoseconds > rhs._picoseconds) return false;

            if (this->_femtoseconds < rhs._femtoseconds) return true;
            else if (this->_femtoseconds > rhs._femtoseconds) return false;

            assert(false); // Should never reach this code
        } else {

            if (this->_hours > rhs._hours) return true;
            else if (this->_hours < rhs._hours) return false;

            if (this->_minutes > rhs._minutes) return true;
            else if (this->_minutes < rhs._minutes) return false;

            if (this->_seconds > rhs._seconds) return true;
            else if (this->_seconds < rhs._seconds) return false;

            if (this->_milliseconds > rhs._milliseconds) return true;
            else if (this->_milliseconds < rhs._milliseconds) return false;

            if (this->_microseconds > rhs._microseconds) return true;
            else if (this->_microseconds < rhs._microseconds) return false;

            if (this->_nanoseconds > rhs._nanoseconds) return true;
            else if (this->_nanoseconds < rhs._nanoseconds) return false;

            if (this->_picoseconds > rhs._picoseconds) return true;
            else if (this->_picoseconds < rhs._picoseconds) return false;

            if (this->_femtoseconds > rhs._femtoseconds) return true;
            else if (this->_femtoseconds < rhs._femtoseconds) return false;

        }
        assert(false); // Should never reach this code
    }

    bool operator>(const NDTime& rhs) const {
        return !(*this < rhs || *this == rhs);
    }

    bool operator<=(const NDTime& rhs) const {
        return !(*this > rhs);
    }

    bool operator>=(const NDTime& rhs) const {
        return !(*this < rhs);
    }

    friend std::ostream& operator<<(std::ostream& os, const NDTime& t);
    friend std::istream& operator>>(std::istream& is, NDTime& t);
};

inline std::ostream& operator<<(std::ostream& os, const NDTime& t) {

    if (!t._possitive) os << "-";
    if (t._inf) {
        os << "inf";
    } else {
        os << ((t._hours < 10) ? "0":"") << t._hours << ":";
        os << ((t._minutes < 10) ? "0":"") << t._minutes << ":";
        os << ((t._seconds < 10) ? "0":"") << t._seconds << ":";
        os << ((t._milliseconds < 10) ? "0":"") << ((t._milliseconds < 100) ? "0":"") << t._milliseconds;
        if (NDTime::deepView(false)) {
            os << ":";
            os << ((t._microseconds < 10) ? "0":"") << ((t._microseconds < 100) ? "0":"") << t._microseconds << ":";
            os << ((t._nanoseconds < 10) ? "0":"") << ((t._nanoseconds < 100) ? "0":"") << t._nanoseconds << ":";
            os << ((t._picoseconds < 10) ? "0":"") << ((t._picoseconds < 100) ? "0":"") << t._picoseconds << ":";
            os << ((t._femtoseconds < 10) ? "0":"") << ((t._femtoseconds < 100) ? "0":"") << t._femtoseconds;
        }
    }
    return os;
}

inline std::istream& operator>>(std::istream& is, NDTime& t) {
    std::string a;
    is >> a;
    t = NDTime(a);
    return is;
}

// specialize numeric_limits
namespace std {
    template<>
    class numeric_limits<NDTime>{
    public:
        static constexpr bool is_specialized = true;
        static NDTime min() noexcept { return NDTime({numeric_limits<int>::min(),59,59,999,999,999,999,999}); }
        static NDTime max() noexcept { return NDTime({numeric_limits<int>::max(),59,59,999,999,999,999,999}); }
        static NDTime lowest() noexcept { return NDTime({numeric_limits<int>::min(),59,59,999,999,999,999,999}); }

        static constexpr int  digits = numeric_limits<int>::digits;
        static constexpr int  digits10 = numeric_limits<int>::digits10;
        static constexpr bool is_signed = true;
        static constexpr bool is_integer = false;
        static constexpr bool is_exact = true;
        static constexpr int radix = 2; // trash_value

        static constexpr int  min_exponent = numeric_limits<int>::min(); // trash_value
        static constexpr int  min_exponent10 = min_exponent/radix; // trash_value
        static constexpr int  max_exponent = numeric_limits<int>::max(); // trash_value
        static constexpr int  max_exponent10 = max_exponent/radix; // trash_value

        static constexpr bool has_infinity = true;
        static constexpr bool has_quiet_NaN = false;
        static constexpr bool has_signaling_NaN = false;
        static constexpr float_denorm_style has_denorm = denorm_indeterminate;
        static constexpr bool has_denorm_loss = false;
        static NDTime infinity() noexcept { return NDTime::infinity(); }

        static constexpr bool is_iec559 = false;
        static constexpr bool is_bounded = false;
        static constexpr bool is_modulo = false;

        static constexpr bool traps = false;
        static constexpr bool tinyness_before = false;
    };
}

#endif


// evolutionaryAlgorithm.hpp
#ifndef evolutionaryAlgorithm
#define evolutionaryAlgorithm

#include <boost/random/mersenne_twister.hpp>

#include "individual.hpp"
#include "population.hpp"
#include "experimentalSetup.hpp"

class EvolutionaryAlgorithm
{
    public:
        std::size_t PopulationSize;
        std::size_t NumberOfFittestIndividuals;
        std::size_t NumberOfIterations;
        std::size_t NumberOfIterationsEqualMinMax;
        std::size_t Iteration;

        // Parent selection
        int ParentSelectionWindowSize;
        bool AllowParentSurvival;

        // Crossover
        double CrossoverProbability;

        // Mutation
        double MutationProbabilityLowerLimit;
        std::size_t MutationIterations;

        double MutationDegreesOfFreedom;
        Eigen::VectorXd MutationDecay;
        double MutationDecay_t;

        double FittestEnsuredSurvivalFraction;
        double FitnessEnsuredSurvivalFraction;
        Eigen::VectorXd::Index FittestIndividualIndex;

        std::size_t HillClimbingIterations;

        Population CurrentPopulation;
        Population FittestMembersOfEntireRun;

        // Constructors
        EvolutionaryAlgorithm();
        EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize, InitialPopulationRandomVariates & RV);

        EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize,
                              const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                              const std::size_t & numberOfFittestIndividuals,
                              const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                              const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                              const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                              const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                              const std::size_t & hillClimbingIterations, InitialPopulationRandomVariates & RV);

        EvolutionaryAlgorithm(ExperimentalSetup & ES, Population & P,
                              const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                              const std::size_t & numberOfFittestIndividuals,
                              const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                              const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                              const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                              const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                              const std::size_t & hillClimbingIterations);

        // Functions
        void RestructingIndividual(Individual & I, const ExperimentalSetup & ES);

        Population InitialisePopulation(ExperimentalSetup & ES, InitialPopulationRandomVariates & RV);

        std::size_t ChoosePartner(const Population & P, int currentIndividual, RandomVariates & RV);

        Individual Crossover(const Individual & I, const Individual & J, const ExperimentalSetup & ES, RandomVariates & RV);

        Eigen::VectorXd CreateMutationProbability(Individual & I, const ExperimentalSetup & ES);
        void Mutation(Individual & I, const ExperimentalSetup & ES, RandomVariates & RV);

        void HillClimbing(Individual & I, ExperimentalSetup & ES, RandomVariates & RV);

        Population SelectionCrossoverMutation(const Population & P, ExperimentalSetup & ES, RandomVariates & RV);

        void Run(ExperimentalSetup & ES, RandomVariates & RV, const bool & trace);
};

#endif


//  (C) Copyright Gennadiy Rozental 2001-2007.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision: 41369 $
//
//  Description : defines algoirthms for comparing 2 floating point values
// ***************************************************************************

#ifndef BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER
#define BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER

#include <boost/limits.hpp>  // for std::numeric_limits

#include <boost/test/utils/class_properties.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {

namespace test_tools {

using unit_test::readonly_property;

// ************************************************************************** //
// **************        floating_point_comparison_type        ************** //
// ************************************************************************** //

enum floating_point_comparison_type {
    FPC_STRONG, // "Very close"   - equation 1' in docs, the default
    FPC_WEAK    // "Close enough" - equation 2' in docs.

};

// ************************************************************************** //
// **************                    details                   ************** //
// ************************************************************************** //

namespace tt_detail {

// FPT is Floating-Point Type: float, double, long double or User-Defined.
template<typename FPT>
inline FPT
fpt_abs( FPT arg )
{
    return arg < static_cast<FPT>(0) ? -arg : arg;
}

//____________________________________________________________________________//

template<typename FPT>
struct fpt_limits {
    static FPT    min_value()
    {
        return std::numeric_limits<FPT>::is_specialized
                    ? (std::numeric_limits<FPT>::min)()
                    : 0;
    }
    static FPT    max_value()
    {
        return std::numeric_limits<FPT>::is_specialized
                    ? (std::numeric_limits<FPT>::max)()
                    : static_cast<FPT>(1000000); // for the our purpuses it doesn't really matter what value is returned here
    }
};

//____________________________________________________________________________//

// both f1 and f2 are unsigned here
template<typename FPT>
inline FPT
safe_fpt_division( FPT f1, FPT f2 )
{
    // Avoid overflow.
    if( f2 < static_cast<FPT>(1)  && f1 > f2*fpt_limits<FPT>::max_value() )
        return fpt_limits<FPT>::max_value();

    // Avoid underflow.
    if( f1 == static_cast<FPT>(0) ||
        f2 > static_cast<FPT>(1) && f1 < f2*fpt_limits<FPT>::min_value() )
        return static_cast<FPT>(0);

    return f1/f2;
}

//____________________________________________________________________________//

} // namespace tt_detail

// ************************************************************************** //
// **************         tolerance presentation types         ************** //
// ************************************************************************** //

template<typename FPT>
struct percent_tolerance_t {
    explicit    percent_tolerance_t( FPT v ) : m_value( v ) {}

    FPT m_value;
};

//____________________________________________________________________________//

template<typename Out,typename FPT>
Out& operator<<( Out& out, percent_tolerance_t<FPT> t )
{
    return out << t.m_value;
}

//____________________________________________________________________________//

template<typename FPT>
inline percent_tolerance_t<FPT>
percent_tolerance( FPT v )
{
    return percent_tolerance_t<FPT>( v );
}

//____________________________________________________________________________//

template<typename FPT>
struct fraction_tolerance_t {
    explicit fraction_tolerance_t( FPT v ) : m_value( v ) {}

    FPT m_value;
};

//____________________________________________________________________________//

template<typename Out,typename FPT>
Out& operator<<( Out& out, fraction_tolerance_t<FPT> t )
{
    return out << t.m_value;
}

//____________________________________________________________________________//

template<typename FPT>
inline fraction_tolerance_t<FPT>
fraction_tolerance( FPT v )
{
    return fraction_tolerance_t<FPT>( v );
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************             close_at_tolerance               ************** //
// ************************************************************************** //

template<typename FPT>
class close_at_tolerance {
public:
    // Public typedefs
    typedef bool result_type;

    // Constructor
    template<typename ToleranceBaseType>
    explicit    close_at_tolerance( percent_tolerance_t<ToleranceBaseType>  tolerance,
                                    floating_point_comparison_type          fpc_type = FPC_STRONG )
    : p_fraction_tolerance( tt_detail::fpt_abs( static_cast<FPT>(0.01)*tolerance.m_value ) )
    , p_strong_or_weak( fpc_type ==  FPC_STRONG )
    {}
    template<typename ToleranceBaseType>
    explicit    close_at_tolerance( fraction_tolerance_t<ToleranceBaseType> tolerance,
                                    floating_point_comparison_type          fpc_type = FPC_STRONG )
    : p_fraction_tolerance( tt_detail::fpt_abs( tolerance.m_value ) )
    , p_strong_or_weak( fpc_type ==  FPC_STRONG )
    {}

    bool        operator()( FPT left, FPT right ) const
    {
        FPT diff = tt_detail::fpt_abs( left - right );
        FPT d1   = tt_detail::safe_fpt_division( diff, tt_detail::fpt_abs( right ) );
        FPT d2   = tt_detail::safe_fpt_division( diff, tt_detail::fpt_abs( left ) );

        return p_strong_or_weak
                   ? (d1 <= p_fraction_tolerance && d2 <= p_fraction_tolerance)
                   : (d1 <= p_fraction_tolerance || d2 <= p_fraction_tolerance);
    }

    // Public properties
    readonly_property<FPT>  p_fraction_tolerance;
    readonly_property<bool> p_strong_or_weak;
};

//____________________________________________________________________________//

// ************************************************************************** //
// **************               check_is_close                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_close_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT, typename ToleranceBaseType>
    bool
    operator()( FPT left, FPT right, percent_tolerance_t<ToleranceBaseType> tolerance,
                floating_point_comparison_type fpc_type = FPC_STRONG )
    {
        close_at_tolerance<FPT> pred( tolerance, fpc_type );

        return pred( left, right );
    }
    template<typename FPT, typename ToleranceBaseType>
    bool
    operator()( FPT left, FPT right, fraction_tolerance_t<ToleranceBaseType> tolerance,
                floating_point_comparison_type fpc_type = FPC_STRONG )
    {
        close_at_tolerance<FPT> pred( tolerance, fpc_type );

        return pred( left, right );
    }
};

namespace {
check_is_close_t check_is_close;
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************               check_is_small                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_small_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT>
    bool
    operator()( FPT fpv, FPT tolerance )
    {
        return tt_detail::fpt_abs( fpv ) < tt_detail::fpt_abs( tolerance );
    }
};

namespace {
check_is_small_t check_is_small;
}

//____________________________________________________________________________//

} // namespace test_tools

} // namespace boost

//____________________________________________________________________________//

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_FLOATING_POINT_COMAPARISON_HPP_071894GER


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * 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.
 * Neither the name of the University of Oxford 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.

*/

#ifndef MIRAMS2010WNTODESYSTEM_HPP_
#define MIRAMS2010WNTODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/shared_ptr.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystem.hpp"
#include "AbstractCellMutationState.hpp"

/**
 * Represents the Mirams et al. system of ODEs, based on Swat et al. (2004)
 * [doi:10.1093/bioinformatics/bth110]
 * and a simple Wnt model (unpublished)
 *
 * The variables are
 *
 * 6. b1 = Beta-Catenin (from 1st allele)
 * 7. b2 = Beta-Catenin (from 1st allele)
 * 8. WntLevel
 */
class Mirams2010WntOdeSystem : public AbstractOdeSystem
{
private:

    /**
     * Parameters for the Mirams et al. (2010) model
     */

    /** Dimensional parameter a. */
    double mA;
    /** Dimensional parameter b. */
    double mB;
    /** Dimensional parameter c. */
    double mC;
    /** Dimensional parameter d. */
    double mD;
    /** Dimensional parameter e. */
    double mE;
    /** Dimensional parameter f. */
    double mF;

    /** The mutation state of the cell (this affects the ODE system). */
    boost::shared_ptr<AbstractCellMutationState> mpMutationState;

    /** The Wnt level (this affects the ODE system). */
    double mWntLevel;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param wntLevel is a non-dimensional Wnt value between 0 and 1. This sets up the Wnt pathway in its steady state.
     * @param pMutationState optional mutation state (affects the ODE system)
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    Mirams2010WntOdeSystem(double wntLevel=0.0,
                           boost::shared_ptr<AbstractCellMutationState> pMutationState=boost::shared_ptr<AbstractCellMutationState>(),
                           std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~Mirams2010WntOdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Set the mutation state of the cell.
     *
     * This should be called by the relevant cell-cycle model before any solving
     * of the ODE system (as it is used to evaluate the Y derivatives).
     *
     * @param pMutationState the mutation state.
     */
    void SetMutationState(boost::shared_ptr<AbstractCellMutationState> pMutationState);

    /**
     * Called by the archive function on the Wnt cell-cycle model.
     *
     * @return #mpMutationState
     */
    const boost::shared_ptr<AbstractCellMutationState> GetMutationState() const;

    /**
     * Compute the RHS of the WntCellCycle system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * @return #mWntLevel.
     */
    double GetWntLevel() const;
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(Mirams2010WntOdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a Mirams2010WntOdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const Mirams2010WntOdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const double wnt_level = t->GetWntLevel();
    ar & wnt_level;

    const boost::shared_ptr<AbstractCellMutationState> p_mutation_state = t->GetMutationState();
    ar & p_mutation_state;

    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a Mirams2010WntOdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, Mirams2010WntOdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    double wnt_level;
    ar & wnt_level;

    boost::shared_ptr<AbstractCellMutationState> p_mutation_state;
    ar & p_mutation_state;

    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)Mirams2010WntOdeSystem(wnt_level, p_mutation_state, state_variables);
}
}
} // namespace ...

#endif /*MIRAMS2010WNTODESYSTEM_HPP_*/


#include <iostream>
#include <fstream>

#include <time.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/variance.hpp>

using namespace boost::accumulators;

#include <ilk/eigen/core-types.h>
#include <ilk/eigen/misc.h>
#include <ilk/eigen/rots.h>
#include <ilk/eigen/test-utils.h>
#include "ur5.h"

using namespace std;

static kul::RoundTheDifference<double> rounder(0.1);

int main(int argc, char** argv)
{
    int different_dataset_repetitions = 10;
    int solver_call_repetitions_per_dataset = 10000000;
    if (argc >= 2) {
        different_dataset_repetitions = atoi(argv[1]);
    }
    if (argc >= 3) {
        solver_call_repetitions_per_dataset = atoi(argv[2]);
    }
    if (different_dataset_repetitions <= 0 || solver_call_repetitions_per_dataset <= 0) {
        std::cerr << "Invalid parameters." << std::endl;
        std::cerr << "Proper parameters:  [number of random datasets to generate] [number of solver calls per dataset]" << std::endl;
        return -1;
    }

    cout << "Execution times of blocks of " << solver_call_repetitions_per_dataset << " solver calls will be measured " << different_dataset_repetitions << " times for different datasets. Please wait..." << endl;

    kul::AxisAngle Rdiff;
    int count = 0;

    accumulator_set<double, stats<tag::variance> > acc;

    ur5::ModelConstants mc;
    ur5::joint_state q;
    ur5::joint_state qd;
    kul::pose_t wrist_3__base;
    kul::twist_t v__wrist_3__base;
    ur5::Jacobian_t J_wrist_3_base;

    for (int i = 0; i < different_dataset_repetitions; i++)
    {
        q.setRandom();

        struct timespec t_before;
        struct timespec t_after;
        double t_delta;
        clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &t_before);

        for (int j = 0; j < solver_call_repetitions_per_dataset; j++) {
            ur5::solver1(mc, q, qd, wrist_3__base, v__wrist_3__base, J_wrist_3_base);
        }
        clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &t_after);
        double before_in_ns = t_before.tv_nsec + t_before.tv_sec * 1e9;
        double after_in_ns = t_after.tv_nsec + t_after.tv_sec * 1e9;
        t_delta = after_in_ns - before_in_ns;
        acc(t_delta);
        count++;
    }
    cout << endl;
    cout << "Mean execution time of a block: "<< mean(acc) * 1e-9 << " s" << endl;
    cout << "Standard deviation of execution time of a block: " << sqrt(variance(acc)) * 1e-9 << " s"<< endl;
    cout << "Standard deviation as percentage of mean: " << sqrt(variance(acc)) / mean(acc) * 100.0 << " %"<< endl;

    return 0;
}


// Copyright (C) 2014 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * 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.
//
//     * Neither the name of The Regents or University of California, Google,
//       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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu), John Flynn (jflynn@google.com)

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <limits>
#include <vector>

#include "theia/math/util.h"
#include "theia/sfm/pose/four_point_relative_pose_partial_rotation.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/test/test_utils.h"
#include "theia/util/random.h"
#include "theia/util/util.h"
#include "gtest/gtest.h"

namespace theia {
namespace {

using Eigen::AngleAxisd;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

RandomNumberGenerator rng(61);

// Tests that the four point pose works correctly by taking the passed
// points_3d, projecting them to view_1_origins to get image one rays,
// transforming by (expected_rotation, expected_translation) to get
// image two rays and then verifying that the FourPointEssentialMatrix
// function returns (expected_rotation, expected_translation) among its
// solutions.
// Noise can be added to the image projections by setting projection_noise.
// The thresholds for rotation and translation similarity can be controlled
// by max_rotation_difference and max_difference_between_translation.
// If projection_noise_std_dev is non-zero then a random noise generator must
// be passed in rng.
void TestFourPointResultWithNoise(const Vector3d& axis,
                                  const Vector3d points_3d[4],
                                  const Vector3d view_1_origins[4],
                                  const Vector3d view_2_origins[4],
                                  double projection_noise_std_dev,
                                  const Quaterniond& expected_rotation,
                                  const Vector3d& expected_translation,
                                  double max_rotation_difference,
                                  double max_translation_difference) {
  Vector3d image_one_rays[4];
  Vector3d image_two_rays[4];

  for (int i = 0; i < 4; ++i) {
    image_one_rays[i] = (points_3d[i] - view_1_origins[i]).normalized();
    const Vector3d transformed_point =
        expected_rotation * points_3d[i] + expected_translation;
    image_two_rays[i] = (transformed_point - view_2_origins[i]).normalized();
  }
  if (projection_noise_std_dev) {
    for (int i = 0; i < 4; ++i) {
      AddNoiseToRay(projection_noise_std_dev, &rng, &image_one_rays[i]);
      AddNoiseToRay(projection_noise_std_dev, &rng, &image_two_rays[i]);
    }
  }
  std::vector<Quaterniond> soln_rotations;
  std::vector<Vector3d> soln_translations;

  FourPointRelativePosePartialRotation(axis,
                                       image_one_rays,
                                       view_1_origins,
                                       image_two_rays,
                                       view_2_origins,
                                       &soln_rotations,
                                       &soln_translations);

  bool matched_transform = false;
  for (int n = 0; n < soln_rotations.size(); ++n) {
    const double rotation_difference =
        expected_rotation.angularDistance(soln_rotations[n]);

    bool matched_rotation = (rotation_difference < max_rotation_difference);

    const double translation_difference =
        (expected_translation - soln_translations[n]).norm();

    bool matched_translation =
        (translation_difference < max_translation_difference);

    if (matched_translation && matched_rotation) {
      matched_transform = true;
      break;
    }
  }
  EXPECT_TRUE(matched_transform);
}

void BasicTest() {
  // Sets up some points in the 3D scene
  const Vector3d kPoints3D[4] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0),
                                 Vector3d(4.0, 3.0, 5.0)};

  const Vector3d kImageOneOrigins[4] = {Vector3d(-1.0, 0.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(2.0, 0.0, 0.0),
                                        Vector3d(3.0, 0.0, 0.0)};

  const Vector3d kImageTwoOrigins[4] = {Vector3d(0.0, 1.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(0.0, 2.0, 0.0),
                                        Vector3d(0.0, 3.0, 0.0)};

  const Vector3d axis = Vector3d(1.0, 1.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(51.0), axis));

  const Vector3d kExpectedTranslation(-2.0, 3.0, -5.0);
  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedTranslationDifference = 1e-4;
  TestFourPointResultWithNoise(axis,
                               kPoints3D,
                               kImageOneOrigins,
                               kImageTwoOrigins,
                               kProjectionNoise,
                               kExpectedRotation,
                               kExpectedTranslation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedTranslationDifference);
}

TEST(FourPointEssentialMatrixTest, Basic) { BasicTest(); }

TEST(FourPointEssentialMatrixTest, NoRotation) {
  // Sets up some points in the 3D scene
  const Vector3d kPoints3D[4] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(2.0, 2.0, 5.0),
                                 Vector3d(4.0, 3.0, 5.0)};

  const Vector3d kImageOneOrigins[4] = {Vector3d(-1.0, 0.0, -1.5),
                                        Vector3d(0.0, 0.0, -1.0),
                                        Vector3d(2.0, 0.0, 0.0),
                                        Vector3d(3.0, 0.0, 0.0)};

  const Vector3d kImageTwoOrigins[4] = {Vector3d(0.0, 1.0, 2.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(0.0, 2.0, 1.0),
                                        Vector3d(0.0, 3.0, 0.0)};

  const Vector3d axis = Vector3d(1.0, 1.0, 1.0).normalized();
  Quaterniond kExpectedRotation(AngleAxisd(DegToRad(0.0), axis));

  const Vector3d kExpectedTranslation(1.0, 1.0, 1.0);
  double kProjectionNoise = 0.0;
  double kMaxAllowedRotationDifference = 1e-5;
  double kMaxAllowedTranslationDifference = 1e-4;
  TestFourPointResultWithNoise(axis,
                               kPoints3D,
                               kImageOneOrigins,
                               kImageTwoOrigins,
                               kProjectionNoise,
                               kExpectedRotation,
                               kExpectedTranslation,
                               kMaxAllowedRotationDifference,
                               kMaxAllowedTranslationDifference);
}

// Tests a variety of axes, angles and translations with added projection noise.
TEST(FourPointEssentialMatrixTest, NoiseTest) {
  const Vector3d kPoints3D[4] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0),
                                 Vector3d(4.0, 3.0, 5.0)};

  const Vector3d kImageOneOrigins[4] = {Vector3d(-1.0, 0.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(2.0, 0.0, 0.0),
                                        Vector3d(3.0, 0.0, 0.0)};

  const Vector3d kImageTwoOrigins[4] = {Vector3d(0.0, 1.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(0.0, 2.0, 0.0),
                                        Vector3d(0.0, 3.0, 0.0)};

  const Vector3d kAxes[5] = {Vector3d(0.0, 0.0, 1.0).normalized(),
                             Vector3d(1.0, 0.0, 0.0).normalized(),
                             Vector3d(1.0, 0.0, 1.0).normalized(),
                             Vector3d(1.0, 1.0, 0.0).normalized(),
                             Vector3d(1.0, 1.0, 1.0).normalized()};

  const double kAngles[5] = {11.0, 5.0, 2.0, 13.0, 12.0};

  const Vector3d kTranslations[5] = {Vector3d(1.0, 1.0, 1.0),
                                     Vector3d(4.0, 5.0, 11.0),
                                     Vector3d(1.0, 2.0, 15.0),
                                     Vector3d(6.0, 3.0, 2.0),
                                     Vector3d(13.0, 1.0, 15.0)};

  for (int transform_index = 0; transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    Quaterniond kExpectedRotation(
        AngleAxisd(DegToRad(kAngles[transform_index]), kAxes[transform_index]));

    const double kProjectionNoise = 0.5 / 512;
    const double kMaxAllowedRotationDifference = DegToRad(4.0);
    const double kMaxAllowedTranslationDifference = 2.0;

    TestFourPointResultWithNoise(kAxes[transform_index],
                                 kPoints3D,
                                 kImageOneOrigins,
                                 kImageTwoOrigins,
                                 kProjectionNoise,
                                 kExpectedRotation,
                                 kTranslations[transform_index],
                                 kMaxAllowedRotationDifference,
                                 kMaxAllowedTranslationDifference);
  }
}

// Tests that the solver degrades gracefully when the passed axis is not exactly
// correct.
TEST(FourPointEssentialMatrixTest, IncorrectAxisTest) {
  const Vector3d kPoints3D[4] = {Vector3d(-1.0, 3.0, 3.0),
                                 Vector3d(1.0, -1.0, 2.0),
                                 Vector3d(2.0, 1.0, 3.0),
                                 Vector3d(4.0, 3.0, 5.0)};

  const Vector3d kImageOneOrigins[4] = {Vector3d(-1.0, 0.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(2.0, 0.0, 0.0),
                                        Vector3d(3.0, 0.0, 0.0)};

  const Vector3d kImageTwoOrigins[4] = {Vector3d(0.0, 1.0, 0.0),
                                        Vector3d(0.0, 0.0, 0.0),
                                        Vector3d(0.0, 2.0, 0.0),
                                        Vector3d(0.0, 3.0, 0.0)};

  const Vector3d kAxes[5] = {Vector3d(0.0, 0.0, 1.0).normalized(),
                             Vector3d(1.0, 0.0, 0.0).normalized(),
                             Vector3d(1.0, 0.0, 1.0).normalized(),
                             Vector3d(1.0, 1.0, 0.0).normalized(),
                             Vector3d(1.0, 1.0, 1.0).normalized()};

  const double kAngles[5] = {11.0, 5.0, 2.0, 13.0, 12.0};

  const Vector3d kTranslations[5] = {Vector3d(1.0, 1.0, 1.0),
                                     Vector3d(4.0, 5.0, 11.0),
                                     Vector3d(1.0, 2.0, 15.0),
                                     Vector3d(6.0, 3.0, 2.0),
                                     Vector3d(13.0, 1.0, 15.0)};

  // The axes are perturbed by these rotation to simulate the axis not being
  // perfectly known.
  const Quaterniond kAxisPerturbations[6] = {
      Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(1.0, 0, 0))),
      Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(1.0, 0, 0))),
      Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 1.0, 0))),
      Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 1.0, 0))),
      Quaterniond(AngleAxisd(DegToRad(1.0), Vector3d(0.0, 0.0, 1.0))),
      Quaterniond(AngleAxisd(DegToRad(-1.0), Vector3d(0.0, 0.0, 1.0)))};

  for (int transform_index = 0; transform_index < THEIA_ARRAYSIZE(kAxes);
       ++transform_index) {
    for (int axis_rotation_index = 0;
         axis_rotation_index < THEIA_ARRAYSIZE(kAxisPerturbations);
         ++axis_rotation_index) {
      // Perturbs the axis by the axis perturbation.
      const Vector3d perturbed_axis =
          kAxisPerturbations[axis_rotation_index] * kAxes[transform_index];
      // Uses the perturbed rotation as the expected rotation, but the original
      // axis is passed as the axis.
      Quaterniond kExpectedRotation(
          AngleAxisd(DegToRad(kAngles[transform_index]), perturbed_axis));

      // Tests the ThreePointEssentialMatrix function with fairly large
      // thresholds on the allowed translation and rotation differences.
      const double kProjectionNoise = 0.0;
      const double kMaxAllowedRotationDifference = DegToRad(2.0);
      const double kMaxAllowedTranslationDifference = 2.0;

      TestFourPointResultWithNoise(kAxes[transform_index],
                                   kPoints3D,
                                   kImageOneOrigins,
                                   kImageTwoOrigins,
                                   kProjectionNoise,
                                   kExpectedRotation,
                                   kTranslations[transform_index],
                                   kMaxAllowedRotationDifference,
                                   kMaxAllowedTranslationDifference);
    }
  }
}

}  // namespace
}  // namespace theia


#ifndef ADMM_LINEARSOLVER_H_
#define ADMM_LINEARSOLVER_H_ 1

#include <Eigen/SparseCholesky>
#include <Eigen/Dense>

namespace mcl {
namespace optlib {

#ifdef USE_PARDISO

// Pariso-project functions
extern "C" {
    void pardisoinit (void   *, int    *,   int *, int *, double *, int *);
    void pardiso     (void   *, int    *,   int *, int *,    int *, int *,
                    double *, int    *,    int *, int *,   int *, int *,
                    int *, double *, double *, int *, double *);
    void pardiso_chkmatrix  (int *, int *, double *, int *, int *, int *);
    void pardiso_chkvec     (int *, int *, double *, int *);
    void pardiso_printstats (int *, int *, double *, int *, int *, int *, double *, int *);
    void pardiso_residual(int *, int *, double *, int *, int *, double *, double *, double *, double *, double *);
}

class PardisoSolver {
public:
    enum {
        SPD = 2,
        SI = -2
    };

	PardisoSolver() :
		mtype(SPD),
		nrhs(1), // number of right hand side
		msglvl(0), // 0=quiet, 1=error checking
		n(0), // dof
		maxfct(1), // max number of factorizations
		mnum(1), // which factorization to use
		solver(0), // sparse direct solver
		m_initialized(false)
		{}

	PardisoSolver(Eigen::SparseMatrix<double, Eigen::RowMajor> &A) : PardisoSolver() { compute(A); }

    ~PardisoSolver(){
		if (m_initialized) {
			pardisoRelease();
		}
	}

	void compute(Eigen::SparseMatrix<double, Eigen::RowMajor> &A) {
		    m_info = Eigen::InvalidInput;

		using namespace Eigen;

		int dof = A.rows();
		m_x.resize(dof);
		if (dof != A.cols() || dof == 0) {
			pardisoErrorCode(-1,"compute",true);
			return;
		}

		// TODO just create CSR from column major A
		// SparseMatrix<double,RowMajor> A = A_;//.triangularView<Upper>();

		n = A.rows();
		int nnz = A.nonZeros();
		int error = 0;

		m_perm = VectorXi::Zero(n);
		m_rows.clear();
		m_rows.reserve(n+1);
		m_cols.clear();
		m_cols.reserve(nnz);
		m_vals.clear();
		m_vals.reserve(nnz);
		VectorXd x_dummy = VectorXd::Zero(n);

		// Convert from row matrix to CSR and Fortran (+1) index
		m_rows.emplace_back(1);
		for (int r=0, v_idx=0; r<n; ++r) {
			m_perm[r] = r+1;
			SparseMatrix<double,RowMajor>::InnerIterator it(A, r);
			for( ; it; ++it ){
				int col = it.col();
				if (col >= r) { // upper triangular
					m_vals.emplace_back(it.value());
					m_cols.emplace_back(col+1);
					++v_idx;
				}
			} // end loop nonzeros for col c
			m_rows.emplace_back(v_idx+1); // Update row index
		} // end loop cols

		// Check input matrix
		if (msglvl>0) {
			pardiso_chkmatrix(&mtype, &n, m_vals.data(), m_rows.data(), m_cols.data(), &error);
			pardisoErrorCode(error,"compute",true);
			if (error != 0) {
				return;
			}
		}

		int n_threads = 0;
		char *ont = getenv("OMP_NUM_THREADS");
		if (ont != NULL) { sscanf(ont, "%d", &n_threads); }
		if (n_threads <= 0) {
			std::string err = "**Error in PardisoSolver::compute: Must set env OMP_NUM_THREADS";
			throw std::runtime_error(err.c_str());
		}

		// Tests for pardiso-project license and inits solver
		// iparm[2] must be OMP_NUM_THREADS
		iparm[2] = n_threads;
		iparm[0] = 0; // Sets parameters to default (except iparm 2)
		pardisoinit(pt, &mtype, &solver, iparm, dparm, &error);
		pardisoErrorCode(error,"compute",true);
		if (error != 0) {
			m_info =  Eigen::InvalidInput;
			return;
		}

		iparm[0] = 1; // no longer use default params
		iparm[33] = 1; // identical perf on multicore
		iparm[29] = 20; // supernodes

		int phase = 11; // Symbolic factorization
		pardiso(pt, &maxfct, &mnum, &mtype, &phase,
				&n, m_vals.data(), m_rows.data(), m_cols.data(), m_perm.data(), &nrhs,
				iparm, &msglvl, NULL, NULL, &error, dparm);
		pardisoErrorCode(error,"compute",true);
		if (error != 0) {
			m_info =  Eigen::NumericalIssue;
			return;
		}

		phase = 22; // Numerical factorization
		pardiso(pt, &maxfct, &mnum, &mtype, &phase,
				&n, m_vals.data(), m_rows.data(), m_cols.data(), m_perm.data(), &nrhs,
				iparm, &msglvl, NULL, NULL, &error, dparm);
		pardisoErrorCode(error,"compute",true);
		if (error != 0) {
			m_info = Eigen::NumericalIssue;
			return;
		}

		m_info = Eigen::Success;
		m_initialized = true;
	}

	void factorize(Eigen::SparseMatrix<double, Eigen::RowMajor> &A) {
		using namespace Eigen;
		// SparseMatrix<double,RowMajor> A = A_;

		// Converge row matrix to CSR and Fortran (+1) index
		m_rows[0]=1;
		for (int r=0, v_idx=0; r<n; ++r) {
			SparseMatrix<double,RowMajor>::InnerIterator it(A, r);
			for( ; it; ++it ){
				int col = it.col();
				if (col >= r) { // upper triangular
					m_vals[v_idx] = it.value();
					m_cols[v_idx] = col+1;
					++v_idx;
				}
			} // end loop nonzeros for col c
			m_rows[r+1] = v_idx+1;
		} // end loop cols

		int error = 0;
		int phase = 22; // Numerical factorization
		pardiso(pt, &maxfct, &mnum, &mtype, &phase,
				&n, m_vals.data(), m_rows.data(), m_cols.data(), m_perm.data(), &nrhs,
				iparm, &msglvl, NULL, NULL, &error, dparm);
		pardisoErrorCode(error,"factorize",true);
		if (error != 0) {
			m_info =  Eigen::NumericalIssue;
		}
	}

	Eigen::ComputationInfo info(){ return m_info; }

	Eigen::VectorXd solve(const Eigen::VectorXd &rhs) {
		using namespace Eigen;
		if (m_info != Eigen::Success) {
			throw std::runtime_error("**PardisoSolver::solve Error: Cannot solve, no factorization\n");
			return VectorXd::Zero(rhs.rows());
		}
		iparm[8] = 1; // ???
		int phase = 33;
		int error = 0;
		pardiso(pt, &maxfct, &mnum, &mtype, &phase,
				&n, m_vals.data(), m_rows.data(), m_cols.data(), m_perm.data(), &nrhs,
				iparm, &msglvl, (double*)rhs.data(), m_x.data(), &error, dparm);
		pardisoErrorCode(error,"solve",true);

		return m_x;
	}

    // Set matrix type. Default SPD
    int &type() { return mtype; }

	void pardisoErrorCode(int error, std::string call, bool ex=false) const {
		std::string err = "";
		switch(error) {
		case 0: // success
			break;
		case -1: err =  "\n**Error in PardisoSolver::%s: input inconsistent\n" + call;
			break;
		case -2: err =  "\n**Error in PardisoSolver::%s: not enough memory\n" + call;
			break;
		case -3: err =  "\n**Error in PardisoSolver::%s: reordering problem\n" + call;
			break;
		case -4: err =  "\n**Error in PardisoSolver::%s: zero pivot, numerical factorization or iterative refinement problem\n" + call;
			break;
		case -5: err =  "\n**Error in PardisoSolver::%s: unclassified (internal) error\n" + call;
			break;
		case -6: err =  "\n**Error in PardisoSolver::%s: pre-ordering failed (matrix types 11, 13 only)\n" + call;
			break;
		case -7: err =  "\n**Error in PardisoSolver::%s: diagonal matrix is singular\n" + call;
			break;
		case -8: err =  "\n**Error in PardisoSolver::%s: 32-bit integer overflow problem\n" + call;
			break;
		case -10: err =  "\n**Error in PardisoSolver::%s: No license file pardiso.lic found.\n" + call;
			break;
		case -11: err =  "\n**Error in PardisoSolver::%s: License is expired.\n" + call;
			break;
		case -12: err =  "\n**Error in PardisoSolver::%s: Wrong username or hostname.\n" + call;
			break;
		case -100: err =  "\n**Error in PardisoSolver::%s: Reached maximum number of Krylov-subspace iteration in iterative solver.\n" + call;
			break;
		case -101: err =  "\n**Error in PardisoSolver::%s: No sufficient convergence in Krylov-subspace iteration within 25 iterations.\n" + call;
			break;
		case -102: err =  "\n**Error in PardisoSolver::%s: Error in Krylov-subspace iteration.\n" + call;
			break;
		case -103: err =  "\n**Error in PardisoSolver::%s: Break-Down in Krylov-subspace iteration\n" + call;
			break;
		default: err =  "\n**Unknown Error in PardisoSolver::%s" + call;
			break;
		}
		if (err.size()>0) {
			if (ex) { throw std::runtime_error(err.c_str()); }
			else { printf("%s",err.c_str()); }
		}
    }

	void pardisoRelease() {
		if (m_info != Eigen::Success) { return; }
		int phase = -1;
		int error = 0;
		pardiso(pt, &maxfct, &mnum, &mtype, &phase,
			&n, m_vals.data(), m_rows.data(), m_cols.data(), m_perm.data(), &nrhs,
			iparm, &msglvl, NULL, NULL, &error, dparm);
		pardisoErrorCode(error,"release",false);
	}

protected:
	int mtype;
	int nrhs;
	int msglvl;
	int n;
	int maxfct, mnum, solver;
	long int pt[64]; // int[64] if 32bit, long int[64] if 64bit, void *pt[64] works for both
	int iparm[64];
	double dparm[64];
	std::vector<int> m_rows;
	std::vector<int> m_cols;
	std::vector<double> m_vals;
	Eigen::VectorXi m_perm;
	Eigen::VectorXd m_x;  // solution from previous solve
	Eigen::ComputationInfo m_info;
	bool m_initialized;

}; // end class PardisoSolver

#endif

// Wrapper for whatever linear solver I decide to go with in the future
class LinearSolver {
protected:
	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper> cholesky;

public:

    LinearSolver() {}
    LinearSolver(const Eigen::SparseMatrix<double> &A) : LinearSolver() { compute(A); }

	void compute(const Eigen::SparseMatrix<double> &A){
		cholesky.analyzePattern(A);
		cholesky.factorize(A);
	}
	void factorize(const Eigen::SparseMatrix<double> &A) { cholesky.factorize(A); }

	Eigen::ComputationInfo info(){ return cholesky.info(); }

	Eigen::VectorXd solve(const Eigen::VectorXd &rhs){ return cholesky.solve(rhs); }

}; // end class linear solver

} // ns optlib
} // ns mcl

#endif


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout;  using std::endl;
#include <cerrno> // for ::errno

//[policy_eg_5

#include <boost/math/special_functions.hpp>
// using boost::math::tgamma; // Would create an ambiguity between
// 'double boost::math::tgamma<int>(T)' and
// 'double 'anonymous-namespace'::tgamma<int>(RT)'.

namespace
{ // unnamed

using namespace boost::math::policies;

typedef policy<
   domain_error<errno_on_error>,
   pole_error<errno_on_error>,
   overflow_error<errno_on_error>,
   evaluation_error<errno_on_error>
> c_policy;

BOOST_MATH_DECLARE_SPECIAL_FUNCTIONS(c_policy)

/*`
So that when we call `tgamma(z)`, we really end up calling
 `boost::math::tgamma(z, anonymous-namespace::c_policy())`.
*/

} // close unnamed namespace

int main()
{
   errno = 0;
   cout << "Result of tgamma(30000) is: "
      << tgamma(30000) << endl;
      // tgamma in unnamed namespace in this translation unit (file) only.
   cout << "errno = " << errno << endl;
   cout << "Result of tgamma(-10) is: "
      << tgamma(-10) << endl;
   cout << "errno = " << errno << endl;
   // Default tgamma policy would throw an exception, and abort.
}

//] //[/policy_eg_5]

/*
Output:

  Result of tgamma(30000) is: 1.#INF
  errno = 34
  Result of tgamma(-10) is: 1.#QNAN
  errno = 33

*/


#ifndef _FRANKA_CARTESIAN_GENERATOR_HPP_
#define _FRANKA_CARTESIAN_GENERATOR_HPP_

#include <Eigen/Core>
#include <Eigen/Dense>

#include <franka/exception.h>
#include <franka/robot.h>

namespace cartesian_franka {
    class CartesianMotionGenerator {
    public:
        ///movements are relative !
        CartesianMotionGenerator(const Eigen::Affine3d& target, double duration = -1)
            : _target(target), _duration(duration), _time(0) {
            }

        franka::CartesianPose operator()(const franka::RobotState& robot_state, franka::Duration period);

    protected:
        void _init(const franka::RobotState& robot_state);
        double _traj(double time, double duration) const;

        // initial values (will be initialized in the operator() at t=0)
        Eigen::Affine3d _i_transform;
        Eigen::Vector3d _i_translation;
        Eigen::Quaterniond _i_rotation;
        // target
        Eigen::Affine3d _target;
        Eigen::Quaterniond _target_rotation; // computed from _target

        double _duration;
        double _time;
    };
} // namespace cartesian_franka
#endif


#include <boost/date_time/gregorian/gregorian.hpp>
#include <iostream>

using namespace boost::gregorian;

int main()
{
  date_duration dd{4};
  std::cout << dd.days() << '\n';
  weeks ws{4};
  std::cout << ws.days() << '\n';
  months ms{4};
  std::cout << ms.number_of_months() << '\n';
  years ys{4};
  std::cout << ys.number_of_years() << '\n';
}


#include <gmp_lib/GMP/GMP_MPC.h>
#include <gmp_lib/math/conversions.h>

#include <cfloat> // for infinity

//#include <osqp_lib/osqp.h>
//#include <osqp_lib/constants.h>
//#include <osqp_lib/csc_mat.h>
#include <osqp_lib/qp_solver.h>

#include <boost/numeric/odeint.hpp>
// using namespace boost::numeric::odeint;

#include <iomanip>

namespace as64_
{

namespace gmp_
{

#define GMP_MPC_fun_ std::string("[GMP_MPC::") + __func__ + "]: "

typedef std::array<double,2> ode_state;
// typedef boost::numeric::odeint::euler< ode_state > euler_stepper_type;
typedef boost::numeric::odeint::runge_kutta_cash_karp54< ode_state > error_stepper_type;
typedef boost::numeric::odeint::controlled_runge_kutta< error_stepper_type > controlled_stepper_type;

arma::mat blkdiag(const arma::mat &A, const arma::mat &B)
{
  if (A.empty()) return B;

  arma::mat C(A.n_rows + B.n_rows, A.n_cols + B.n_cols);
  C.submat(0,0, A.n_rows-1, A.n_cols-1) = A;
  C.submat(A.n_rows, A.n_cols, C.n_rows-1, C.n_cols-1) = B;
  return C;
}

arma::mat blkdiag(const arma::mat &A, const arma::mat &B, const arma::mat &C)
{
  return blkdiag( blkdiag(A, B), C );
}

arma::mat blkdiag(const arma::mat &A, double b)
{
  arma::mat C(A.n_rows + 1, A.n_cols + 1);
  if (!A.empty()) C.submat(0,0, A.n_rows-1, A.n_cols-1) = A;
  C.submat(A.n_rows, A.n_cols, C.n_rows-1, C.n_cols-1) = b;
  return C;
}

void addTriplets(std::vector<Eigen::Triplet<double>> &a, const std::vector<Eigen::Triplet<double>> &b, int row_offset=0, int col_offset=0)
{
  for (const Eigen::Triplet<double> &t : b)
    a.push_back( Eigen::Triplet<double>( t.row()+row_offset , t.col()+col_offset, t.value() ) );
}

void addDiagonalTriplets(std::vector<Eigen::Triplet<double>> &coeff, const Eigen::VectorXd &v, int row_offset=0, int col_offset=0)
{
  for (int i=0; i<v.size(); i++) coeff.push_back(Eigen::Triplet<double>(row_offset+i,col_offset+i,v[i]));
}

void addDiagonalTriplets(std::vector<Eigen::Triplet<double>> &coeff, int dim, double value, int row_offset=0, int col_offset=0)
{
  for (int i=0; i<dim; i++) coeff.push_back(Eigen::Triplet<double>(row_offset+i,col_offset+i,value));
}

void addToAineqTriplets(std::vector<Eigen::Triplet<double>> &Aineq_triplets, int n_dof,
    const arma::vec &phi, const arma::vec &phi_dot, const arma::vec &phi_ddot, int row_offset)
{
  int N_kernels = phi.size();

  for (int j=0; j<N_kernels; j++)
  {
    if (phi(j))
    {
      for (int i=0; i<n_dof; i++) Aineq_triplets.push_back( Eigen::Triplet<double>( row_offset+i, j+i*N_kernels, phi(j) ) );
    }
    if (phi_dot(j))
    {
      for (int i=0; i<n_dof; i++) Aineq_triplets.push_back( Eigen::Triplet<double>( row_offset+n_dof+i, j+i*N_kernels, phi_dot(j) ) );
    }
    if (phi_ddot(j))
    {
      for (int i=0; i<n_dof; i++) Aineq_triplets.push_back( Eigen::Triplet<double>( row_offset+2*n_dof+i, j+i*N_kernels, phi_ddot(j) ) );
    }
  }

}

Eigen::SpMat createPsiMat(int n_dof, const arma::vec &phi, const arma::vec &phi_dot)
{
  int N_kernels = phi.size();

  std::vector<Eigen::Triplet<double>> coeff;
  coeff.reserve(N_kernels*n_dof*2);  // upper bound of nonzero elements
  for (int j=0; j<N_kernels; j++)
  {
    if (phi(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( i, j+i*N_kernels, phi(j) ) );
    }
    if (phi_dot(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( n_dof+i, j+i*N_kernels, phi_dot(j) ) );
    }
  }

  Eigen::SpMat sm(3*n_dof, n_dof*N_kernels);
  sm.setFromTriplets(coeff.begin(), coeff.end());
  return sm;
}

Eigen::SpMat GMP_MPC::getAjMat(double s, double s_dot, double s_ddot) const
{
  arma::vec phi = this->gmp_mpc->regressVec(s);
  arma::vec phi_dot = this->gmp_mpc->regressVecDot(s, s_dot);
  arma::vec phi_ddot = this->gmp_mpc->regressVecDDot(s, s_dot, s_ddot);

  std::vector<Eigen::Triplet<double>> coeff;
  coeff.reserve( 3*n_dof*N_kernels ); // upper bound of nonzero elements
  for (int j=0; j<N_kernels; j++)
  {
    if (phi(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( i, j+i*N_kernels, phi(j) ) );
    }
    if (phi_dot(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( n_dof+i, j+i*N_kernels, phi_dot(j) ) );
    }
    if (phi_ddot(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( 2*n_dof+i, j+i*N_kernels, phi_ddot(j) ) );
    }
  }

  Eigen::SpMat sm(3*n_dof, n_dof*N_kernels);
  sm.setFromTriplets(coeff.begin(), coeff.end());
  return sm;
}

// ====================================================================
// ====================================================================
// ====================================================================

GMP_MPC::GMP_MPC(const GMP *gmp, unsigned N_horizon, double pred_time_step, unsigned N_kernels,
                double kernel_std_scaling, const std::array<double,3> &slack_gains, double trunc_kern_thres)
{
  this->inf = OSQP_INFTY;

  this->settings.time_limit = 0;
  this->settings.max_iter = 12000;
  this->settings.abs_tol = 1e-3;
  this->settings.rel_tol = 1e-5;

  this->n_dof = gmp->numOfDoFs();
  this->n_dof3 = 3*n_dof;
  this->I_ndof = Eigen::SpMat(n_dof,n_dof);
  this->I_ndof.setIdentity();

  this->inf_ndof = Eigen::VectorXd(n_dof).setConstant(this->inf);
  this->ones_ndof = Eigen::VectorXd(n_dof).setOnes();
  this->zeros_ndof = Eigen::VectorXd(n_dof).setZero();

  this->gmp_ref = gmp;

  this->gmp_mpc.reset(new GMP(n_dof, N_kernels, kernel_std_scaling));
  this->gmp_mpc->setTruncatedKernels(trunc_kern_thres);

  this->N_kernels = N_kernels;

  this->N = N_horizon;
  this->dt_.resize(this->N);
  for (int i=0; i<dt_.size(); i++) this->dt_[i] = pred_time_step;

  this->pos_slack = slack_gains[0] > 0;
  this->vel_slack = slack_gains[1] > 0;
  this->accel_slack = slack_gains[2] > 0;
  this->n_slack = n_dof * (this->pos_slack + this->vel_slack + this->accel_slack);

  this->n_via = 3; // number of via-points to consider at each optimization window

  this->n_var = n_dof*N_kernels + n_slack;
  this->n_ineq = N*n_dof3 + n_dof3 + n_via*n_dof3 + n_slack; //  pos,vel,accel limits, via-pints, final state and slacks
  this->n_eq = n_dof3; // initial state constraint

  this->Z0 = arma::vec().zeros(n_var);
  this->Z0_dual_ineq = arma::vec().zeros(n_ineq);
  this->Z0_dual_eq = arma::vec().zeros(n_eq);

  Eigen::VectorXd Qp_s, Qv_s, Qa_s;
  // Eigen::SpMat Ap_s, Av_s, Aa_s;
  Aineq_slack_triplets.reserve(3*n_dof);
  if (this->pos_slack)
  {
    Qp_s = slack_gains[0]*ones_ndof;
    for (int i=0; i<n_dof; i++) Aineq_slack_triplets.push_back(Eigen::Triplet<double>(i,i, -1.0));
    //Ap_s = Eigen::join_vert( -I_ndof, Eigen::SpMat(2*n_dof, n_dof) );
  }
  if (this->vel_slack)
  {
    Qv_s = slack_gains[1]*ones_ndof;
    for (int i=n_dof; i<2*n_dof; i++) Aineq_slack_triplets.push_back(Eigen::Triplet<double>(i,i, -1.0));
    // Av_s = Eigen::join_vert( Eigen::SpMat(n_dof, n_dof), -I_ndof, Eigen::SpMat(n_dof, n_dof) );
  }
  if (this->accel_slack)
  {
    Qa_s = slack_gains[2]*ones_ndof;
    for (int i=2*n_dof; i<3*n_dof; i++) Aineq_slack_triplets.push_back(Eigen::Triplet<double>(i,i, -1.0));
    // Aa_s = Eigen::join_vert( Eigen::SpMat(2*n_dof, n_dof), -I_ndof );
  }

  this->Qs_vec.resize(Qp_s.size() + Qv_s.size() + Qa_s.size());
  this->Qs_vec << Qp_s, Qv_s, Qa_s;

  setObjCostGains(1.0, 0.0);

  this->setPosLimits(-inf_ndof, inf_ndof);
  this->setVelLimits(-inf_ndof, inf_ndof);
  this->setAccelLimits(-inf_ndof, inf_ndof);

  this->setPosSlackLimit(this->inf);
  this->setVelSlackLimit(this->inf);
  this->setAccelSlackLimit(this->inf);

  Qf.resize(n_dof3, n_dof3);
  Qf.setIdentity();
  Eigen::VectorXd vf(n_dof3);
  vf << 1e3*ones_ndof, 1e2*ones_ndof, 1e1*ones_ndof;
  Qf.diagonal() = vf;
}

void GMP_MPC::setObjCostGains(double pos_gain, double vel_gain)
{
  if (pos_gain < 0) throw std::runtime_error(GMP_MPC_fun_ + " 'pos_gain' must be non-negative.");
  if (vel_gain < 0) throw std::runtime_error(GMP_MPC_fun_ + " 'vel_gain' must be non-negative.");

  this->sqrt_pos_gain = std::sqrt(pos_gain);
  this->sqrt_vel_gain = std::sqrt(vel_gain);

  sqrt_pos_vel_gain_vec.resize(2*n_dof);
  sqrt_pos_vel_gain_vec << Eigen::VectorXd::Constant(n_dof, sqrt_pos_gain), Eigen::VectorXd::Constant(n_dof, sqrt_vel_gain);

  // this->Qi = Eigen::SpMat(2*n_dof,2*n_dof);
  // this->Qi.setIdentity();
  // this->Qi.diagonal() = v;

  // // this->Qi = blkdiag( pos_gain*I_ndof, vel_gain*I_ndof );
  // this->QN = this->Qi;
}

void GMP_MPC::setInitialState(const arma::vec &y0, const arma::vec &y0_dot, const arma::vec &y0_ddot, double s, double s_dot, double s_ddot)
{
  if (y0.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "y0.size() must be equal to n_dof");
  if (y0_dot.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "y0_dot.size() must be equal to n_dof");
  if (y0_ddot.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "y0_ddot.size() must be equal to n_dof");

  arma::vec phi0_ = this->gmp_mpc->regressVec(s);
  arma::vec phi0_dot_ = this->gmp_mpc->regressVecDot(s, s_dot);
  arma::vec phi0_ddot_ = this->gmp_mpc->regressVecDDot(s, s_dot, s_ddot);

  this->Phi0 = getAjMat(s, s_dot, s_ddot);
  this->x0.resize(3*n_dof);
  this->x0 << Eigen::Map<const Eigen::VectorXd>(y0.memptr(), n_dof),
              Eigen::Map<const Eigen::VectorXd>(y0_dot.memptr(), n_dof),
              Eigen::Map<const Eigen::VectorXd>(y0_ddot.memptr(), n_dof);
}

void GMP_MPC::setFinalState(const arma::vec &yf, const arma::vec &yf_dot, const arma::vec &yf_ddot, double s, double s_dot, double s_ddot)
{
  setFinalState(yf, yf_dot, yf_ddot, s, s_dot, s_ddot, arma::vec().zeros(3) );
}

void GMP_MPC::setFinalState(const arma::vec &yf, const arma::vec &yf_dot, const arma::vec &yf_ddot, double s, double s_dot, double s_ddot, const arma::vec &err_tol)
{

  if (yf.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "yf.size() must be equal to n_dof");
  if (yf_dot.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "yf_dot.size() must be equal to n_dof");
  if (yf_ddot.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "yf_ddot.size() must be equal to n_dof");
  if (err_tol.size() != 3) throw std::runtime_error(GMP_MPC_fun_ + "err_tol.size() must be equal to 3");

  this->s_f = s;

  this->err_tol_f.resize(3*n_dof);
  this->err_tol_f << err_tol(0)*ones_ndof, err_tol(1)*ones_ndof, err_tol(2)*ones_ndof;

  this->phi_f = this->gmp_mpc->regressVec(s);
  this->phi_f_dot = this->gmp_mpc->regressVecDot(s, s_dot);
  this->phi_f_ddot = this->gmp_mpc->regressVecDDot(s, s_dot, s_ddot);

  Phi_f_triplets.clear();
  Phi_f_triplets.reserve(N_kernels*n_dof*3);
  for (int j=0; j<N_kernels; j++)
  {
    if (phi_f(j))
    {
      for (int i=0; i<n_dof; i++) Phi_f_triplets.push_back( Eigen::Triplet<double>( i, j+i*N_kernels, phi_f(j) ) );
    }
    if (phi_f_dot(j))
    {
      for (int i=0; i<n_dof; i++) Phi_f_triplets.push_back( Eigen::Triplet<double>( n_dof+i, j+i*N_kernels, phi_f_dot(j) ) );
    }
    if (phi_f_ddot(j))
    {
      for (int i=0; i<n_dof; i++) Phi_f_triplets.push_back( Eigen::Triplet<double>( 2*n_dof+i, j+i*N_kernels, phi_f_ddot(j) ) );
    }
  }

  Phi_f.resize(3*n_dof, N_kernels*n_dof);
  Phi_f.setFromTriplets(Phi_f_triplets.begin(), Phi_f_triplets.end());

  this->x_f.resize(3*n_dof);
  this->x_f << Eigen::Map<const Eigen::VectorXd>(yf.memptr(), n_dof),
               Eigen::Map<const Eigen::VectorXd>(yf_dot.memptr(), n_dof),
               Eigen::Map<const Eigen::VectorXd>(yf_ddot.memptr(), n_dof);
}

void GMP_MPC::addViaPoint(double s, const arma::vec &y, double err_tol)
{
  if (y.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "y.size() must be equal to n_dof");

  ViaPoint vp(s, Eigen::Map<const Eigen::VectorXd>(y.memptr(),y.size()), err_tol);

  arma::vec phi = gmp_mpc->regressVec(s);
  std::vector<Eigen::Triplet<double>> coeff;
  coeff.reserve(N_kernels*n_dof);
  for (int j=0; j<N_kernels; j++)
  {
    if (phi(j))
    {
      for (int i=0; i<n_dof; i++) coeff.push_back( Eigen::Triplet<double>( i, j+i*N_kernels, phi(j) ) );
    }
  }

  vp.Phi.resize(n_dof, n_dof*N_kernels);
  vp.Phi.setFromTriplets(coeff.begin(), coeff.end());

  vp.z_dual = Eigen::VectorXd::Zero(this->n_dof3);

  // find where to insert the new via-point, so that they are sorted
  unsigned n = via_points.size();
  auto it = via_points.rbegin();
  while (it != via_points.rend()) // search backwards, since new via-points will probably refer to future time
  {
    if (s > it->s) break;
    it++;
  }
  via_points.insert(it.base(), vp); // base returns one element after 'it'
}

// -----------------------------------------------

void GMP_MPC::setPosLimits(const arma::vec &lb, const arma::vec &ub)
{
  Eigen::Map<const Eigen::VectorXd> lb_(lb.memptr(), lb.size());
  Eigen::Map<const Eigen::VectorXd> ub_(ub.memptr(), ub.size());
  setPosLimits(lb_, ub_);
}

void GMP_MPC::setPosLimits(const Eigen::VectorXd &lb, const Eigen::VectorXd &ub)
{
  if (lb.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "lb.size() must be equal to n_dof");
  if (ub.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "ub.size() must be equal to n_dof");

  this->pos_lb = lb;
  this->pos_ub = ub;
}

// -----------------------------------------------

void GMP_MPC::setVelLimits(const arma::vec &lb, const arma::vec &ub)
{
  Eigen::Map<const Eigen::VectorXd> lb_(lb.memptr(), lb.size());
  Eigen::Map<const Eigen::VectorXd> ub_(ub.memptr(), ub.size());
  setVelLimits(lb_, ub_);
}

void GMP_MPC::setVelLimits(const Eigen::VectorXd &lb, const Eigen::VectorXd &ub)
{
  if (lb.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "lb.size() must be equal to n_dof");
  if (ub.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "ub.size() must be equal to n_dof");

  this->vel_lb = lb;
  this->vel_ub = ub;
}

// -----------------------------------------------

void GMP_MPC::setAccelLimits(const arma::vec &lb, const arma::vec &ub)
{
  Eigen::Map<const Eigen::VectorXd> lb_(lb.memptr(), lb.size());
  Eigen::Map<const Eigen::VectorXd> ub_(ub.memptr(), ub.size());
  setAccelLimits(lb_, ub_);
}

void GMP_MPC::setAccelLimits(const Eigen::VectorXd &lb, const Eigen::VectorXd &ub)
{
  if (lb.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "lb.size() must be equal to n_dof");
  if (ub.size() != this->n_dof) throw std::runtime_error(GMP_MPC_fun_ + "ub.size() must be equal to n_dof");

  this->accel_lb = lb;
  this->accel_ub = ub;
}

// -----------------------------------------------

void GMP_MPC::setPosSlackLimit(double s_lim)
{
  if (this->pos_slack) this->pos_slack_lim = s_lim*ones_ndof;
}

void GMP_MPC::setVelSlackLimit(double s_lim)
{
  if (this->vel_slack) this->vel_slack_lim = s_lim*ones_ndof;
}

void GMP_MPC::setAccelSlackLimit(double s_lim)
{
  if (this->accel_slack) this->accel_slack_lim = s_lim*ones_ndof;
}

void GMP_MPC::setCanonicalSystemFunction(std::function<std::array<double,2>(double, double)> can_sys_fun)
{
  this->can_sys_fun = can_sys_fun;
}

void GMP_MPC::integratePhase(double s, double s_dot, std::vector<double> &si_data, std::vector<double> &si_dot_data, std::vector<double> &si_ddot_data)
{
  si_data.resize(N);
  si_dot_data.resize(N);
  si_ddot_data.resize(N);

  std::vector<double> ti(this->N);
  ti[0] = 0;
  for (int i=1; i<ti.size(); i++) ti[i] = ti[i-1] + this->dt_[i-1];
  auto ode_fun = [this](const ode_state &state, ode_state &state_dot, double t)
                          { state_dot = this->can_sys_fun(state[0], state[1]); };
  controlled_stepper_type controlled_stepper;
  ode_state state = {s, s_dot};
  for (int i=0; i<ti.size()-1; i++)
  {
    si_data[i] = state[0];
    si_dot_data[i] = state[1];
    si_ddot_data[i] = ( this->can_sys_fun(state[0], state[1]) )[1];
    boost::numeric::odeint::integrate_adaptive(controlled_stepper, ode_fun, state, ti[i], ti[i+1], ti[i+1] - ti[i]);
  }
  si_data[this->N-1] = state[0];
  si_dot_data[this->N-1] = state[1];
  si_ddot_data[this->N-1] = ( this->can_sys_fun(state[0], state[1]) )[1];
}

GMP_MPC::Solution GMP_MPC::solve(double s, double s_dot)
{
  #ifdef GMP_MPC_L0G_STATISTICS
    timer.tic();
  #endif

  if (!this->can_sys_fun)
    throw std::runtime_error(GMP_MPC_fun_ + "The canonical system function has to be set at least once!");

  arma::vec y = arma::vec(&x0(0), n_dof, false);
  arma::vec y_dot = arma::vec(&x0(n_dof), n_dof, false);
  arma::vec y_ddot = arma::vec(&x0(2*n_dof), n_dof, false);
  arma::vec slack_var = arma::vec().zeros(n_slack);

  unsigned N_kernels = this->gmp_mpc->numOfKernels();

  Eigen::VectorXd q = Eigen::VectorXd::Zero(n_var);

  Eigen::VectorXd lb_i(3*n_dof);
  lb_i << this->pos_lb, this->vel_lb, this->accel_lb;
  Eigen::VectorXd ub_i(3*n_dof);
  ub_i << this->pos_ub, this->vel_ub, this->accel_ub;

  Eigen::VectorXd slack_lim(n_slack);
  slack_lim << this->pos_slack_lim, this->vel_slack_lim, this->accel_slack_lim;

  // Eigen::SpMat Hi(n,n);
  std::vector<Eigen::Triplet<double>> H_triplets;
  H_triplets.reserve( N*n_var*n_var );

  Eigen::VectorXd qi = Eigen::VectorXd::Zero(n_var);

  std::vector<Eigen::Triplet<double>> Aineq_triplets;
  Aineq_triplets.reserve( n_ineq * n_var );

  //if (this->n_slack) Hi.submat(n-n_slack, n-n_slack, n-1, n-1) = this->Q_slack;

  // DMP phase variable
  std::vector<double> si_data, si_dot_data, si_ddot_data;
  integratePhase(s, s_dot, si_data, si_dot_data, si_ddot_data);
  double s_ddot = ( this->can_sys_fun(s, s_dot) )[1];

  #ifdef GMP_MPC_L0G_STATISTICS
    timer_H.tic();
  #endif

  for (int i=0; i<this->N; i++)
  {
    double si = si_data[i];
    double si_dot = si_dot_data[i];
    double si_ddot = si_ddot_data[i];

    arma::vec yd_i_ = this->gmp_ref->getYd(si);
    arma::vec dyd_i_ = this->gmp_ref->getYdDot(si, si_dot);
    Eigen::Map<Eigen::VectorXd> yd_i(yd_i_.memptr(), n_dof);
    Eigen::Map<Eigen::VectorXd> dyd_i(dyd_i_.memptr(), n_dof);

    if (si >= 1)
    {
      yd_i = x_f.segment(0,n_dof);
      dyd_i = x_f.segment(n_dof, n_dof);
    }

    arma::vec phi = this->gmp_mpc->regressVec(si);
    arma::vec phi_dot = this->gmp_mpc->regressVecDot(si, si_dot);
    arma::vec phi_ddot = this->gmp_mpc->regressVecDDot(si, si_dot, si_ddot);

    // Eigen::SpMat Qi_;
    // if (i==this->N-1) Qi_ = this->QN;
    // else Qi_ = this->Qi;

    Eigen::SpMat Psi_gain = createPsiMat(n_dof, sqrt_pos_gain*phi, sqrt_vel_gain*phi_dot);
    Eigen::VectorXd xd_i(2*n_dof);
    xd_i << yd_i, dyd_i;

    // Eigen::SpMat Hi_ = Psi.transpose()*Qi_*Psi;
    Eigen::SpMat Hi_ = Psi_gain.transpose()*Psi_gain;

    Eigen::appendTriplets(H_triplets, Hi_);
    // Hi.submat(0,0, n-n_slack-1, n-n_slack-1) =
    // H = H + Hi;

    // std::cout << "Hi_:\n" << std::setprecision(2) << std::setw(4) << Hi_.toDense() << "\n";

    //qi.segment(0, n_var-n_slack) = -Psi.transpose()*Qi_*xd_i;
    qi.segment(0, n_var-n_slack) = -Psi_gain.transpose()*(xd_i.cwiseProduct(sqrt_pos_vel_gain_vec));
    q = q + qi;

    // arma::mat Aineq_i = arma::join_vert( arma::kron(I_ndof,phi.t()), arma::kron(I_ndof,phi_dot.t()), arma::kron(I_ndof,phi_ddot.t()) );
    // Aineq.rows(i*n_dof3, (i+1)*n_dof3-1) = arma::join_horiz(Aineq_i, this->Aineq_slack);
    addToAineqTriplets(Aineq_triplets, n_dof, phi, phi_dot, phi_ddot, i*n_dof3);

    // triplets for identity to insert slacks in constraints: [Aj I] * [w; s]
    addTriplets(Aineq_triplets, Aineq_slack_triplets, i*n_dof3, n_var-n_slack);

    // si = si + si_dot*this->dt_(i-1);
    // si_dot = si_dot + si_ddot*this->dt_(i-1);
    // si_ddot = ... (if it changes too)
  }

  // =======  initial state constraint  =======
  Eigen::SpMat Aeq = Eigen::join_horiz( this->Phi0, Eigen::SpMat(n_dof3, this->n_slack) );
  Eigen::VectorXd beq = this->x0;

  // =======  final state constraint  =======
  // Aineq = arma::join_vert(Aineq, arma::join_horiz(this->Phi_f, arma::mat().zeros(n_dof3, this->n_slack) ) );
  addTriplets(Aineq_triplets, Phi_f_triplets, N*n_dof3, 0);
  // addToAineqTriplets(Aineq_triplets, n_dof, phi_f, phi_f_dot, phi_f_ddot, N*n_dof3);
  Eigen::VectorXd lb_f = this->x_f - this->err_tol_f;
  Eigen::VectorXd ub_f = this->x_f + this->err_tol_f;

  // add final state to cost function too, to help the optimizer
  Eigen::SpMat Hi_ = Phi_f.transpose()*Qf*Phi_f;
  Eigen::appendTriplets(H_triplets, Hi_);
  q.segment(0, n_var - n_slack) -= Phi_f.transpose()*Qf*this->x_f;

  // =======  process via-points  =======

  // discard past (non active) via-points
  for (auto it = via_points.begin(); it != via_points.end(); )
  {
    if ( it->s < s ) it = via_points.erase(it);
    else it++;
  }

  // consider only up to the first 'n_via' active via-points
  unsigned n_vp = std::min( (size_t)n_via, this->via_points.size() );

  Eigen::VectorXd lb_v = -inf*Eigen::VectorXd::Ones(n_via * n_dof3);
  Eigen::VectorXd  ub_v = -lb_v;

  auto vp_it = via_points.begin();
  for (int i=0; i<n_vp; i++)
  {
      ViaPoint vp = *vp_it++;
      arma::vec phi = this->gmp_mpc->regressVec(vp.s);
      arma::vec phi_dot = this->gmp_mpc->regressVecDot(vp.s, s_dot);
      arma::vec phi_ddot = this->gmp_mpc->regressVecDDot(vp.s, s_dot, 0); // ? s_ddot

      addToAineqTriplets(Aineq_triplets, n_dof, phi, phi_dot, phi_ddot, (N+1+i)*n_dof3);
      lb_v.segment(i*n_dof3, n_dof3) << (vp.pos.array() - vp.err_tol), vel_lb, accel_lb;
      ub_v.segment(i*n_dof3, n_dof3) << (vp.pos.array() + vp.err_tol), vel_ub, accel_ub;

      // add via-point to cost function too, to help the optimizer
      double Qi_ = 1e3;
      Eigen::appendTriplets(H_triplets, vp.Phi.transpose()*(Qi_*vp.Phi) );
      q.segment(0, n_var - n_slack) -= vp.Phi.transpose()*(Qi_*vp.pos);
  }

  // // =======  expand/shrink dual solution guess, in case constraints were added/removed =======
  // int n_ineq_plus = Aineq.n_rows - this->Z0_dual_ineq.size();
  // if (n_ineq_plus>0) this->Z0_dual_ineq = arma::join_vert(this->Z0_dual_ineq, arma::vec().zeros(n_ineq_plus) );
  // else this->Z0_dual_ineq.resize(Aineq.n_rows);

  // add diagonal to H for numerical stability
  addDiagonalTriplets(H_triplets, n_var, 1e-6);
  addDiagonalTriplets(H_triplets, N*Qs_vec, n_var-n_slack, n_var-n_slack);
  Eigen::SpMat H(n_var, n_var);
  H.setFromTriplets(H_triplets.begin(), H_triplets.end());
  // H.setIdentity();
  // H.diagonal() *= 1e-6; // for numerical stability

  // triplets for slack variable limits
  addDiagonalTriplets(Aineq_triplets, n_slack, 1.0, n_ineq-n_slack, n_var-n_slack);
  // if (this->n_slack)
  // {
  //   int i_end = Aineq.rows() - 1;
  //   int j_end = Aineq.cols() - 1;
  //   // identity matrix for slack variable bounds
  //   Aineq.submat(i_end-this->n_slack+1, j_end-this->n_slack+1, i_end, j_end) = arma::mat().eye(this->n_slack,this->n_slack);
  // }

  Eigen::SpMat Aineq(n_ineq, n_var);
  Aineq.setFromTriplets(Aineq_triplets.begin(), Aineq_triplets.end());

  Eigen::VectorXd lb(n_ineq);
  lb << lb_i.replicate(this->N,1), lb_f, lb_v, -slack_lim;
  Eigen::VectorXd ub(n_ineq);
  ub << ub_i.replicate(this->N,1), ub_f, ub_v, slack_lim;

  Eigen::MatrixXd lb_A_ub( lb.rows() , lb.cols() + Aineq.cols() + ub.cols() );
  lb_A_ub << lb, Aineq.toDense(), ub;
  Eigen::MatrixXd Aeq_beq( beq.rows() , beq.cols() + Aeq.cols() );
  Aeq_beq << Aeq.toDense(), beq;

  // std::cout << "========== QP problem ============\n";
  // std::cout << std::setprecision(2) << std::setw(4)
  //           << "H: \n" << H.toDense() << "\n"
  //           << "q: \n" << q.transpose() << "\n"
  //           << "lb < A < ub :\n" << lb_A_ub << "\n"
  //           << "Aeq = beq: \n" << Aeq_beq << "\n";

  // exit(-1);

  #ifdef GMP_MPC_L0G_STATISTICS
    calcH_elaps_t_data.push_back(timer_H.toc() * 1000);
  #endif

  #ifdef GMP_MPC_L0G_STATISTICS
    if (s > s_rec)
    {
      s_rec = 1e10;
      this->H_ = H;
      this->A_ = Aineq;
    }
  #endif

  // ===========  solve optimization problem  ==========
  #ifdef GMP_MPC_L0G_STATISTICS
    timer_solve.tic();
  #endif

  osqp_::QPsolver solver(H, q, Aineq, lb, ub, Aeq, beq);
  solver.setPrimalSolutionGuess(this->Z0);
  solver.setDualSolutionGuess(this->Z0_dual_ineq, this->Z0_dual_eq);

  solver.settings->time_limit = this->settings.time_limit;
  solver.settings->max_iter = this->settings.max_iter;
  solver.settings->eps_abs = this->settings.abs_tol;
  solver.settings->eps_rel = this->settings.rel_tol;
  solver.settings->warm_start = false;
  solver.settings->verbose = false;

  int exit_flag = solver.solve();

  #ifdef GMP_MPC_L0G_STATISTICS
    solve_elaps_t_data.push_back( timer_solve.toc()*1000 );
  #endif

  std::string exit_msg = solver.getExitMsg();
  if (exit_flag >= 0)
  {
    // =======  extract the solution  =======
    arma::vec X = solver.getPrimalSolution();
    arma::vec w = X.subvec(0, X.n_elem-1-this->n_slack);
    if (this->n_slack) slack_var = X.subvec(X.n_elem-this->n_slack, X.n_elem-1);
    this->W_mpc = arma::reshape(w, N_kernels, n_dof).t();

    this->Z0 = X;
    this->Z0_dual_ineq = solver.getIneqDualSolution();
    this->Z0_dual_eq = solver.getEqDualSolution();

    // =======  Generate optimal output  =======
    y = this->getYd(s);
    y_dot = this->getYdDot(s, s_dot);
    y_ddot = this->getYdDDot(s, s_dot, s_ddot);

    if (this->n_slack)
    {
      // check, because if the optimization finishes prematurely,
      // sometimes it can happen that the slacks are violated...
      // Not sure why this happens...?
      double max_violation = arma::max( arma::abs(slack_var) - arma::vec(slack_lim.data(), slack_lim.size(), false) );
      if ( max_violation > 5e-4 )
      {
        exit_msg = "Slack variable limits violated: max violation = " + std::to_string(max_violation);
        exit_flag = -1;
        goto end_solve;
      }
    }

    this->setInitialState(y, y_dot, y_ddot, s, s_dot, s_ddot);
  }

  #ifdef GMP_MPC_L0G_STATISTICS
    elaps_t_data.push_back( timer.toc()*1000 );
  #endif

  end_solve:
  Solution sol;
  sol.y = y;
  sol.y_dot = y_dot;
  sol.y_ddot = y_ddot;
  int i1=0, i2=n_dof-1;
  if (pos_slack)
  {
    sol.pos_slack = slack_var.subvec(i1,i2);
    i1 += n_dof;
    i2 += n_dof;
  }
  if (vel_slack)
  {
    sol.vel_slack = slack_var.subvec(i1,i2);
    i1 += n_dof;
    i2 += n_dof;
  }
  if (accel_slack) sol.accel_slack = slack_var.subvec(i1,i2);
  sol.exit_flag = exit_flag;
  sol.exit_msg = exit_msg;
  return sol;
}

arma::vec GMP_MPC::getYd(double s) const
{
  return W_mpc*gmp_mpc->regressVec(s);
}

arma::vec GMP_MPC::getYdDot(double s, double s_dot) const
{
  return W_mpc*gmp_mpc->regressVecDot(s, s_dot);
}

arma::vec GMP_MPC::getYdDDot(double s, double s_dot, double s_ddot) const
{
  return W_mpc*gmp_mpc->regressVecDDot(s, s_dot, s_ddot);
}

} // namespace gmp_

} // namespace as64_


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE set_commitment_component_test

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>

#include <nil/crypto3/zk/components/hashes/sha256/sha256_component.hpp>
#include <nil/crypto3/zk/snark/components/set_commitment/set_commitment_component.hpp>

using namespace nil::crypto3::zk;
using namespace nil::crypto3::algebra;

template<typename FieldT, typename HashT>
void test_set_commitment_component(){

    const std::size_t digest_len = HashT::digest_bits;
    const std::size_t max_set_size = 16;
    const std::size_t value_size = (HashT::block_bits > 0 ? HashT::block_bits : 10);

    set_commitment_accumulator<HashT> accumulator(max_set_size, value_size);

    std::vector<algebra::bit_vector> set_elems;
    for (std::size_t i = 0; i < max_set_size; ++i){
        algebra::bit_vector elem(value_size);
        std::generate(elem.begin(), elem.end(), [&]() { return std::rand() % 2; });
        set_elems.emplace_back(elem);
        accumulator.add(elem);
        BOOST_CHECK(accumulator.is_in_set(elem));
    }

    components::blueprint<FieldT> bp;
    components::blueprint_variable_array<FieldT> element_bits;
    element_bits.allocate(bp, value_size);
    set_commitment_variable<FieldT, HashT> root_digest(bp, digest_len);

    bp_variable<FieldT> check_succesful;
    check_succesful.allocate(bp);

    set_membership_proof_variable<FieldT, HashT> proof(bp, max_set_size);

    set_commitment_component<FieldT, HashT> sc(bp, max_set_size, element_bits, root_digest, proof, check_succesful);
    sc.generate_r1cs_constraints();

    /* test all elements from set */
    for (std::size_t i = 0; i < max_set_size; ++i){
        element_bits.fill_with_bits(bp, set_elems[i]);
        bp.val(check_succesful) = FieldT::one();
        proof.generate_r1cs_witness(accumulator.get_membership_proof(set_elems[i]));
        sc.generate_r1cs_witness();
        root_digest.generate_r1cs_witness(accumulator.get_commitment());
        BOOST_CHECK(bp.is_satisfied());
    }
    std::cout << "membership tests OK" << std::endl;

    /* test an element not in set */
    for (std::size_t i = 0; i < value_size; ++i){
        bp.val(element_bits[i]) = FieldT(std::rand() % 2);
    }

    bp.val(check_succesful) = FieldT::zero(); /* do not require the check result to be successful */
    proof.generate_r1cs_witness(accumulator.get_membership_proof(set_elems[0])); /* try it with invalid proof */
    sc.generate_r1cs_witness();
    root_digest.generate_r1cs_witness(accumulator.get_commitment());
    BOOST_CHECK(bp.is_satisfied());

    bp.val(check_succesful) = FieldT::one(); /* now require the check result to be succesful */
    proof.generate_r1cs_witness(accumulator.get_membership_proof(set_elems[0])); /* try it with invalid proof */
    sc.generate_r1cs_witness();
    root_digest.generate_r1cs_witness(accumulator.get_commitment());
    BOOST_CHECK(!bp.is_satisfied()); /* the blueprint should be unsatisfied */
    std::cout << "non-membership test OK" << std::endl;
}

template<typename CurveType>
void test_all_set_commitment_components() {
    typedef typename CurveType::scalar_field_type scalar_field_type;

    // for now all CRH components are knapsack CRH's; can be easily extended
    // later to more expressive selector types.
    using crh_with_field_out_component = knapsack_crh_with_field_out_component<scalar_field_type>;
    using crh_with_bit_out_component = knapsack_crh_with_bit_out_component<scalar_field_type>;

    test_set_commitment_component<scalar_field_type, crh_with_bit_out_component>();
    test_set_commitment_component<scalar_field_type, sha256_two_to_one_hash_component<scalar_field_type>>();
}

int main(void) {
    test_all_set_commitment_components<curves::bls12<381>>();
    test_all_set_commitment_components<curves::mnt4<298>>();
    test_all_set_commitment_components<curves::mnt6<298>>();
}