// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP

#include <boost/geometry/core/assert.hpp>

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>
#include <boost/geometry/algorithms/detail/overlay/turn_info.hpp>
#include <boost/geometry/iterators/closing_iterator.hpp>
#include <boost/geometry/iterators/ever_circling_iterator.hpp>
#include <boost/geometry/strategies/side.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

// TODO: move this to /util/
template <typename T>
inline std::pair<T, T> ordered_pair(T const& first, T const& second)
{
    return first < second ? std::make_pair(first, second) : std::make_pair(second, first);
}

namespace left_turns
{

template <typename Vector>
inline int get_quadrant(Vector const& vector)
{
    // Return quadrant as layouted in the code below:
    // 3 | 0
    // -----
    // 2 | 1
    return geometry::get<1>(vector) >= 0
        ? (geometry::get<0>(vector)  < 0 ? 3 : 0)
        : (geometry::get<0>(vector)  < 0 ? 2 : 1)
        ;
}

template <typename Vector>
inline int squared_length(Vector const& vector)
{
    return geometry::get<0>(vector) * geometry::get<0>(vector)
         + geometry::get<1>(vector) * geometry::get<1>(vector)
         ;
}

template <typename Point>
struct angle_less
{
    typedef Point vector_type;
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point>::type
    >::type side_strategy_type;

    angle_less(Point const& origin)
        : m_origin(origin)
    {}

    template <typename Angle>
    inline bool operator()(Angle const& p, Angle const& q) const
    {
        // Vector origin -> p and origin -> q
        vector_type pv = p.point;
        vector_type qv = q.point;
        geometry::subtract_point(pv, m_origin);
        geometry::subtract_point(qv, m_origin);

        int const quadrant_p = get_quadrant(pv);
        int const quadrant_q = get_quadrant(qv);
        if (quadrant_p != quadrant_q)
        {
            return quadrant_p < quadrant_q;
        }
        // Same quadrant, check if p is located left of q
        int const side = side_strategy_type::apply(m_origin, q.point,
                    p.point);
        if (side != 0)
        {
            return side == 1;
        }
        // Collinear, check if one is incoming, incoming angles come first
        if (p.incoming != q.incoming)
        {
            return int(p.incoming) < int(q.incoming);
        }
        // Same quadrant/side/direction, return longest first
        // TODO: maybe not necessary, decide this
        int const length_p = squared_length(pv);
        int const length_q = squared_length(qv);
        if (length_p != length_q)
        {
            return squared_length(pv) > squared_length(qv);
        }
        // They are still the same. Just compare on seg_id
        return p.seg_id < q.seg_id;
    }

private:
    Point m_origin;
};

template <typename Point>
struct angle_equal_to
{
    typedef Point vector_type;
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Point>::type
    >::type side_strategy_type;

    inline angle_equal_to(Point const& origin)
        : m_origin(origin)
    {}

    template <typename Angle>
    inline bool operator()(Angle const& p, Angle const& q) const
    {
        // Vector origin -> p and origin -> q
        vector_type pv = p.point;
        vector_type qv = q.point;
        geometry::subtract_point(pv, m_origin);
        geometry::subtract_point(qv, m_origin);

        if (get_quadrant(pv) != get_quadrant(qv))
        {
            return false;
        }
        // Same quadrant, check if p/q are collinear
        int const side = side_strategy_type::apply(m_origin, q.point,
                    p.point);
        return side == 0;
    }

private:
    Point m_origin;
};

template <typename AngleCollection, typename Turns>
inline void get_left_turns(AngleCollection const& sorted_angles,
        Turns& turns)
{
    std::set<int> good_incoming;
    std::set<int> good_outgoing;

    for (typename boost::range_iterator<AngleCollection const>::type it =
        sorted_angles.begin(); it != sorted_angles.end(); ++it)
    {
        if (!it->blocked)
        {
            if (it->incoming)
            {
                good_incoming.insert(it->turn_index);
            }
            else
            {
                good_outgoing.insert(it->turn_index);
            }
        }
    }

    if (good_incoming.empty() || good_outgoing.empty())
    {
        return;
    }

    for (typename boost::range_iterator<AngleCollection const>::type it =
        sorted_angles.begin(); it != sorted_angles.end(); ++it)
    {
        if (good_incoming.count(it->turn_index) == 0
            || good_outgoing.count(it->turn_index) == 0)
        {
            turns[it->turn_index].remove_on_multi = true;
        }
    }
}

//! Returns the number of clusters
template <typename Point, typename AngleCollection>
inline std::size_t assign_cluster_indices(AngleCollection& sorted, Point const& origin)
{
    // Assign same cluster_index for all turns in same direction
    BOOST_GEOMETRY_ASSERT(boost::size(sorted) >= 4u);

    angle_equal_to<Point> comparator(origin);
    typename boost::range_iterator<AngleCollection>::type it = sorted.begin();

    std::size_t cluster_index = 0;
    it->cluster_index = cluster_index;
    typename boost::range_iterator<AngleCollection>::type previous = it++;
    for (; it != sorted.end(); ++it)
    {
        if (!comparator(*previous, *it))
        {
            cluster_index++;
            previous = it;
        }
        it->cluster_index = cluster_index;
    }
    return cluster_index + 1;
}

template <typename AngleCollection>
inline void block_turns(AngleCollection& sorted, std::size_t cluster_size)
{
    BOOST_GEOMETRY_ASSERT(boost::size(sorted) >= 4u && cluster_size > 0);

    std::vector<std::pair<bool, bool> > directions;
    for (std::size_t i = 0; i < cluster_size; i++)
    {
        directions.push_back(std::make_pair(false, false));
    }

    for (typename boost::range_iterator<AngleCollection const>::type it = sorted.begin();
        it != sorted.end(); ++it)
    {
        if (it->incoming)
        {
            directions[it->cluster_index].first = true;
        }
        else
        {
            directions[it->cluster_index].second = true;
        }
    }

    for (typename boost::range_iterator<AngleCollection>::type it = sorted.begin();
        it != sorted.end(); ++it)
    {
        int cluster_index = it->cluster_index;
        int previous_index = cluster_index - 1;
        if (previous_index < 0)
        {
            previous_index = cluster_size - 1;
        }
        int next_index = cluster_index + 1;
        if (next_index >= static_cast<int>(cluster_size))
        {
            next_index = 0;
        }

        if (directions[cluster_index].first
            && directions[cluster_index].second)
        {
            it->blocked = true;
        }
        else if (!directions[cluster_index].first
            && directions[cluster_index].second
            && directions[previous_index].second)
        {
            // Only outgoing, previous was also outgoing: block this one
            it->blocked = true;
        }
        else if (directions[cluster_index].first
            && !directions[cluster_index].second
            && !directions[previous_index].first
            && directions[previous_index].second)
        {
            // Only incoming, previous was only outgoing: block this one
            it->blocked = true;
        }
        else if (directions[cluster_index].first
            && !directions[cluster_index].second
            && directions[next_index].first
            && !directions[next_index].second)
        {
            // Only incoming, next also incoming, block this one
            it->blocked = true;
        }
    }
}

#if defined(BOOST_GEOMETRY_BUFFER_ENLARGED_CLUSTERS)
template <typename AngleCollection, typename Point>
inline bool has_rounding_issues(AngleCollection const& angles, Point const& origin)
{
    for (typename boost::range_iterator<AngleCollection const>::type it =
        angles.begin(); it != angles.end(); ++it)
    {
        // Vector origin -> p and origin -> q
        typedef Point vector_type;
        vector_type v = it->point;
        geometry::subtract_point(v, origin);
        return geometry::math::abs(geometry::get<0>(v)) <= 1
            || geometry::math::abs(geometry::get<1>(v)) <= 1
            ;
    }
    return false;
}
#endif

}  // namespace left_turns

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_GET_LEFT_TURNS_HPP


#include <iostream>
#include "GUI/GlutGUI/GLApp.h"
#include "Framework/Framework/SceneGraph.h"
#include "Dynamics/EmbeddedMethod/EmbeddedFiniteElement.h"
#include "Dynamics/ParticleSystem/StaticBoundary.h"
#include "Dynamics/ParticleSystem/ElasticityModule.h"
#include "Dynamics/ParticleSystem/ParticleElasticBody.h"
#include "Rendering/SurfaceMeshRender.h"
#include "Rendering/PointRenderModule.h"
#include <boost/property_tree/json_parser.hpp>

#include "Dynamics/EmbeddedMethod/EmbeddedFiniteElement.h"
#include "Dynamics/EmbeddedMethod/EmbeddedMassSpring.h"

using namespace PhysIKA;
using namespace std;

