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

// vim: foldenable foldmethod=marker

#pragma once

#include "detail/lattice.hpp"
#include "detail/memory.hpp"
#include "detail/utility.hpp"
#include <boost/container/small_vector.hpp>
#include <boost/pool/pool.hpp>
#include <gsl/gsl-lite.hpp>
#include <sleef.h>
#include <memory>
#include <optional>
#include <utility>
#include <vector>

#if !defined(__AVX__)
#    error "WTF?"
#endif

TCM_NAMESPACE_BEGIN

using size_t = std::size_t;

/// A tag indicating that a function should not optimise the energy.
struct no_optimize_t {};
inline constexpr no_optimize_t no_optimize{};

#if 0
/// Let the direction of spin on site `site` be `x`. Denote the spins on the
/// nearest neighbours of `x` by `{αᵢ}`. Furthermore, let `cᵢ = +1` if
/// interaction between sites is antiferromagnetic and `cᵢ = -1` if interaction
/// is ferromagnetic.
///
/// Then our goal here is to find `argmin{E(x) | x∈[0, 2π)}` where
///
///     E(x) = ∑cᵢcos(x - αᵢ) = ∑cᵢ(sin(x)sin(αᵢ) + cos(x)cos(αᵢ))
///          = [∑cᵢsin(αᵢ)] * sin(x) + [∑cᵢcos(αᵢ)] * cos(x) .
///            ^^^^^^^^^^^^            ^^^^^^^^^^^^
///                := A                    := B
///
/// Since `E` is continuously differentiable, let's just compute its derivative
///
///     dE/dx = Acos(x) - Bsin(x) .
///
/// Now,
///
///     dE/dx = 0  <=>  Acos(x) = Bsin(x)  <=>  tan(x) = A/B, if B != 0
///                                             Acos(x) = 0,  if B == 0
///
/// * B != 0: We have a choice between
///
///        x₁ = tan⁻¹(A/B) + 2π | mod 2π,   and
///        x₂ = x₁ + π          | mod 2π
///
///   We simply compute `E` for both values and choose the one that results in
///   lower energy.
///
/// * `B == 0 && A != 0`: We have a choice between
///
///        x₁ = π/2,    and
///        x₂ = 3π/2
///
///   Again, we compute `E` for both `x`'s and determine the best.
///
/// * `B == 0 && A == 0`: `E` is constant, so we can choose arbitrary `x`. For
/// simplicity, we pick `x = 0`.
///
template <class System>
auto minimise_local_energy(size_t const site, System const& system) -> angle_t
{
    // Determines the coefficients cᵢ
    struct get_c_t {
        using Lattice = typename System::lattice_type;
        Lattice const& lattice;

        auto operator()(size_t const i, size_t const j) const TCM_NOEXCEPT
            -> float
        {
            switch (::TCM_NAMESPACE::interaction(lattice, i, j)) {
            case interaction_t::Ferromagnetic: return -1.0f;
            case interaction_t::Antiferromagnetic: return 1.0f;
            } // end switch
        }
    } get_c{system.lattice()};

    auto A = 0.0f;
    auto B = 0.0f;
    for (int64_t const _i : system.lattice().neighbours[site]) {
        if (_i >= 0 && !system.is_empty(static_cast<size_t>(_i))) {
            auto const i = static_cast<size_t>(_i);
            auto const c = get_c(site, i);
            // Projection of spin on the y-axis is the sine
            A += c * system.S_y(i);
            // Projection of spin on the x-axis is the cosine
            B += c * system.S_x(i);
        }
    }

    auto const E = [A, B](angle_t const x) {
        auto const v     = Sleef_sincosf_u10(static_cast<float>(x));
        auto const sin_x = v.x;
        auto const cos_x = v.y;
        return A * sin_x + B * cos_x;
    };

    if (B != 0.0f) {
        auto x1 = [A, B]() {
            constexpr auto epsilon = -4.7683716E-7f;
            constexpr auto two_pi  = detail::two_pi<float>;
            auto           result  = Sleef_atanf_u10(A / B);
            if (result < epsilon) { return angle_t{result + two_pi}; }
            else if (result < 0.0f) {
                return angle_t{0.0f};
            }
            else {
                return angle_t{result};
            }
        }();
        auto const x2 = x1 + angle_t{detail::pi<float>};
        return (E(x1) <= E(x2)) ? x1 : x2;
    }
    else if (A != 0.0f) {
        auto const x1 = angle_t{1.5707964f}; // pi / 2
        auto const x2 = angle_t{4.712389f};  // 3 * pi / 2
        return (E(x1) <= E(x2)) ? x1 : x2;
    }
    else {
        return angle_t{0.0f};
    }
}
#endif

class system_base_t;

struct magnetic_cluster_base_t {
  public:
    using unique_ptr = pool_unique_ptr<magnetic_cluster_base_t>;

    /// Information about the connection between this cluster and a child
    /// cluster.
    struct child_conn_t {
        /// The edge which connects this cluster to the child. `edge.first`
        /// belongs to this cluster and `edge.second` belongs to the child.
        std::pair<uint32_t, uint32_t> edge;
        /// The child cluster itself.
        ///
        /// \note Yes, parents own their children. No shared ownership.
        unique_ptr data;
    };

    /// Information about the connection between this cluster and its parent.
    struct parent_conn_t {
        /// Index in the #_children array of the parent.
        uint32_t index;
        /// The edge which connects the parent to this cluster. `edge.first`
        /// belongs to the parent and `edge.second` belongs to this cluster.
        std::pair<uint32_t, uint32_t> edge;
        /// Pointer to the parent node.
        ///
        /// \note The parent owns this cluster so it's safe to store a
        /// non-owning pointer here.
        gsl::not_null<magnetic_cluster_base_t*> data;
    };

  private:
    /// Connection to the parent node.
    std::optional<parent_conn_t> _parent;
    /// Connection to children nodes.
    boost::container::small_vector<child_conn_t, 1> _children;
    /// A list of indices of all the sites that belong to this cluster.
    std::vector<uint32_t> _sites;
    /// A reference to the global system state
    system_base_t& _system_state;

  public:
    /// Constructs a new magnetic cluster consisting of just one site
    ///
    /// \param index Index of the site.
    /// \param angle Direction of the spin at site \p index.
    /// \param system A reference to the system state.
    TCM_FORCEINLINE magnetic_cluster_base_t(uint32_t index, angle_t angle,
                                            system_base_t& system);

    /// **Deleted** copy constructor.
    magnetic_cluster_base_t(magnetic_cluster_base_t const&) = delete;
    /// **Deleted** move constructor.
    magnetic_cluster_base_t(magnetic_cluster_base_t&&) = delete;
    /// **Deleted** copy assignment operator.
    auto operator                   =(magnetic_cluster_base_t const&)
        -> magnetic_cluster_base_t& = delete;
    /// **Deleted** move assignment operator.
    auto operator                   =(magnetic_cluster_base_t &&)
        -> magnetic_cluster_base_t& = delete;

    /// Attaches a child node.
    ///
    /// \param connection Specifies the child node and the edge connecting it to
    /// `*this`.
    ///
    /// \throws std::bad_alloc if memory allocation fails.
    TCM_FORCEINLINE auto attach(child_conn_t connection) -> void;

    /// Detaches the `i`'th child.
    ///
    /// \param i Index of the child to detach. \p i must be less than
    /// number_children().
    /// \return The child connection.
    TCM_FORCEINLINE auto detach(size_t i) -> child_conn_t;

    /// Adds site `i` to the cluster.
    ///
    /// \param i     Index of the site.
    /// \param angle Direction of the spin.
    TCM_FORCEINLINE auto insert(uint32_t i, angle_t angle) -> void;

    /// Merges \p cluster into `*this`.
    ///
    /// The merging is done in two steps:
    ///   -# stealing all the *children* from \p cluster, and
    ///   -# stealing all the *sites* from \p cluster.
    TCM_FORCEINLINE auto merge(unique_ptr cluster) -> void;

    /// Rotates the subtree (i.e. `*this` and all the children recursively) by
    /// \p angle.
    ///
    /// Rotation amounts to a simle depth-first search over the subtree.
    TCM_FORCEINLINE auto rotate(angle_t angle) -> void;

    /// Returns whether the cluster is the root of the geometric cluster.
    ///
    /// \noexcept
    constexpr auto is_root() const noexcept -> bool;

    /// Returns the index of `*this` in the array of children of its parent.
    ///
    /// \pre `*this` is not the root node, i.e. is_root() returns `false`.
    /// \noexcept
    constexpr auto index_in_parent() const TCM_NOEXCEPT -> uint32_t;

