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

// Adapted from GLAS implementation by Karl Meerbergen and Toon Knappen

#ifndef MTL_DENSE_VECTOR_INCLUDE
#define MTL_DENSE_VECTOR_INCLUDE

#include <iostream>
#include <cassert>
#include <vector>
#include <algorithm>
#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_integral.hpp>

#include <boost/numeric/mtl/utility/assert.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/ashape.hpp>
#include <boost/numeric/mtl/vector/all_vec_expr.hpp>
#include <boost/numeric/mtl/vector/parameter.hpp>
#include <boost/numeric/mtl/detail/contiguous_memory_block.hpp>
#include <boost/numeric/mtl/vector/crtp_base_vector.hpp>
#include <boost/numeric/mtl/utility/dense_el_cursor.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/utility/is_static.hpp>
#include <boost/numeric/mtl/utility/is_row_major.hpp>
#include <boost/numeric/mtl/utility/transposed_orientation.hpp>
#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#ifdef MTL_WITH_INITLIST
# include <initializer_list>
#endif

namespace mtl { namespace vec {

/// Dense vector
template <class Value, typename Parameters = parameters<> >
class dense_vector
  : public vec_expr<dense_vector<Value, Parameters> >,
    public ::mtl::detail::contiguous_memory_block< Value, Parameters::on_stack, Parameters::dimension::value >,
    public crtp_base_vector< dense_vector<Value, Parameters>, Value, std::size_t >
{
  public:
    typedef dense_vector<Value, Parameters>                                          self;
    typedef ::mtl::detail::contiguous_memory_block< Value, Parameters::on_stack,
                                                    Parameters::dimension::value >   memory_base;
    typedef crtp_base_vector< self, Value, std::size_t >                             crtp_base;
    typedef crtp_vector_assign< self, Value, std::size_t >                           assign_base;
    typedef vec_expr<dense_vector<Value, Parameters> >                               expr_base;
    typedef Value             value_type ;
    typedef std::size_t       size_type ;
    typedef value_type&       reference ;
    typedef value_type const& const_reference ;
    typedef Value*            pointer ;
    typedef Value const*      const_pointer ;
    typedef typename Parameters::orientation  orientation;

    typedef const_pointer     key_type;

    /// Check whether index is non-negative and less than size
    void check_index( size_type MTL_DEBUG_ARG(i) ) const
    {
	MTL_CRASH_IF( is_negative(i) ||  i >= this->used_memory(), "Index out of range!");
    }

    /// Check for a given vector if the sizes are equal or this has size 0 (and can take the size of source)
    void check_dim( size_type MTL_DEBUG_ARG(s) ) const
    {
	MTL_CRASH_IF( this->used_memory() != 0 && this->used_memory() != s, "Incompatible size!");
    }

    /// Check at compile time for a given vector if the sizes are equal
    void static_check( size_type MTL_DEBUG_ARG(s) ) const
    {
	assert(!traits::is_static<self>::value || s == size(typename Parameters::dimension()));
    }

    /// Check if a given vector expression whether it has the same shape as the object
    /** Both must be row vectors or column vectors. The elements must have the same
	algebraic shape, e.g. a row vector of scalars is not compatible with a row
	vector of matrices. **/
    template <class E>
    void check_consistent_shape( vec_expr<E> const& ) const
    {
	MTL_CRASH_IF((!boost::is_same<
			        typename ashape::ashape<self>::type
			      , typename ashape::ashape<E>::type
			    >::value),
			   "Incompatible shape!");
    }

    /// Default constructor
    dense_vector( ) : memory_base( Parameters::dimension::value ) {}

    /// Constructor for size \p n
    explicit dense_vector( size_type n ) : memory_base( n ) { static_check( n ); }

    /// Constructor for size \p n and value \p value that is set in all entries
    explicit dense_vector( size_type n, value_type value )
      : memory_base( n )
    {
	static_check( n );
	std::fill(begin(), end(), value);
    }

    /// Constructor for size \p n and \p address
    /** Can be used to handle vectors from other libraries, even in other languages like Fortran **/
    explicit dense_vector( size_type n, value_type *address )
      : memory_base( address, n, true )
    { static_check( n ); }

    /// Copy constructor
    dense_vector( const self& src )
      : memory_base( src )
    {
	vampir_trace<2042> tracer;
	using std::copy;
	copy(src.begin(), src.end(), this->begin());
    }

#ifdef MTL_WITH_MOVE
    dense_vector(self&& src) : memory_base(std::move(src)) {}
#endif

    /// Clone constructor
    /** Copies every vector, even those that refer to external data, sub-vectors, or rows and columns in a matrix **/
    dense_vector( const self& src, clone_ctor )
      : memory_base( src, clone_ctor()) {}

    struct dummy_type {};

    /// Constructor from vector expressions
    template <typename VectorSrc>
    explicit dense_vector(const VectorSrc& src,
			  typename boost::disable_if<boost::is_integral<VectorSrc>, dummy_type>::type= dummy_type())
    {	vampir_trace<2043> tracer; *this= src;    }

#ifdef MTL_WITH_INITLIST
    /// Constructor for initializer list \p values
    template <typename Value2>
    dense_vector(std::initializer_list<Value2> values)
      : memory_base(values.size())
    {
	static_check(values.size());
	std::copy(values.begin(), values.end(), begin());
    }

    /// Assignment from initializer list \p values
    template <typename Value2>
    self& operator=(std::initializer_list<Value2> values)
    {
	checked_change_dim(values.size());
	std::copy(values.begin(), values.end(), begin());
	return *this;
    }
#endif

    /// Constructor from std::vector; value_type must be identic
    explicit dense_vector(const std::vector<value_type>& src)
      : memory_base(src.size())
    {	std::copy(src.begin(), src.end(), this->begin());    }

    /// Stride is always 1
    size_type stride() const { return 1 ; }

    /// i-th entry
    reference operator()( size_type i )
    {
        check_index(i);
        return this->value_n( i ) ;
    }

    /// i-th entry (constant)
    const_reference operator()( size_type i ) const
    {
        check_index(i);
        return this->value_n( i ) ;
    }

    reference operator[]( size_type i ) { return (*this)( i ) ; } ///< i-th entry
    const_reference operator[]( size_type i ) const { return (*this)( i ) ;  } ///< i-th entry (constant)

    self operator[]( irange r ) { return sub_vector(*this, r.start(), r.finish()); } ///< sub-vector
    const self  operator[]( irange r ) const { return sub_vector(*this, r.start(), r.finish());  } ///< sub-vector

    void delay_assign() const {}

    // Compatibility with STL
    const_pointer begin() const { return this->elements() ; }
    const_pointer end() const { return this->elements() + this->used_memory(); }
    pointer begin() { return this->elements() ; }
    pointer end() { return this->elements() + this->used_memory(); }
    bool empty() const { return this->used_memory() == 0; } ///< Whether it is empty

    /// Address of first data entry; to be used with care.
    value_type* address_data() { return begin(); }
    const value_type* address_data() const { return begin(); }

#ifdef MTL_VECTOR_MOVE_EMULATION
    /// Move assignment
    self& operator=(self src)
    {
	// Self-copy would be an indication of an error
	assert(this != &src);

	checked_change_dim(src.used_memory());
	memory_base::move_assignment(src);
	return *this;
    }
#else
// #if defined(_MSC_VER) && !defined(MTL_VECTOR_MOVE_EMULATION)
    /// Copy assignment
    self& operator=(const self& src)
    {
	if (this == &src)
	    return *this;

	checked_change_dim(src.used_memory());
	memory_base::operator=(src);
	return *this;
    }
#endif

#ifdef MTL_WITH_MOVE
    self& operator=(self&& src)
    {
	checked_change_dim(src.used_memory());
	memory_base::move_assignment(src);
	return *this;
    }
#endif

#if 0 // def __PGI
    using crtp_base::operator=;
#else
    using assign_base::operator=;
#endif

    template <typename Value2> friend void fill(self&, const Value2&);

    /// swap two vectors
    friend void swap(self& vector1, self& vector2)
    {
	swap(static_cast<memory_base&>(vector1), static_cast<memory_base&>(vector2));
    }

    void change_resource(size_type n) { this->realloc(n); } ///< Change resource, like \ref change_dim
    void change_dim(size_type n) { this->realloc(n); } ///< Change dimension of vector
    void checked_change_dim(size_type n) { check_dim(n); change_dim(n); } ///< Only change dim if it was empty before

    void crop() {} ///< Delete structural zeros, only dummy here for completeness

} ; // dense_vector

/// Size of v
template <typename Value, typename Parameters>
inline typename dense_vector<Value, Parameters>::size_type
size(const dense_vector<Value, Parameters>& v)
{ return v.used_memory() ; }

// ================
// Free functions
// ================

/// Fill \p vector with \p value
template <typename Value, typename Parameters, typename Value2>
inline void fill(dense_vector<Value, Parameters>& vector, const Value2& value)
{
    std::fill(vector.begin(), vector.end(), value);
}

template <typename Value, typename Parameters>
typename dense_vector<Value, Parameters>::size_type
inline num_rows_aux(const dense_vector<Value, Parameters>& , tag::row_major)
{
    return 1;
}

template <typename Value, typename Parameters>
typename dense_vector<Value, Parameters>::size_type
inline num_rows_aux(const dense_vector<Value, Parameters>& vector, tag::col_major)
{
    return vector.used_memory();
}

/// Number of rows: is size for column vectors and 1 for row vectors
template <typename Value, typename Parameters>
typename dense_vector<Value, Parameters>::size_type
inline num_rows(const dense_vector<Value, Parameters>& vector)
{
    return num_rows_aux(vector, typename Parameters::orientation());
}

/// Number of columns: is size for row vectors and 1 for column vectors
template <typename Value, typename Parameters>
typename dense_vector<Value, Parameters>::size_type
inline num_cols(const dense_vector<Value, Parameters>& vector)
{
    return num_rows_aux(vector, typename mtl::traits::transposed_orientation<typename Parameters::orientation>::type());
}

/// Sub-vector function, more convenient with irange
template <typename Value, typename Parameters>
dense_vector<Value, Parameters>
inline sub_vector(dense_vector<Value, Parameters>& v,
		  typename dense_vector<Value, Parameters>::size_type start,
		  typename dense_vector<Value, Parameters>::size_type finish)
{
    typedef dense_vector<Value, Parameters>    Vector;

    MTL_CRASH_IF( is_negative(start) || is_negative(finish), "Index out of range!");
    irange r= intersection(irange(start, finish), irange(0, mtl::vec::size(v)));
    return r.empty() ? Vector() : Vector(r.size(), &v[r.start()]);

#if 0
    finish= min(finish, size(v));
    start= min(start, finish); // implies min(start, size(v))
    return start < finish ? Vector(finish - start, &v[start]) : Vector();
#endif
}

template <typename Value, typename Parameters>
const dense_vector<Value, Parameters>
inline sub_vector(const dense_vector<Value, Parameters>& v,
		  typename dense_vector<Value, Parameters>::size_type start,
		  typename dense_vector<Value, Parameters>::size_type finish)
{
    typedef dense_vector<Value, Parameters>    Vector;
    return sub_vector(const_cast<Vector&>(v), start, finish);
}

}} // namespace mtl::vector

