// Copyright 2004 The Trustees of Indiana University.

// 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)

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP
#define BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP

// Gursoy-Atun graph layout, based on:
// "Neighbourhood Preserving Load Balancing: A Self-Organizing Approach"
// in EuroPar 2000, p. 234 of LNCS 1900
// http://springerlink.metapress.com/link.asp?id=pcu07ew5rhexp9yt

#include <cmath>
#include <vector>
#include <exception>
#include <algorithm>

#include <boost/graph/visitors.hpp>
#include <boost/graph/properties.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>

namespace boost {

namespace detail {

struct over_distance_limit : public std::exception {};

template <typename PositionMap, typename NodeDistanceMap,  typename Topology,
          typename Graph>
struct update_position_visitor {
  typedef typename Topology::point_type Point;
  PositionMap position_map;
  NodeDistanceMap node_distance;
  const Topology& space;
  Point input_vector;
  double distance_limit;
  double learning_constant;
  double falloff_ratio;

  typedef boost::on_examine_vertex event_filter;

  typedef typename graph_traits<Graph>::vertex_descriptor
    vertex_descriptor;

  update_position_visitor(PositionMap position_map,
                          NodeDistanceMap node_distance,
                          const Topology& space,
                          const Point& input_vector,
                          double distance_limit,
                          double learning_constant,
                          double falloff_ratio):
    position_map(position_map), node_distance(node_distance),
    space(space),
    input_vector(input_vector), distance_limit(distance_limit),
    learning_constant(learning_constant), falloff_ratio(falloff_ratio) {}

  void operator()(vertex_descriptor v, const Graph&) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::pow;
#endif

    if (get(node_distance, v) > distance_limit)
      throw over_distance_limit();
    Point old_position = get(position_map, v);
    double distance = get(node_distance, v);
    double fraction =
      learning_constant * pow(falloff_ratio, distance * distance);
    put(position_map, v,
        space.move_position_toward(old_position, fraction, input_vector));
  }
};

template<typename EdgeWeightMap>
struct gursoy_shortest
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      EdgeWeightMap weight)
  {
    boost::dijkstra_shortest_paths(g, s, weight_map(weight).
      visitor(boost::make_dijkstra_visitor(std::make_pair(
       boost::record_distances(node_distance, boost::on_edge_relaxed()),
        update_position))));
  }
};

template<>
struct gursoy_shortest<dummy_property_map>
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      dummy_property_map)
  {
    boost::breadth_first_search(g, s,
      visitor(boost::make_bfs_visitor(std::make_pair(
        boost::record_distances(node_distance, boost::on_tree_edge()),
        update_position))));
  }
};

} // namespace detail

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename Diameter, typename VertexIndexMap,
          typename EdgeWeightMap>
void
gursoy_atun_step
  (const VertexListAndIncidenceGraph& graph,
   const Topology& space,
   PositionMap position,
   Diameter diameter,
   double learning_constant,
   VertexIndexMap vertex_index_map,
   EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<double> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  point_type input_vector = space.random_point();
  vertex_descriptor min_distance_loc
    = graph_traits<VertexListAndIncidenceGraph>::null_vertex();
  double min_distance = 0.0;
  bool min_distance_unset = true;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    double this_distance = space.distance(get(position, *i), input_vector);
    put(distance_from_input, *i, this_distance);
    if (min_distance_unset || this_distance < min_distance) {
      min_distance = this_distance;
      min_distance_loc = *i;
    }
    min_distance_unset = false;
  }
  assert (!min_distance_unset); // Graph must have at least one vertex
  boost::detail::update_position_visitor<
      PositionMap, NodeDistanceMap, Topology,
      VertexListAndIncidenceGraph>
    update_position(position, node_distance, space,
                    input_vector, diameter, learning_constant,
                    exp(-1. / (2 * diameter * diameter)));
  std::fill(node_distance_map_vector.begin(), node_distance_map_vector.end(), 0);
  try {
    typedef detail::gursoy_shortest<EdgeWeightMap> shortest;
    shortest::run(graph, min_distance_loc, node_distance, update_position,
                  weight);
  } catch (detail::over_distance_limit) {
    /* Thrown to break out of BFS or Dijkstra early */
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_refine(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  double diameter_ratio = (double)diameter_final / diameter_initial;
  double learning_constant_ratio =
    learning_constant_final / learning_constant_initial;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<int> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<int>::iterator,
                                       VertexIndexMap, double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  for (int round = 0; round < nsteps; ++round) {
    double part_done = (double)round / (nsteps - 1);
    int diameter = (int)(diameter_initial * pow(diameter_ratio, part_done));
    double learning_constant =
      learning_constant_initial * pow(learning_constant_ratio, part_done);
    gursoy_atun_step(graph, space, position, diameter, learning_constant,
                     vertex_index_map, weight);
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  vertex_iterator i, iend;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    put(position, *i, space.random_point());
  }
  gursoy_atun_refine(graph, space,
                     position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, weight);
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map)
{
  gursoy_atun_layout(graph, space, position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, dummy_property_map());
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final = 1.0,
                        double learning_constant_initial = 0.8,
                        double learning_constant_final = 0.2)
{
  gursoy_atun_layout(graph, space, position, nsteps, diameter_initial,
                     diameter_final, learning_constant_initial,
                     learning_constant_final, get(vertex_index, graph));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  gursoy_atun_layout(graph, space, position, nsteps,
                     sqrt((double)num_vertices(graph)));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position)
{
  gursoy_atun_layout(graph, space, position, num_vertices(graph));
}

template<typename VertexListAndIncidenceGraph, typename Topology,
         typename PositionMap, typename P, typename T, typename R>
void
gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                   const Topology& space,
                   PositionMap position,
                   const bgl_named_params<P,T,R>& params)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  std::pair<double, double> diam(sqrt(double(num_vertices(graph))), 1.0);
  std::pair<double, double> learn(0.8, 0.2);
  gursoy_atun_layout(graph, space, position,
                     choose_param(get_param(params, iterations_t()),
                                  num_vertices(graph)),
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).first,
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).second,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).first,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).second,
                     choose_const_pmap(get_param(params, vertex_index), graph,
                                       vertex_index),
                     choose_param(get_param(params, edge_weight),
                                  dummy_property_map()));
}