    /// Returns a non-owning pointer to the parent node.
    ///
    /// \pre `*this` has a parent, i.e. is_root() returns `false`.
    /// \noexcept
    constexpr auto parent() const TCM_NOEXCEPT
        -> gsl::not_null<magnetic_cluster_base_t*>;

    /// Returns a non-owning view of the sites in the cluster.
    ///
    /// \noexcept
    inline auto sites() const noexcept -> gsl::span<uint32_t const>;

    constexpr auto system() const noexcept -> system_base_t const&;
    constexpr auto system() noexcept -> system_base_t&;

    /// Returns the number of sites in this cluster.
    ///
    /// \noexcept
    inline auto number_sites() const noexcept -> size_t;

    /// Returns the number of child nodes.
    ///
    /// \noexcept
    inline auto number_children() const noexcept -> size_t;

    friend TCM_FORCEINLINE auto
    move_root_down(magnetic_cluster_base_t::unique_ptr root, size_t child_index)
        -> magnetic_cluster_base_t::unique_ptr;

    /// Runs Depth-First Search on the subtree applying \p fn to each node.
    ///
    /// \param fn Function to apply to each node. \p fn should be callable with
    /// a reference to magnetic_cluster_t. Return value is ignored.
    template <class Function> TCM_FORCEINLINE auto dfs(Function&& fn) -> void;

    template <class Lattice>
    inline auto align_with_parent(Lattice const& lattice) -> void;
};

#if 0

/// A magnetic cluster, i.e. an irreducible component in our graph.
template <class System> class magnetic_cluster_t { // {{{
  public:
    using system_type = System;
#    if !defined(DOXYGEN_IS_IN_THE_HOUSE)
    using unique_ptr =
        typename System::template unique_ptr<magnetic_cluster_t<System>>;
#    else
    using unique_ptr = std::unique_ptr<magnetic_cluster_t<System>>;
#    endif

  private:
    /// Information about the connection between this cluster and a child
    /// cluster.
    struct child_conn_info_t {
        /// The edge which connects this cluster to the child. `edge.first`
        /// belongs to this cluster and `edge.second` belongs to the child.
        std::pair<size_t, size_t> edge;
        /// The child cluster itself.
        ///
        /// \note Yes, parents own their children. No shared ownership.
        unique_ptr data;
    };

    /// Information about the connection between this cluster and its parent.
    struct parent_conn_info_t {
        /// Index in the #_children array of the parent.
        size_t index;
        /// The edge which connects the parent to this cluster. `edge.first`
        /// belongs to the parent and `edge.second` belongs to this cluster.
        std::pair<size_t, size_t> edge;
        /// Pointer to the parent node.
        ///
        /// \note The parent owns this cluster so it's safe to store a
        /// non-owning pointer here.
        gsl::not_null<magnetic_cluster_t*> data;
    };

    /// Connection to the parent node.
    std::optional<parent_conn_info_t> _parent;
    /// Connection to children nodes.
    boost::container::small_vector<child_conn_info_t, 1> _children;
    /// A list of indices of all the sites that belong to this cluster.
    std::vector<size_t> _sites;
    /// A reference to the global system state
    system_type& _system_state;

    // static_assert(sizeof(boost::container::small_vector<child_conn_info_t, 1>)
    //               == 56);

  public:
    /// Constructs a new magnetic cluster consisting of just one site
    ///
    /// \param index Index of the site.
    /// \param angle Direction of the spin at site \p index.
    /// \param system A reference to the system state.
    inline magnetic_cluster_t(size_t index, angle_t angle, system_type& system);

    /// **Deleted** copy constructor.
    magnetic_cluster_t(magnetic_cluster_t const&) = delete;
    /// **Deleted** move constructor.
    magnetic_cluster_t(magnetic_cluster_t&&) = delete;
    /// **Deleted** copy assignment operator.
    magnetic_cluster_t& operator=(magnetic_cluster_t const&) = delete;
    /// **Deleted** move assignment operator.
    magnetic_cluster_t& operator=(magnetic_cluster_t&&) = delete;

    /// Attaches a child node.
    ///
    /// \param connection Specifies the child node and the edge connecting it to
    /// `*this`.
    ///
    /// \throws std::bad_alloc if memory allocation fails.
    inline auto attach(child_conn_info_t connection) -> void;
    // inline auto attach(no_optimize_t, child_conn_info_t /*connection*/) -> void;

    /// Detaches the `i`'th child.
    ///
    /// \param i Index of the child to detach. \p i must be less than
    /// number_children().
    /// \return The child connection.
    inline auto detach(size_t i) -> child_conn_info_t;

    /// Adds site `i` to the cluster.
    ///
    /// \param i Index of the site.
    inline auto insert(size_t i) -> void;
    // inline auto insert(no_optimize_t, size_t /*site index*/) -> void;

    /// Merges \p cluster into `*this`.
    ///
    /// The merging is done in two steps:
    ///   -# stealing all the *children* from \p cluster, and
    ///   -# stealing all the *sites* from \p cluster.
    inline auto merge(unique_ptr cluster) -> void;
    // auto        merge(no_optimize_t, unique_ptr cluster) -> void;

    /// Rotates the subtree (i.e. `*this` and all the children recursively) by
    /// \p angle.
    ///
    /// Rotation amounts to a simle depth-first search over the subtree.
    inline auto rotate(angle_t angle) -> void;

    /// Optimizes the energy of the magnetic cluster assuming that the cluster
    /// was optimized and then a new site `site` was added.
    // auto optimize_one(size_t site) -> void;

    /// Optimizes the energy of the magnetic cluster.
    // TCM_NOINLINE auto optimize_full() -> void;

    /// Returns whether the cluster is the root of the geometric cluster.
    ///
    /// \noexcept
    constexpr auto is_root() const noexcept { return !_parent.has_value(); }

    /// Returns the index of `*this` in the array of children of its parent.
    ///
    /// \pre `*this` is not the root node, i.e. is_root() returns `false`.
    /// \noexcept
    constexpr auto index_in_parent() const TCM_NOEXCEPT
    {
        TCM_ASSERT(!is_root(), "Root nodes are orphans");
        TCM_ASSERT(_parent->index < _parent->data->_children.size(),
                   "Index out of bounds");
        TCM_ASSERT(_parent->data->_children[_parent->index].data.get() == this,
                   "Bug! Invalid index in parent");
        return _parent->index;
    }

    /// Returns a non-owning pointer to the parent node.
    ///
    /// \pre `*this` has a parent, i.e. is_root() returns `false`.
    /// \noexcept
    constexpr auto parent() const TCM_NOEXCEPT
    {
        TCM_ASSERT(!is_root(), "Root nodes are orphans");
        return _parent->data;
    }

    /// Returns a non-owning view of the sites in the cluster.
    ///
    /// \noexcept
    constexpr auto sites() const noexcept -> gsl::span<size_t const>
    {
        return {_sites};
    }

    constexpr auto system() const noexcept -> system_type const&
    {
        return _system_state;
    }

    constexpr auto system() noexcept -> system_type& { return _system_state; }

    /// Returns the number of sites in this cluster.
    ///
    /// \noexcept
    auto number_sites() const noexcept -> size_t { return _sites.size(); }

    /// Returns the number of sites in this cluster.
    ///
    /// \noexcept
    auto size() const noexcept -> size_t { return number_sites(); }

    /// Returns the number of child nodes.
    ///
    /// \noexcept
    auto number_children() const noexcept -> size_t { return _children.size(); }

    template <class S>
    friend inline auto
        move_root_down(typename magnetic_cluster_t<S>::unique_ptr /*root*/,
                       size_t /*child index*/) ->
        typename magnetic_cluster_t<S>::unique_ptr;

    /// Runs Depth-First Search on the subtree applying \p fn to each node.
    ///
    /// \param fn Function to apply to each node. \p fn should be callable with
    /// a reference to magnetic_cluster_t. Return value is ignored.
    template <class Function> TCM_FORCEINLINE auto dfs(Function&& fn)
    {
        using container_type =
            boost::container::small_vector<magnetic_cluster_t*, 10>;
        using stack_type = std::stack<magnetic_cluster_t*, container_type>;
        static_assert(sizeof(stack_type) == sizeof(container_type),
                      "std::stack is wasting memory");

        stack_type todo;
        todo.push(this);
        while (!todo.empty()) {
            auto& x = *todo.top();
            todo.pop();
            for (auto& child : x._children) {
                todo.push(child.data.get());
            }
            fn(x);
        }
    }

  private:
    /// Aligns `*this` with its parent.
    ///
    /// Let `(i, j)` be the edge connecting `*this` to its parent. Site `i`
    /// belongs to the parent and site `j` belongs to `*this`. If the
    /// interaction between sites `i` and `j` is of ferromagnetic character, we
    /// rotate `*this` (and its children) such that sites `i` and `j` become
    /// aligned.  Otherwise (i.e. if the interaction is of antiferromagnetic
    /// character) we rotate `*this` such that the angle between `i` and `j`
    /// becomes *π*.
    ///
    /// \throws std::bad_alloc If dfs() fails to allocate memory.
    auto align_with_parent() -> void
    {
        TCM_ASSERT(_parent.has_value(), "Can't align an orphan");
        auto const delta_angle = [this]() {
            auto const [i, j] = _parent->edge;
            switch (
                ::TCM_NAMESPACE::interaction(_system_state.lattice(), i, j)) {
            case interaction_t::Ferromagnetic:
                return _system_state.get_angle(i) - _system_state.get_angle(j);
            case interaction_t::Antiferromagnetic:
                return _system_state.get_angle(i) - _system_state.get_angle(j)
                       + angle_t{detail::pi<float>};
            } // end switch
        }();
        rotate(delta_angle);
    }
}; // }}}