int main(int argc, char* argv[])
{
    Log::sendMessage(Log::Info, "Simulation start");

    SceneGraph& scene = SceneGraph::getInstance();

    std::shared_ptr<StaticBoundary<DataType3f>> root = scene.createNewScene<StaticBoundary<DataType3f>>();
    root->loadCube(Vector3f(0), Vector3f(1), 0.005f, true);

    const string discret_m = argc != 3 ? "fem" : argv[1];
    const string calcu_m   = argc != 3 ? "implicit_euler" : argv[2];

    if (discret_m == "tet_fem")
    {
        std::shared_ptr<EmbeddedFiniteElement<DataType3f>> bunny = std::make_shared<EmbeddedFiniteElement<DataType3f>>();
        root->addParticleSystem(bunny);
        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");

        // bunny->scale(1.0 / 6);
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        // bunny->getElasticitySolver()->setIterationNumber(10);
        boost::property_tree::ptree pt;
        const std::string           jsonfile_path = "../../Media/bunny/embedded_finite_element.json";
        read_json(jsonfile_path, pt);
        bunny->init_problem_and_solver(pt);
    }
    else if (discret_m == "mass_spring")
    {
        std::shared_ptr<EmbeddedMassSpring<DataType3f>> bunny = std::make_shared<EmbeddedMassSpring<DataType3f>>();
        root->addParticleSystem(bunny);

        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");

        // bunny->scale(1.0 / 6);
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        // bunny->getElasticitySolver()->setIterationNumber(10);
        boost::property_tree::ptree pt;
        const std::string           jsonfile_path = "../../Media/bunny/embedded_mass_spring.json";
        read_json(jsonfile_path, pt);
        bunny->init_problem_and_solver(pt);
    }
    else if (discret_m == "voxel")
    {
        std::shared_ptr<ParticleElasticBody<DataType3f>> bunny = std::make_shared<ParticleElasticBody<DataType3f>>();
        root->addParticleSystem(bunny);

        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        bunny->getElasticitySolver()->setIterationNumber(10);
    }
    else if (discret_m == "particle")
    {
        std::shared_ptr<ParticleElasticBody<DataType3f>> bunny = std::make_shared<ParticleElasticBody<DataType3f>>();
        root->addParticleSystem(bunny);

        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        bunny->getElasticitySolver()->setIterationNumber(10);
    }
    else if (discret_m == "hybrid")
    {
        std::shared_ptr<ParticleElasticBody<DataType3f>> bunny = std::make_shared<ParticleElasticBody<DataType3f>>();
        root->addParticleSystem(bunny);

        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        bunny->getElasticitySolver()->setIterationNumber(10);
    }
    else
    {
        std::shared_ptr<EmbeddedFiniteElement<DataType3f>> bunny = std::make_shared<EmbeddedFiniteElement<DataType3f>>();
        root->addParticleSystem(bunny);
        auto m_pointsRender = std::make_shared<PointRenderModule>();
        m_pointsRender->setColor(Vector3f(0, 1, 1));
        bunny->addVisualModule(m_pointsRender);

        bunny->setMass(1.0);
        bunny->loadParticles("../../Media/bunny/bunny_points.obj");
        bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");

        // bunny->scale(1.0 / 6);
        bunny->translate(Vector3f(0.5, 0.2, 0.5));
        bunny->setVisible(true);

        auto sRender = std::make_shared<SurfaceMeshRender>();
        bunny->getSurfaceNode()->addVisualModule(sRender);
        sRender->setColor(Vector3f(1, 1, 0));

        // bunny->getElasticitySolver()->setIterationNumber(10);
        boost::property_tree::ptree pt;
        const std::string           jsonfile_path = "../../Media/bunny/embedded_finite_element.json";
        read_json(jsonfile_path, pt);
        bunny->init_problem_and_solver(pt);
    }

    GLApp window;
    window.createWindow(1024, 768);

    window.mainLoop();

    return 0;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <boost/math/special_functions/factorials.hpp>
#include <eve/function/double_factorial.hpp>
#include <eve/constant/inf.hpp>
#include <type_traits>

TTS_CASE_TPL("Check eve::double_factorial return type", EVE_TYPE)
{
  if constexpr(eve::simd_value<T>)
  {
    using r_t = eve::wide<double, eve::cardinal_t<T>>;
    TTS_EXPR_IS(eve::double_factorial(T()), r_t);
  }
  else
  {
    TTS_EXPR_IS(eve::double_factorial(T()), double);
  }
}

TTS_CASE_TPL("Check eve::double_factorial behavior", EVE_TYPE)
{
  using r_t = std::conditional_t<eve::simd_value<T>
    , eve::wide<double, eve::cardinal_t<T>>
   , double>;
  TTS_ULP_EQUAL(eve::double_factorial(T(10)) , r_t(boost::math::double_factorial<double>(10)), 0.5);
  TTS_ULP_EQUAL(eve::double_factorial(T( 5)) , r_t(boost::math::double_factorial<double>( 5)), 0.5);
  TTS_ULP_EQUAL(eve::double_factorial(T(180))    , r_t(boost::math::double_factorial<double>( 180)), 1.0);
  TTS_ULP_EQUAL(eve::double_factorial(T(181))    , r_t(boost::math::double_factorial<double>( 181)), 1.0);
  if constexpr(sizeof(eve::element_type_t<T>) > 1)
  {
    TTS_ULP_EQUAL(eve::double_factorial(T(301))    , eve::inf(eve::as<r_t>()), 0);
    TTS_ULP_EQUAL(eve::double_factorial(T(302))    , eve::inf(eve::as<r_t>()), 0);
  }
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/arithmetic/dec.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/control/expr_iif.hpp>
#include <boost/preprocessor/list/adt.hpp>
#include <boost/preprocessor/repetition/for.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>
#include <pup.h>
#include <vector>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "Parallel/CharmPupable.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"  // IWYU pragma: keep
#include "Utilities/Math.hpp"
#include "Utilities/TMPL.hpp"

/// \cond
class DataVector;

/// \endcond

// IWYU pragma: no_forward_declare Tensor

namespace EquationsOfState {

/*!
 * \ingroup EquationsOfStateGroup
 * \brief An equation of state given by parametrized enthalpy
 *
 * This equation of state is determined as a function of \f$x =
 * \ln(\rho/\rho_0)\f$ where \f$\rho\f$ is the rest mass density and
 * \f$\rho_0\f$ is the provided reference density.
 * The pseudo-enthalpy \f$h \equiv (p + rho  + u)/rho\f$
 * is expanded as
 *
 * \f{equation}
 * h(x) = \sum_i a_i x^i + \sum_j b_j \sin(jkx) + c_j \cos(jkx)
 * \f}
 *
 * This form allows for convenient calculation of thermodynamic
 * quantities for a cold equation of state. For example
 *
 * \f{equation}
 * h(x) = \frac{d e} {d \rho} |_{x = \log(\rho/\rho_0)}
 * \f}
 *
 * where \f$e\f$ is the total energy density.  At the same time \f$ dx =
 * d\rho/\rho \f$ so \f$ \rho_0 e^x dx = d \rho \f$ Therefore,
 *
 * \f{equation}
 * e(x) - e(x_0) = \int_{x_0}^x h(x') e^{x'} dx '
 * \f}
 *
 * This can be computed analytically because
 *
 * \f{equation}
 *  \int a_i \frac{x^i}{i!} e^{x} dx = \sum_{j \leq i} a_i (-1)^{i-j}
 * \frac{(x)^{j}}{j!}
 * + C \f}
 *
 * and
 *
 * \f{equation}
 * \int b_j \sin(j k x) e^x dx = b_j e^x \frac{\sin(jkx) - j k \cos(jkx)}{j^2
 * k^2 + 1} \f}
 *
 * \f{equation}
 * \int c_j \cos(j k x) e^x dx = b_j e^x \frac{\cos(jkx) + j k \sin(jkx)}{j^2
 * k^2 + 1} \f}
 *
 * From this most other thermodynamic quantities can be computed
 * analytically
 *
 * The internal energy density
 * \f{equation}
 * \epsilon(x)\rho(x) = e(x)  - \rho(x)
 * \f}
 *
 * The pressure
 * \f{equation}
 * p(x) = \rho(x) h(x) - e(x)
 * \f}
 *
 * The derivative of the pressure with respect to the rest mass density
 * \f{equation}
 * \chi(x) = \frac{dp}{d\rho} |_{x = x(\rho)} = \frac{dh}{dx}
 * \f}
 *
 * Below the minimum density, a spectral parameterization
 * is used.
 *
 *
 *
 */
template <typename LowDensityEoS>
class Enthalpy : public EquationOfState<true, 1> {
 private:
  struct Coefficients {
    std::vector<double> polynomial_coefficients;
    std::vector<double> sin_coefficients;
    std::vector<double> cos_coefficients;
    double trig_scale;
    double reference_density;
    bool has_exponential_prefactor;
    double exponential_external_constant;
    Coefficients() = default;
    ~Coefficients() = default;
    Coefficients(const Coefficients& coefficients) = default;
    Coefficients(std::vector<double> in_polynomial_coefficients,
                 std::vector<double> in_sin_coefficients,
                 std::vector<double> in_cos_coefficients, double in_trig_scale,
                 double in_reference_density,
                 double in_exponential_constant =
                     std::numeric_limits<double>::quiet_NaN());

    Enthalpy<LowDensityEoS>::Coefficients compute_exponential_integral(
        const std::pair<double, double>& initial_condition);
    Enthalpy<LowDensityEoS>::Coefficients compute_derivative();
    void pup(PUP::er& p);
  };

 public:
  static constexpr size_t thermodynamic_dim = 1;
  static constexpr bool is_relativistic = true;

  struct ReferenceDensity {
    using type = double;
    static constexpr Options::String help = {"Reference density rho_0"};
    static double lower_bound() { return 0.0; }
  };

  struct MinimumDensity {
    using type = double;
    static constexpr Options::String help = {
        "Minimum valid density rho_min,"
        " for this parametrization"};
    static double lower_bound() { return 0.0; }
  };
  struct MaximumDensity {
    using type = double;
    static constexpr Options::String help = {"Maximum density for this EoS"};
    static double lower_bound() { return 0.0; }
  };

  struct PolynomialCoefficients {
    using type = std::vector<double>;
    static constexpr Options::String help = {"Polynomial coefficients a_i"};
  };

  struct TrigScaling {
    using type = double;
    static constexpr Options::String help = {
        "Fundamental wavenumber of trig "
        "functions, k"};
    static double lower_bound() { return 0.0; }
  };

  struct SinCoefficients {
    using type = std::vector<double>;
    static constexpr Options::String help = {"Sine coefficients b_j"};
  };
  struct CosCoefficients {
    using type = std::vector<double>;
    static constexpr Options::String help = {"Cosine coefficients c_j"};
  };
  struct StitchedLowDensityEoS {
    using type = LowDensityEoS;
    static std::string name() {
      return pretty_type::short_name<LowDensityEoS>();
    }
    static constexpr Options::String help = {
        "Low density EoS stitched at the MinimumDensity"};
  };

  struct TransitionDeltaEpsilon {
    using type = double;
    static constexpr Options::String help = {
        "the change in internal energy across the low-"
        "to-high-density transition, generically 0.0"};
    static double lower_bound() { return 0.0; }
  };

  static constexpr Options::String help = {
      "An EoS with a parametrized value h(log(rho/rho_0)) with h the specific "
      "enthalpy and rho the baryon rest mass density.  The enthalpy is "
      "expanded as a sum of polynomial terms and trigonometric corrections. "
      "let x = log(rho/rho_0) in"
      "h(x) = \\sum_i a_ix^i + \\sum_j b_jsin(k * j * x) + c_jcos(k * j * x) "
      "Note that rho(x)(1+epsilon(x)) = int_0^x e^x' h((x') dx' can be "
      "computed "
      "analytically, and therefore so can "
      "P(x) = rho(x) * (h(x) - (1 + epsilon(x))) "};

  using options =
      tmpl::list<ReferenceDensity, MaximumDensity, MinimumDensity, TrigScaling,
                 PolynomialCoefficients, SinCoefficients, CosCoefficients,
                 StitchedLowDensityEoS, TransitionDeltaEpsilon>;

  Enthalpy() = default;
  Enthalpy(const Enthalpy&) = default;
  Enthalpy& operator=(const Enthalpy&) = default;
  Enthalpy(Enthalpy&&) = default;
  Enthalpy& operator=(Enthalpy&&) = default;
  ~Enthalpy() override = default;

  Enthalpy(double reference_density, double max_density, double min_density,
           double trig_scale,
           const std::vector<double>& polynomial_coefficients,
           const std::vector<double>& sin_coefficients,
           const std::vector<double>& cos_coefficients,
           const LowDensityEoS& low_density_eos,
           const double transition_delta_epsilon);

  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBERS(Enthalpy, 1)

  WRAPPED_PUPable_decl_base_template(  // NOLINT
      SINGLE_ARG(EquationOfState<true, 1>), Enthalpy);

  /// The lower bound of the rest mass density that is valid for this EOS
  double rest_mass_density_lower_bound() const override { return 0.0; }

  /// The upper bound of the rest mass density that is valid for this EOS
  double rest_mass_density_upper_bound() const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_lower_bound(
      const double /* rest_mass_density */) const override {
    return 0.0;
  }

  /// The upper bound of the specific internal energy that is valid for this EOS
  /// at the given rest mass density \f$\rho\f$
  double specific_internal_energy_upper_bound(
      const double /* rest_mass_density */) const override {
    return std::numeric_limits<double>::max();
  }

  /// The lower bound of the specific enthalpy that is valid for this EOS
  double specific_enthalpy_lower_bound() const override { return 1.0; }

 private:
  EQUATION_OF_STATE_FORWARD_DECLARE_MEMBER_IMPLS(1)

  SPECTRE_ALWAYS_INLINE
  bool in_low_density_domain(const double density) const {
    return density < minimum_density_;
  }

  double x_from_density(const double density) const;
  double density_from_x(const double x) const;
  double energy_density_from_log_density(const double x) const;
  static double evaluate_coefficients(
      const Enthalpy::Coefficients& coefficients, const double x);

  double chi_from_density(const double density) const;
  double specific_internal_energy_from_density(const double density) const;
  double specific_enthalpy_from_density(const double density) const;
  double pressure_from_density(const double density) const;
  double pressure_from_log_density(const double x) const;
  double rest_mass_density_from_enthalpy(const double specific_enthalpy) const;

  double reference_density_ = std::numeric_limits<double>::signaling_NaN();
  double minimum_density_ = std::numeric_limits<double>::signaling_NaN();
  double maximum_density_ = std::numeric_limits<double>::signaling_NaN();
  double minimum_enthalpy_ = std::numeric_limits<double>::signaling_NaN();

  LowDensityEoS low_density_eos_;
  Coefficients coefficients_;
  Coefficients exponential_integral_coefficients_;
  Coefficients derivative_coefficients_;
};

}  // namespace EquationsOfState


/**
 * @author Luca Marchionni
 * @author Bence Magyar
 * @author Enrique Fernández
 * @author Paul Mathieu
 * @author Gérald Lelong
 */

#include <ackermann_controller/odometry.h>
#include <boost/bind.hpp>

namespace ackermann_controller
{

namespace bacc = boost::accumulators;

Odometry::Odometry(size_t velocity_rolling_window_size)
    : timestamp_(0.0)
    , x_(0.0)
    , y_(0.0)
    , heading_(0.0)
    , linear_(0.0)
    , angular_(0.0)
    , wheelbase_(1.0)
    , velocity_rolling_window_size_(velocity_rolling_window_size)
    , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
    , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
    , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
{
}

void Odometry::init(const ros::Time& time)
{
    // Reset accumulators and timestamp:
    resetAccumulators();
    timestamp_ = time;
}

bool Odometry::update(
    const std::vector<ActuatedJoint>& steering_joints,
    const std::vector<Wheel>& odometry_joints,
    const ros::Time &time)
{
    double linear_sum = 0.0;
    double angular_sum = 0.0;
    double steering_angle_sum = 0.0;

    const double dt = (time - timestamp_).toSec();
    timestamp_ = time;

    for (std::vector<Wheel>::const_iterator it = odometry_joints.begin(); it != odometry_joints.end(); ++it)
    {
        const double wheel_est_vel = it->handle_.getVelocity() * dt;
        linear_sum += wheel_est_vel * it->radius_;
    }

    const double linear = linear_sum / odometry_joints.size();

    for (std::vector<ActuatedJoint>::const_iterator it = steering_joints.begin(); it != steering_joints.end(); ++it)
    {
        const double steering_angle = it->getPosition();
        double virtual_steering_angle = std::atan(wheelbase_ * std::tan(steering_angle)/std::abs(wheelbase_ + it->lateral_deviation_ * std::tan(steering_angle)));
        steering_angle_sum += virtual_steering_angle;
        angular_sum += linear * tan(virtual_steering_angle) / wheelbase_;
    }

    const double angular = angular_sum / steering_joints.size();
    const double steering_angle = steering_angle_sum / steering_joints.size();

    /// Integrate odometry:
    const double curvature_radius = wheelbase_ / cos(M_PI/2.0 - steering_angle);

    if (fabs(curvature_radius) > 0.0001)
    {
        const double elapsed_distance = linear;
        const double elapsed_angle = elapsed_distance / curvature_radius;
        const double x_curvature = curvature_radius * sin(elapsed_angle);
        const double y_curvature = curvature_radius * (cos(elapsed_angle) - 1.0);
        const double wheel_heading = heading_ + steering_angle;
        y_ += x_curvature * sin(wheel_heading) + y_curvature * cos(wheel_heading);
        x_ += x_curvature * cos(wheel_heading) - y_curvature * sin(wheel_heading);
        heading_ += elapsed_angle;
    }

    if (dt < 0.0001)
        return false; // Interval too small to integrate with

    /// Estimate speeds using a rolling mean to filter them out:
    linear_acc_(linear/dt);
    angular_acc_(angular/dt);

    linear_ = bacc::rolling_mean(linear_acc_);
    angular_ = bacc::rolling_mean(angular_acc_);

    return true;
}

void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
{
    /// Save last linear and angular velocity:
    linear_ = linear;
    angular_ = angular;

    /// Integrate odometry:
    const double dt = (time - timestamp_).toSec();
    timestamp_ = time;
    integrate_fun_(linear * dt, angular * dt);
}

void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
{
    velocity_rolling_window_size_ = velocity_rolling_window_size;

    resetAccumulators();
}

void Odometry::integrateRungeKutta2(double linear, double angular)
{
    const double direction = heading_ + angular * 0.5;

    /// Runge-Kutta 2nd order integration:
    x_       += linear * cos(direction);
    y_       += linear * sin(direction);
    heading_ += angular;
}

/**
* \brief Other possible integration method provided by the class
* \param linear linear speed
* \param angular angular speed
*/
void Odometry::integrateExact(double linear, double angular)
{
    if (fabs(angular) < 1e-6)
        integrateRungeKutta2(linear, angular);
    else
    {
        /// Exact integration (should solve problems when angular is zero):
        const double heading_old = heading_;
        const double r = linear/angular;
        heading_ += angular;
        x_       +=  r * (sin(heading_) - sin(heading_old));
        y_       += -r * (cos(heading_) - cos(heading_old));
    }
}

void Odometry::resetAccumulators()
{
    linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
    angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
}

}


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ALLSOL_SIMPLE_HPP
#define ALLSOL_SIMPLE_HPP

#include <iostream>
#include <list>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/autodif.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

namespace allsol_simple_sub {

// return index of I_i which has maximum width

template <class T> int search_maxwidth (const ub::vector< interval<T> >& I) {
	int s = I.size();
	int i, mi;
	T m, tmp;

	m = 0.;
	for (i=0; i<s; i++) {
		tmp = width(I(i));
		if (tmp > m) {
			m = tmp; mi = i;
		}
	}

	return mi;
}

// return max width(I_i) / width(J_i)

template <class T> T widthratio_max (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = 0.;

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

// return min width(I_i) / width(J_i)

template <class T> T widthratio_min (const ub::vector< interval<T> >& I, const ub::vector< interval<T> >& J) {
	int s = I.size();
	int i;
	T tmp, r;

	r = std::numeric_limits<T>::max();

	for (i=0; i<s; i++) {
		tmp = width(I(i)) / width(J(i));
		if (tmp < r) r = tmp;
	}

	return r;
}

} // namespace allsol_simple_sub

// find all solution of f in I

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_simple (F f, const ub::vector< interval<T> >& I, int verbose=1)
{
	std::list< ub::vector < interval<T> > > targets;
	targets.push_back(I);
	return allsol_list_simple(f, targets, verbose);
}

// find all solution of f in targets (list of intervals)

template <class T, class F> std::list< ub::vector< interval<T> > >
allsol_list_simple (F f, std::list< ub::vector< interval<T> > > targets, int verbose=1)
{
	int s = (targets.front()).size();
	ub::vector< interval<T> > I, fc, fi, C, CK, K, mvf, I1, I2;
	ub::matrix< interval<T> > fdi, M;
	ub::matrix<T> L, R, E;
	std::list< ub::vector< interval<T> > > solutions, solutions_big;
	typename std::list< ub::vector< interval<T> > >::iterator p, p2;
	int i, j, k, mi;
	T tmp;
	bool r, flag, flag2;
	int count_ne_test = 0;
	int count_ex_test = 0;
	int count_unknown = targets.size();
	int count_ne = 0;
	int count_ex = 0;

	E = ub::identity_matrix<T>(s);

	while (!targets.empty()) {
		if (verbose >= 2) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", unknown: " << count_unknown << ", ne: " << count_ne << ", ex: " << count_ex << "    \r" << std::flush;
		}

		I = targets.front();
		targets.pop_front();
		count_unknown--;

		// non-existence test

		count_ne_test++;

		try {
			fi = f(I);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		if (!zero_in(fi)) {
			count_ne++;
			continue;
		}

		C = mid(I);
		try {
			fc = f(C);
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
		}
		catch (std::domain_error& e) {
			goto label;
		}

		mvf = fc + prod(fdi, I - C);
		if (!zero_in(mvf)) {
			count_ne++;
			continue;
		}

		// existence test

		L = mid(fdi);

		count_ex_test++;

		r = invert(L, R);
		if (!r) goto label;

		M = E - prod(R, fdi);
		CK = C - prod(R, fc);
		K = CK +  prod(M, I - C);
		if (!overlap(K, I)) {
			count_ne++;
			continue;
		}
		if (proper_subset(K, I)) {
			// check whether the solution is already found or not
			flag = true;
			p = solutions.begin();
			p2 = solutions_big.begin();
			while (p != solutions.end()) {
				if (overlap(K, *p)) {
					if (subset(K, *p2)||subset(*p, I)) {
						flag = false;
						break;
					}
					while (true) {
						C = mid(K);
						I1 = C - prod(R, f(C)) + prod(M, K - C);
						K = intersect(K, I1);
						if (subset(K, *p2)) {
							flag2 = true;
							break;
						}
						if (!overlap(K, *p)) {
							flag2 = false;
							break;
						}
					}
					if (flag2 == true) {
						flag = false;
						break;
					} else {
						/* never reach? */
						std::cout << "two overlap intervals includes different solutions";
					}
				}
				p++;
				p2++;
			}
			if (flag) { // new solution found
				if (verbose >= 1) std::cout << I << "(ex)\n";
				solutions_big.push_back(I);
				// iterative refinement
				while (1) {
					C = mid(K);
					I1 = C - prod(R, f(C)) + prod(M, K - C);
					I1 = intersect(K, I1);
					tmp = allsol_simple_sub::widthratio_min(I1, K);
					K = I1;
					if (tmp > 0.9) break;
				}
				solutions.push_back(K);
				count_ex++;
				if (verbose >= 1) std::cout << K << "(ex:improved)\n";
			}
			continue;
		}

		// check the case that solution may exist near boundary.
		// If so, use K as next interval

		if (allsol_simple_sub::widthratio_max(K, I) < 0.9) {
			targets.push_back(K);
			count_unknown++;
			continue;
		}

		I = intersect(I, K);

		label:

		// divide interval

		mi = allsol_simple_sub::search_maxwidth(I);

		tmp = mid(I(mi));
		if (tmp == I(mi).lower() || tmp == I(mi).upper()) {
			std::cout << "too small interval (may be multiple root?):\n" << I << "\n";
			continue;
		}

		I1 = I; I2 = I;
		I1(mi).assign(I1(mi).lower(), tmp);
		I2(mi).assign(tmp, I2(mi).upper());
		targets.push_back(I1);
		targets.push_back(I2);
		count_unknown += 2;
	}

	if (verbose >= 1) {
			std::cout << "ne_test: " << count_ne_test << ", ex_test: " << count_ex_test << ", ne: " << count_ne << ", ex: " << count_ex << "    \n";
	}

	return solutions;
}

} // namespace kv

#endif // ALLSOL_SIMPLE_HPP


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/comparison.hh>
#include <gcs/constraints/linear_equality.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>

#include <util/for_each.hh>

#include <boost/program_options.hpp>

#include <cstdlib>
#include <iostream>
#include <optional>
#include <vector>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::nullopt;
using std::optional;
using std::pair;
using std::string;
using std::to_string;
using std::vector;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof")
        ("extra-constraints", "Use extra constraints described in the MiniCP paper");

    po::options_description all_options{ "All options" };
    all_options.add_options()
        ("size", po::value<int>()->default_value(300), "Size of the problem to solve")
        ;

    all_options.add(display_options);

    po::positional_options_description positional_options;
    positional_options
        .add("size", -1);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .positional(positional_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options] [size]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    cout << "Replicating the MiniCP Magic Series benchmark." << endl;
    cout << "See Laurent D. Michel, Pierre Schaus, Pascal Van Hentenryck:" << endl;
    cout << "\"MiniCP: a lightweight solver for constraint programming.\"" << endl;
    cout << "Math. Program. Comput. 13(1): 133-184 (2021)." << endl;
    cout << "This should take 1193 recursions with default options." << endl;
    cout << endl;

    int size = options_vars["size"].as<int>();
    Problem p = options_vars.count("prove") ? Problem{ Proof{ "magic_series.opb", "magic_series.veripb" } } : Problem{ };

    vector<IntegerVariableID> series;
    for (int v = 0 ; v != size ; ++v)
        series.push_back(p.create_integer_variable(0_i, Integer{ size - 1 }, "series" + to_string(v)));

    for (int i = 0 ; i < size ; ++i) {
        Linear coeff_vars;
        for (int j = 0 ; j < size ; ++j) {
            auto series_j_eq_i = p.create_integer_variable(0_i, 1_i);
            p.post(EqualsIff{ series[j], constant_variable(Integer{ i }), series_j_eq_i == 1_i });
            coeff_vars.emplace_back(1_i, series_j_eq_i);
        }

        coeff_vars.emplace_back(-1_i, series[i]);
        p.post(LinearEquality{ move(coeff_vars), 0_i });
    }

    Linear sum_s;
    for (auto & s : series)
        sum_s.emplace_back(1_i, s);
    p.post(LinearEquality{ move(sum_s), Integer{ size } });

    // Although this is discussed in the text, it isn't included in the executed
    // benchmarks.
    if (options_vars.count("extra-constraints")) {
        Linear sum_mul_s;
        for_each_with_index(series, [&] (IntegerVariableID s, auto idx) {
                sum_mul_s.emplace_back(Integer(idx), s);
                });
        p.post(LinearEquality{ move(sum_mul_s), Integer{ size } });
    }

    p.branch_on(series);

    auto stats = solve_with(p, SolveCallbacks{
            .solution = [&] (const State & s) -> bool {
                cout << "solution:";
                for (auto & v : series)
                    cout << " " << s(v);
                cout << endl;

                return true;
                },
            .guess = [&] (const State & state, IntegerVariableID var) -> vector<Literal> {
                return vector<Literal>{ var == state.lower_bound(var), var != state.lower_bound(var) };
            }
            });

    cout << stats;

    return EXIT_SUCCESS;
}


//
// Copyright (c) 2016-2019 CNRS, INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;
using namespace Eigen;

template<bool local>
Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
{
  Data::Matrix6x res(6,model.nv); res.setZero();
  VectorXd q_integrate (model.nq);
  VectorXd v_integrate (model.nv); v_integrate.setZero();

  forwardKinematics(model,data,q);
  const SE3 oMi_ref = data.oMi[joint_id];

  double eps = 1e-8;
  for(int k=0; k<model.nv; ++k)
  {
    // Integrate along kth direction
    v_integrate[k] = eps;
    q_integrate = integrate(model,q,v_integrate);

    forwardKinematics(model,data,q_integrate);
    const SE3 & oMi = data.oMi[joint_id];

    if (local)
      res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
    else
      res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();

    res.col(k) /= eps;

    v_integrate[k] = 0.;
  }

  return res;
}

template<typename Matrix>
void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
{
  for(int k = 0; k < mat.size(); ++k)
    mat.derived().data()[k] =  math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
}

template<typename JointModel_> struct init;

template<typename JointModel_>
struct init
{
  static JointModel_ run()
  {
    JointModel_ jmodel;
    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    JointModel jmodel((JointModelRX()));

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    JointModel jmodel((JointModelRX()));
    jmodel.addJoint(JointModelRY());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;

  static JointModel run()
  {
    JointModel_ jmodel_ref = init<JointModel_>::run();

    JointModel jmodel(jmodel_ref,1.,0.);
    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

struct FiniteDiffJoint
{
  void operator()(JointModelComposite & /*jmodel*/) const
  {}

  template<typename JointModel>
  void operator()(JointModelBase<JointModel> & /*jmodel*/) const
  {
    typedef typename JointModel::ConfigVector_t CV;
    typedef typename JointModel::TangentVector_t TV;
    typedef typename LieGroup<JointModel>::type LieGroupType;

    JointModel jmodel = init<JointModel>::run();
    std::cout << "name: " << jmodel.classname() << std::endl;

    typename JointModel::JointDataDerived jdata_ = jmodel.createData();
    typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
    DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);

    CV q = LieGroupType().random();
    jmodel.calc(jdata.derived(),q);
    SE3 M_ref(jdata.M());

    CV q_int(q);
    const Eigen::DenseIndex nv = jdata.S().nv();
    TV v(nv); v.setZero();
    double eps = 1e-8;

    Eigen::Matrix<double,6,JointModel::NV> S(6,nv), S_ref(jdata.S().matrix());

    for(int k=0;k<nv;++k)
    {
      v[k] = eps;
      q_int = LieGroupType().integrate(q,v);
      jmodel.calc(jdata.derived(),q_int);
      SE3 M_int = jdata.M();

      S.col(k) = log6(M_ref.inverse()*M_int).toVector();
      S.col(k) /= eps;

      v[k] = 0.;
    }

    BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
    std::cout << "S_ref:\n" << S_ref << std::endl;
    std::cout << "S:\n" << S << std::endl;
  }
};

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE (test_S_finit_diff)
{
  boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
}

BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();
  computeJointJacobians(model,data,q);

  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

  getJointJacobian(model,data,idx,WORLD,Jrh);
  Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));

  getJointJacobian(model,data,idx,LOCAL,Jrh);
  Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
}