/***********************************************************
 * Topologies                                              *
 ***********************************************************/
template<std::size_t Dims>
class convex_topology
{
  struct point
  {
    point() { }
    double& operator[](std::size_t i) {return values[i];}
    const double& operator[](std::size_t i) const {return values[i];}

  private:
    double values[Dims];
  };

 public:
  typedef point point_type;

  double distance(point a, point b) const
  {
    double dist = 0;
    for (std::size_t i = 0; i < Dims; ++i) {
      double diff = b[i] - a[i];
      dist += diff * diff;
    }
    // Exact properties of the distance are not important, as long as
    // < on what this returns matches real distances
    return dist;
  }

  point move_position_toward(point a, double fraction, point b) const
  {
    point result;
    for (std::size_t i = 0; i < Dims; ++i)
      result[i] = a[i] + (b[i] - a[i]) * fraction;
    return result;
  }
};

template<std::size_t Dims,
         typename RandomNumberGenerator = minstd_rand>
class hypercube_topology : public convex_topology<Dims>
{
  typedef uniform_01<RandomNumberGenerator, double> rand_t;

 public:
  typedef typename convex_topology<Dims>::point_type point_type;

  explicit hypercube_topology(double scaling = 1.0)
    : gen_ptr(new RandomNumberGenerator), rand(new rand_t(*gen_ptr)),
      scaling(scaling)
  { }

  hypercube_topology(RandomNumberGenerator& gen, double scaling = 1.0)
    : gen_ptr(), rand(new rand_t(gen)), scaling(scaling) { }

  point_type random_point() const
  {
    point_type p;
    for (std::size_t i = 0; i < Dims; ++i)
      p[i] = (*rand)() * scaling;
    return p;
  }

 private:
  shared_ptr<RandomNumberGenerator> gen_ptr;
  shared_ptr<rand_t> rand;
  double scaling;
};

template<typename RandomNumberGenerator = minstd_rand>
class square_topology : public hypercube_topology<2, RandomNumberGenerator>
{
  typedef hypercube_topology<2, RandomNumberGenerator> inherited;

 public:
  explicit square_topology(double scaling = 1.0) : inherited(scaling) { }

  square_topology(RandomNumberGenerator& gen, double scaling = 1.0)
    : inherited(gen, scaling) { }
};

template<typename RandomNumberGenerator = minstd_rand>
class cube_topology : public hypercube_topology<3, RandomNumberGenerator>
{
  typedef hypercube_topology<3, RandomNumberGenerator> inherited;

 public:
  explicit cube_topology(double scaling = 1.0) : inherited(scaling) { }

  cube_topology(RandomNumberGenerator& gen, double scaling = 1.0)
    : inherited(gen, scaling) { }
};

template<std::size_t Dims,
         typename RandomNumberGenerator = minstd_rand>
class ball_topology : public convex_topology<Dims>
{
  typedef uniform_01<RandomNumberGenerator, double> rand_t;

 public:
  typedef typename convex_topology<Dims>::point_type point_type;

  explicit ball_topology(double radius = 1.0)
    : gen_ptr(new RandomNumberGenerator), rand(new rand_t(*gen_ptr)),
      radius(radius)
  { }

  ball_topology(RandomNumberGenerator& gen, double radius = 1.0)
    : gen_ptr(), rand(new rand_t(gen)), radius(radius) { }

  point_type random_point() const
  {
    point_type p;
    double dist_sum;
    do {
      dist_sum = 0.0;
      for (std::size_t i = 0; i < Dims; ++i) {
        double x = (*rand)() * 2*radius - radius;
        p[i] = x;
        dist_sum += x * x;
      }
    } while (dist_sum > radius*radius);
    return p;
  }

 private:
  shared_ptr<RandomNumberGenerator> gen_ptr;
  shared_ptr<rand_t> rand;
  double radius;
};

template<typename RandomNumberGenerator = minstd_rand>
class circle_topology : public ball_topology<2, RandomNumberGenerator>
{
  typedef ball_topology<2, RandomNumberGenerator> inherited;

 public:
  explicit circle_topology(double radius = 1.0) : inherited(radius) { }

  circle_topology(RandomNumberGenerator& gen, double radius = 1.0)
    : inherited(gen, radius) { }
};