// {{{ IMPLEMENTATION: magnetic_cluster_t
template <class System>
TCM_FORCEINLINE magnetic_cluster_t<System>::magnetic_cluster_t(
    size_t const site, angle_t const phase, System& system_state)
    : _parent{std::nullopt}
    , _children{}
    // TODO(twesterhout): Should we reserve some memory in advance?
    , _sites({site})
    , _system_state{system_state}
{
    _system_state.set_angle(site, phase);
    _system_state.set_magnetic_cluster(site, *this);
}

template <class System>
TCM_FORCEINLINE auto magnetic_cluster_t<System>::insert(size_t const site)
    -> void
{
    _sites.push_back(site);
    _system_state.set_magnetic_cluster(site, *this);
    _system_state.set_angle(site, minimise_local_energy(site, _system_state));
}

#    if 0
template <class System>
TCM_FORCEINLINE auto magnetic_cluster_t<System>::insert(size_t const site)
    -> void
{
    TCM_ASSERT(_is_optimised, "It is assumed that the cluster is optimized");
    insert(no_optimize, site);
    optimize_one(site);
    _is_optimised = true;
}
#    endif

#    if 0
template <class System>
TCM_FORCEINLINE auto magnetic_cluster_t<System>::attach(child_conn_info_t child)
    -> void
{
    // TCM_ASSERT(_is_optimised, "It is assumed that the cluster is optimized");
    attach(no_optimize, std::move(child));
    // _children.back().data->align_with_parent();
    // _is_optimised = true;
}
#    endif

template <class System>
TCM_FORCEINLINE auto magnetic_cluster_t<System>::attach(child_conn_info_t child)
    -> void
{
    using std::begin, std::end;
    TCM_ASSERT(child.data != nullptr, "Can't attach a non-existent cluster");
    TCM_ASSERT(!child.data->_parent.has_value(), "Cluster must be an orphan");
    TCM_ASSERT(std::addressof(child.data->_system_state)
                   == std::addressof(_system_state),
               "Cluster must belong to the same system");
    TCM_ASSERT(std::count(begin(child.data->_sites), end(child.data->_sites),
                          child.edge.second)
                   == 1,
               "Invalid child_conn_info_t");
    TCM_ASSERT(std::count(begin(_sites), end(_sites), child.edge.first) == 1,
               "Invalid child_conn_info_t");

    // Attach the child
    child.data->_parent = {_children.size(), child.edge,
                           gsl::not_null<magnetic_cluster_t*>{this}};
    _children.emplace_back(std::move(child));
    _children.back().data->align_with_parent();
}

template <class System>
auto magnetic_cluster_t<System>::merge(unique_ptr cluster) -> void
{
    using std::begin, std::end;
    TCM_ASSERT(cluster != nullptr, "Can't merge with a non-existant cluster");
    TCM_ASSERT(!cluster->_parent.has_value(), "Cluster must be an orphan");
    TCM_ASSERT(std::addressof(cluster->_system_state)
                   == std::addressof(_system_state),
               "Cluster must belong to the same system");

    // Steal children from `cluster`. For each child we have to update its
    // `parent_conn_info_t.data` to point to this. After that, the child can be
    // safely moved into `_children`.
    _children.reserve(_children.size() + cluster->_children.size());
    for (auto& conn : cluster->_children) {
        conn.data->_parent->index = _children.size();
        conn.data->_parent->data  = this;
        _children.emplace_back(std::move(conn));
    }
    // cluster->_children.clear();

    // Now we steal `cluster->_sites`. Each site now belongs to a different
    // magnetic cluster. This info has to be propagated to the `_system_state`.
    for (auto const site : cluster->_sites) {
        _system_state.set_magnetic_cluster(site, *this);
    }
    _sites.reserve(_sites.size() + cluster->_sites.size());
    _sites.insert(end(_sites), begin(cluster->_sites), end(cluster->_sites));
}

#    if 0
template <class System>
TCM_FORCEINLINE auto magnetic_cluster_t<System>::merge(unique_ptr cluster)
    -> void
{
    merge(no_optimize, std::move(cluster));
    optimize_full();
}
#    endif

template <class System>
auto magnetic_cluster_t<System>::rotate(angle_t const angle) -> void
{
    dfs([angle, this](auto& x) {
        using std::begin, std::end;
        _system_state.rotate(begin(x._sites), end(x._sites), angle);
    });
}

template <class System>
TCM_FORCEINLINE auto
magnetic_cluster_t<System>::detach(size_t const child_index)
    -> child_conn_info_t
{
    using std::swap;
    TCM_ASSERT(child_index < _children.size(), "Index out of bounds");

    if (child_index != _children.size() - 1) {
        _children.back().data->_parent->index = child_index;
        swap(_children[child_index], _children.back());
    }
    auto conn = std::move(_children.back());
    _children.pop_back();
    conn.data->_parent = std::nullopt;
    return conn;
}

#    if 0
template <class System>
auto magnetic_cluster_t<System>::thermalise(sa_pars_t const& parameters)
    -> gsl::span<float>
{
    auto& sa_buffers = _system_state.sa_buffers();
    sa_buffers.resize(_sites.size());

    auto& energy_buffers = _system_state.energy_buffers();
    auto energy_fn = energy_buffers.energy_fn(sites(), _system_state.lattice());

    return optimise(std::move(energy_fn), parameters, sa_buffers,
                    _system_state.rng_stream(),
                    [this](auto initial) {
                        using std::begin, std::end;
                        std::transform(begin(_sites), end(_sites),
                                       begin(initial), [this](auto const i) {
                                           return static_cast<float>(
                                               _system_state.get_angle(i));
                                       });
                    })
        .buffer;
}

template <class System>
auto magnetic_cluster_t<System>::optimize_one(size_t const /*site*/) -> void
{
    using std::begin, std::end;
    sa_pars_t params{/*q_V = */ 2.62f, /*q_A = */ -1.0f, /*t_0 = */ 10.0f,
                     /*n = */ 1000u};

    auto const angles = thermalise(params);

    if (_parent.has_value()) {
        auto const delta_angle = [this, &angles]() {
            auto const [i, j] = _parent->edge;
            auto const local_j =
                _system_state.energy_buffers().global_to_local(j);
            switch (interaction(_system_state.lattice(), i, j)) {
            case interaction_t::Ferromagnetic:
                return _system_state.get_angle(i) - angle_t{angles[local_j]};
            case interaction_t::Antiferromagnetic:
                return _system_state.get_angle(i) - angle_t{angles[local_j]}
                       + angle_t{static_cast<float>(M_PI)};
            } // end switch
        }();

        std::transform(begin(angles), end(angles), begin(angles),
                       [delta_angle](auto const x) {
                           return static_cast<float>(angle_t{x} + delta_angle);
                       });
    }

    for (auto i = size_t{0}; i < _sites.size(); ++i) {
        _system_state.set_angle(_sites[i], angle_t{angles[i]});
    }
    for (auto& child_conn : _children) {
        child_conn.data->align_with_parent();
    }
    _is_optimised = true;
}

template <class System>
TCM_NOINLINE auto magnetic_cluster_t<System>::optimize_full() -> void
{
    using std::begin, std::end;
    if (_parent.has_value()) {
        auto const fixed_index = _parent->edge.second;
        auto const fixed_angle = _system_state.get_angle(fixed_index);
        TCM_ASSERT(std::find(begin(_sites), end(_sites), fixed_index)
                       != end(_sites),
                   "`fixed_index` must belong to `*this`.");
        _system_state.set_angle(begin(_sites), end(_sites), fixed_angle);
        /*
        for (auto& conn : _children) {
            TCM_ASSERT(conn.data->is_optimized(),
                       "It makes little sense to align a child which is not "
                       "optimised.");
            conn.data->align_with_parent();
        }
        */
    }
    else {
        auto const fixed_index = _sites.front();
        auto const fixed_angle = _system_state.get_angle(fixed_index);
        _system_state.set_angle(begin(_sites), end(_sites), fixed_angle);
    }
    _is_optimised = true;
}
#    endif