namespace mtl {

    // Enable cloning of dense vectors
    template <typename Value, typename Parameters>
    struct is_clonable< vec::dense_vector<Value, Parameters> > : boost::mpl::true_ {};

} // namespace mtl

namespace mtl { namespace traits {

// ================
// Range generators
// For cursors
// ================

    template <typename Value, class Parameters>
    struct range_generator<tag::all, mtl::vec::dense_vector<Value, Parameters> >
      : public detail::dense_element_range_generator<mtl::vec::dense_vector<Value, Parameters>,
						     dense_el_cursor<Value>, complexity_classes::linear_cached>
    {};

    template <typename Value, class Parameters>
    struct range_generator<tag::nz, mtl::vec::dense_vector<Value, Parameters> >
	: public range_generator<tag::all, mtl::vec::dense_vector<Value, Parameters> >
    {};

    template <typename Value, class Parameters>
    struct range_generator<tag::iter::all, mtl::vec::dense_vector<Value, Parameters> >
    {
	typedef mtl::vec::dense_vector<Value, Parameters>   collection_t;
	typedef complexity_classes::linear_cached complexity;
	static int const                          level = 1;
	typedef typename collection_t::pointer    type;

	type begin(collection_t& collection)
	{
	    return collection.begin();
	}
	type end(collection_t& collection)
	{
	    return collection.end();
	}
    };

    template <typename Value, class Parameters>
    struct range_generator<tag::iter::nz, mtl::vec::dense_vector<Value, Parameters> >
	: public range_generator<tag::iter::all, mtl::vec::dense_vector<Value, Parameters> >
    {};

    template <typename Value, class Parameters>
    struct range_generator<tag::const_iter::all, mtl::vec::dense_vector<Value, Parameters> >
    {
	typedef mtl::vec::dense_vector<Value, Parameters>   collection_t;
	typedef complexity_classes::linear_cached complexity;
	static int const                          level = 1;
	typedef typename collection_t::const_pointer type;

	type begin(const collection_t& collection) const
	{
	    return collection.begin();
	}
	type end(const collection_t& collection) const
	{
	    return collection.end();
	}
    };

    template <typename Value, class Parameters>
    struct range_generator<tag::const_iter::nz, mtl::vec::dense_vector<Value, Parameters> >
	: public range_generator<tag::const_iter::all, mtl::vec::dense_vector<Value, Parameters> >
    {};

}} // namespace mtl::traits

namespace mtl {
    using vec::dense_vector;
}

#endif // MTL_DENSE_VECTOR_INCLUDE


text:
#define BOOST_TEST_MODULE VR
#include <boost/test/unit_test.hpp>

#include "odil/Exception.h"
#include "odil/VR.h"

BOOST_AUTO_TEST_CASE(as_string)
{
    odil::VR const vr(odil::VR::AT);
    std::string const string = odil::as_string(vr);
    BOOST_CHECK_EQUAL(string, "AT");
}

BOOST_AUTO_TEST_CASE(as_string_invalid)
{
    odil::VR const vr(odil::VR::INVALID);
    BOOST_CHECK_THROW(odil::as_string(vr), odil::Exception);
}