template<typename RandomNumberGenerator = minstd_rand>
class sphere_topology : public ball_topology<3, RandomNumberGenerator>
{
  typedef ball_topology<3, RandomNumberGenerator> inherited;

 public:
  explicit sphere_topology(double radius = 1.0) : inherited(radius) { }

  sphere_topology(RandomNumberGenerator& gen, double radius = 1.0)
    : inherited(gen, radius) { }
};

template<typename RandomNumberGenerator = minstd_rand>
class heart_topology
{
  // Heart is defined as the union of three shapes:
  // Square w/ corners (+-1000, -1000), (0, 0), (0, -2000)
  // Circle centered at (-500, -500) radius 500*sqrt(2)
  // Circle centered at (500, -500) radius 500*sqrt(2)
  // Bounding box (-1000, -2000) - (1000, 500*(sqrt(2) - 1))

  struct point
  {
    point() { values[0] = 0.0; values[1] = 0.0; }
    point(double x, double y) { values[0] = x; values[1] = y; }

    double& operator[](std::size_t i)       { return values[i]; }
    double  operator[](std::size_t i) const { return values[i]; }

  private:
    double values[2];
  };

  bool in_heart(point p) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::abs;
    using std::pow;
#endif

    if (p[1] < abs(p[0]) - 2000) return false; // Bottom
    if (p[1] <= -1000) return true; // Diagonal of square
    if (pow(p[0] - -500, 2) + pow(p[1] - -500, 2) <= 500000)
      return true; // Left circle
    if (pow(p[0] - 500, 2) + pow(p[1] - -500, 2) <= 500000)
      return true; // Right circle
    return false;
  }

  bool segment_within_heart(point p1, point p2) const
  {
    // Assumes that p1 and p2 are within the heart
    if ((p1[0] < 0) == (p2[0] < 0)) return true; // Same side of symmetry line
    if (p1[0] == p2[0]) return true; // Vertical
    double slope = (p2[1] - p1[1]) / (p2[0] - p1[0]);
    double intercept = p1[1] - p1[0] * slope;
    if (intercept > 0) return false; // Crosses between circles
    return true;
  }

  typedef uniform_01<RandomNumberGenerator, double> rand_t;

 public:
  typedef point point_type;

  heart_topology()
    : gen_ptr(new RandomNumberGenerator), rand(new rand_t(*gen_ptr)) { }

  heart_topology(RandomNumberGenerator& gen)
    : gen_ptr(), rand(new rand_t(gen)) { }

  point random_point() const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif

    point result;
    double sqrt2 = sqrt(2.);
    do {
      result[0] = (*rand)() * (1000 + 1000 * sqrt2) - (500 + 500 * sqrt2);
      result[1] = (*rand)() * (2000 + 500 * (sqrt2 - 1)) - 2000;
    } while (!in_heart(result));
    return result;
  }

  double distance(point a, point b) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    if (segment_within_heart(a, b)) {
      // Straight line
      return sqrt((b[0] - a[0]) * (b[0] - a[0]) + (b[1] - a[1]) * (b[1] - a[1]));
    } else {
      // Straight line bending around (0, 0)
      return sqrt(a[0] * a[0] + a[1] * a[1]) + sqrt(b[0] * b[0] + b[1] * b[1]);
    }
  }

  point move_position_toward(point a, double fraction, point b) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif

    if (segment_within_heart(a, b)) {
      // Straight line
      return point(a[0] + (b[0] - a[0]) * fraction,
                   a[1] + (b[1] - a[1]) * fraction);
    } else {
      double distance_to_point_a = sqrt(a[0] * a[0] + a[1] * a[1]);
      double distance_to_point_b = sqrt(b[0] * b[0] + b[1] * b[1]);
      double location_of_point = distance_to_point_a /
                                   (distance_to_point_a + distance_to_point_b);
      if (fraction < location_of_point)
        return point(a[0] * (1 - fraction / location_of_point),
                     a[1] * (1 - fraction / location_of_point));
      else
        return point(
          b[0] * ((fraction - location_of_point) / (1 - location_of_point)),
          b[1] * ((fraction - location_of_point) / (1 - location_of_point)));
    }
  }

 private:
  shared_ptr<RandomNumberGenerator> gen_ptr;
  shared_ptr<rand_t> rand;
};

} // namespace boost

#endif // BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP


#ifndef QC_UTIL_MATRIX_H
#define QC_UTIL_MATRIX_H

#define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/array.hpp>
#include <boost/mpl/void.hpp>

namespace ublas = boost::numeric::ublas;

namespace  matrix {

    namespace mpl = boost::mpl;

    template<typename T>
    struct matrix_semantics {
	typedef int index_type;
	typedef T& reference;
	typedef const T& const_reference;
	virtual T& operator()(index_type i, index_type j) = 0;
	virtual const T& operator()(index_type i, index_type j) const = 0;
    };

    template<typename T>
    struct abstract_matrix {
	virtual T* operator()(int i, int j) = 0;
	virtual const T* operator()(int i, int j) const = 0;
    };

    template<typename T> class block_matrix;
    template<class Matrix> class meta_matrix;
    template<class Matrix> class block_meta_matrix;

}