BOOST_AUTO_TEST_SUITE_END()


#include "setupScene.h"
#include "optimization.h"
#include "types.h"

#include <iostream>
#include <vector>

#include <Eigen/Geometry>

int main()
{
	//using Rotation = roto::AngleAxisRotation; // Convenience switch
	using Rotation = roto::QuaternionRotation; // Convenience switch
	//using Rotation = roto::MatrixRotation; // Convenience switch

	roto::MeasuredScene<Rotation> measurements;
	roto::OptimizedScene<Rotation> parameters;

	double totalTime = 0.;
	const std::size_t numberOfIterations = 100;
	for(std::size_t i = 0; i < numberOfIterations; i++)
	{
		//std::tie(measurements, parameters) = roto::setupSmallTestScene<Rotation>();
		std::tie(measurements, parameters) = roto::setupBigTestScene<Rotation>();

		totalTime += roto::optimize(measurements, parameters);
	}
	std::cout << "Average processing time: "
						<< totalTime / double(numberOfIterations) << std::endl;

	return 0;
}


#include "splineGeneration.h"

#include <Eigen/Dense>
#include <iostream>
#include <sstream>
#include <numeric>
#include <stdexcept>

using namespace std;
using namespace Eigen;

template <typename DerivedM>
void setConstraintMatrixPart(double time, int derivative_order, MatrixBase<DerivedM> & constraint_matrix, double scaling = 1.0)
{
  double time_power = 1.0;
  typename MatrixBase<DerivedM>::Index num_coefficients = constraint_matrix.cols();

  for (int col = derivative_order; col < num_coefficients; col++)
  {
     double column_power = 1.0;
     for (int i = 0; i< derivative_order; i++)
     {
        column_power *= (col - i);
     }
     constraint_matrix(0, col) = scaling * time_power * column_power;
     time_power *= time;
  }
}