template <class System>
TCM_FORCEINLINE auto
move_root_down(typename magnetic_cluster_t<System>::unique_ptr root,
               size_t const                                    child_index) ->
    typename magnetic_cluster_t<System>::unique_ptr
{
    // root->check_index_in_parent();
    using child_conn_info_t =
        typename magnetic_cluster_t<System>::child_conn_info_t;
    TCM_ASSERT(root->is_root(), "`root` must be the of the tree");
    TCM_ASSERT(child_index < root->_children.size(), "Index out of bounds");

    // Remove `child_index` from `root`'s list of children
    auto [edge, new_root] = root->detach(child_index);
    // Construct the new connection with the direction inverted.
    auto conn = child_conn_info_t{{edge.second, edge.first}, std::move(root)};
    // Attach it to the new root
    // auto const old_state = new_root->_is_optimised;
    new_root->attach(std::move(conn));
    // new_root->_is_optimised = old_state;
    TCM_ASSERT(new_root->is_root(), "Post-condition violated");
    // new_root->check_index_in_parent();
    return std::move(new_root);
}
#endif

TCM_NAMESPACE_END


//---------------------------------Spheral++----------------------------------//
// IvanoviSALEDamageModel
//
// The Ivanov damage model, hopefully close to how it's implemented in iSALE.
// This damage model is most appropriate for rocky materials.
//
// Refs:
//
// Collins, G. S., Melosh, H. J., & Ivanov, B. A. (2004). Modeling damage and deformation in impact simulations.
//   Meteoritics & Planetary Science, 39(2), 217–231. http://doi.wiley.com/10.1111/j.1945-5100.2004.tb00337.x
//
// Raducan, S. D., Davison, T. M., Luther, R., & Collins, G. S. (2019). The role of asteroid strength, porosity and
//   internal friction in impact momentum transfer. Icarus. https://doi.org/10.1016/J.ICARUS.2019.03.040
//
// Lundborg, N. (1967). The strength-size relation of granite. International Journal of Rock Mechanics and
//   Mining Sciences & Geomechanics Abstracts, 4(3):269–272.f
//
// Created by JMO, Sat Jun 26 11:35:44 PDT 2021
//----------------------------------------------------------------------------//
#include "FileIO/FileIO.hh"
#include "IvanoviSALEDamageModel.hh"
#include "TensorStrainPolicy.hh"
#include "IvanoviSALEDamagePolicy.hh"
#include "YoungsModulusPolicy.hh"
#include "LongitudinalSoundSpeedPolicy.hh"
#include "DamageGradientPolicy.hh"
#include "Strength/SolidFieldNames.hh"
#include "NodeList/SolidNodeList.hh"
#include "DataBase/DataBase.hh"
#include "DataBase/State.hh"
#include "DataBase/StateDerivatives.hh"
#include "DataBase/ReplaceState.hh"
#include "Hydro/HydroFieldNames.hh"
#include "Field/FieldList.hh"
#include "Boundary/Boundary.hh"
#include "Neighbor/Neighbor.hh"
#include "Utilities/mortonOrderIndices.hh"
#include "Utilities/allReduce.hh"
#include "Utilities/uniform_random.hh"

#include <boost/functional/hash.hpp>  // hash_combine

#include <string>
#include <vector>
#include <algorithm>
#include <limits>
using std::vector;
using std::string;
using std::pair;
using std::make_pair;
using std::cout;
using std::cerr;
using std::endl;
using std::min;
using std::max;
using std::abs;

namespace Spheral {

//------------------------------------------------------------------------------
// Constructor.
//------------------------------------------------------------------------------
template<typename Dimension>
IvanoviSALEDamageModel<Dimension>::
IvanoviSALEDamageModel(SolidNodeList<Dimension>& nodeList,
                       const TableKernel<Dimension>& W,
                       const double minPlasticFailure,
                       const double plasticFailurePressureSlope,
                       const double plasticFailurePressureOffset,
                       const double tensileFailureStress,
                       const double crackGrowthMultiplier,
                       const DamageCouplingAlgorithm damageCouplingAlgorithm,
                       const double criticalDamageThreshold,
                       const Field<Dimension, int>& mask):
  DamageModel<Dimension>(nodeList, W, crackGrowthMultiplier, damageCouplingAlgorithm),
  mEpsPfb(minPlasticFailure),
  mB(plasticFailurePressureSlope),
  mPc(plasticFailurePressureOffset),
  mTensileFailureStress(tensileFailureStress),
  mCriticalDamageThreshold(criticalDamageThreshold),
  mMask(mask),
  mYoungsModulus(SolidFieldNames::YoungsModulus, nodeList),
  mLongitudinalSoundSpeed(SolidFieldNames::longitudinalSoundSpeed, nodeList),
  mDdamageDt(IvanoviSALEDamagePolicy<Dimension>::prefix() + SolidFieldNames::scalarDamage, nodeList),
  mStrain(SolidFieldNames::strainTensor, nodeList),
  mEffectiveStrain(SolidFieldNames::effectiveStrainTensor, nodeList) {
}

//------------------------------------------------------------------------------
// Destructor.
//------------------------------------------------------------------------------
template<typename Dimension>
IvanoviSALEDamageModel<Dimension>::
~IvanoviSALEDamageModel() {
}

//------------------------------------------------------------------------------
// Evaluate derivatives.
//
// In this model we compute the scalar damage derivative assuming unresolved
// crack growth for every point. However, that is not applied in the tensor
// damage update policy unless the flaws are actually activated.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
evaluateDerivatives(const Scalar time,
                    const Scalar dt,
                    const DataBase<Dimension>& dataBase,
                    const State<Dimension>& state,
                    StateDerivatives<Dimension>& derivs) const {

  // Set the scalar magnitude of the damage evolution.
  const auto* nodeListPtr = &(this->nodeList());
  auto&       DDDt = derivs.field(state.buildFieldKey(IvanoviSALEDamagePolicy<Dimension>::prefix() + SolidFieldNames::scalarDamage, nodeListPtr->name()), 0.0);
  this->computeScalarDDDt(dataBase,
                          state,
                          time,
                          dt,
                          DDDt);
}

//------------------------------------------------------------------------------
// Vote on a time step.
//------------------------------------------------------------------------------
template<typename Dimension>
typename IvanoviSALEDamageModel<Dimension>::TimeStepType
IvanoviSALEDamageModel<Dimension>::
dt(const DataBase<Dimension>& /*dataBase*/,
   const State<Dimension>& /*state*/,
   const StateDerivatives<Dimension>& /*derivs*/,
   const Scalar /*currentTime*/) const {

  // // Look at how quickly we're trying to change the damage.
  // double dt = DBL_MAX;
  // const Field<Dimension, SymTensor>& damage = this->nodeList().damage();
  // const ConnectivityMap<Dimension>& connectivityMap = dataBase.connectivityMap();
  // const vector<const NodeList<Dimension>*>& nodeLists = connectivityMap.nodeLists();
  // const size_t nodeListi = distance(nodeLists.begin(), find(nodeLists.begin(), nodeLists.end(), &(this->nodeList())));
  // for (typename ConnectivityMap<Dimension>::const_iterator iItr = connectivityMap.begin(nodeListi);
  //      iItr != connectivityMap.end(nodeListi);
  //      ++iItr) {
  //   const int i = *iItr;
  //   const double D0 = damage(i).Trace() / Dimension::nDim;
  //   dt = min(dt, 0.8*max(D0, 1.0 - D0)/
  //            std::sqrt(mDdamageDt(i)*mDdamageDt(i) + 1.0e-20));
  // }
  // return TimeStepType(dt, "Rate of damage change");

  return TimeStepType(1.0e100, "Rate of damage change -- NO VOTE.");
}

//------------------------------------------------------------------------------
// Register our state.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
registerState(DataBase<Dimension>& dataBase,
              State<Dimension>& state) {

  typedef typename State<Dimension>::PolicyPointer PolicyPointer;

  // Register Youngs modulus and the longitudinal sound speed.
  PolicyPointer EPolicy(new YoungsModulusPolicy<Dimension>());
  PolicyPointer clPolicy(new LongitudinalSoundSpeedPolicy<Dimension>());
  state.enroll(mYoungsModulus, EPolicy);
  state.enroll(mLongitudinalSoundSpeed, clPolicy);

  // Set the initial values
  typename StateDerivatives<Dimension>::PackageList dummyPackages;
  StateDerivatives<Dimension> derivs(dataBase, dummyPackages);
  EPolicy->update(state.key(mYoungsModulus), state, derivs, 1.0, 0.0, 0.0);
  clPolicy->update(state.key(mLongitudinalSoundSpeed), state, derivs, 1.0, 0.0, 0.0);

  // Register the strain and effective strain.
  PolicyPointer effectiveStrainPolicy(new TensorStrainPolicy<Dimension>(TensorStrainAlgorithm::PseudoPlasticStrain));
  state.enroll(mStrain);
  state.enroll(mEffectiveStrain, effectiveStrainPolicy);

  // Register the damage and state it requires.
  // Note we are overriding the default no-op policy for the damage
  // as originally registered by the SolidSPHHydroBase class.
  auto& damage = this->nodeList().damage();
  PolicyPointer damagePolicy(new IvanoviSALEDamagePolicy<Dimension>(mEpsPfb,
                                                                    mB,
                                                                    mPc,
                                                                    mTensileFailureStress));
  state.enroll(damage, damagePolicy);

  // Mask out nodes beyond the critical damage threshold from setting the timestep.
  auto maskKey = state.buildFieldKey(HydroFieldNames::timeStepMask, this->nodeList().name());
  auto& mask = state.field(maskKey, 0);
  const auto nlocal = this->nodeList().numInternalNodes();
#pragma omp parallel for
  for (auto i = 0u; i < nlocal; ++i) {
    if (damage(i).Trace() > mCriticalDamageThreshold) mask(i) = 0;
  }
}

//------------------------------------------------------------------------------
// Register the derivatives.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
registerDerivatives(DataBase<Dimension>& /*dataBase*/,
                    StateDerivatives<Dimension>& derivs) {
  derivs.enroll(mDdamageDt);
}

//------------------------------------------------------------------------------
// Apply the boundary conditions to the ghost nodes.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
applyGhostBoundaries(State<Dimension>& state,
                     StateDerivatives<Dimension>& /*derivs*/) {

  // Grab this models damage field from the state.
  typedef typename State<Dimension>::KeyType Key;
  const Key nodeListName = this->nodeList().name();
  const Key DKey = state.buildFieldKey(SolidFieldNames::tensorDamage, nodeListName);
  CHECK(state.registered(DKey));
  auto& D = state.field(DKey, SymTensor::zero);

  // Apply ghost boundaries to the damage.
  for (auto boundaryItr = this->boundaryBegin();
       boundaryItr < this->boundaryEnd();
       ++boundaryItr) {
    (*boundaryItr)->applyGhostBoundary(D);
  }
}

//------------------------------------------------------------------------------
// Enforce boundary conditions for the physics specific fields.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
enforceBoundaries(State<Dimension>& state,
                  StateDerivatives<Dimension>& /*derivs*/) {

  // Grab this models damage field from the state.
  typedef typename State<Dimension>::KeyType Key;
  const Key nodeListName = this->nodeList().name();
  const Key DKey = state.buildFieldKey(SolidFieldNames::tensorDamage, nodeListName);
  CHECK(state.registered(DKey));
  auto& D = state.field(DKey, SymTensor::zero);

  // Enforce!
  for (auto boundaryItr = this->boundaryBegin();
       boundaryItr < this->boundaryEnd();
       ++boundaryItr) {
    (*boundaryItr)->enforceBoundary(D);
  }
}

//------------------------------------------------------------------------------
// Dump the current state to the given file.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
dumpState(FileIO& file, const string& pathName) const {
  DamageModel<Dimension>::dumpState(file, pathName);
  file.write(mStrain, pathName + "/strain");
  file.write(mEffectiveStrain, pathName + "/effectiveStrain");
  file.write(mDdamageDt, pathName + "/DdamageDt");
  file.write(mMask, pathName + "/mask");
}

//------------------------------------------------------------------------------
// Restore the state from the given file.
//------------------------------------------------------------------------------
template<typename Dimension>
void
IvanoviSALEDamageModel<Dimension>::
restoreState(const FileIO& file, const string& pathName) {
  DamageModel<Dimension>::restoreState(file, pathName);
  file.read(mStrain, pathName + "/strain");
  file.read(mEffectiveStrain, pathName + "/effectiveStrain");
  file.read(mDdamageDt, pathName + "/DdamageDt");
  file.read(mMask, pathName + "/mask");
}

}