#include "matrix/rectangular.hpp"
#include "matrix/block.hpp"
#include "matrix/meta.hpp"
#include "matrix/symmetric.hpp"

#include "matrix/ops.hpp"
#include "matrix/permute.hpp"

typedef ublas::matrix<double> Matrix;
typedef ublas::symmetric_matrix<double> SymmetricMatrix;
typedef matrix::block_matrix<double> BlockMatrix;

#endif // _MATRIX_H


// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Base implementations for math library.
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @author Moses Turner <mosesturner@protonmail.com>
 * @ingroup aux_math
 */

#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <assert.h>

using namespace xrt::auxiliary::math;

/*
 *
 * Copy helpers.
 *
 */

static inline Eigen::Quaternionf
copy(const struct xrt_quat &q)
{
	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
	//  Eigen: `float w, x, y, z`.
	// OpenXR: `float x, y, z, w`.
	return Eigen::Quaternionf(q.w, q.x, q.y, q.z);
}

static inline Eigen::Quaternionf
copy(const struct xrt_quat *q)
{
	return copy(*q);
}

XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
copyd(const struct xrt_quat &q)
{
	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
	//  Eigen: `float w, x, y, z`.
	// OpenXR: `float x, y, z, w`.
	return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
}

XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
copyd(const struct xrt_quat *q)
{
	return copyd(*q);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 &v)
{
	return Eigen::Vector3f(v.x, v.y, v.z);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 *v)
{
	return copy(*v);
}

XRT_MAYBE_UNUSED static inline Eigen::Vector3d
copyd(const struct xrt_vec3 &v)
{
	return Eigen::Vector3d(v.x, v.y, v.z);
}

XRT_MAYBE_UNUSED static inline Eigen::Vector3d
copyd(const struct xrt_vec3 *v)
{
	return copyd(*v);
}

static inline Eigen::Matrix3f
copy(const struct xrt_matrix_3x3 *m)
{
	Eigen::Matrix3f res;
	// clang-format off
	res << m->v[0], m->v[3], m->v[6],
	       m->v[1], m->v[4], m->v[7],
	       m->v[2], m->v[5], m->v[8];
	// clang-format on
	return res;
}

static inline Eigen::Matrix4f
copy(const struct xrt_matrix_4x4 *m)
{
	Eigen::Matrix4f res;
	// clang-format off
	res << m->v[0], m->v[4], m->v[8],  m->v[12],
	       m->v[1], m->v[5], m->v[9],  m->v[13],
	       m->v[2], m->v[6], m->v[10], m->v[14],
	       m->v[3], m->v[7], m->v[11], m->v[15];
	// clang-format on
	return res;
}

/*
 *
 * Exported vector functions.
 *
 */

extern "C" bool
math_vec3_validate(const struct xrt_vec3 *vec3)
{
	assert(vec3 != NULL);

	return map_vec3(*vec3).allFinite();
}

extern "C" void
math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut)
{
	assert(additional != NULL);
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) += map_vec3(*additional);
}

extern "C" void
math_vec3_subtract(const struct xrt_vec3 *subtrahend, struct xrt_vec3 *inAndOut)
{
	assert(subtrahend != NULL);
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) -= map_vec3(*subtrahend);
}

extern "C" void
math_vec3_scalar_mul(float scalar, struct xrt_vec3 *inAndOut)
{
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) *= scalar;
}

extern "C" void
math_vec3_cross(const struct xrt_vec3 *l, const struct xrt_vec3 *r, struct xrt_vec3 *result)
{
	map_vec3(*result) = map_vec3(*l).cross(map_vec3(*r));
}

extern "C" void
math_vec3_normalize(struct xrt_vec3 *in)
{
	map_vec3(*in) = map_vec3(*in).normalized();
}

/*
 *
 * Exported quaternion functions.
 *
 */

extern "C" void
math_quat_from_angle_vector(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result)
{
	map_quat(*result) = Eigen::AngleAxisf(angle_rads, copy(vector));
}

extern "C" void
math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, struct xrt_quat *result)
{
	Eigen::Matrix3f m;
	m << mat->v[0], mat->v[1], mat->v[2], mat->v[3], mat->v[4], mat->v[5], mat->v[6], mat->v[7], mat->v[8];

	Eigen::Quaternionf q(m);
	map_quat(*result) = q;
}

extern "C" void
math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x, const struct xrt_vec3 *plus_z, struct xrt_quat *result)
{
	xrt_vec3 plus_y;
	math_vec3_cross(plus_z, plus_x, &plus_y);

	xrt_matrix_3x3 m = {{
	    plus_x->x,
	    plus_y.x,
	    plus_z->x,
	    plus_x->y,
	    plus_y.y,
	    plus_z->y,
	    plus_x->z,
	    plus_y.z,
	    plus_z->z,
	}};

	math_quat_from_matrix_3x3(&m, result);
}