BOOST_AUTO_TEST_CASE(as_vr)
{
    std::string const string("AT");
    odil::VR const vr = odil::as_vr(string);
    BOOST_CHECK(vr == odil::VR::AT);
}

BOOST_AUTO_TEST_CASE(as_vr_wrong)
{
    std::string const string("XX");
    BOOST_CHECK_THROW(odil::as_vr(string), odil::Exception);
}

ext: cpp

max_stars_repo_name: genisysram/odil

max_issues_repo_name: genisysram/odil

max_forks_repo_name: genisysram/odil


text:
#include <boost/text/collate.hpp>
#include <boost/text/text.hpp>
#include <boost/text/data/da.hpp>

#include <boost/container/flat_set.hpp>
#include <boost/container/flat_map.hpp>

#include <benchmark/benchmark.h>

#include <iostream>
#include <set>

#define BE_REASONABLE 1
#if BE_REASONABLE
template<typename T, typename Cmp = std::less<T>>
using set_t = boost::container::flat_multiset<T, Cmp>;
template<typename Key, typename T>
using map_t = boost::container::flat_multimap<Key, T>;
#else
template<typename T, typename Cmp = std::less<T>>
using set_t = std::multiset<T, Cmp>;
template<typename Key, typename T>
using map_t = std::multimap<Key, T>;
#endif

//[ collation_text_cmp_naive
struct text_cmp
{
    bool operator()(
        boost::text::text const & lhs, boost::text::text const & rhs) const
        noexcept
    {
        // Binary comparison of code point values.
        return std::lexicographical_compare(
            lhs.begin().base(),
            lhs.end().base(),
            rhs.begin().base(),
            rhs.end().base());
    }
};
//]

//[ collation_text_cmp
struct text_cmp_2
{
    bool operator()(
        boost::text::text const & lhs, boost::text::text const & rhs) const
        noexcept
    {
        // Binary comparison of code point values.
        return std::lexicographical_compare(
            lhs.begin().base().base(),
            lhs.end().base().base(),
            rhs.begin().base().base(),
            rhs.end().base().base());
    }
};
//]

//[ collation_text_coll_cmp
struct text_coll_cmp
{
    bool operator()(
        boost::text::text const & lhs, boost::text::text const & rhs) const
        noexcept
    {
        return boost::text::collate(lhs, rhs, table) < 0;
    }

    boost::text::collation_table table; // Cheap to copy.
};
//]

std::vector<boost::text::text> make_texts()
{
    std::vector<boost::text::text> retval;
    for (int i = 0; i < 2 << 15; ++i) {
        retval.push_back(boost::text::text(std::to_string(i)));
    }
    return retval;
}
std::vector<boost::text::text> const texts = make_texts();

boost::text::collation_table default_table =
    boost::text::default_collation_table();

void BM_set_inserts_binary_compare_text_naive(benchmark::State & state)
{
    while (state.KeepRunning()) {
        set_t<boost::text::text, text_cmp> set;
        for (int i = 0, end = state.range(0); i < end; ++i) {
            benchmark::DoNotOptimize(set.insert(texts[i]));
        }
    }
}

void BM_set_inserts_binary_compare_text(benchmark::State & state)
{
    while (state.KeepRunning()) {
        set_t<boost::text::text, text_cmp_2> set;
        for (int i = 0, end = state.range(0); i < end; ++i) {
            benchmark::DoNotOptimize(set.insert(texts[i]));
        }
    }
}

void BM_set_inserts_collate(benchmark::State & state)
{
    while (state.KeepRunning()) {
        set_t<boost::text::text, text_coll_cmp> set(
            text_coll_cmp{default_table});
        for (int i = 0, end = state.range(0); i < end; ++i) {
            benchmark::DoNotOptimize(set.insert(texts[i]));
        }
    }
}

void BM_map_inserts(benchmark::State & state)
{
    while (state.KeepRunning()) {
        map_t<boost::text::text_sort_key, boost::text::text> map;
        for (int i = 0, end = state.range(0); i < end; ++i) {
            benchmark::DoNotOptimize(map.emplace(
                boost::text::collation_sort_key(texts[i], default_table),
                texts[i]));
        }
    }
}

BENCHMARK(BM_set_inserts_binary_compare_text_naive)->Range(16, 2 << 12);
BENCHMARK(BM_set_inserts_binary_compare_text)->Range(16, 2 << 12);
BENCHMARK(BM_set_inserts_collate)->Range(16, 2 << 12);
BENCHMARK(BM_map_inserts)->Range(16, 2 << 12);

BENCHMARK_MAIN()

max_stars_repo_name: eightysquirrels/text

max_issues_repo_name: eightysquirrels/text

max_forks_repo_name: eightysquirrels/text


text:
/*
 * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 * Copyright 2020 ANYbotics AG
 */

#include <algorithm>
#include <memory>
#include <numeric>
#include <vector>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "copra/LMPC.h"
#include "copra/constraints.h"
#include "copra/costFunctions.h"
#include "copra/solvers/all.h"
#include "copra/systems/TimeInvariantSystem.h"

#include "time_invariant_systems.h"
#include "tools.h"

/********************************************************************************************************
 *                               Check Bound constraint                                                 *
 ********************************************************************************************************/