// This is an advanced implementation of the algorithm described in the
// following paper:
//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms with
//    sound   state   representationsthrough  encapsulation  of  manifolds. CoRR,  vol.  abs/1107.1119,  2011.[Online].
//    Available: http://arxiv.org/abs/1107.1119

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

/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/src/mtkmath.hpp
 * @brief several math utility functions.
 */

#ifndef MTKMATH_H_
#define MTKMATH_H_

#include <cmath>

#include <boost/math/tools/precision.hpp>

#include "../types/vect.hpp"

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

namespace MTK {

namespace internal {

template <class Manifold>
struct traits {
    typedef typename Manifold::scalar scalar;
    enum { DOF = Manifold::DOF };
    typedef vect<DOF, scalar> vectorized_type;
    typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};

template <>
struct traits<float> : traits<Scalar<float> > {};
template <>
struct traits<double> : traits<Scalar<double> > {};

}  // namespace internal

/**
 * \defgroup MTKMath Mathematical helper functions
 */
//@{

//! constant @f$ \pi @f$
const double pi = M_PI;

template <class scalar>
inline scalar tolerance();

template <>
inline float tolerance<float>() {
    return 1e-5f;
}
template <>
inline double tolerance<double>() {
    return 1e-11;
}

/**
 * normalize @a x to @f$[-bound, bound] @f$.
 *
 * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
 */
template <class scalar>
inline scalar normalize(scalar x, scalar bound) {  // not used
    if (std::fabs(x) <= bound) return x;
    int r = (int)(x * (scalar(1.0) / bound));
    return x - ((r + (r >> 31) + 1) & ~1) * bound;
}

/**
 * Calculate cosine and sinc of sqrt(x2).
 * @param x2 the squared angle must be non-negative
 * @return a pair containing cos and sinc of sqrt(x2)
 */
template <class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar& x2) {
    using std::cos;
    using std::sin;
    using std::sqrt;
    static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
    static scalar const taylor_2_bound = sqrt(taylor_0_bound);
    static scalar const taylor_n_bound = sqrt(taylor_2_bound);

    assert(x2 >= 0 && "argument must be non-negative");

    // FIXME check if bigger bounds are possible
    if (x2 >= taylor_n_bound) {
        // slow fall-back solution
        scalar x = sqrt(x2);
        return std::make_pair(cos(x), sin(x) / x);  // x is greater than 0.
    }

    // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can
    // be calculated in parallel using SSE2 mulpd/addpd)
    // TODO Find optimal coefficients using Remez algorithm
    static scalar const inv[] = {1 / 3., 1 / 4., 1 / 5., 1 / 6., 1 / 7., 1 / 8., 1 / 9.};
    scalar cosi = 1., sinc = 1;
    scalar term = -1 / 2. * x2;
    for (int i = 0; i < 3; ++i) {
        cosi += term;
        term *= inv[2 * i];
        sinc += term;
        term *= -inv[2 * i + 1] * x2;
    }

    return std::make_pair(cosi, sinc);
}

template <typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
    Eigen::Matrix<typename Base::scalar, 3, 3> res;
    res << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
    return res;
}

template <typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v) {
    Eigen::Matrix<typename Base::scalar, 3, 3> res;
    if (v.norm() > MTK::tolerance<typename Base::scalar>()) {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) +
              (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();

    } else {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
    }

    return res;
}

template <typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v) {
    Eigen::Matrix<typename Base::scalar, 3, 3> res;
    if (v.norm() > MTK::tolerance<typename Base::scalar>()) {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) +
              (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();

    } else {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
    }

    return res;
}

template <typename scalar>
Eigen::Matrix<scalar, 2, 3> S2_w_expw_(Eigen::Matrix<scalar, 2, 1> v, scalar length) {
    Eigen::Matrix<scalar, 2, 3> res;
    scalar norm = std::sqrt(v[0] * v[0] + v[1] * v[1]);
    if (norm < MTK::tolerance<scalar>()) {
        res = Eigen::Matrix<scalar, 2, 3>::Zero();
        res(0, 1) = 1;
        res(1, 2) = 1;
        res /= length;
    } else {
        res << -v[0] * (1 / norm - 1 / std::tan(norm)) / std::sin(norm), norm / std::sin(norm), 0,
            -v[1] * (1 / norm - 1 / std::tan(norm)) / std::sin(norm), 0, norm / std::sin(norm);
        res /= length;
    }
}

template <typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base& v) {
    Eigen::Matrix<typename Base::scalar, 3, 3> res;
    double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
    double norm = std::sqrt(squaredNorm);
    if (norm < MTK::tolerance<typename Base::scalar>()) {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
    } else {
        res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) +
              (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
    }
    return res;
}