static bool
quat_validate(const float precision, const struct xrt_quat *quat)
{
	assert(quat != NULL);
	auto rot = copy(*quat);

	/*
	 * This was originally squaredNorm, but that could result in a norm
	 * value that was further from 1.0f then FLOAT_EPSILON (two).
	 *
	 * Our tracking system would produce such orientations and looping those
	 * back into say a quad layer would cause this to fail. And even
	 * normalizing the quat would not fix this as normalizations uses
	 * non-squared "length" which does fall into the range and doesn't
	 * change the elements of the quat.
	 */
	auto norm = rot.norm();
	if (norm > 1.0f + precision || norm < 1.0f - precision) {
		return false;
	}

	// Technically not yet a required check, but easier to stop problems
	// now than once denormalized numbers pollute the rest of our state.
	// see https://gitlab.khronos.org/openxr/openxr/issues/922
	if (!rot.coeffs().allFinite()) {
		return false;
	}

	return true;
}

extern "C" bool
math_quat_validate(const struct xrt_quat *quat)
{
	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
	return quat_validate(FLOAT_EPSILON, quat);
}

extern "C" bool
math_quat_validate_within_1_percent(const struct xrt_quat *quat)
{
	return quat_validate(0.01, quat);
}

extern "C" void
math_quat_invert(const struct xrt_quat *quat, struct xrt_quat *out_quat)
{
	map_quat(*out_quat) = map_quat(*quat).conjugate();
}

extern "C" void
math_quat_normalize(struct xrt_quat *inout)
{
	assert(inout != NULL);
	map_quat(*inout).normalize();
}

extern "C" bool
math_quat_ensure_normalized(struct xrt_quat *inout)
{
	assert(inout != NULL);

	if (math_quat_validate(inout))
		return true;

	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
	const float TOLERANCE = FLOAT_EPSILON * 5;

	auto rot = copy(*inout);
	auto norm = rot.norm();
	if (norm > 1.0f + TOLERANCE || norm < 1.0f - TOLERANCE) {
		return false;
	}

	map_quat(*inout).normalize();
	return true;
}

extern "C" void
math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto q = l * r;

	map_quat(*result) = q;
}

extern "C" void
math_quat_rotate_vec3(const struct xrt_quat *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto v = l * r;

	map_vec3(*result) = v;
}

extern "C" void
math_quat_rotate_derivative(const struct xrt_quat *quat, const struct xrt_vec3 *deriv, struct xrt_vec3 *result)
{
	assert(quat != NULL);
	assert(deriv != NULL);
	assert(result != NULL);

	auto l = copy(quat);
	auto m = Eigen::Quaternionf(0.0f, deriv->x, deriv->y, deriv->z);
	auto r = l.conjugate();

	auto v = l * m * r;

	struct xrt_vec3 ret = {v.x(), v.y(), v.z()};
	*result = ret;
}

extern "C" void
math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float t, struct xrt_quat *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	map_quat(*result) = l.slerp(t, r);
}

/*
 *
 * Exported matrix functions.
 *
 */

void
math_matrix_2x2_multiply(const struct xrt_matrix_2x2 *left,
                         const struct xrt_matrix_2x2 *right,
                         struct xrt_matrix_2x2 *result)
{
	result->v[0] = left->v[0] * right->v[0] + left->v[1] * right->v[2];
	result->v[1] = left->v[0] * right->v[1] + left->v[1] * right->v[3];
	result->v[2] = left->v[2] * right->v[0] + left->v[3] * right->v[2];
	result->v[3] = left->v[2] * right->v[1] + left->v[3] * right->v[3];
}

extern "C" void
math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
{
	Eigen::Matrix3f m;
	m << left->v[0], left->v[1], left->v[2], // 1
	    left->v[3], left->v[4], left->v[5],  // 2
	    left->v[6], left->v[7], left->v[8];  // 3

	map_vec3(*result) = m * copy(right);
}

extern "C" void
math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
                         const struct xrt_matrix_3x3 *right,
                         struct xrt_matrix_3x3 *result)
{
	result->v[0] = left->v[0] * right->v[0] + left->v[1] * right->v[3] + left->v[2] * right->v[6];
	result->v[1] = left->v[0] * right->v[1] + left->v[1] * right->v[4] + left->v[2] * right->v[7];
	result->v[2] = left->v[0] * right->v[2] + left->v[1] * right->v[5] + left->v[2] * right->v[8];

	result->v[3] = left->v[3] * right->v[0] + left->v[4] * right->v[3] + left->v[5] * right->v[6];
	result->v[4] = left->v[3] * right->v[1] + left->v[4] * right->v[4] + left->v[5] * right->v[7];
	result->v[5] = left->v[3] * right->v[2] + left->v[4] * right->v[5] + left->v[5] * right->v[8];

	result->v[6] = left->v[6] * right->v[0] + left->v[7] * right->v[3] + left->v[8] * right->v[6];
	result->v[7] = left->v[6] * right->v[1] + left->v[7] * right->v[4] + left->v[8] * right->v[7];
	result->v[8] = left->v[6] * right->v[2] + left->v[7] * right->v[5] + left->v[8] * right->v[8];
}

extern "C" void
math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
{
	Eigen::Matrix3f m = copy(in);
	map_matrix_3x3(*result) = m.inverse();
}

void
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
{
	map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
}

void
math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
                         const struct xrt_matrix_4x4 *right,
                         struct xrt_matrix_4x4 *result)
{
	map_matrix_4x4(*result) = copy(left) * copy(right);
}