TEST_F(BoundedSystem, mpcTargetCostWithBoundConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    ps->update();

    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver) {
            controller.useSolver(std::move(solver));
        } else {
            controller.selectQPSolver(sFlag);
        }
        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constraints
        ASSERT_LE(posTraj.maxCoeff(), x0(0));
        ASSERT_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
        ASSERT_LE(control.maxCoeff(), uUpper(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(BoundedSystem, mpcTrajectoryCostWithBoundConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

#ifdef EIGEN_OSQP_FOUND
    // Increase precision
    if (sFlag == copra::SolverFlag::OSQP) {
        auto& bs = static_cast<copra::OSQPSolver&>(controller.solver()).baseSolver();
        bs.scalingIter(0);
        bs.absConvergenceTol(1e-6);
        bs.relConvergenceTol(1e-6);
        bs.primalInfeasibilityTol(1e-7);
        bs.dualInfeasibilityTol(1e-7);
    }
#endif

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check system dynamics on full solution
        for (int k = 0; 2 * (k + 1) + 1 < trajLen; k++) {
            Eigen::VectorXd x_k = fullTraj.segment<2>(2 * k);
            Eigen::VectorXd x_next = fullTraj.segment<2>(2 * (k + 1));
            ASSERT_LE((x_next - A * x_k - B * control(k) - c).norm(), 1e-10);
        }

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constraints
        ASSERT_LE(posTraj.maxCoeff(), x0(0));  // actually not an explicit constraint
        ASSERT_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
        ASSERT_LE(control.maxCoeff(), uUpper(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(BoundedSystem, mpcMixedCostWithBoundConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd); // min(||X - Xt||^2)
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud); // min(||U - Ut||^2)
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    ASSERT_TRUE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check system dynamics on full solution
    for (int k = 0; 2 * (k + 1) + 1 < trajLen; k++) {
        Eigen::VectorXd x_k = fullTraj.segment<2>(2 * k);
        Eigen::VectorXd x_next = fullTraj.segment<2>(2 * (k + 1));
        ASSERT_LE((x_next - A * x_k - B * control(k) - c).norm(), 1e-10);
    }

    // Check result
    EXPECT_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    ASSERT_LE(posTraj.maxCoeff(), x0(0));
    ASSERT_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
    ASSERT_LE(control.maxCoeff(), uUpper(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
}

/********************************************************************************************************
 *                                      Check Simple cost                                               *
 ********************************************************************************************************/

TEST_F(BoundedSystem, mpcSimpleTrajectoryCostEquivalenceTest) {  // NOLINT
  Eigen::VectorXd trajectory1 ;
  Eigen::VectorXd trajectory2;
  Eigen::VectorXd control1;
  Eigen::VectorXd control2;

  {
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    N.setIdentity();
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);
    ASSERT_TRUE(controller.solve());
    trajectory1 = controller.trajectory();
    control1 = controller.trajectory();
  }

  {
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::SimpleControlCost>(ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);
    ASSERT_TRUE(controller.solve());
    trajectory2 = controller.trajectory();
    control2 = controller.trajectory();
  }

  ASSERT_TRUE(trajectory1.isApprox(trajectory2));
  ASSERT_TRUE(control1.isApprox(control2));

}

TEST_F(BoundedSystem, mpcSimpleControlCostEquivalenceTest) {  // NOLINT
  Eigen::VectorXd trajectory1 ;
  Eigen::VectorXd trajectory2;
  Eigen::VectorXd control1;
  Eigen::VectorXd control2;

  {
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    M.setIdentity();
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);
    ASSERT_TRUE(controller.solve());
    trajectory1 = controller.trajectory();
    control1 = controller.trajectory();
  }

  {
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::SimpleTrajectoryCost>(xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
    auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);
    ASSERT_TRUE(controller.solve());
    trajectory2 = controller.trajectory();
    control2 = controller.trajectory();
  }

  ASSERT_TRUE(trajectory1.isApprox(trajectory2));
  ASSERT_TRUE(control1.isApprox(control2));

}

/********************************************************************************************************
 *                            Check inequality constraint                                               *
 ********************************************************************************************************/

TEST_F(IneqSystem, mpcTargetCostWithInequalityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

#ifdef EIGEN_OSQP_FOUND
    // Increase precision
    if (sFlag == copra::SolverFlag::OSQP) {
        auto& bs = static_cast<copra::OSQPSolver&>(controller.solver()).baseSolver();
        bs.scalingIter(0);
        bs.absConvergenceTol(1e-6);
        bs.relConvergenceTol(1e-6);
        bs.primalInfeasibilityTol(1e-7);
        bs.dualInfeasibilityTol(1e-7);
        bs.relaxationParam(1);
        bs.maxIter(6000);
    }
#endif

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0));
        ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
        ASSERT_LE(control.maxCoeff(), h(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(IneqSystem, mpcTrajectoryCostWithInequalityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

#ifdef EIGEN_OSQP_FOUND
    // Increase precision
    if (sFlag == copra::SolverFlag::OSQP) {
        auto& bs = static_cast<copra::OSQPSolver&>(controller.solver()).baseSolver();
        bs.scalingIter(0);
        bs.absConvergenceTol(1e-6);
        bs.relConvergenceTol(1e-6);
        bs.primalInfeasibilityTol(1e-7);
        bs.dualInfeasibilityTol(1e-7);
    }
#endif

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0));
        ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
        ASSERT_LE(control.maxCoeff(), h(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(IneqSystem, mpcMixedCostWithInequalityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
    auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);
    controller.addConstraint(contConstr);

    ASSERT_TRUE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    EXPECT_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    ASSERT_LE(posTraj.maxCoeff(), x0(0));
    ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    ASSERT_LE(control.maxCoeff(), h(0) + 1e-6); // QuadProg allows to exceeds the constrain of a small amount.
}

/********************************************************************************************************
 *                               Check Mixed constraint                                                 *
 ********************************************************************************************************/

TEST_F(MixedSystem, mpcTargetCostWithMixedConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0));
        for (int i = 0; i < nbStep; ++i) {
            auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
            if (!(res(0) <= p(0) + 1e-6)) {
                ASSERT_TRUE(false) << "Mixed constraint violated!";
            }
        }
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(MixedSystem, mpcTrajectoryCostWithMixedConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0));
        for (int i = 0; i < nbStep; ++i) {
            auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
            if (!(res(0) <= p(0) + 1e-6)) {
                ASSERT_TRUE(false) << "Mixed constraint violated!";
            }
        }
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(MixedSystem, mpcMixedCostWithMixedConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(mixedConstr);

    ASSERT_TRUE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    EXPECT_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    ASSERT_LE(posTraj.maxCoeff(), x0(0));
    for (int i = 0; i < nbStep; ++i) {
        auto res = E * fullTraj.segment(i * E.cols(), E.cols()) + G * control.segment(i * G.cols(), G.cols());
        if (!(res(0) <= p(0) + 1e-6)) {
            ASSERT_TRUE(false) << "Mixed constraint violated!";
        }
    }
}

/********************************************************************************************************
 *                              Check Equality constraint                                               *
 ********************************************************************************************************/

TEST_F(EqSystem, mpcTargetCostWithEqualityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TargetCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

#ifdef EIGEN_OSQP_FOUND
    // Increase precision
    if (sFlag == copra::SolverFlag::OSQP) {
        auto& bs = static_cast<copra::OSQPSolver&>(controller.solver()).baseSolver();
        bs.scalingIter(0);
        bs.absConvergenceTol(1e-7);
        bs.relConvergenceTol(1e-7);
        bs.primalInfeasibilityTol(1e-8);
        bs.dualInfeasibilityTol(1e-8);
        bs.relaxationParam(0.01);
    }
#endif

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
#ifdef EIGEN_OSQP_FOUND
        if (sFlag == copra::SolverFlag::OSQP)
            ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-4); // I could not get a better precision...
        else
#endif
        ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(EqSystem, mpcTrajectoryCostWithEqualityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::TrajectoryCost>(M, xd);
    auto uCost = std::make_shared<copra::ControlCost>(N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
        if (solver)
            controller.useSolver(std::move(solver));
        else
            controller.selectQPSolver(sFlag);

        ASSERT_TRUE(controller.solve());

        Eigen::VectorXd fullTraj = controller.trajectory();
        auto trajLen = fullTraj.rows() / 2;
        Eigen::VectorXd posTraj(trajLen);
        Eigen::VectorXd velTraj(trajLen);
        for (auto i = 0; i < trajLen; ++i) {
            posTraj(i) = fullTraj(2 * i);
            velTraj(i) = fullTraj(2 * i + 1);
        }
        Eigen::VectorXd control = controller.control();

        // Check result
        EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

        // Check constrains
        ASSERT_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
#ifdef EIGEN_OSQP_FOUND
        if (sFlag == copra::SolverFlag::OSQP)
            ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-4); // I could not get a better precision...
        else
#endif
        ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
    };

    for (auto s : tools::Solvers) {
        std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
        if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
        pcCheck(s.first, s.second, std::move(solver));
    }
}

TEST_F(EqSystem, mpcMixedCostWithEqualityConstraints) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto xCost = std::make_shared<copra::MixedCost>(M, Eigen::MatrixXd::Zero(2, 1), xd);
    auto uCost = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Zero(1, 2), N, ud);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p, false);
    xCost->weights(wx);
    uCost->weights(wu);

    controller.addCost(xCost);
    controller.addCost(uCost);
    controller.addConstraint(trajConstr);

    ASSERT_TRUE(controller.solve());

    Eigen::VectorXd fullTraj = controller.trajectory();
    auto trajLen = fullTraj.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
        posTraj(i) = fullTraj(2 * i);
        velTraj(i) = fullTraj(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();

    // Check result
    EXPECT_LE(std::abs(xd(1) - velTraj.tail(3)(0)), 0.001); // Check X_{N-1} for mixed cost because X_N is not evaluated.

    // Check constrains
    ASSERT_LE(posTraj.maxCoeff(), x0(0) + 1e-6);
    ASSERT_LE(velTraj.maxCoeff(), p(0) + 1e-6);
}