template <class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
    scalar norm2 = vec.squaredNorm();
    std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale * scale * norm2);
    scalar mult = cos_sinc.second * scale;
    result = mult * vec;
    return cos_sinc.first;
}

/**
 * Inverse function to @c exp.
 *
 * @param result @c vectview to the result
 * @param w      scalar part of input
 * @param vec    vector part of input
 * @param scale  scale result by this value
 * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
 */
template <class scalar, int n>
void log(vectview<scalar, n> result, const scalar& w, const vectview<const scalar, n> vec, const scalar& scale,
         bool plus_minus_periodicity) {
    // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~>
    // only one division
    scalar nv = vec.norm();
    if (nv < tolerance<scalar>()) {
        if (!plus_minus_periodicity && w < 0) {
            // find the maximal entry:
            int i;
            nv = vec.cwiseAbs().maxCoeff(&i);
            result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
            return;
        }
        nv = tolerance<scalar>();
    }
    scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w));

    result = s * vec;
}

}  // namespace MTK

#endif /* MTKMATH_H_ */


#ifndef STAN_MATH_TORSTEN_ONECTP_HPP
#define STAN_MATH_TORSTEN_ONECTP_HPP

#include <Eigen/Dense>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/pmx_solve_ode.hpp>
#include <stan/math/torsten/pmx_solve_cpt.hpp>
#include <stan/math/prim/err/check_positive_finite.hpp>
#include <stan/math/torsten/ev_solver.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <string>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a one-compartment model with analytical solution.
 *
 * @tparam Ts types of parameters, see pmx_solve_cpt for
 *         details.
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 */
  template <typename... Ts>
  auto pmx_solve_onecpt(Ts... args) {
    return PMXSolveCPT<PMXOneCptModel>::solve(args...);
  }

  // old version
  template <typename T0, typename T1, typename T2, typename T3, typename T4,
            typename T5, typename T6>
  stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
  PKModelOneCpt(const std::vector<T0>& time,
                const std::vector<T1>& amt,
                const std::vector<T2>& rate,
                const std::vector<T3>& ii,
                const std::vector<int>& evid,
                const std::vector<int>& cmt,
                const std::vector<int>& addl,
                const std::vector<int>& ss,
                const std::vector<std::vector<T4> >& pMatrix,
                const std::vector<std::vector<T5> >& biovar,
                const std::vector<std::vector<T6> >& tlag) {
    auto x = pmx_solve_onecpt(time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, biovar, tlag);
    return x.transpose();
  }

  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  PKModelOneCpt(const std::vector<T0>& time,
                   const std::vector<T1>& amt,
                   const std::vector<T2>& rate,
                   const std::vector<T3>& ii,
                   const std::vector<int>& evid,
                   const std::vector<int>& cmt,
                   const std::vector<int>& addl,
                   const std::vector<int>& ss,
                   const std::vector<T_par>& pMatrix,
                   const std::vector<T_biovar>& biovar,
                   const std::vector<T_tlag>& tlag) {
    auto x = pmx_solve_onecpt(time, amt, rate, ii, evid, cmt, addl, ss,
                            pMatrix, biovar, tlag);
    return x.transpose();
  }

}

#endif