void
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
{
	Eigen::Vector3f position = copy(&pose->position);
	Eigen::Quaternionf orientation = copy(&pose->orientation);

	Eigen::Translation3f translation(position);
	Eigen::Affine3f transformation = translation * orientation;

	map_matrix_4x4(*result) = transformation.matrix().inverse();
}

void
math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
{
	Eigen::Vector3f position = copy(&pose->position);
	Eigen::Quaternionf orientation = copy(&pose->orientation);

	auto scale = Eigen::Scaling(size->x, size->y, size->z);

	Eigen::Translation3f translation(position);
	Eigen::Affine3f transformation = translation * orientation * scale;

	map_matrix_4x4(*result) = transformation.matrix();
}

void
math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view,
                                        const struct xrt_matrix_4x4 *projection,
                                        struct xrt_matrix_4x4 *result)
{
	Eigen::Matrix4f v = copy(view);
	Eigen::Matrix4f v3 = Eigen::Matrix4f::Identity();
	v3.block<3, 3>(0, 0) = v.block<3, 3>(0, 0);
	Eigen::Matrix4f vp = copy(projection) * v3;
	map_matrix_4x4(*result) = vp.inverse();
}

/*
 *
 * Exported Matrix 4x4 functions.
 *
 */

extern "C" void
m_mat4_f64_identity(struct xrt_matrix_4x4_f64 *result)
{
	map_matrix_4x4_f64(*result) = Eigen::Matrix4d::Identity();
}

extern "C" void
m_mat4_f64_invert(const struct xrt_matrix_4x4_f64 *matrix, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Matrix4d m = map_matrix_4x4_f64(*matrix);
	map_matrix_4x4_f64(*result) = m.inverse();
}

extern "C" void
m_mat4_f64_multiply(const struct xrt_matrix_4x4_f64 *left,
                    const struct xrt_matrix_4x4_f64 *right,
                    struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Matrix4d l = map_matrix_4x4_f64(*left);
	Eigen::Matrix4d r = map_matrix_4x4_f64(*right);

	map_matrix_4x4_f64(*result) = l * r;
}

extern "C" void
m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
{
	map_matrix_4x4_f64(*result) = Eigen::Affine3d(copyd(*quat)).matrix();
}

extern "C" void
m_mat4_f64_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Vector3d position = copyd(pose->position);
	Eigen::Quaterniond orientation = copyd(pose->orientation);

	auto scale = Eigen::Scaling(copyd(size));

	Eigen::Translation3d translation(position);
	Eigen::Affine3d transformation = translation * orientation * scale;

	map_matrix_4x4_f64(*result) = transformation.matrix();
}

extern "C" void
m_mat4_f64_view(const struct xrt_pose *pose, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Vector3d position = copyd(pose->position);
	Eigen::Quaterniond orientation = copyd(pose->orientation);

	Eigen::Translation3d translation(position);
	Eigen::Affine3d transformation = translation * orientation;

	map_matrix_4x4_f64(*result) = transformation.matrix().inverse();
}

/*
 *
 * Exported pose functions.
 *
 */

extern "C" bool
math_pose_validate(const struct xrt_pose *pose)
{
	assert(pose != NULL);

	return math_vec3_validate(&pose->position) && math_quat_validate(&pose->orientation);
}

extern "C" void
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(outPose != NULL);

	// Store results to temporary locals so we can do this "in-place"
	// (pose == outPose) if desired. Pure copies here.
	Eigen::Vector3f newPosition = position(*pose);
	Eigen::Quaternionf newOrientation = orientation(*pose);

	// Conjugate legal here since pose must be normalized/unit length.
	newOrientation = newOrientation.conjugate();
	// Use the newly inverted rotation, to rotate position.
	newPosition = -(newOrientation * newPosition);

	position(*outPose) = newPosition;
	orientation(*outPose) = newOrientation;
}

extern "C" void
math_pose_identity(struct xrt_pose *pose)
{
	pose->position.x = 0.0;
	pose->position.y = 0.0;
	pose->position.z = 0.0;
	pose->orientation.x = 0.0;
	pose->orientation.y = 0.0;
	pose->orientation.z = 0.0;
	pose->orientation.w = 1.0;
}

/*!
 * Return the result of transforming a point by a pose/transform.
 */
static inline Eigen::Vector3f
transform_point(const xrt_pose &transform, const xrt_vec3 &point)
{
	return orientation(transform) * map_vec3(point) + position(transform);
}

/*!
 * Return the result of transforming a pose by a pose/transform.
 */
static inline xrt_pose
transform_pose(const xrt_pose &transform, const xrt_pose &pose)
{
	xrt_pose ret;
	position(ret) = transform_point(transform, pose.position);
	orientation(ret) = orientation(transform) * orientation(pose);
	return ret;
}

extern "C" void
math_pose_transform(const struct xrt_pose *transform, const struct xrt_pose *pose, struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(transform != NULL);
	assert(outPose != NULL);

	xrt_pose newPose = transform_pose(*transform, *pose);
	memcpy(outPose, &newPose, sizeof(xrt_pose));
}

extern "C" void
math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec3 *point, struct xrt_vec3 *out_point)
{
	assert(transform != NULL);
	assert(point != NULL);
	assert(out_point != NULL);

	map_vec3(*out_point) = transform_point(*transform, *point);
}