/********************************************************************************************************
 *                                   Check Autospan                                                     *
 ********************************************************************************************************/

TEST_F(BoundedSystem, checkAutospanAndWholeMatrixOnBoundConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::VectorXd& xLower, const Eigen::VectorXd& xUpper, const Eigen::VectorXd& uLower, const Eigen::VectorXd& uUpper) {
        auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
        trajConstr->autoSpan();

        auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
        contConstr->autoSpan();

        ASSERT_NO_THROW(controller.addConstraint(trajConstr));
        ASSERT_NO_THROW(controller.addConstraint(contConstr));
    };

    auto fullxLower = tools::spanVector(xLower, nbXStep);
    auto fullxUpper = tools::spanVector(xUpper, nbXStep);
    auto fulluLower = tools::spanVector(uLower, nbStep);
    auto fulluUpper = tools::spanVector(uUpper, nbStep);

    checkSpan(xLower, xUpper, uLower, uUpper);
    checkSpan(fullxLower, xUpper, fulluLower, uUpper);
    checkSpan(xLower, fullxUpper, uLower, fulluUpper);
    checkSpan(fullxLower, fullxUpper, fulluLower, fulluUpper);
}

TEST_F(IneqSystem, checkAutospanAndWholeMatrixOnInequalityConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::MatrixXd& E, const Eigen::VectorXd& p, const Eigen::MatrixXd& G, const Eigen::VectorXd& h) {
        auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
        trajConstr->autoSpan();

        auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);
        contConstr->autoSpan();

        ASSERT_NO_THROW(controller.addConstraint(trajConstr));
        ASSERT_NO_THROW(controller.addConstraint(contConstr));
    };

    auto fullE = tools::spanMatrix(E, nbXStep);
    auto fullf = tools::spanVector(p, nbXStep);
    auto fullG = tools::spanMatrix(G, nbStep);
    auto fullh = tools::spanVector(h, nbStep);

    checkSpan(E, p, G, h);
    checkSpan(fullE, p, fullG, h);
    checkSpan(E, fullf, G, fullh);
    checkSpan(fullE, fullf, fullG, fullh);
}

TEST_F(MixedSystem, checkAutospanAndWholeMatrixOnMixedConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& E, const Eigen::MatrixXd& G, const Eigen::VectorXd& p) {
        auto mixedConstr = std::make_shared<copra::MixedConstraint>(E, G, p);
        mixedConstr->autoSpan();

        ASSERT_NO_THROW(controller.addConstraint(mixedConstr));
    };

    auto fullE = tools::spanMatrix(E, nbStep, 1);
    auto fullG = tools::spanMatrix(G, nbStep);
    auto fullf = tools::spanVector(p, nbStep);

    checkSpan(E, G, p);
    checkSpan(fullE, G, p);
    checkSpan(fullE, fullG, p);
    checkSpan(fullE, G, fullf);
    checkSpan(E, fullG, p);
    checkSpan(fullE, fullG, p);
    checkSpan(E, fullG, fullf);
    checkSpan(E, G, fullf);
    checkSpan(fullE, G, fullf);
    checkSpan(E, fullG, fullf);
    checkSpan(fullE, fullG, fullf);
}

TEST_F(IneqSystem, checkAutospanAndWholeMatrixOnTrajectoryCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    int nbXStep = nbStep + 1;

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::TrajectoryCost>(M, p);
        cost->weights(weights);
        cost->autoSpan();

        ASSERT_NO_THROW(controller.addCost(cost));
    };

    auto fullM = tools::spanMatrix(M, nbXStep);
    auto fullxd = tools::spanVector(xd, nbXStep);

    checkSpan(M, xd, wx);
    checkSpan(M, fullxd, wx);
    checkSpan(fullM, xd, wx);
    checkSpan(fullM, fullxd, wx);
}

TEST_F(IneqSystem, checkAutospanAndWholeMatrixOnControlCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::ControlCost>(M, p);
        cost->weights(weights);
        cost->autoSpan();

        ASSERT_NO_THROW(controller.addCost(cost));
    };

    auto fullN = tools::spanMatrix(N, nbStep);
    auto fullud = tools::spanVector(ud, nbStep);

    checkSpan(N, ud, wu);
    checkSpan(N, fullud, wu);
    checkSpan(fullN, ud, wu);
    checkSpan(fullN, fullud, wu);
}

TEST_F(IneqSystem, checkAutospanAndWholeMatrixOnMixedCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto checkSpan = [&](const Eigen::MatrixXd& M, const Eigen::MatrixXd& N, const Eigen::VectorXd& p, const Eigen::VectorXd& weights) {
        auto cost = std::make_shared<copra::MixedCost>(M, N, p);
        cost->weights(weights);
        cost->autoSpan();

        ASSERT_NO_THROW(controller.addCost(cost));
    };

    auto MVec = std::vector<Eigen::MatrixXd>();
    MVec.push_back(M);
    MVec.push_back(tools::spanMatrix(M, nbStep, 1));
    auto nnVec = std::vector<Eigen::MatrixXd>();
    nnVec.push_back(Eigen::MatrixXd::Ones(2, 1));
    nnVec.push_back(tools::spanMatrix(Eigen::MatrixXd::Ones(2, 1), nbStep));
    auto xdVec = std::vector<Eigen::VectorXd>();
    xdVec.push_back(xd);
    xdVec.push_back(tools::spanVector(xd, nbStep));

    for (auto& i : MVec)
        for (auto& j : nnVec)
            for (auto& k : xdVec)
                checkSpan(i, j, k, wx);
}

/********************************************************************************************************
 *                                Check Error Messages                                                  *
 ********************************************************************************************************/

TEST_F(IneqSystem, errorHandlerForSystem) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ASSERT_THROW(ps->reset(Eigen::MatrixXd::Ones(5, 2), B, c, x0, nbStep), std::domain_error);
    ASSERT_THROW(ps->reset(Eigen::MatrixXd::Ones(2, 5), B, c, x0, nbStep), std::domain_error);
    ASSERT_THROW(ps->reset(A, Eigen::MatrixXd::Ones(5, 1), c, x0, nbStep), std::domain_error);
    ASSERT_THROW(ps->reset(A, B, Eigen::VectorXd::Ones(5), x0, nbStep), std::domain_error);
    ASSERT_THROW(ps->reset(A, B, c, x0, -1), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForWeigths) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    auto cost = std::make_shared<copra::TrajectoryCost>(M, xd);

    ASSERT_NO_THROW(cost->weight(2));
    ASSERT_THROW(cost->weights(Eigen::VectorXd::Ones(5)), std::domain_error);
    ASSERT_NO_THROW(cost->weights(wx));
    ASSERT_NO_THROW(controller.addCost(cost));
    ASSERT_NO_THROW(cost->weights(Eigen::VectorXd::Ones(2)));
}