PiecewisePolynomial<double> generateSpline(const SplineInformation& spline_information) {
  int num_segments = spline_information.getNumberOfSegments();
  int num_constraints = spline_information.getNumberOfConstraints();
  int num_coefficients = spline_information.getTotalNumberOfCoefficients();

  if (num_constraints != num_coefficients) {
    stringstream msg;
    msg << "Only the case where the number of coefficients equals the number of variables is currently handled." << endl;
    msg << "Number of coefficients: " << num_coefficients << ", Number of constraints: " << num_constraints << endl;
    throw runtime_error(msg.str().c_str());
  }

  MatrixXd constraint_matrix = MatrixXd::Zero(num_constraints, num_coefficients);
  VectorXd right_hand_side(num_constraints); // should get overwritten completely

  int constraint_row_start = 0;
  std::vector<int> segment_col_starts;
  int segment_col_start = 0;
  for (int i = 0; i < num_segments; i++) {
    segment_col_starts.push_back(segment_col_start);
    segment_col_start += spline_information.getNumberOfCoefficients(i);
  }

  // handle value constraints
  for (int i = 0; i < num_segments; i++) {
    std::vector<ValueConstraint> const & value_constraints = spline_information.getValueConstraints(i);
    for (auto it = value_constraints.begin(); it != value_constraints.end(); ++it) {
      const ValueConstraint& constraint = *it;
      int number_of_coefficients = spline_information.getNumberOfCoefficients(i);
      int segment_col_start = segment_col_starts[i];
      auto constraint_matrix_segment_part = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_start, 1, number_of_coefficients);
      double t_local = constraint.getTime() - spline_information.getStartTime(i);
      setConstraintMatrixPart(t_local, constraint.getDerivativeOrder(), constraint_matrix_segment_part);
      right_hand_side(constraint_row_start) = constraint.getValue();
      constraint_row_start += 1;
    }
  }

  // handle continuity constraints
  std::vector<ContinuityConstraint> continuity_constraints = spline_information.getContinuityConstraints();
  for (auto it = continuity_constraints.begin(); it != continuity_constraints.end(); ++it) {
    const ContinuityConstraint& constraint = *it;
    int first_spline_index = constraint.getFirstSplineIndex();
    int number_of_coefficients_1 = spline_information.getNumberOfCoefficients(first_spline_index);
    auto constraint_matrix_segment_part_1 = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_starts[first_spline_index], 1, number_of_coefficients_1);
    double t_local_1 = spline_information.getEndTime(first_spline_index) - spline_information.getStartTime(first_spline_index);
    setConstraintMatrixPart(t_local_1, constraint.getDerivativeOrder(), constraint_matrix_segment_part_1, 1.0);

    int second_spline_index = constraint.getSecondSplineIndex();
    int number_of_coefficients_2 = spline_information.getNumberOfCoefficients(second_spline_index);
    auto constraint_matrix_segment_part_2 = constraint_matrix.block<1, Dynamic>(constraint_row_start, segment_col_starts[second_spline_index], 1, number_of_coefficients_2);
    setConstraintMatrixPart(0.0, constraint.getDerivativeOrder(), constraint_matrix_segment_part_2, -1.0);

    right_hand_side(constraint_row_start) = 0.0;

    constraint_row_start += 1;
  }

  // solve
  auto decomposition = constraint_matrix.colPivHouseholderQr();
  if (!decomposition.isInvertible())
    throw ConstraintMatrixSingularError();
  VectorXd solution = decomposition.solve(right_hand_side);

  // create Polynomials
  std::vector<Polynomial<double>> polynomials;
  for (int i = 0; i < num_segments; i++) {
    auto coefficients = solution.segment(segment_col_starts[i], spline_information.getNumberOfCoefficients(i));
    polynomials.push_back(Polynomial<double>(coefficients));
  }

  // return a PiecewisePolynomial
  return PiecewisePolynomial<double>(polynomials, spline_information.getSegmentTimes());
}