#define BOOST_TEST_MODULE "test_afm_fitting_interaction"

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

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/AFMFit/AFMFitInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>

#include <random>

namespace mjolnir
{
namespace test
{
template<typename realT, typename coordT>
std::vector<realT>
make_image(const std::vector<realT>& radii, const std::vector<coordT>& positions,
           const realT pixel_width, const realT sigma, const realT gamma,
           const std::size_t n_x, const std::size_t n_y)
{
    std::vector<realT> image; image.reserve(n_x * n_y);
    for(std::size_t i_y=0; i_y < n_y; ++i_y)
    {
        const realT y0 = pixel_width * (i_y + 0.5);
        for(std::size_t i_x=0; i_x < n_x; ++i_x)
        {
            const realT x0 = pixel_width * (i_x + 0.5);

            realT sumexp = 1.0; // exp(0)
            for(std::size_t i=0; i<positions.size(); ++i)
            {
                const auto& p = positions.at(i);
                const auto& r = radii.at(i);
                const realT dx = mjolnir::math::X(p) - x0;
                const realT dy = mjolnir::math::Y(p) - y0;
                const realT dz = mjolnir::math::Z(p) - 0.0;

                const realT gx = -0.5 * dx * dx / (sigma * sigma);
                const realT gy = -0.5 * dy * dy / (sigma * sigma);
                const realT hz = (dz + r) / gamma;

                sumexp += std::exp(gx + gy + hz);
            }
            image.push_back(gamma * std::log(sumexp));
        }
    }
    assert(image.size() == n_x * n_y);
    return image;
}
} // test
} // mjolnir