TEST_F(IneqSystem, errorHandlerForTrajectoryCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::TrajectoryCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::TrajectoryCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addCost(badCost2), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForTargetCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::TargetCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::TargetCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addCost(badCost2), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForControlCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::ControlCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::ControlCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addCost(badCost2), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForMixedCost) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badCost1 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(2, 1), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addCost(badCost1), std::domain_error);
    auto badCost2 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(2, 1), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addCost(badCost2), std::domain_error);
    auto badCost3 = std::make_shared<copra::MixedCost>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addCost(badCost3), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForTrajectoryConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr = std::make_shared<copra::TrajectoryConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr), std::domain_error);
    auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addConstraint(trajConstr), std::domain_error);
}

TEST_F(IneqSystem, errorHandlerForControlConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::ControlConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::ControlConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addConstraint(badConstr2), std::domain_error);

    auto goodConstr = std::make_shared<copra::ControlConstraint>(G, h);
    controller.addConstraint(goodConstr);
    ASSERT_THROW(controller.addConstraint(goodConstr), std::runtime_error);
}

TEST_F(IneqSystem, errorHandlerForMixedConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(2, 1), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(2, 1), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr2), std::domain_error);
    auto badConstr3 = std::make_shared<copra::MixedConstraint>(Eigen::MatrixXd::Identity(5, 5), Eigen::MatrixXd::Identity(5, 5), Eigen::VectorXd::Ones(5));
    ASSERT_THROW(controller.addConstraint(badConstr3), std::domain_error);
}

TEST_F(BoundedSystem, errorHandlerForTrajectoryBoundConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr = std::make_shared<copra::TrajectoryBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr), std::domain_error);
    auto tbConstr = std::make_shared<copra::TrajectoryBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(3));
    ASSERT_THROW(controller.addConstraint(tbConstr), std::domain_error);
}

TEST_F(BoundedSystem, errorHandlerForControlBoundConstraint) {  // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>();
    ps->reset(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);

    auto badConstr1 = std::make_shared<copra::ControlBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(2));
    ASSERT_THROW(controller.addConstraint(badConstr1), std::domain_error);
    auto badConstr2 = std::make_shared<copra::ControlBoundConstraint>(Eigen::VectorXd::Ones(3), Eigen::VectorXd::Ones(3));
    ASSERT_THROW(controller.addConstraint(badConstr2), std::domain_error);

    auto goodConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
    controller.addConstraint(goodConstr);
    ASSERT_THROW(controller.addConstraint(goodConstr), std::runtime_error);
}

/********************************************************************************************************
 *                               Check remove functions                                                 *
 ********************************************************************************************************/

TEST_F(IneqSystem, removeCostAndConstraint) { // NOLINT
    auto ps = std::make_shared<copra::TimeInvariantSystem>(A, B, c, x0, nbStep);
    auto controller = copra::LMPC(ps);
    {
        auto xCost = std::make_shared<copra::TargetCost>(M, xd);
        auto uCost = std::make_shared<copra::ControlCost>(N, ud);
        auto trajConstr = std::make_shared<copra::TrajectoryConstraint>(E, p);
        auto contConstr = std::make_shared<copra::ControlConstraint>(G, h);

        controller.addCost(xCost);
        controller.addCost(uCost);
        controller.addConstraint(trajConstr);
        controller.addConstraint(contConstr);

        controller.removeCost(xCost);
        controller.removeCost(uCost);
        controller.removeConstraint(trajConstr);
        controller.removeConstraint(contConstr);
    }
    ASSERT_TRUE(controller.solve());
}

TEST_F(SmallSystem, solveSmallSystem) {  // NOLINT
  auto ps = std::make_shared<copra::TimeInvariantSystem>();
  ps->reset(A, B, c, x0, nbStep);
  ps->update();

  auto controller = copra::LMPC(ps);
  auto xCost = std::make_shared<copra::TargetCost>(M, xd);
  auto uCost = std::make_shared<copra::ControlCost>(N, ud);
  auto trajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(xLower, xUpper);
  auto contConstr = std::make_shared<copra::ControlBoundConstraint>(uLower, uUpper);
  xCost->weights(wx);
  uCost->weights(wu);

  controller.addCost(xCost);
  controller.addCost(uCost);
  controller.addConstraint(trajConstr);
  controller.addConstraint(contConstr);

  auto pcCheck = [&](const std::string& /* solverName */, copra::SolverFlag sFlag, std::unique_ptr<copra::SolverInterface>&& solver = nullptr) {
    if (solver) {
      controller.useSolver(std::move(solver));
    } else {
      controller.selectQPSolver(sFlag);
    }
    ASSERT_TRUE(controller.solve());

    Eigen::VectorXd trajectory = controller.trajectory();
    ASSERT_LE((trajectory - expectedTrajectory).norm(), 1e-4);

    auto trajLen = trajectory.rows() / 2;
    Eigen::VectorXd posTraj(trajLen);
    Eigen::VectorXd velTraj(trajLen);
    for (auto i = 0; i < trajLen; ++i) {
      posTraj(i) = trajectory(2 * i);
      velTraj(i) = trajectory(2 * i + 1);
    }
    Eigen::VectorXd control = controller.control();
    ASSERT_LE((control - expectedControl).norm(), 2e-4);

    // Check system dynamics on full solution
    for (int k = 0; 2 * (k + 1) + 1 < trajLen; k++) {
      Eigen::VectorXd x_k = trajectory.segment<2>(2 * k);
      Eigen::VectorXd x_next = trajectory.segment<2>(2 * (k + 1));
      ASSERT_LE((x_next - A * x_k - B * control(k) - c).norm(), 1e-10);
    }

    // Check that terminal condition is fulfilled
    EXPECT_LE(std::abs(xd(1) - velTraj.tail(1)(0)), 0.001);

    // Check constraints
    ASSERT_LE(posTraj.maxCoeff(), x0(0));  // actually not an explicit constraint
    ASSERT_LE(velTraj.maxCoeff(), xUpper(1) + 1e-6);
    ASSERT_LE(control.maxCoeff(), uUpper(0) + 1e-6);
  };

  for (auto s : tools::Solvers) {
    std::unique_ptr<copra::SolverInterface> solver(nullptr);
#ifdef EIGEN_LSSOL_FOUND
    if (s.second == copra::SolverFlag::LSSOL) {
            solver = copra::solverFactory(copra::SolverFlag::LSSOL);
            solver->SI_maxIter(200);
        }
#endif
    pcCheck(s.first, s.second, std::move(solver));
  }
}

max_stars_repo_name: ANYbotics/copra

max_issues_repo_name: ANYbotics/copra

max_forks_repo_name: ANYbotics/copra


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

#include "roc_result.h"

#include <boost/format.hpp>
#include <algorithm>
#include <numeric>