PiecewisePolynomial<double> twoWaypointCubicSpline(const vector<double>& segment_times, double x0, double xd0, double xf, double xdf, double x1, double x2) {
  const int num_segments = 3;
  assert(segment_times.size() == num_segments + 1);

  int polynomial_order = 3;
  vector<int> segment_polynomial_orders;
  for (int i = 0; i < num_segments; ++i) {
    segment_polynomial_orders.push_back(polynomial_order);
  }

  SplineInformation spline_information(segment_polynomial_orders, segment_times);
  spline_information.addValueConstraint(0, ValueConstraint(0, spline_information.getStartTime(0), x0));
  spline_information.addValueConstraint(0, ValueConstraint(1, spline_information.getStartTime(0), xd0));
  spline_information.addValueConstraint(num_segments - 1, ValueConstraint(0, spline_information.getEndTime(num_segments - 1), xf));
  spline_information.addValueConstraint(num_segments - 1, ValueConstraint(1, spline_information.getEndTime(num_segments - 1), xdf));
  spline_information.addValueConstraint(1, ValueConstraint(0, spline_information.getStartTime(1), x1));
  spline_information.addValueConstraint(2, ValueConstraint(0, spline_information.getStartTime(2), x2));

  int num_knots = num_segments - 1;
  for (int i = 0; i < num_knots; i++) {
    for (int derivative_order = 0; derivative_order < 3; derivative_order++) {
      spline_information.addContinuityConstraint(ContinuityConstraint(derivative_order, i, i + 1));
    }
  }

  return generateSpline(spline_information);
}


#include<bits/stdc++.h>
using namespace std;
#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;

// This header file needs to be included for using Centroid decomposition.
#include "boost/graph/CND/cnd_ds.hpp"
using namespace boost::graph;
int main()
{
    int i,n,a,b;
    scanf("%d",&n);
    //Graph type
    typedef adjacency_list<vecS, vecS, undirectedS> Graph;
    //Initialisation of Graph object
    Graph graph(n);
    for(i=1;i<n;i++)
    {
        scanf("%d %d",&a,&b);
        add_edge(a,b,graph);
    }
    // "CND" class is called which constructs an object which contains the decomposed tree and its root.
    CND<Graph> decomposed_tree(graph,0,n);

    // Printing the centroid of entire input tree or root of the centroid decomposition of input tree.
    printf("%d\n",decomposed_tree.decomposed_root);

    return 0;

}


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

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

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

/**
 * @file    testRegularJacobianFactor.cpp
 * @brief   unit test regular jacobian factors
 * @author  Sungtae An
 * @date    Nov 12, 2014
 */

#include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/TestableAssertions.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

static const size_t fixedDim = 3;
static const size_t nrKeys = 3;

// Keys are assumed to be from 0 to n
namespace {
  namespace simple {
    // Terms we'll use
    const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
      (make_pair(0, Matrix3::Identity()))
      (make_pair(1, 2*Matrix3::Identity()))
      (make_pair(2, 3*Matrix3::Identity()));

    // RHS and sigmas
    const Vector b = (Vector(3) << 1., 2., 3.).finished();
    const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished());
  }

  namespace simple2 {
    // Terms
    const vector<pair<Key, Matrix> > terms2 = list_of<pair<Key,Matrix> >
      (make_pair(0, 2*Matrix3::Identity()))
      (make_pair(1, 4*Matrix3::Identity()))
      (make_pair(2, 6*Matrix3::Identity()));

    // RHS
    const Vector b2 = (Vector(3) << 2., 4., 6.).finished();
  }
}

/* ************************************************************************* */
// Convert from double* to VectorValues
VectorValues double2vv(const double* x,
    const  size_t nrKeys, const size_t dim) {
  // create map with dimensions
  std::map<gtsam::Key, size_t> dims;
  for (size_t i = 0; i < nrKeys; i++)
    dims.insert(make_pair(i, dim));

  size_t n = nrKeys*dim;
  Vector xVec(n);
  for (size_t i = 0; i < n; i++){
    xVec(i) = x[i];
  }
  return VectorValues(xVec, dims);
}

/* ************************************************************************* */
void vv2double(const VectorValues& vv, double* y,
    const  size_t nrKeys, const size_t dim) {
  // create map with dimensions
  std::map<gtsam::Key, size_t> dims;
  for (size_t i = 0; i < nrKeys; i++)
    dims.insert(make_pair(i, dim));

  Vector yvector = vv.vector(dims);
  size_t n = nrKeys*dim;
  for (size_t j = 0; j < n; j++)
    y[j] = yvector[j];
}

/* ************************************************************************* */
TEST(RegularJacobianFactor, constructorNway)
{
  using namespace simple;
  JacobianFactor factor(terms[0].first, terms[0].second,
        terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);

  LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back());
  EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1)));
  EXPECT(assert_equal(b, factor.getb()));
  EXPECT(assert_equal(b, regularFactor.getb()));
  EXPECT(noise == factor.get_model());
  EXPECT(noise == regularFactor.get_model());
}

/* ************************************************************************* */
TEST(RegularJacobianFactor, hessianDiagonal)
{
  using namespace simple;
  JacobianFactor factor(terms[0].first, terms[0].second,
          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);

  // we compute hessian diagonal from the standard Jacobian
  VectorValues expectedHessianDiagonal = factor.hessianDiagonal();

  // we compare against the Raw memory access implementation of hessianDiagonal
  double actualValue[9]={0};
  regularFactor.hessianDiagonal(actualValue);
  VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw));
}

/* ************************************************************************* */
TEST(RegularJacobian, gradientAtZero)
{
  using namespace simple;
  JacobianFactor factor(terms[0].first, terms[0].second,
          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);

  // we compute gradient at zero from the standard Jacobian
  VectorValues expectedGradientAtZero = factor.gradientAtZero();

  //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));

  // we compare against the Raw memory access implementation of gradientAtZero
  double actualValue[9]={0};
  regularFactor.gradientAtZero(actualValue);
  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
}

/* ************************************************************************* */
TEST(RegularJacobian, gradientAtZero_multiFactors)
{
  using namespace simple;
  JacobianFactor factor(terms[0].first, terms[0].second,
          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);

  // we compute gradient at zero from the standard Jacobian
  VectorValues expectedGradientAtZero = factor.gradientAtZero();

  // we compare against the Raw memory access implementation of gradientAtZero
  double actualValue[9]={0};
  regularFactor.gradientAtZero(actualValue);
  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));

  // One more factor
  using namespace simple2;
  JacobianFactor factor2(terms2[0].first, terms2[0].second,
          terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise);
  RegularJacobianFactor<fixedDim> regularFactor2(terms2, b2, noise);

  // we accumulate computed gradient at zero from the standard Jacobian
  VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero());

  // we compare against the Raw memory access implementation of gradientAtZero
  regularFactor2.gradientAtZero(actualValue);
  VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2));

}

/* ************************************************************************* */
TEST(RegularJacobian, multiplyHessianAdd)
{
  using namespace simple;
  JacobianFactor factor(terms[0].first, terms[0].second,
            terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);

  // arbitrary vector X
  VectorValues X;
  X.insert(0, (Vector(3) << 10.,20.,30.).finished());
  X.insert(1, (Vector(3) << 10.,20.,30.).finished());
  X.insert(2, (Vector(3) << 10.,20.,30.).finished());

  // arbitrary vector Y
  VectorValues Y;
  Y.insert(0, (Vector(3) << 10.,10.,10.).finished());
  Y.insert(1, (Vector(3) << 20.,20.,20.).finished());
  Y.insert(2, (Vector(3) << 30.,30.,30.).finished());

  // multiplyHessianAdd Y += alpha*A'A*X
  double alpha = 2.0;
  VectorValues expectedMHA = Y;
  factor.multiplyHessianAdd(alpha, X, expectedMHA);

  // create data for raw memory access
  double XRaw[9];
  vv2double(X, XRaw, nrKeys, fixedDim);

  // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y)
  double actualMHARaw[9];
  vv2double(Y, actualMHARaw, nrKeys, fixedDim);
  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw);
  VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedMHA,actualMHARawVV));

  // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys)
  double actualMHARaw2[9];
  vv2double(Y, actualMHARaw2, nrKeys, fixedDim);
  vector<size_t> dims;
  size_t accumulatedDim = 0;
  for (size_t i = 0; i < nrKeys+1; i++){
    dims.push_back(accumulatedDim);
    accumulatedDim += fixedDim;
  }
  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims);
  VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim);
  EXPECT(assert_equal(expectedMHA,actualMHARawVV2));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