BOOST_AUTO_TEST_CASE(AFMFitting_calc_force)
{
    namespace test = mjolnir::test;

    mjolnir::LoggerManager::set_default_logger("test_afm_fitting_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using interaction_type = mjolnir::AFMFitInteraction<traits_type>;

    const std::size_t num_particles = 5;

    const real_type   k        =  5.0;
    const real_type   gamma    =  1.0;
    const real_type   z0       =  0.0;
    const real_type   cutoff   =  5.01;
    const real_type   margin   =  0.5;
    const real_type   sigma_x  =  2.0;
    const real_type   sigma_y  = sigma_x;
    const real_type   pixel_x  = 10.0;
    const real_type   pixel_y  = pixel_x;
    const std::size_t length_x = 10u;
    const std::size_t length_y = 10u;
    const std::vector<std::pair<std::size_t, real_type>> radii = {
        {0, 1.0}, {1, 2.0}, {2, 3.0}, {3, 4.0}, {4, 5.0}
    };

    std::vector<coord_type> initial_coordinates = {
        coord_type(20.0, 20.0, 10.0),
        coord_type(50.0, 50.0, 15.0),
        coord_type(80.0, 80.0,  5.0),
        coord_type(20.0, 50.0, 20.0),
        coord_type(50.0, 80.0, 10.0)
    };

    const std::vector<real_type> image = test::make_image(
            {1.0, 2.0, 3.0, 4.0, 5.0}, initial_coordinates,
            pixel_x, sigma_x, gamma, length_x, length_y);

    interaction_type interaction(k, gamma, z0, cutoff, margin,
            sigma_x, sigma_y, pixel_x, pixel_y, length_x, length_y,
            radii, image);

    std::mt19937 mt(123456789);

    for(std::size_t trial = 0; trial < 1000; ++trial)
    {
        system_type sys(num_particles, boundary_type{});
        test::clear_everything(sys);

        for(std::size_t i=0; i<sys.size(); ++i)
        {
            sys.position(i)  = initial_coordinates.at(i);
        }
        test::apply_random_perturbation(sys, mt, 0.01);

        interaction.initialize(sys);

        constexpr real_type tol = 2e-3;
        constexpr real_type dr  = 1e-4;

        test::check_force(sys, interaction, tol, dr, false);
        test::check_force_and_energy(sys, interaction, tol);
    }
}


/*
 * 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));
  }
}


/**
 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/
#include <boost/program_options.hpp>
#include <libff/common/profiling.hpp>

#include <libsnark/common/default_types/ram_zksnark_pp.hpp>
#include <libsnark/relations/ram_computations/memory/examples/memory_contents_examples.hpp>
#include <libsnark/relations/ram_computations/rams/examples/ram_examples.hpp>
#include <libsnark/relations/ram_computations/rams/tinyram/tinyram_params.hpp>
#include <libsnark/zk_proof_systems/zksnark/ram_zksnark/examples/run_ram_zksnark.hpp>
#include <libsnark/zk_proof_systems/zksnark/ram_zksnark/ram_zksnark.hpp>

using namespace libsnark;

template<typename FieldT>
void simulate_random_memory_contents(const tinyram_architecture_params &ap, const size_t input_size, const size_t program_size)
{
    const size_t num_addresses = 1ul<<ap.dwaddr_len();
    const size_t value_size = 2 * ap.w;
    memory_contents init_random = random_memory_contents(num_addresses, value_size, program_size + (input_size + 1)/2);

    libff::enter_block("Initialize random delegated memory");
    delegated_ra_memory<FieldT> dm_random(num_addresses, value_size, init_random);
    libff::leave_block("Initialize random delegated memory");
}

template<typename ppT>
void profile_ram_zksnark_verifier(const tinyram_architecture_params &ap, const size_t input_size, const size_t program_size)
{
    typedef ram_zksnark_machine_pp<ppT> ramT;
    const size_t time_bound  = 10;

    const size_t boot_trace_size_bound = program_size + input_size;
    const ram_example<ramT> example = gen_ram_example_complex<ramT>(ap, boot_trace_size_bound, time_bound, true);

    ram_zksnark_proof<ppT> pi;
    ram_zksnark_verification_key<ppT> vk = ram_zksnark_verification_key<ppT>::dummy_verification_key(ap);

    libff::enter_block("Verify fake proof");
    ram_zksnark_verifier<ppT>(vk, example.boot_trace, time_bound, pi);
    libff::leave_block("Verify fake proof");
}

template<typename ppT>
void print_ram_zksnark_verifier_profiling()
{
    libff::inhibit_profiling_info = true;
    for (size_t w : { 16, 32 })
    {
        const size_t k = 16;

        for (size_t input_size : { 0, 10, 100 })
        {
            for (size_t program_size = 10; program_size <= 10000; program_size *= 10)
            {
                const tinyram_architecture_params ap(w, k);

                profile_ram_zksnark_verifier<ppT>(ap, input_size, program_size);

                const double input_map = libff::last_times["Call to ram_zksnark_verifier_input_map"];
                const double preprocessing = libff::last_times["Call to r1cs_ppzksnark_verifier_process_vk"];
                const double accumulate = libff::last_times["Call to r1cs_ppzksnark_IC_query::accumulate"];
                const double pairings = libff::last_times["Online pairing computations"];
                const double total = libff::last_times["Call to ram_zksnark_verifier"];
                const double rest = total - (input_map + preprocessing + accumulate + pairings);

                const double delegated_ra_memory_init = libff::last_times["Construct delegated_ra_memory from memory map"];
                simulate_random_memory_contents<libff::Fr<typename ppT::curve_A_pp> >(ap, input_size, program_size);
                const double delegated_ra_memory_init_random = libff::last_times["Initialize random delegated memory"];
                const double input_map_random = input_map - delegated_ra_memory_init + delegated_ra_memory_init_random;
                const double total_random = total - delegated_ra_memory_init + delegated_ra_memory_init_random;

                printf("w = %zu, k = %zu, program_size = %zu, input_size = %zu, input_map = %0.2fms, preprocessing = %0.2fms, accumulate = %0.2fms, pairings = %0.2fms, rest = %0.2fms, total = %0.2fms (input_map_random = %0.2fms, total_random = %0.2fms)\n",
                       w, k, program_size, input_size, input_map * 1e-6, preprocessing * 1e-6, accumulate * 1e-6, pairings * 1e-6, rest * 1e-6, total * 1e-6, input_map_random * 1e-6, total_random * 1e-6);
            }
        }
    }
}

template<typename ppT>
void profile_ram_zksnark(const tinyram_architecture_params &ap, const size_t program_size, const size_t input_size, const size_t time_bound)
{
    typedef ram_zksnark_machine_pp<ppT> ramT;

    const size_t boot_trace_size_bound = program_size + input_size;
    const ram_example<ramT> example = gen_ram_example_complex<ramT>(ap, boot_trace_size_bound, time_bound, true);
    const bool test_serialization = true;
    const bool bit = run_ram_zksnark<ppT>(example, test_serialization);
    assert(bit);
}

namespace po = boost::program_options;

bool process_command_line(const int argc, const char** argv,
                          bool &profile_gp,
                          size_t &w,
                          size_t &k,
                          bool &profile_v,
                          size_t &l)
{
    try
    {
        po::options_description desc("Usage");
        desc.add_options()
            ("help", "print this help message")
            ("profile_gp", "profile generator and prover")
            ("w", po::value<size_t>(&w)->default_value(16), "word size")
            ("k", po::value<size_t>(&k)->default_value(16), "register count")
            ("profile_v", "profile verifier")
            ("v", "print version info")
            ("l", po::value<size_t>(&l)->default_value(10), "program length");

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("v"))
        {
            libff::print_compilation_info();
            exit(0);
        }

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        profile_gp = vm.count("profile_gp");
        profile_v = vm.count("profile_v");

        if (!(vm.count("profile_gp") ^ vm.count("profile_v")))
        {
            std::cout << "Must choose between profiling generator/prover and profiling verifier (see --help)\n";
            return false;
        }

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}

int main(int argc, const char* argv[])
{
    libff::start_profiling();
    ram_zksnark_PCD_pp<default_ram_zksnark_pp>::init_public_params();

    bool profile_gp;
    size_t w;
    size_t k;
    bool profile_v;
    size_t l;

    if (!process_command_line(argc, argv, profile_gp, w, k, profile_v, l))
    {
        return 1;
    }

    tinyram_architecture_params ap(w, k);

    if (profile_gp)
    {
        profile_ram_zksnark<default_ram_zksnark_pp>(ap, 100, 100, 10); // w, k, l, n, T
    }

    if (profile_v)
    {
        profile_ram_zksnark_verifier<default_ram_zksnark_pp>(ap, l/2, l/2);
    }
}