namespace nnforge
{
	roc_result::roc_result(
		const output_neuron_value_set& predicted_value_set,
		const output_neuron_value_set& actual_value_set,
		float threshold,
		float beta,
		unsigned int segment_count,
		float min_val,
		float max_val)
		: segment_count(segment_count)
		, min_val(min_val)
		, max_val(max_val)
		, threshold(threshold)
		, beta(beta)
		, actual_positive_elem_count(0)
		, actual_negative_elem_count(0)
		, values_for_positive_elems(segment_count)
		, values_for_negative_elems(segment_count)
	{
		float mult = 1.0F / (max_val - min_val);
		float segment_count_f = static_cast<float>(segment_count);
		std::vector<std::vector<float> >::const_iterator predicted_it = predicted_value_set.neuron_value_list.begin();
		for(std::vector<std::vector<float> >::const_iterator actual_it = actual_value_set.neuron_value_list.begin();
			actual_it != actual_value_set.neuron_value_list.end();
			actual_it++, predicted_it++)
		{
			const std::vector<float>& actual_value_list = *actual_it;
			const std::vector<float>& predicted_value_list = *predicted_it;

			std::vector<float>::const_iterator predicted_value_it = predicted_value_list.begin();
			for(std::vector<float>::const_iterator actual_value_it = actual_value_list.begin();
				actual_value_it != actual_value_list.end();
				actual_value_it++, predicted_value_it++)
			{
				float actual_value = *actual_value_it;
				float predicted_value = *predicted_value_it;

				unsigned int bucket_id = std::min<unsigned int>(static_cast<unsigned int>(std::max<float>(std::min<float>((predicted_value - min_val) * mult, 1.0F), 0.0F) * segment_count_f), (segment_count - 1));

				if (actual_value > 0.0F)
				{
					values_for_positive_elems[bucket_id]++;
					actual_positive_elem_count++;
				}
				else
				{
					values_for_negative_elems[bucket_id]++;
					actual_negative_elem_count++;
				}
			}
		}
	}

	float roc_result::get_accuracy() const
	{
		unsigned int starting_segment_id = static_cast<unsigned int>(std::max(std::min((threshold - min_val) / (max_val - min_val), 1.0F), 0.0F) * static_cast<float>(segment_count));

		unsigned int true_positive = std::accumulate(values_for_positive_elems.begin() + starting_segment_id, values_for_positive_elems.end(), 0);
		unsigned int true_negative = std::accumulate(values_for_negative_elems.begin(), values_for_negative_elems.begin() + starting_segment_id, 0);

		return static_cast<float>(true_positive + true_negative) / static_cast<float>(actual_positive_elem_count + actual_negative_elem_count);
	}

	float roc_result::get_f_score() const
	{
		unsigned int starting_segment_id = static_cast<unsigned int>(std::max(std::min((threshold - min_val) / (max_val - min_val), 1.0F), 0.0F) * static_cast<float>(segment_count));

		unsigned int true_positive = std::accumulate(values_for_positive_elems.begin() + starting_segment_id, values_for_positive_elems.end(), 0);
		unsigned int false_positive = std::accumulate(values_for_negative_elems.begin() + starting_segment_id, values_for_negative_elems.end(), 0);
		// unsigned int true_negative = std::accumulate(values_for_negative_elems.begin(), values_for_negative_elems.begin() + starting_segment_id, 0);
		unsigned int false_negative = std::accumulate(values_for_positive_elems.begin(), values_for_positive_elems.begin() + starting_segment_id, 0);

		return (1.0F + beta * beta) * static_cast<float>(true_positive) /
			((1.0F + beta * beta) * static_cast<float>(true_positive) + (beta * beta) * static_cast<float>(false_negative) + static_cast<float>(false_positive));
	}

	float roc_result::get_precision() const
	{
		unsigned int starting_segment_id = static_cast<unsigned int>(std::max(std::min((threshold - min_val) / (max_val - min_val), 1.0F), 0.0F) * static_cast<float>(segment_count));

		unsigned int true_positive = std::accumulate(values_for_positive_elems.begin() + starting_segment_id, values_for_positive_elems.end(), 0);
		unsigned int false_positive = std::accumulate(values_for_negative_elems.begin() + starting_segment_id, values_for_negative_elems.end(), 0);

		return static_cast<float>(true_positive) / (static_cast<float>(true_positive) + static_cast<float>(false_positive));
	}

	float roc_result::get_recall() const
	{
		unsigned int starting_segment_id = static_cast<unsigned int>(std::max(std::min((threshold - min_val) / (max_val - min_val), 1.0F), 0.0F) * static_cast<float>(segment_count));

		unsigned int true_positive = std::accumulate(values_for_positive_elems.begin() + starting_segment_id, values_for_positive_elems.end(), 0);
		unsigned int false_negative = std::accumulate(values_for_positive_elems.begin(), values_for_positive_elems.begin() + starting_segment_id, 0);

		return static_cast<float>(true_positive) / (static_cast<float>(true_positive) + static_cast<float>(false_negative));
	}

	float roc_result::get_auc() const
	{
		std::vector<float> true_positive_rates;
		{
			unsigned int current_positive_elems_count = 0;
			float mult = 1.0F / static_cast<float>(actual_positive_elem_count);
			for(std::vector<unsigned int>::const_reverse_iterator it = values_for_positive_elems.rbegin(); it != values_for_positive_elems.rend(); ++it)
			{
				current_positive_elems_count += *it;
				true_positive_rates.push_back(mult * static_cast<float>(current_positive_elems_count));
			}
		}
		true_positive_rates.push_back(1.0F);

		std::vector<float> false_positive_rates;
		{
			unsigned int current_negative_elems_count = 0;
			float mult = 1.0F / static_cast<float>(actual_negative_elem_count);
			for(std::vector<unsigned int>::const_reverse_iterator it = values_for_negative_elems.rbegin(); it != values_for_negative_elems.rend(); ++it)
			{
				current_negative_elems_count += *it;
				false_positive_rates.push_back(mult * static_cast<float>(current_negative_elems_count));
			}
		}
		false_positive_rates.push_back(1.0F);

		float sum = 0.0F;
		float previous_fpr = 0.0F;
		float previous_tpr = 0.0F;
		std::vector<float>::const_iterator tpr_it = true_positive_rates.begin();
		for(std::vector<float>::const_iterator fpr_it = false_positive_rates.begin(); fpr_it != false_positive_rates.end(); ++fpr_it, ++tpr_it)
		{
			float current_fpr = *fpr_it;
			float current_tpr = *tpr_it;

			if (current_fpr != previous_fpr)
				sum += (current_fpr - previous_fpr) * (previous_tpr + current_tpr) * 0.5F;

			previous_fpr = current_fpr;
			previous_tpr = current_tpr;
		}

		return sum;
	}

	std::ostream& operator<< (std::ostream& out, const roc_result& val)
	{
		out << (boost::format("AUC %|1$.5f|, (using threshold %|2$.3f|) Accuracy %|3$.5f|, Precision %|4$.5f|, Recall %|5$.5f|, F-score %|6$.5f| (beta %|7$.3f|)") % val.get_auc() % val.threshold % val.get_accuracy() % val.get_precision() % val.get_recall() % val.get_f_score() % val.beta).str();

		return out;
	}
}