/* test_uniform_smallint_distribution.cpp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/uniform_smallint.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::uniform_smallint<>
#define BOOST_RANDOM_ARG1 a
#define BOOST_RANDOM_ARG2 b
#define BOOST_RANDOM_ARG1_DEFAULT 0
#define BOOST_RANDOM_ARG2_DEFAULT 9
#define BOOST_RANDOM_ARG1_VALUE 5
#define BOOST_RANDOM_ARG2_VALUE 250

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX 9
#define BOOST_RANDOM_DIST1_MIN 5
#define BOOST_RANDOM_DIST1_MAX 9
#define BOOST_RANDOM_DIST2_MIN 5
#define BOOST_RANDOM_DIST2_MAX 250

#define BOOST_RANDOM_TEST1_PARAMS (0, 9)
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 9

#define BOOST_RANDOM_TEST2_PARAMS (10, 19)
#define BOOST_RANDOM_TEST2_MIN 10
#define BOOST_RANDOM_TEST2_MAX 19

#include "test_distribution.ipp"


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#if defined(_MSC_VER)
#pragma warning( disable : 4305 ) // truncation double -> float
#endif // defined(_MSC_VER)

#include <boost/core/ignore_unused.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/srs/epsg.hpp>
#include <boost/geometry/srs/projection.hpp>

#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <test_common/test_point.hpp>

namespace srs = bg::srs;

template <int E, typename P1, typename P2>
void test_one(double lon, double lat,
              typename bg::coordinate_type<P2>::type x,
              typename bg::coordinate_type<P2>::type y)
{
    srs::projection<srs::static_epsg<E> > prj;

    P1 ll;
    bg::set<0>(ll, lon);
    bg::set<1>(ll, lat);

    P2 xy;
    bg::set<0>(xy, 0.0);
    bg::set<1>(xy, 0.0);
    prj.forward(ll, xy);

    BOOST_CHECK_CLOSE(bg::get<0>(xy), x, 0.001);
    BOOST_CHECK_CLOSE(bg::get<1>(xy), y, 0.001);
}

template <typename D, typename P>
void test_deg_rad(double factor)
{
    typedef typename bg::coordinate_type<P>::type coord_type;
    typedef bg::model::point<coord_type, 2, bg::cs::geographic<D> > point_type;

    test_one<28992, point_type, P>(4.897000 * factor, 52.371000 * factor, 121590.388077, 487013.903377);
    test_one<29118, point_type, P>(4.897000 * factor, 52.371000 * factor, 4852882, 9129373);
}

template <typename P>
void test_all()
{
    test_deg_rad<bg::degree, P>(1.0);
    test_deg_rad<bg::radian, P>(bg::math::d2r<double>());
}

int test_main(int, char* [])
{
    // Commented out most the types because otherwise it cannot be linked
    //test_all<int[2]>();
    //test_all<float[2]>();
    //test_all<double[2]>();
    //test_all<test::test_point>();
    //test_all<bg::model::d2::point_xy<int> >();
    ////test_all<bg::model::d2::point_xy<float> >();
    ////test_all<bg::model::d2::point_xy<long double> >();

    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


#ifndef HYPSYS1D_RUNGE_KUTTA_HPP
#define HYPSYS1D_RUNGE_KUTTA_HPP

#include <Eigen/Dense>
#include <map>
#include <memory>

#include <ancse/includes.hpp>
#include <ancse/config.hpp>
#include <ancse/dg_limiting.hpp>
#include <ancse/boundary_condition.hpp>
#include <ancse/rate_of_change.hpp>

/// Interface advancing the solution of a PDE by one step.
class TimeIntegrator {
  public:
    virtual ~TimeIntegrator() = default;

    /// Update 'u1' starting from 'u0' using a time-step `dt`.
    virtual void operator()(Eigen::MatrixXd &u1,
                            const Eigen::MatrixXd &u0,
                            double dt) const = 0;
};

/// Runge-Kutta time integration methods.
/** Note: not all methods are Runge-Kutta methods. Therefore, 'RungeKutta' can
 *        not be the interface. It should be the base for all (explicit)
 *        Runge-Kutta methods.
 */
class RungeKutta : public TimeIntegrator {
  public:
    RungeKutta(std::shared_ptr<RateOfChange> rate_of_change_,
               std::shared_ptr<BoundaryCondition> boundary_condition_,
               std::shared_ptr<Limiting> limiting_)
  : rate_of_change(std::move(rate_of_change_)),
    boundary_condition(std::move(boundary_condition_)),
    limiting(std::move(limiting_))
  {}

  protected:
    void post_euler_step(Eigen::MatrixXd &u) const {
      (*boundary_condition)(u);
      if(limiting != nullptr) {
        (*limiting)(u);
      }
      (*boundary_condition)(u);
    }

  protected:
    std::shared_ptr<RateOfChange> rate_of_change;
    std::shared_ptr<BoundaryCondition> boundary_condition;
    std::shared_ptr<Limiting> limiting;
};

class ForwardEuler : public RungeKutta {
  private:
    using super = RungeKutta;

  public:
    ForwardEuler(std::shared_ptr<RateOfChange> rate_of_change_,
                 std::shared_ptr<BoundaryCondition> boundary_condition_,
                 std::shared_ptr<Limiting> limiting_,
                 int n_rows,
                 int n_cols)
        : super(std::move(rate_of_change_),
                std::move(boundary_condition_),
                std::move(limiting_)),
          dudt(n_rows, n_cols) {}

    virtual void operator()(Eigen::MatrixXd &u1,
                            const Eigen::MatrixXd &u0,
                            double dt) const override {

        (*rate_of_change)(dudt, u0);
        u1 = u0 + dt * dudt;
        post_euler_step(u1);
    }

  private:
    mutable Eigen::MatrixXd dudt;
};

// Mishra lecture notes 5.8.1. p. 76, cf. Toro p. 539
class SSP2 : public RungeKutta {
    private:
        using super= RungeKutta;

    public:
        SSP2(std::shared_ptr<RateOfChange> rate_of_change_,
             std::shared_ptr<BoundaryCondition> boundary_condition_,
             std::shared_ptr<Limiting> limiting_,
             int n_rows,
             int n_cols)
             : super(std::move(rate_of_change_),
                     std::move(boundary_condition_),
                     std::move(limiting_)),
               dudt(n_rows, n_cols) {}

        virtual void operator() (Eigen::MatrixXd &u1,
                                 const Eigen::MatrixXd &u0,
                                 double dt) const override
        {
            // u* step: using u1 as temp variable:
            (*rate_of_change)(dudt, u0);
            u1= u0 + dt * dudt;
            post_euler_step(u1);

            // u** step: again using u1 as temp variable:
            (*rate_of_change)(dudt, u1);
            u1= u1 + dt * dudt;
            post_euler_step(u1);

            // final RK2 assembly:
            u1= 0.5 * (u0 + u1);
            (*boundary_condition)(u1);
        }

    private:
        mutable Eigen::MatrixXd dudt;
};

/// make Runge Kutta for FVM
std::shared_ptr<RungeKutta>
make_runge_kutta(const nlohmann::json &config,
                 const std::shared_ptr<RateOfChange> &rate_of_change,
                 const std::shared_ptr<BoundaryCondition> &boundary_condition,
                 int n_rows,
                 int n_cols);

/// make Runge Kutta for DG
std::shared_ptr<RungeKutta>
make_runge_kutta(const nlohmann::json &config,
                 const std::shared_ptr<RateOfChange> &rate_of_change,
                 const std::shared_ptr<BoundaryCondition> &boundary_condition,
                 const std::shared_ptr<Limiting> &dg_limiting,
                 int n_rows,
                 int n_cols);

#endif // HYPSYS1D_RUNGE_KUTTA_HPP


/**
 * @file Camera.hpp
 * @author Takashi Michikawa <michikawa@acm.org>
 */
#ifndef MI4_CAMERA_HPP
#define MI4_CAMERA_HPP 1

#include <Eigen/Dense>
namespace mi4
{
        class Camera
        {
        public:
                Camera (void) : _rotation(Eigen::Quaterniond(1, 0, 0, 0)), _center(Eigen::Vector3d(0, 0, 0)), _dist(0), _radius(100), _fov(40)
                {
                        Eigen::Vector3d b ( 1, 1, 1 );
                        b.normalize();
                        b *= 100;
                        this->init ( -b, b );
                        return;
                }
                Camera ( const Camera& that ) = default;
                Camera ( Camera&& that ) = default;
                Camera& operator = ( const Camera& that ) = default;
                Camera& operator = ( Camera&& that ) = default;
                ~Camera ( void ) = default;

                void init ( const Eigen::Vector3d& bmin, const Eigen::Vector3d& bmax )
                {
                        this->_center = ( bmin + bmax ) * 0.5;
                        this->_radius = ( bmin - bmax ).norm() * 0.5;
                        this->_dist   = this->_radius * 1.0 / std::sin ( this->_fov / 360.0 * 3.1415926 ) ;
                        return;
                }
                void init ( const Eigen::Vector3d& center, const double radius, const double dist )
                {
                        this->_center = center;
                        this->_radius = radius;
                        this->_dist   = dist;
                }

                void clone ( const Camera& that )
                {
                        this->init ( that._center, that._radius, that._dist );
                        this->_rotation = that._rotation;
                        return;
                }

                void getLookAt ( Eigen::Vector3d& eye, Eigen::Vector3d& center, Eigen::Vector3d& up )
                {
                        const auto& rotation = this->_rotation;
                        center = this->_center;
                        const auto& dist   = this->_dist;

                        const auto r = rotation.toRotationMatrix();
                        eye = r * Eigen::Vector3d(0, 0, dist) + center;
                        up = r * Eigen::Vector3d(0, 1, 0);
                        return;
                }

                void zoom ( bool isUp  )
                {
                        this->_dist *= isUp ? 0.99 : 1.0 / 0.99;
                }

                void getZNearFar ( double& zNear, double& zFar )
                {
                        const double& radius = this->_radius;
                        const double& dist   = this->_dist;

                        zNear = dist - radius;
                        zFar  = dist + radius;

                        zNear = std::max(0.01, zNear);

                }

                double getFov ( void ) const
                {
                        return this->_fov;
                }

                void rotate ( const double  oldx, const double oldy, const double newx, const double newy )
                {
                        auto& rotation = this->_rotation;
                        Eigen::Vector3d oldp ( oldx, oldy, 0.0 );
                        Eigen::Vector3d newp ( newx, newy, 0.0 );

                        if ( oldp.isApprox ( newp, 1.0e-16 ) ) {
                                return;
                        }

                        double radius_virtual_sphere = 0.9; // @todo move to attribute
                        this->project_onto_sphere ( radius_virtual_sphere, oldp );
                        this->project_onto_sphere ( radius_virtual_sphere, newp );
                        Eigen::Quaterniond dr;
                        dr.setFromTwoVectors ( newp, oldp );
                        rotation *= dr;
                        return;
                }
                /**
                 * @brief Rotate by axis and angle.
                 * @param [in] axis Axis of rotation.
                 * @param [in] angle Rotation angle [rad].
                 */
                void rotate ( const Eigen::Vector3d& axis,  const double angle )
                {
                        Eigen::Quaterniond& rotation = this->_rotation;
                        Eigen::AngleAxisd aa ( angle, axis );
                        Eigen::Quaterniond dr ( aa );
                        rotation *= dr;
                        return;
                }

                /**
                 * @brief Get radius of virtual sphere.
                 */
                double getRadius ( void ) const
                {
                        return this->_radius;
                }

                void moveTo ( const Eigen::Vector3d& newRay )
                {
                        this->_rotation.setIdentity();
                        auto m = this->_rotation.toRotationMatrix();
                        auto oldRay =  m *  Eigen::Vector3d ( 0, 0, 1 );
                        this->_rotation.setFromTwoVectors ( newRay, oldRay );
                }
        private:
                void
                project_onto_sphere ( const double& radius, Eigen::Vector3d& p )
                {
                        p.z() = 0; // project onto xy-plane.
                        const double d = p.x() * p.x() + p.y() * p.y();
                        const double r = radius * radius;

                        if ( d < r )	{
                                p.z() = std::sqrt ( r - d );        // on sphere
                        } else {
                                p *= radius / p.norm();        // on silhouette
                        }

                        return;
                }
        private:
                Eigen::Quaterniond _rotation;	///< rotation
                Eigen::Vector3d _center;	///< center point
                double	 _dist;			///< distance between eye-center.
                double   _radius;		///< radius of bounding sphere.
                double   _fov;			///< field-of-view angle
        };
}
#endif


#ifndef GP_KERNELS_HPP
#define GP_KERNELS_HPP

#include <iostream>
#include <memory>
#include <vector>
#include <Eigen/Core>
#include <math.h>

namespace librav{
    class Kernel{
        public:
            typedef std::shared_ptr<Kernel> Ptr;
            typedef std::shared_ptr<const Kernel> ConstPtr;
            virtual ~Kernel() {};

            // Pure virtual methods to be implemented in a derived class
            virtual double Evaluate(const Eigen::VectorXd& x, const Eigen::VectorXd& y) const = 0;
            virtual long double EvaluateGradient(const Eigen::VectorXd& x, const Eigen::VectorXd& y, size_t ii, size_t jj) const = 0;
            virtual double Partial(const Eigen::VectorXd& x, const Eigen::VectorXd& y, size_t ii) const = 0;
            virtual void Gradient(const Eigen::VectorXd& x, const Eigen::VectorXd& y, Eigen::VectorXd& gradient) const = 0;

            // Access and reset params.
            Eigen::VectorXd Params() {return params_;};
            const Eigen::VectorXd& ImmutableParams() const {return params_;};
            void Reset(const Eigen::VectorXd& params){
                params_ = params;
            }
            void Adjust(double diff, size_t ii){
                params_(ii) += diff;
            }
            void SetParams(const size_t ii, const double param_ii){
                params_(ii) = std::exp(param_ii);
                // params_(ii) = param_ii;
            }

        protected:
            explicit Kernel(const Eigen::VectorXd& params): params_(params) {};
            Eigen::VectorXd params_;
    };
}

#endif /* GP_KERNELS_HPP */


#include "definition.h"

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/dofs/dof_renumbering.h>

namespace bart {

namespace domain {

template <int dim>
Definition<dim>::Definition(
    std::unique_ptr<domain::mesh::MeshI<dim>> mesh,
    std::shared_ptr<domain::finite_element::FiniteElementI<dim>> finite_element,
    problem::DiscretizationType discretization)
    : mesh_(std::move(mesh)),
      finite_element_(finite_element),
      triangulation_(MPI_COMM_WORLD,
                     typename dealii::Triangulation<dim>::MeshSmoothing(
                         dealii::Triangulation<dim>::smoothing_on_refinement |
                         dealii::Triangulation<dim>::smoothing_on_coarsening)),
      dof_handler_(triangulation_),
      discretization_type_(discretization) {
  std::string description{"Domain, " + std::to_string(dim) + "D"};
  if (discretization == problem::DiscretizationType::kContinuousFEM) {
    description += ", Continuous";
  } else if (discretization == problem::DiscretizationType::kDiscontinuousFEM ){
    description += ", Discontinuous";
  }
  this->set_description(description, utility::DefaultImplementation(true));
}

template <>
Definition<1>::Definition(
    std::unique_ptr<domain::mesh::MeshI<1>> mesh,
    std::shared_ptr<domain::finite_element::FiniteElementI<1>> finite_element,
    problem::DiscretizationType discretization)
    : mesh_(std::move(mesh)),
      finite_element_(finite_element),
      triangulation_(typename dealii::Triangulation<1>::MeshSmoothing(
                         dealii::Triangulation<1>::smoothing_on_refinement |
                             dealii::Triangulation<1>::smoothing_on_coarsening)),
      dof_handler_(triangulation_),
      discretization_type_(discretization) {
  std::string description{"Domain, 1D"};
  if (discretization == problem::DiscretizationType::kContinuousFEM) {
    description += ", Continuous";
  } else if (discretization == problem::DiscretizationType::kDiscontinuousFEM ){
    description += ", Discontinuous";
  }
  this->set_description(description, utility::DefaultImplementation(true));
}

template <int dim>
Definition<dim>& Definition<dim>::SetUpMesh() {
  // SetUp triangulation using the mesh dependency
  AssertThrow(mesh_->has_material_mapping(),
                    dealii::ExcMessage("Mesh object must have initialized material mapping"));
  mesh_->FillTriangulation(triangulation_);
  mesh_->FillBoundaryID(triangulation_);
  mesh_->FillMaterialID(triangulation_);
  return *this;
}

template <int dim>
Definition<dim>& Definition<dim>::SetUpDOF() {
  // Setup dof Handler
  dof_handler_.distribute_dofs(*(finite_element_->finite_element()));
  // Populate dof IndexSets
  locally_owned_dofs_ = dof_handler_.locally_owned_dofs();
  dealii::DoFTools::extract_locally_relevant_dofs(dof_handler_,
                                                  locally_relevant_dofs_);
  // Create constraint matrix
  constraint_matrix_.clear();
  constraint_matrix_.reinit(locally_relevant_dofs_);
  dealii::DoFTools::make_hanging_node_constraints(dof_handler_,
                                                  constraint_matrix_);
  constraint_matrix_.close();

  for (auto cell = dof_handler_.begin_active();
       cell != dof_handler_.end(); ++cell) {
    if (cell->is_locally_owned())
      local_cells_.push_back(cell);
  }

  // Set up dynamic sparsity pattern
  dynamic_sparsity_pattern_.reinit(locally_relevant_dofs_.size(),
                                   locally_relevant_dofs_.size(),
                                   locally_relevant_dofs_);

  if (discretization_type_ ==  problem::DiscretizationType::kDiscontinuousFEM) {
    dealii::DoFTools::make_flux_sparsity_pattern(dof_handler_,
                                                 dynamic_sparsity_pattern_,
                                                 constraint_matrix_, false);
  } else {
    dealii::DoFTools::make_sparsity_pattern(dof_handler_,
                                            dynamic_sparsity_pattern_,
                                            constraint_matrix_, false);
  }

  dealii::SparsityTools::distribute_sparsity_pattern(
      dynamic_sparsity_pattern_,
      dof_handler_.n_locally_owned_dofs_per_processor(),
      MPI_COMM_WORLD, locally_relevant_dofs_);

  constraint_matrix_.condense(dynamic_sparsity_pattern_);

  return *this;
}

template <>
Definition<1>& Definition<1>::SetUpDOF() {
  auto n_mpi_processes = dealii::Utilities::MPI::n_mpi_processes(MPI_COMM_WORLD);
  auto this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);

  dealii::GridTools::partition_triangulation(n_mpi_processes, triangulation_);
  dof_handler_.distribute_dofs(*(finite_element_)->finite_element());
  dealii::DoFRenumbering::subdomain_wise(dof_handler_);

  for (auto cell = dof_handler_.begin_active();
       cell != dof_handler_.end(); ++cell) {
    if (cell->is_locally_owned())
      local_cells_.push_back(cell);
  }

  auto locally_owned_dofs_vector =
      dealii::DoFTools::locally_owned_dofs_per_subdomain(dof_handler_);
  locally_owned_dofs_ = locally_owned_dofs_vector.at(this_process);

  constraint_matrix_.clear();
  dealii::DoFTools::make_hanging_node_constraints(dof_handler_,
                                                  constraint_matrix_);
  constraint_matrix_.close();

  dynamic_sparsity_pattern_.reinit(dof_handler_.n_dofs(), dof_handler_.n_dofs());

  if (discretization_type_ ==  problem::DiscretizationType::kDiscontinuousFEM) {
    dealii::DoFTools::make_flux_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_,
                                                 constraint_matrix_, false);
  } else {
    dealii::DoFTools::make_sparsity_pattern(dof_handler_, dynamic_sparsity_pattern_,
                                            constraint_matrix_, false);
  }

  return *this;
}

template<int dim>
std::shared_ptr<system::MPISparseMatrix> Definition<dim>::MakeSystemMatrix() const {
  auto system_matrix_ptr = std::make_shared<system::MPISparseMatrix>();
  system_matrix_ptr->reinit(locally_owned_dofs_,
      locally_owned_dofs_,
      dynamic_sparsity_pattern_,
      MPI_COMM_WORLD);
  return system_matrix_ptr;
}

template<int dim>
std::shared_ptr<system::MPIVector> Definition<dim>::MakeSystemVector() const {
  auto system_vector_ptr = std::make_shared<system::MPIVector>();
  system_vector_ptr->reinit(locally_owned_dofs_, MPI_COMM_WORLD);
  return system_vector_ptr;
}

template <int dim>
int Definition<dim>::total_degrees_of_freedom() const {
  if (total_degrees_of_freedom_ == 0)
    total_degrees_of_freedom_ = dof_handler_.n_dofs();
  return total_degrees_of_freedom_;
}

template class Definition<1>;
template class Definition<2>;
template class Definition<3>;

} // namespace bart

} // namespace domain


#include "iris.h"
#include <stdexcept>
#include <iostream>
#include <numeric>
#include <chrono>
#include <Eigen/LU>
#include <Eigen/StdVector>
#include "iris/iris_mosek.h"
#include "iris/cvxgen_ldp.h"

namespace iris {

template <typename T>
std::vector<size_t> arg_sort(const std::vector<T> &vec) {
  std::vector<size_t> idx(vec.size());
  std::iota(idx.begin(), idx.end(), 0);
  std::sort(idx.begin(), idx.end(), [&vec](size_t i0, size_t i1) {return vec[i0] < vec[i1];});
  return idx;
}

typedef std::pair<Eigen::VectorXd, double> hyperplane;

hyperplane tangent_plane_through_point(const Ellipsoid &ellipsoid, const Eigen::MatrixXd &Cinv2, const Eigen::VectorXd &x) {
  Eigen::VectorXd nhat = (2 * Cinv2 * (x - ellipsoid.getD())).normalized();
  std::pair<Eigen::VectorXd, double> plane(nhat,
                                    nhat.transpose() * x);
  // std::cout << "tangent plane through point: " << x.transpose() << std::endl;
  // std::cout << plane.first.transpose() << " | " << plane.second << std::endl;
  return plane;
}

void choose_closest_point_solver(const Eigen::MatrixXd &Points, Eigen::VectorXd &result, MSKenv_t &env) {
  // std::cout << "points: " << std::endl << Points << std::endl;
  if (Points.rows() <= IRIS_CVXGEN_LDP_MAX_ROWS && Points.cols() <= IRIS_CVXGEN_LDP_MAX_COLS) {
    iris_cvxgen::closest_point_in_convex_hull(Points, result);
  } else {
    if (!env) {
      // std::cout << "making env" << std::endl;
      iris_mosek::check_res(MSK_makeenv(&env, NULL));
    }
    iris_mosek::closest_point_in_convex_hull(Points, result, &env);
  }
  // std::cout << "closest point: " << result.transpose() << std::endl;
}

void separating_hyperplanes(const std::vector<Eigen::MatrixXd> obstacle_pts, const Ellipsoid &ellipsoid, Polyhedron &polyhedron, bool &infeasible_start) {

  int dim = ellipsoid.getDimension();
  infeasible_start = false;
  int n_obs = obstacle_pts.size();

  if (n_obs == 0) {
    polyhedron.setA(Eigen::MatrixXd::Zero(0, dim));
    polyhedron.setB(Eigen::VectorXd::Zero(0));
    return;
  }

  Eigen::MatrixXd Cinv = ellipsoid.getC().inverse();
  Eigen::MatrixXd Cinv2 = Cinv * Cinv.transpose();

  Eigen::Matrix<bool, Eigen::Dynamic, 1> uncovered_obstacles = Eigen::Matrix<bool, Eigen::Dynamic, 1>::Constant(n_obs, true);

  std::vector<Eigen::MatrixXd> image_pts(n_obs);
  for (int i=0; i < n_obs; i++) {
    image_pts[i] = Cinv * (obstacle_pts[i].colwise() - ellipsoid.getD());
  }

  std::vector<Eigen::VectorXd> image_squared_dists(n_obs);
  for (int i=0; i < n_obs; i++) {
    image_squared_dists[i] = image_pts[i].colwise().squaredNorm();
  }

  std::vector<double> obs_min_squared_image_dists(n_obs);
  for (int i=0; i < n_obs; i++) {
    obs_min_squared_image_dists[i] = image_squared_dists[i].minCoeff();
  }
  std::vector<size_t> obs_sort_idx = arg_sort(obs_min_squared_image_dists);

  std::vector<std::pair<Eigen::VectorXd, double>> planes;

  MSKenv_t env = NULL;
  for (auto it = obs_sort_idx.begin(); it != obs_sort_idx.end(); ++it) {
    size_t i = *it;
    if (!uncovered_obstacles(i)) {
      continue;
    }
    Eigen::DenseIndex idx;
    image_squared_dists[i].minCoeff(&idx);
    hyperplane plane = tangent_plane_through_point(ellipsoid, Cinv2, obstacle_pts[i].col(idx));
    if ((((plane.first.transpose() * obstacle_pts[i]).array() - plane.second) >= 0).all()) {
      // nhat already separates the ellipsoid from obstacle i, so we can skip the optimization
      planes.push_back(plane);
    } else {
      Eigen::VectorXd ystar(dim);
      choose_closest_point_solver(image_pts[i], ystar, env);

      if (ystar.squaredNorm() < 1e-6) {
        // d is inside the obstacle. So we'll just reverse nhat to try to push the
        // ellipsoid out of the obstacle.
        infeasible_start = true;
        planes.emplace_back(-plane.first, -plane.first.transpose() * obstacle_pts[i].col(idx));
      } else {
        Eigen::VectorXd xstar = ellipsoid.getC() * ystar + ellipsoid.getD();
        planes.push_back(tangent_plane_through_point(ellipsoid, Cinv2, xstar));
      }
    }

    for (size_t j=0; j < n_obs; j++) {
      if (((planes.back().first.transpose() * obstacle_pts[j]).array() >= planes.back().second).all()) {
        uncovered_obstacles(j) = false;
      }
    }
    uncovered_obstacles(i) = false; // even if it doesn't pass the strict check, we're done with this obstacle

    if (!uncovered_obstacles.any()) {
      break;
    }
  }

  // Eigen::MatrixXd A = polyhedron.getA();
  // Eigen::VectorXd b = polyhedron.getB();
  // A.resize(planes.size(), dim);
  // b.resize(planes.size(), 1);
  Eigen::MatrixXd A(planes.size(), dim);
  Eigen::VectorXd b(planes.size());

  for (auto it = planes.begin(); it != planes.end(); ++it) {
    A.row(it - planes.begin()) = it->first.transpose();
    b(it - planes.begin()) = it->second;
  }
  polyhedron.setA(A);
  polyhedron.setB(b);

  return;
}

IRISRegion inflate_region(const IRISProblem &problem, const IRISOptions &options, IRISDebugData *debug) {
  // std::cout << "running IRIS with the following inputs: " << std::endl;
  // std::cout << "bounds: " << std::endl << problem.getBounds().getA() << std::endl << problem.getBounds().getB() << std::endl;
  // std::cout << "obstacles: " << std::endl;
  // auto debug_obstacles = problem.getObstacles();
  // for (auto it = debug_obstacles.begin(); it != debug_obstacles.end(); ++it) {
  //   std::cout << *it << std::endl;
  // }

  IRISRegion region(problem.getDimension());
  region.ellipsoid.setC(problem.getSeed().getC());
  region.ellipsoid.setD(problem.getSeed().getD());

  double best_vol = pow(ELLIPSOID_C_EPSILON, problem.getDimension());
  double volume;
  long int iter = 0;
  bool infeasible_start;
  Polyhedron new_poly(problem.getDimension());

  if (debug) {
    // std::cout << "starting debug" << std::endl;
    debug->bounds = problem.getBounds();
    debug->ellipsoid_history.push_back(region.ellipsoid);
    // std::cout << "pushing back obstacles" << std::endl;
    auto obstacles = problem.getObstacles();
    for (auto obs = obstacles.begin(); obs != obstacles.end(); ++obs) {
      // std::cout << "pushing back obstacle: " << *obs << std::endl;
      debug->obstacles.push_back(*obs);
    }
    // debug->obstacles = std::vector<Eigen::MatrixXd>(problem.getObstacles().begin(), problem.getObstacles().end());
  }

  float p_time = 0;
  float e_time = 0;

  while (1) {
    auto begin = std::chrono::high_resolution_clock::now();
    // std::cout << "calling hyperplanes with: " << std::endl;
    // std::cout << "C: " << region.ellipsoid->getC() << std::endl;
    // std::cout << "d: " << region.ellipsoid->getD() << std::endl;
    separating_hyperplanes(problem.getObstacles(), region.ellipsoid, new_poly, infeasible_start);
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
    p_time += elapsed.count();

    if (options.error_on_infeasible_start && infeasible_start) {
      throw(InitialPointInfeasibleError());
    }

    new_poly.appendConstraints(problem.getBounds());

    // std::cout << "A: " << std::endl << new_poly.getA() << std::endl;
    // std::cout << "b: " << new_poly.getB().transpose() << std::endl;

    if (options.require_containment) {
      bool all_points_contained;
      if (options.required_containment_points.size()) {
        all_points_contained = true;
        for (auto pt = options.required_containment_points.begin(); pt != options.required_containment_points.end(); ++pt) {
          if (!new_poly.contains(*pt, 0.0)) {
            all_points_contained = false;
            break;
          }
        }
      } else {
        all_points_contained = new_poly.contains(problem.getSeed().getD(), 0.0);
      }

      if (all_points_contained || infeasible_start) {
        region.polyhedron = new_poly;
        if (debug) {
          debug->polyhedron_history.push_back(new_poly);
        }
      } else {
        std::cout << "breaking early because the start point is no longer contained in the polyhedron" << std::endl;
        return region;
      }
    } else {
      region.polyhedron = new_poly;
      if (debug) {
        debug->polyhedron_history.push_back(new_poly);
      }
    }

    // std::cout << "calling inner_ellipsoid with: " << std::endl;
    // std::cout << "A: " << region.polyhedron->getA() << std::endl;
    // std::cout << "b: " << region.polyhedron->getB() << std::endl;
    begin = std::chrono::high_resolution_clock::now();
    volume = iris_mosek::inner_ellipsoid(region.polyhedron, &region.ellipsoid);
    end = std::chrono::high_resolution_clock::now();
    elapsed = std::chrono::duration_cast<std::chrono::duration<float>>(end - begin);
    e_time += elapsed.count();

    if (debug) {
      debug->ellipsoid_history.push_back(region.ellipsoid);
    }
    // std::cout << "C: " << region.ellipsoid->getC() << std::endl;
    // std::cout << "volume: " << volume << std::endl;
    // std::cout << "det: " << region.ellipsoid->getC().determinant() << std::endl;

    const bool at_iter_limit = (options.iter_limit > 0) && (iter + 1 >= options.iter_limit);
    const bool insufficient_progress = (std::abs(volume - best_vol) / best_vol) < options.termination_threshold;
    if (at_iter_limit || insufficient_progress) {
      // std::cout << "(abs(volume - best_vol) / best_vol): " << (std::abs(volume - best_vol) / best_vol) << std::endl;
      // std::cout << "term thresh: " << options.termination_threshold << std::endl;
      break;
    }

    best_vol = volume; // always true because ellipsoid volume is guaranteed to be non-decreasing (see Deits14).
    iter++;
    if (debug) {
      debug->iters = iter;
    }
  }

  // std::cout << "c++ p time: " << p_time << std::endl;
  // std::cout << "c++ e time: " << e_time << std::endl;
  // std::cout << "c++ iters: " << iter << std::endl;
  return region;
}

}