/*******************************************************************************
 *
 * Data structures for the symbolic manipulation of linear constraints.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#ifndef IKOS_LINEAR_CONSTRAINTS_HPP
#define IKOS_LINEAR_CONSTRAINTS_HPP

#include <boost/optional.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/container/flat_map.hpp>
#include <boost/container/slist.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/functional/hash.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/patricia_trees.hpp>

namespace ikos {

  template< typename Number, typename VariableName >
  class linear_expression: public writeable {

  public:
    typedef Number number_t;
    typedef VariableName varname_t;
    typedef variable< Number, VariableName > variable_t;
    typedef linear_expression< Number, VariableName > linear_expression_t;
    typedef std::pair< Number, variable_t > component_t;
    typedef patricia_tree_set< variable_t > variable_set_t;

  private:
    typedef boost::container::flat_map< variable_t, Number > map_t;
    typedef boost::shared_ptr< map_t > map_ptr;
    typedef typename map_t::value_type pair_t;

  private:
    map_ptr _map;
    Number _cst;

  private:
    linear_expression(map_ptr map, Number cst):
        _map(map), _cst(cst) { }

    linear_expression(const map_t& map, Number cst):
        _map(map_ptr(new map_t)), _cst(cst) {
      *this->_map = map;
    }

    void add(variable_t x, Number n) {
      typename map_t::iterator it = this->_map->find(x);
      if (it != this->_map->end()) {
        Number r = it->second + n;
        if (r == 0) {
          this->_map->erase(it);
        } else {
          it->second = r;
        }
      } else {
        if (n != 0) {
          this->_map->insert(pair_t(x, n));
        }
      }
    }

   public:
    class iterator: public boost::iterator_facade< iterator,
                                                   component_t,
                                                   boost::forward_traversal_tag,
                                                   component_t> {

      friend class boost::iterator_core_access;

     private:
      typename map_t::const_iterator _it;
      map_ptr _m;

     public:
      iterator(map_ptr m, bool b): _it(b ? m->begin() : m->end()), _m(m) { }

     private:
      void increment() {
        ++this->_it;
      }

      bool equal(const iterator& other) const {
        return (this->_m == other._m && this->_it == other._it);
      }

      component_t dereference() const {
        if (this->_it != this->_m->end()) {
          return component_t(this->_it->second, this->_it->first);
        } else {
          CRAB_ERROR("Linear expression: trying to dereference an empty iterator");
        }
      }

    }; // class iterator

  public:
    linear_expression():
        _map(map_ptr(new map_t)), _cst(0) { }

    linear_expression(Number n):
        _map(map_ptr(new map_t)), _cst(n) { }

    linear_expression(signed long long int n):
        _map(map_ptr(new map_t)), _cst(Number(n)) { }

    linear_expression(variable_t x):
        _map(map_ptr(new map_t)), _cst(0) {
      this->_map->insert(pair_t(x, Number(1)));
    }

    linear_expression(Number n, variable_t x):
        _map(map_ptr(new map_t)), _cst(0) {
      this->_map->insert(pair_t(x, n));
    }

    linear_expression_t& operator=(const linear_expression_t &e) {
      if (this != &e) {
        this->_map = e._map;
        this->_cst = e._cst;
      }
      return *this;
    }

    iterator begin() const {
      return iterator(this->_map, true);
    }

    iterator end() const {
      return iterator(this->_map, false);
    }

    size_t hash () const {
      size_t res = 0;
      for (iterator it=begin(), et=end (); it!=et; ++it) {
        boost::hash_combine (res, std::make_pair((*it).second, (*it).first));
      }
      boost::hash_combine (res, _cst);
      return res;
    }

    bool is_constant() const {
      return (this->_map->size() == 0);
    }

    Number constant() const {
      return this->_cst;
    }

    std::size_t size() const {
      return this->_map->size();
    }

    Number operator[](variable_t x) const {
      typename map_t::const_iterator it = this->_map->find(x);
      if (it != this->_map->end()) {
	return it->second;
      } else {
	return 0;
      }
    }

    template<typename VarMap>
    boost::optional<linear_expression_t> rename (const VarMap& map) const {
      Number cst(this->_cst);
      linear_expression_t ren_exp(cst);
      for(auto v : this->variables()) {
        auto const it = map.find(v);
        if (it != map.end()) {
          variable_t v_out ((*it).second);
          ren_exp = ren_exp + this->operator[](v) * v_out;
        }
        else
          return boost::optional<linear_expression_t>();
      }
      return ren_exp;
    }

    linear_expression_t operator+(Number n) const {
      linear_expression_t r(this->_map, this->_cst + n);
      return r;
    }

    linear_expression_t operator+(int n) const {
      return this->operator+(Number(n));
    }

    linear_expression_t operator+(variable_t x) const {
      linear_expression_t r(*this->_map, this->_cst);
      r.add(x, Number(1));
      return r;
    }

    linear_expression_t operator+(const linear_expression_t &e) const {
      linear_expression_t r(*this->_map, this->_cst + e._cst);
      for (typename map_t::const_iterator it = e._map->begin();
           it != e._map->end(); ++it) {
        r.add(it->first, it->second);
      }
      return r;
    }

    linear_expression_t operator-(Number n) const {
      return this->operator+(-n);
    }

    linear_expression_t operator-(int n) const {
      return this->operator+(-Number(n));
    }

    linear_expression_t operator-(variable_t x) const {
      linear_expression_t r(*this->_map, this->_cst);
      r.add(x, Number(-1));
      return r;
    }

    linear_expression_t operator-() const {
      return this->operator*(Number(-1));
    }

    linear_expression_t operator-(const linear_expression_t &e) const {
      linear_expression_t r(*this->_map, this->_cst - e._cst);
      for (typename map_t::const_iterator it = e._map->begin();
           it != e._map->end(); ++it) {
        r.add(it->first, -it->second);
      }
      return r;
    }

    linear_expression_t operator*(Number n) const {
      if (n == 0) {
        return linear_expression_t();
      } else {
        map_ptr map = map_ptr(new map_t);
        for (typename map_t::const_iterator it = this->_map->begin();
             it != this->_map->end(); ++it) {
          Number c = n * it->second;
          if (c != 0) {
            map->insert(pair_t(it->first, c));
          }
        }
        return linear_expression_t(map, n * this->_cst);
      }
    }

    linear_expression_t operator*(int n) const {
      return operator*(Number(n));
    }

    variable_set_t variables() const {
      variable_set_t variables;
      for (iterator it = this->begin(); it != this->end(); ++it) {
	variables += it->second;
      }
      return variables;
    }

    boost::optional<variable_t> get_variable() const {
      if (this->is_constant())
        return boost::optional<variable_t>();
      else{
        if ((this->constant() == 0) && (this->size() == 1)){
          typename linear_expression_t::iterator it = this->begin();
          Number coeff = it->first;
          if (coeff == 1)
            return boost::optional<variable_t>(it->second);
        }
        return boost::optional<variable_t>();
      }
    }

    void write(crab::crab_os& o) {
      for (typename map_t::iterator it = this->_map->begin();
           it != this->_map->end(); ++it) {
        Number n = it->second;
        variable_t v = it->first;
        if (n > 0 && it != this->_map->begin()) {
          o << "+";
        }
        if (n == -1) {
          o << "-";
        } else if (n != 1) {
          o << n;
        }
        o << v;
      }
      if (this->_cst > 0 && this->_map->size() > 0) {
        o << "+";
      }
      if (this->_cst != 0 || this->_map->size() == 0) {
        o << this->_cst;
      }
    }

  }; // class linear_expression

  template<typename Number, typename VariableName>
  inline std::size_t hash_value(const linear_expression<Number,VariableName>& e) {
    return e.hash ();
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(Number n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(n, x);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(int n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(Number(n), x);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(variable< Number, VariableName > x, Number n) {
    return linear_expression< Number, VariableName >(n, x);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(variable< Number, VariableName > x, int n) {
    return linear_expression< Number, VariableName >(Number(n), x);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(Number n, const linear_expression< Number, VariableName > &e) {
    return e.operator*(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator*(int n, const linear_expression< Number, VariableName > &e) {
    return e.operator*(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(variable< Number, VariableName > x, Number n) {
    return linear_expression< Number, VariableName >(x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(variable< Number, VariableName > x, int n) {
    return linear_expression< Number, VariableName >(x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(Number n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(int n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(variable< Number, VariableName > x,
            variable< Number, VariableName > y) {
    return linear_expression< Number, VariableName >(x).operator+(y);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(Number n, const linear_expression< Number, VariableName > &e) {
    return e.operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(int n, const linear_expression< Number, VariableName > &e) {
    return e.operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator+(variable< Number, VariableName > x,
            const linear_expression< Number, VariableName > &e) {
    return e.operator+(x);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(variable< Number, VariableName > x, Number n) {
    return linear_expression< Number, VariableName >(x).operator-(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(variable< Number, VariableName > x, int n) {
    return linear_expression< Number, VariableName >(x).operator-(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(Number n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(Number(-1), x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(int n, variable< Number, VariableName > x) {
    return linear_expression< Number, VariableName >(Number(-1), x).operator+(n);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(variable< Number, VariableName > x,
            variable< Number, VariableName > y) {
    return linear_expression< Number, VariableName >(x).operator-(y);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(Number n, const linear_expression< Number, VariableName > &e) {
    return linear_expression< Number, VariableName >(n).operator-(e);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(int n, const linear_expression< Number, VariableName > &e) {
    return linear_expression< Number, VariableName >(Number(n)).operator-(e);
  }

  template< typename Number, typename VariableName >
  inline linear_expression< Number, VariableName >
  operator-(variable< Number, VariableName > x,
            const linear_expression< Number, VariableName > &e) {
    return linear_expression< Number, VariableName >(Number(1), x).operator-(e);
  }

  template< typename Number, typename VariableName >
  class linear_constraint: public writeable {

  public:
    typedef Number number_t;
    typedef VariableName varname_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef variable< Number, VariableName > variable_t;
    typedef linear_expression< Number, VariableName > linear_expression_t;
    typedef patricia_tree_set< variable_t > variable_set_t;
    typedef enum {
      EQUALITY,
      DISEQUATION,
      INEQUALITY
    } kind_t;
    typedef typename linear_expression_t::iterator iterator;

  private:
    kind_t _kind;
    linear_expression_t _expr;

  public:
    linear_constraint(): _kind(EQUALITY) { }

    linear_constraint(const linear_expression_t &expr,
                      kind_t kind): _kind(kind), _expr(expr) { }

    static linear_constraint_t get_true () {
      linear_constraint_t res(linear_expression_t(Number(0)), EQUALITY);
      return res;
    }

    static linear_constraint_t get_false () {
      linear_constraint_t res(linear_expression_t(Number(0)), DISEQUATION);
      return res;
    }

    bool is_tautology() const {
      switch (this->_kind) {
        case DISEQUATION:
          return (this->_expr.is_constant() && this->_expr.constant() != 0);
        case EQUALITY:
          return (this->_expr.is_constant() && this->_expr.constant() == 0);
        case INEQUALITY:
	return (this->_expr.is_constant() && this->_expr.constant() <= 0);
        default:
          CRAB_ERROR("Unreachable");
      }
    }

    bool is_contradiction() const {
      switch (this->_kind) {
        case DISEQUATION:
          return (this->_expr.is_constant() && this->_expr.constant() == 0);
        case EQUALITY:
	return (this->_expr.is_constant() && this->_expr.constant() != 0);
        case INEQUALITY:
	return (this->_expr.is_constant() && this->_expr.constant() > 0);
        default:
	CRAB_ERROR("Unreachable");
      }
    }

    bool is_inequality() const {
      return (this->_kind == INEQUALITY);
    }

    bool is_equality() const {
      return (this->_kind == EQUALITY);
    }

    bool is_disequation() const {
      return (this->_kind == DISEQUATION);
    }

    const linear_expression_t& expression() const {
      return this->_expr;
    }

    kind_t kind() const {
      return this->_kind;
    }

    iterator begin() const {
      return this->_expr.begin();
    }

    iterator end() const {
      return this->_expr.end();
    }

    Number constant() const {
      return -this->_expr.constant();
    }

    std::size_t size() const {
      return this->_expr.size();
    }

    size_t hash () const {
      size_t res = 0;
      boost::hash_combine (res, _expr);
      boost::hash_combine (res, _kind);
      return res;
    }

    Number operator[](variable_t x) const {
      return this->_expr.operator[](x);
    }

    variable_set_t variables() const {
      return this->_expr.variables();
    }

    linear_constraint_t negate () const {
      if (is_tautology ())
         return linear_constraint_t ( linear_expression_t (0) >= linear_expression_t (1));
      else if (is_contradiction ())
         return linear_constraint_t ( linear_expression_t (1) >= linear_expression_t (0));
      else {
        switch (kind ()) {
          case INEQUALITY: {
            linear_expression_t e = -(this->_expr - 1);
            return linear_constraint_t (e, INEQUALITY);
          }
          case EQUALITY:
            return linear_constraint_t (this->_expr, DISEQUATION);
          case DISEQUATION:
            return linear_constraint_t (this->_expr, EQUALITY);
          default: ;;
        }
      }
      CRAB_ERROR("unreachable");
    }

    template<typename VarMap>
    boost::optional<linear_constraint_t> rename(const VarMap& map) const {

      boost::optional<linear_expression_t> e = this->_expr.rename(map);
      if (e) {
        return linear_constraint_t(*e, this->_kind);
      } else {
        return boost::optional<linear_constraint_t>();
      }
    }

    void write(crab::crab_os& o) {
      if (this->is_contradiction()) {
        o << "false";
      } else if (this->is_tautology()) {
        o << "true";
      } else {
        linear_expression_t e = this->_expr - this->_expr.constant();
        o << e;
        switch (this->_kind) {
	case INEQUALITY: {
            o << " <= ";
            break;
          }
	case EQUALITY: {
            o << " = ";
            break;
          }
	case DISEQUATION: {
            o << " != ";
            break;
          }
        }
        Number c = -this->_expr.constant();
        o << c;
      }
    }

  }; // class linear_constraint

  template<typename Number, typename VariableName>
  inline std::size_t hash_value(const linear_constraint<Number,VariableName>& e) {
    return e.hash ();
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(const linear_expression< Number, VariableName > &e, Number n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(const linear_expression< Number, VariableName > &e, int n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(Number n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (n - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(int n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (n - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(const linear_expression< Number, VariableName > &e,
             variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(variable< Number, VariableName > x,
             const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (x - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(variable< Number, VariableName > x, Number n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(variable< Number, VariableName > x, int n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(Number n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (n - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(int n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (n - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(variable< Number, VariableName > x, variable< Number, VariableName > y) {
    return linear_constraint< Number, VariableName >
        (x - y, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator<=(const linear_expression< Number, VariableName >& e1,
             const linear_expression< Number, VariableName >& e2) {
    return linear_constraint< Number, VariableName >
        (e1 - e2, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(const linear_expression< Number, VariableName > &e, Number n) {
    return linear_constraint< Number, VariableName >
        (n - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(const linear_expression< Number, VariableName > &e, int n) {
    return linear_constraint< Number, VariableName >
        (n - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(Number n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(int n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(const linear_expression< Number, VariableName > &e,
             variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - e, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(variable< Number, VariableName > x,
             const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(variable< Number, VariableName > x, Number n) {
    return linear_constraint< Number, VariableName >
        (n - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(variable< Number, VariableName > x, int n) {
    return linear_constraint< Number, VariableName >
        (n - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(Number n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(int n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(variable< Number, VariableName > x,
             variable< Number, VariableName > y) {
    return linear_constraint< Number, VariableName >
        (y - x, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator>=(const linear_expression< Number, VariableName > &e1,
             const linear_expression< Number, VariableName > &e2) {
    return linear_constraint< Number, VariableName >
        (e2 - e1, linear_constraint< Number, VariableName >::INEQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(const linear_expression< Number, VariableName > &e, Number n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(const linear_expression< Number, VariableName > &e, int n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(Number n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(int n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(const linear_expression< Number, VariableName > &e,
             variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(variable< Number, VariableName > x,
             const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(variable< Number, VariableName > x, Number n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(variable< Number, VariableName > x, int n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(Number n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(int n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(variable< Number, VariableName > x,
             variable< Number, VariableName > y) {
    return linear_constraint< Number, VariableName >
        (x - y, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator==(const linear_expression< Number, VariableName > &e1,
             const linear_expression< Number, VariableName > &e2) {
    return linear_constraint< Number, VariableName >
        (e1 - e2, linear_constraint< Number, VariableName >::EQUALITY);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(const linear_expression< Number, VariableName > &e, Number n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(const linear_expression< Number, VariableName > &e, int n) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(Number n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(int n, const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(const linear_expression< Number, VariableName > &e,
             variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(variable< Number, VariableName > x,
             const linear_expression< Number, VariableName > &e) {
    return linear_constraint< Number, VariableName >
        (e - x, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(variable< Number, VariableName > x, Number n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(variable< Number, VariableName > x, int n) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(Number n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(int n, variable< Number, VariableName > x) {
    return linear_constraint< Number, VariableName >
        (x - n, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(variable< Number, VariableName > x,
             variable< Number, VariableName > y) {
    return linear_constraint< Number, VariableName >
        (x - y, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  inline linear_constraint< Number, VariableName >
  operator!=(const linear_expression< Number, VariableName > &e1,
             const linear_expression< Number, VariableName > &e2) {
    return linear_constraint< Number, VariableName >
        (e1 - e2, linear_constraint< Number, VariableName >::DISEQUATION);
  }

  template< typename Number, typename VariableName >
  class linear_constraint_system: public writeable {

  public:
    typedef Number number_t;
    typedef VariableName varname_t;
    typedef linear_constraint< Number, VariableName > linear_constraint_t;
    typedef linear_constraint_system< Number, VariableName > linear_constraint_system_t;
    typedef variable< Number, VariableName > variable_t;
    typedef patricia_tree_set< variable_t > variable_set_t;

  private:
    typedef std::vector< linear_constraint_t > cst_collection_t;

  public:
    typedef typename cst_collection_t::const_iterator iterator;

  private:
    cst_collection_t _csts;

  public:
    linear_constraint_system() { }

    linear_constraint_system(const linear_constraint_t &cst) {
      _csts.push_back(cst);
    }

    linear_constraint_system(const linear_constraint_system_t &o)
        : _csts (o._csts) { }

    linear_constraint_system(linear_constraint_system_t &&o)
        : _csts (std::move(o._csts)) { }

    linear_constraint_system_t&
    operator+=(const linear_constraint_t &cst) {
      this->_csts.push_back(cst);
      return *this;
    }

    linear_constraint_system_t&
    operator+=(const linear_constraint_system_t &s) {
      for (auto c: s)
        this->_csts.push_back(c);
      return *this;
    }

    linear_constraint_system_t
    operator+(const linear_constraint_system_t &s) const {
      linear_constraint_system_t r;
      r.operator+=(s);
      r.operator+=(*this);
      return r;
    }

    iterator begin() const { return _csts.begin(); }

    iterator end() const { return _csts.end(); }

    variable_set_t variables() const {
      variable_set_t variables;
      for (auto c: *this)
        variables |= c.variables();
      return variables;
    }

    // TODO: expensive linear operation.
    // XXX: We can keep track of whether the system is false in an
    // incremental manner.
    bool is_false () const {
      if (_csts.empty ())
	return false; // empty is considered true

      for (auto it = this->begin(); it != this->end();) {
        auto c = *it;
	if (!c.is_contradiction ()) {
	  return false;
	}
      }
      return true; // all constraints are false
    }

    std::size_t size() const { return _csts.size(); }

    void write(crab::crab_os& o) {
      o << "{";
      for (iterator it = this->begin(); it != this->end();) {
        auto c = *it;
        o << c;
        ++it;
        if (it != end()) {
        o << "; ";
        }
      }
      o << "}";
    }

  }; // class linear_constraint_system

  // This class contains a disjunction of linear constraints
  template< typename Number, typename VariableName >
  class disjunctive_linear_constraint_system: public writeable {

  public:
    typedef Number number_t;
    typedef VariableName varname_t;
    typedef linear_constraint<Number,VariableName> linear_constraint_t;
    typedef linear_constraint_system<Number,VariableName> linear_constraint_system_t;
    typedef disjunctive_linear_constraint_system<Number,VariableName> this_type;

  private:
    typedef std::vector<linear_constraint_system_t> cst_collection_t;
    cst_collection_t _csts;
    bool _is_false;

  public:
    typedef typename cst_collection_t::const_iterator iterator;

    disjunctive_linear_constraint_system()
      : _is_false (false) {}

    disjunctive_linear_constraint_system(const linear_constraint_system_t &cst)
      : _is_false (false) {
      if (cst.is_false ()) {
	_is_false = true;
      } else {
	_csts.push_back(cst);
      }
    }

    disjunctive_linear_constraint_system(const this_type &o)
        : _csts (o._csts) { }

    disjunctive_linear_constraint_system(this_type &&o)
        : _csts (std::move(o._csts)) { }

    bool is_false () const {
      return _is_false;
    }

    this_type& operator+=(const linear_constraint_system_t &cst) {
      if (_csts.empty () && cst.is_false ()) {
	_is_false = true;
      } else {
	_csts.push_back(cst);
	_is_false = false;
      }
      return *this;
    }

    this_type& operator+=(const this_type &s) {
      if (this->is_false ()) {
	return s;
      } else if (s.is_false ()) {
	return *this;
      } else {
	for (auto c: s) {
	  _csts.push_back(c);
	}
	return *this;
      }
    }

    this_type operator+(const this_type &s) const {
      this_type r;
      r.operator+=(s);
      r.operator+=(*this);
      return r;
    }

    // To enumerate all the conjunctions
    iterator begin() const {
      if (is_false ())
	CRAB_ERROR("Disjunctive Linear constraint: trying to call begin() when false");
      return _csts.begin();
    }

    iterator end() const {
      if (is_false ())
	CRAB_ERROR("Disjunctive Linear constraint: trying to call end() when false");
      return _csts.end();
    }

    // Return the number of conjunctions
    std::size_t size() const { return _csts.size(); }

    void write(crab::crab_os& o) {
      if (is_false ()) {
	o << "_|_";
      } else if (_csts.empty ()) {
	o << "{}";
      } else if (size () == 1) {
	o << _csts[0];
      } else {
	assert (size () > 1);
	for (iterator it = this->begin(); it != this->end();) {
	  auto c = *it;
	  o << c;
	  ++it;
	  if (it != end()) {
	    o << " or \n";
	  }
	}
      }
    }
  };

} // namespace ikos

#endif // IKOS_LINEAR_CONSTRAINTS_HPP