max_stars_repo_name: anshumang/nnForgeINST

max_issues_repo_name: anshumang/nnForgeINST

max_forks_repo_name: anshumang/nnForgeINST


text:
/*
 * @COPYRIGHT@
 *
 * 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
 */

// dimbo/clinfo/platform_info.hpp

/** // // doc: dimbo/clinfo/platform_info.hpp {{{
 * \file dimbo/clinfo/platform_info.hpp
 *
 * This file contains definition of Dimbo::Clinfo::Platform_Info class, which
 * encapsulates several parameters retrieved from OpenCL platfrom.
 */ // }}}
#ifndef DIMBO_CLINFO_PLATFORM_INFO_HPP_INCLUDED
#define DIMBO_CLINFO_PLATFORM_INFO_HPP_INCLUDED

#include <string>
#include <boost/optional.hpp>

#include <dimbo/clinfo/exceptions/common.hpp>
#include <dimbo/clinfo/exceptions/bad_alloc.hpp>
#include <dimbo/clinfo/exceptions/length_error.hpp>
#include <dimbo/clinfo/exceptions/uninitialized_value.hpp>

namespace Dimbo {
namespace Clinfo {

/** // doc: class Platform_Info {{{
 * \ingroup Dimbo_Clinfo_Platform
 * \brief Encapsulates parameters retrieved from OpenCL platfrom.
 *
 * An object of type Platform_Info keeps the result of composite query to
 * a single OpenCL platform. It may be used, for example, to pass that
 * information between comuters, serialize it or write it to a stream.
 * It also keeps a copy of recently used Platform_Query object to remember
 * which parameters were obtained from OpenCL platform and are actually valid.
 */ // }}}
class Platform_Info
{
  template <class Archive>
  friend void _serialize(Archive&, Platform_Info&, const unsigned int);
public:
  static constexpr unsigned int class_version = 0x000001;
public:
  /** // doc: Platform_Info() {{{
   * \brief Default constructor.
   */ // }}}
  Platform_Info();
  /** // doc: ~Platform_Info() {{{
   * \brief Destructor.
   */ // }}}
  virtual ~Platform_Info() { }
  /** // doc: clear() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear();
  /** // doc: cmp() {{{
   * \todo Write documentation
   */ // }}}
  bool cmp(Platform_Info const&) const throw();
  // getters {{{
  /** // // doc: id() {{{
   * \brief Identifier of the recently queried OpenCL platform.
   * \return The \c cl_platform_id identifier of an OpenCL platform, whose
   *    parameters are keeps in this object.
   */ // }}}
  unsigned long id() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  /** // // doc: profile() {{{
   * \brief Platform profile string retrieved with Platform::get_profile().
   * \todo Refine documentation
   */ // }}}
  std::string const& profile() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  /** // // doc: version() {{{
   * \brief Platform version string retrieved with Platform::get_version().
   * \todo Refine documentation
   */ // }}}
  std::string const& version() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  /** // // doc: name() {{{
   * \brief Platform version string retrieved with Platform::get_version().
   * \todo Refine documentation
   */ // }}}
  std::string const& name() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  /** // // doc: vendor() {{{
   * \todo Refine documentation
   * \brief Platform vendor string retrieved with Platform::get_vendor().
   */ // }}}
  std::string const& vendor() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  /** // // doc: extensions() {{{
   * \todo Refine documentation
   * \brief Platform extensions string retrieved with
   *        Platform::get_extensions().
   */ // }}}
  std::string const& extensions() const
    throw(DIMBO_CLINFO_EXCEPTION(Uninitialized_Value));
  // }}}
  // setters {{{
  /** // doc: set_id() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_id(unsigned long val) throw();
  /** // doc: set_profile() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_profile(std::string const& val)
    throw(DIMBO_CLINFO_EXCEPTION(Bad_Alloc), DIMBO_CLINFO_EXCEPTION(Length_Error));
  /** // doc: set_version() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_version(std::string const& val)
    throw(DIMBO_CLINFO_EXCEPTION(Bad_Alloc), DIMBO_CLINFO_EXCEPTION(Length_Error));
  /** // doc: set_name() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_name(std::string const& val)
    throw(DIMBO_CLINFO_EXCEPTION(Bad_Alloc), DIMBO_CLINFO_EXCEPTION(Length_Error));
  /** // doc: set_vendor() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_vendor(std::string const& val)
    throw(DIMBO_CLINFO_EXCEPTION(Bad_Alloc), DIMBO_CLINFO_EXCEPTION(Length_Error));
  /** // doc: set_extensions() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& set_extensions(std::string const& val)
    throw(DIMBO_CLINFO_EXCEPTION(Bad_Alloc), DIMBO_CLINFO_EXCEPTION(Length_Error));
  // }}}
  // cleaners {{{
  /** // doc: clear_id() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_id() throw();
  /** // doc: clear_profile() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_profile() throw();
  /** // doc: clear_version() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_version() throw();
  /** // doc: clear_name() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_name() throw();
  /** // doc: clear_vendor() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_vendor() throw();
  /** // doc: clear_extensions() {{{
   * \todo Write documentation
   */ // }}}
  Platform_Info& clear_extensions() throw();
  // }}}
  // presence {{{
  /** // doc: has_id() {{{
   * \todo Write documentation
   */ // }}}
  bool has_id() const throw();
  /** // doc: has_profile() {{{
   * \todo Write documentation
   */ // }}}
  bool has_profile() const throw();
  /** // doc: has_version() {{{
   * \todo Write documentation
   */ // }}}
  bool has_version() const throw();
  /** // doc: has_name() {{{
   * \todo Write documentation
   */ // }}}
  bool has_name() const throw();
  /** // doc: has_vendor() {{{
   * \todo Write documentation
   */ // }}}
  bool has_vendor() const throw();
  /** // doc: has_extensions() {{{
   * \todo Write documentation
   */ // }}}
  bool has_extensions() const throw();
  // }}}
private:
  void _init();
  void _clear();
  // attributes {{{
  boost::optional<unsigned long> _id;
  boost::optional<std::string> _profile;
  boost::optional<std::string> _version;
  boost::optional<std::string> _name;
  boost::optional<std::string> _vendor;
  boost::optional<std::string> _extensions;
  // }}}
};

/** // doc: operator==(a,b) {{{
 * \todo Write documentation
 */ // }}}
inline bool operator== (Platform_Info const& a, Platform_Info const& b)
  throw()
{ return a.cmp(b); }
/** // doc: operator==(a,b) {{{
 * \todo Write documentation
 */ // }}}
inline bool operator!= (Platform_Info const& a, Platform_Info const& b)
  throw()
{ return !(a == b); }

} /* namespace Dimbo */
} /* namespace Clinfo */

#endif /* DIMBO_CLINFO_PLATFORM_INFO_HPP_INCLUDED */
// vim: set expandtab tabstop=2 shiftwidth=2:
// vim: set foldmethod=marker foldcolumn=4:

ext: hpp

max_stars_repo_name: ptomulik/dimbo

max_issues_repo_name: ptomulik/dimbo

max_forks_repo_name: ptomulik/dimbo


text: #include <boost/spirit/home/karma/binary/padding.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino