//  Copyright (c) 2007-2021 Hartmut Kaiser
//
//  SPDX-License-Identifier: BSL-1.0
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <hpx/config.hpp>
#include <hpx/async_base/launch_policy.hpp>
#include <hpx/async_distributed/continuation.hpp>
#include <hpx/components_base/agas_interface.hpp>
#include <hpx/functional/bind_front.hpp>
#include <hpx/performance_counters/counter_creators.hpp>
#include <hpx/performance_counters/counters.hpp>
#include <hpx/performance_counters/performance_counter.hpp>
#include <hpx/performance_counters/server/statistics_counter.hpp>
#include <hpx/runtime_components/derived_component_factory.hpp>
#include <hpx/runtime_local/runtime_local_fwd.hpp>
#include <hpx/thread_support/unlock_guard.hpp>
#include <hpx/timing/high_resolution_clock.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <boost/accumulators/statistics/rolling_variance.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <hpx/statistics/rolling_max.hpp>
#include <hpx/statistics/rolling_min.hpp>

#if defined(HPX_MSVC)
#pragma warning(push)
#pragma warning(disable : 4244)
#endif
#include <boost/accumulators/statistics/median.hpp>
#if defined(HPX_MSVC)
#pragma warning(pop)
#endif

#include <boost/spirit/home/x3/char.hpp>
#include <boost/spirit/home/x3/core.hpp>
#include <boost/spirit/home/x3/numeric.hpp>
#include <boost/spirit/home/x3/operator.hpp>

#include <boost/version.hpp>

#include <cstddef>
#include <cstdint>
#include <mutex>
#include <string>
#include <vector>

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace performance_counters { namespace server {
    ///////////////////////////////////////////////////////////////////////////
    namespace detail {
        template <typename Statistic>
        struct counter_type_from_statistic;

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::mean>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::mean aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t /*parameter2*/) {}

            double get_value() override
            {
                return boost::accumulators::mean(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::variance>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::variance aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t /*parameter2*/) {}

            double get_value() override
            {
                return sqrt(boost::accumulators::variance(accum_));
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::median>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::median aggregating_tag;
            typedef boost::accumulators::with_p_square_quantile
                aggregating_type_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag(
                    aggregating_type_tag)>>
                accumulator_type;

            counter_type_from_statistic(std::size_t /*parameter2*/) {}

            double get_value() override
            {
                return boost::accumulators::median(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<
            boost::accumulators::tag::rolling_mean>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::rolling_mean aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t parameter2)
              : accum_(boost::accumulators::tag::rolling_window::window_size =
                           parameter2)
            {
                if (parameter2 == 0)
                {
                    HPX_THROW_EXCEPTION(bad_parameter,
                        "counter_type_from_statistic<Statistic>",
                        "base rolling window size is specified to be zero");
                }
            }

            double get_value() override
            {
                return boost::accumulators::rolling_mean(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<
            boost::accumulators::tag::rolling_variance>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::rolling_variance aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t parameter2)
              : accum_(boost::accumulators::tag::rolling_window::window_size =
                           parameter2)
            {
                if (parameter2 == 0)
                {
                    HPX_THROW_EXCEPTION(bad_parameter,
                        "counter_type_from_statistic<Statistic>",
                        "base rolling window size is specified to be zero");
                }
            }

            double get_value() override
            {
                return sqrt(boost::accumulators::rolling_variance(accum_));
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::max>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::max aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t /*parameter2*/) {}

            double get_value() override
            {
                return (boost::accumulators::max)(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return true;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::min>
          : counter_type_from_statistic_base
        {
            typedef boost::accumulators::tag::min aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t /*parameter2*/) {}

            double get_value() override
            {
                return (boost::accumulators::min)(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return true;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<hpx::util::tag::rolling_min>
          : counter_type_from_statistic_base
        {
            typedef hpx::util::tag::rolling_min aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t parameter2)
              : accum_(boost::accumulators::tag::rolling_window::window_size =
                           parameter2)
            {
                if (parameter2 == 0)
                {
                    HPX_THROW_EXCEPTION(bad_parameter,
                        "counter_type_from_statistic<Statistic>",
                        "base rolling window size is specified to be zero");
                }
            }

            double get_value() override
            {
                return hpx::util::rolling_min(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };

        template <>
        struct counter_type_from_statistic<hpx::util::tag::rolling_max>
          : counter_type_from_statistic_base
        {
            typedef hpx::util::tag::rolling_max aggregating_tag;
            typedef boost::accumulators::accumulator_set<double,
                boost::accumulators::stats<aggregating_tag>>
                accumulator_type;

            counter_type_from_statistic(std::size_t parameter2)
              : accum_(boost::accumulators::tag::rolling_window::window_size =
                           parameter2)
            {
                if (parameter2 == 0)
                {
                    HPX_THROW_EXCEPTION(bad_parameter,
                        "counter_type_from_statistic<Statistic>",
                        "base rolling window size is specified to be zero");
                }
            }

            double get_value() override
            {
                return hpx::util::rolling_max(accum_);
            }

            void add_value(double value) override
            {
                accum_(value);
            }

            bool need_reset() const override
            {
                return false;
            }

        private:
            accumulator_type accum_;
        };
    }    // namespace detail

    ///////////////////////////////////////////////////////////////////////////
    template <typename Statistic>
    statistics_counter<Statistic>::statistics_counter(counter_info const& info,
        std::string const& base_counter_name, std::size_t parameter1,
        std::size_t parameter2, bool reset_base_counter)
      : base_type_holder(info)
      , timer_(util::bind_front(&statistics_counter::evaluate, this_()),
            util::bind_front(&statistics_counter::on_terminate, this_()),
            1000 * parameter1, info.fullname_, true)
      , base_counter_name_(ensure_counter_prefix(base_counter_name))
      , value_(new detail::counter_type_from_statistic<Statistic>(parameter2))
      , has_prev_value_(false)
      , parameter1_(parameter1)
      , parameter2_(parameter2)
      , reset_base_counter_(reset_base_counter)
    {
        if (parameter1 == 0)
        {
            HPX_THROW_EXCEPTION(bad_parameter,
                "statistics_counter<Statistic>::statistics_counter",
                "base interval is specified to be zero");
        }

        if (info.type_ != counter_aggregating)
        {
            HPX_THROW_EXCEPTION(bad_parameter,
                "statistics_counter<Statistic>::statistics_counter",
                "unexpected counter type specified");
        }

        // make sure this counter starts collecting data
        start();
    }

    template <typename Statistic>
    hpx::performance_counters::counter_value
    statistics_counter<Statistic>::get_counter_value(bool reset)
    {
        std::lock_guard<mutex_type> l(mtx_);

        hpx::performance_counters::counter_value value;

        prev_value_.value_ = static_cast<std::int64_t>(value_->get_value());
        prev_value_.status_ = status_new_data;
        prev_value_.time_ = static_cast<std::int64_t>(hpx::get_system_uptime());
        prev_value_.count_ = ++invocation_count_;
        has_prev_value_ = true;

        value = prev_value_;    // return value

        if (reset || value_->need_reset())
        {
            value_.reset(new detail::counter_type_from_statistic<Statistic>(
                parameter2_));    // reset accumulator
            value_->add_value(static_cast<double>(prev_value_.value_));
            // start off with last base value
        }

        return value;
    }

    template <typename Statistic>
    bool statistics_counter<Statistic>::evaluate()
    {
        // gather current base value
        counter_value base_value;
        if (!evaluate_base_counter(base_value))
            return false;

        // simply average the measured base counter values since it got queried
        // for the last time
        counter_value value;
        if (base_value.scaling_ != prev_value_.scaling_ ||
            base_value.scale_inverse_ != prev_value_.scale_inverse_)
        {
            // not supported right now
            HPX_THROW_EXCEPTION(not_implemented,
                "statistics_counter<Statistic>::evaluate",
                "base counter should keep scaling constant over time");
            return false;
        }
        else
        {
            // accumulate new value
            std::lock_guard<mutex_type> l(mtx_);
            value_->add_value(static_cast<double>(base_value.value_));
        }
        return true;
    }

    template <typename Statistic>
    bool statistics_counter<Statistic>::ensure_base_counter()
    {
        // lock here to avoid checking out multiple reference counted GIDs
        // from AGAS. This
        std::unique_lock<mutex_type> l(mtx_);

        if (!base_counter_id_)
        {
            // get or create the base counter
            error_code ec(lightweight);
            hpx::id_type base_counter_id;
            {
                // We need to unlock the lock here since get_counter might suspend
                util::unlock_guard<std::unique_lock<mutex_type>> unlock(l);
                base_counter_id = get_counter(base_counter_name_, ec);
            }

            // After reacquiring the lock, we need to check again if base_counter_id_
            // hasn't been set yet
            if (!base_counter_id_)
            {
                base_counter_id_ = base_counter_id;
            }
            else
            {
                // If it was set already by a different thread, return true.
                return true;
            }

            if (HPX_UNLIKELY(ec || !base_counter_id_))
            {
                // base counter could not be retrieved
                HPX_THROW_EXCEPTION(bad_parameter,
                    "statistics_counter<Statistic>::evaluate_base_counter",
                    "could not get or create performance counter: '{}'",
                    base_counter_name_);
                return false;
            }
        }

        return true;
    }

    template <typename Statistic>
    bool statistics_counter<Statistic>::evaluate_base_counter(
        counter_value& value)
    {
        // query the actual value
        if (!base_counter_id_ && !ensure_base_counter())
            return false;

        performance_counters::performance_counter c(base_counter_id_);
        value = c.get_counter_value(launch::sync, reset_base_counter_);

        if (!has_prev_value_)
        {
            has_prev_value_ = true;
            prev_value_ = value;
        }

        return true;
    }

    ///////////////////////////////////////////////////////////////////////////
    // Start and stop this counter. We dispatch the calls to the base counter
    // and control our own interval_timer.
    template <typename Statistic>
    bool statistics_counter<Statistic>::start()
    {
        if (!timer_.is_started())
        {
            // start base counter
            if (!base_counter_id_ && !ensure_base_counter())
                return false;

            performance_counters::performance_counter c(base_counter_id_);
            bool result = c.start(launch::sync);
            if (result)
            {
                // acquire the current value of the base counter
                counter_value base_value;
                if (evaluate_base_counter(base_value))
                {
                    std::lock_guard<mutex_type> l(mtx_);
                    value_->add_value(static_cast<double>(base_value.value_));
                    prev_value_ = base_value;
                }

                // start timer
                timer_.start();
            }
            else
            {
                // start timer even if base counter does not support being
                // start/stop operations
                timer_.start(true);
            }
            return result;
        }
        return false;
    }

    template <typename Statistic>
    bool statistics_counter<Statistic>::stop()
    {
        if (timer_.is_started())
        {
            timer_.stop();

            if (!base_counter_id_ && !ensure_base_counter())
                return false;

            performance_counters::performance_counter c(base_counter_id_);
            return c.stop(launch::sync);
        }
        return false;
    }

    template <typename Statistic>
    void statistics_counter<Statistic>::reset_counter_value()
    {
        std::lock_guard<mutex_type> l(mtx_);

        // reset accumulator
        value_.reset(
            new detail::counter_type_from_statistic<Statistic>(parameter2_));

        // start off with last base value
        value_->add_value(static_cast<double>(prev_value_.value_));
    }
}}}    // namespace hpx::performance_counters::server

///////////////////////////////////////////////////////////////////////////////
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::mean>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::variance>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::rolling_variance>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::median>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::rolling_mean>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::max>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    boost::accumulators::tag::min>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    hpx::util::tag::rolling_min>;
template class HPX_EXPORT hpx::performance_counters::server::statistics_counter<
    hpx::util::tag::rolling_max>;

///////////////////////////////////////////////////////////////////////////////
// Average
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::mean>>
    average_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(average_count_counter_type,
    average_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(average_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Rolling variance
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::rolling_variance>>
    rolling_variance_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(rolling_variance_count_counter_type,
    rolling_variance_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(rolling_variance_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Variance
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::variance>>
    variance_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(variance_count_counter_type,
    variance_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(variance_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Rolling average
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::rolling_mean>>
    rolling_mean_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(rolling_mean_count_counter_type,
    rolling_mean_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(rolling_mean_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Median
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::median>>
    median_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(median_count_counter_type,
    median_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(median_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Max
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::max>>
    max_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(max_count_counter_type,
    max_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(max_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Min
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<boost::accumulators::tag::min>>
    min_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(min_count_counter_type,
    min_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(min_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Rolling min
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<hpx::util::tag::rolling_min>>
    rolling_min_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(rolling_min_count_counter_type,
    rolling_min_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(rolling_min_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
// Rolling max
typedef hpx::components::component<hpx::performance_counters::server::
        statistics_counter<hpx::util::tag::rolling_max>>
    rolling_max_count_counter_type;

HPX_REGISTER_DERIVED_COMPONENT_FACTORY(rolling_max_count_counter_type,
    rolling_max_count_counter, "base_performance_counter",
    hpx::components::factory_enabled)
HPX_DEFINE_GET_COMPONENT_TYPE(rolling_max_count_counter_type::wrapped_type)

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace performance_counters { namespace detail {
    /// Creation function for aggregating performance counters to be registered
    /// with the counter types.
    naming::gid_type statistics_counter_creator(
        counter_info const& info, error_code& ec)
    {
        switch (info.type_)
        {
        case counter_aggregating:
        {
            counter_path_elements paths;
            get_counter_path_elements(info.fullname_, paths, ec);
            if (ec)
                return naming::invalid_gid;

            if (!paths.parentinstance_is_basename_)
            {
                HPX_THROWS_IF(ec, bad_parameter, "statistics_counter_creator",
                    "invalid aggregate counter "
                    "name (instance name must be valid base counter name)");
                return naming::invalid_gid;
            }

            std::string base_name;
            get_counter_name(paths.parentinstancename_, base_name, ec);
            if (ec)
                return naming::invalid_gid;

            std::vector<std::size_t> parameters;
            if (!paths.parameters_.empty())
            {
                // try to interpret the additional parameters
                namespace x3 = boost::spirit::x3;
                if (!x3::parse(paths.parameters_.begin(),
                        paths.parameters_.end(), x3::uint_ % ',', parameters))
                {
                    HPX_THROWS_IF(ec, bad_parameter,
                        "statistics_counter_creator",
                        "invalid parameter specification format for "
                        "this counter: {}",
                        paths.parameters_);
                    return naming::invalid_gid;
                }
                if (paths.countername_.find("rolling") != std::string::npos)
                {
                    if (parameters.size() > 3)
                    {
                        HPX_THROWS_IF(ec, bad_parameter,
                            "statistics_counter_creator",
                            "too many parameter specifications for "
                            "this counter: {}",
                            paths.parameters_);
                        return naming::invalid_gid;
                    }
                }
                else if (parameters.size() > 2)
                {
                    HPX_THROWS_IF(ec, bad_parameter,
                        "statistics_counter_creator",
                        "too many parameter specifications for "
                        "this counter: {}",
                        paths.parameters_);
                    return naming::invalid_gid;
                }
            }
            else if (paths.countername_.find("rolling") != std::string::npos)
            {
                parameters.push_back(1000);    // sample interval
                parameters.push_back(10);      // rolling window
                parameters.push_back(0);       // don't reset underlying counter
            }
            else
            {
                parameters.push_back(1000);    // sample interval
                parameters.push_back(0);       // don't reset underlying counter
            }
            return create_statistics_counter(info, base_name, parameters, ec);
        }
        break;

        default:
            HPX_THROWS_IF(ec, bad_parameter, "statistics_counter_creator",
                "invalid counter type requested");
            return naming::invalid_gid;
        }
    }
}}}    // namespace hpx::performance_counters::detail


// Copyright (c) 2020 INRA Distributed under the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <irritator/core.hpp>

#include <boost/ut.hpp>

#include <fmt/format.h>

#include <cstdio>

static void
dot_graph_save(const irt::simulation& sim, std::FILE* os)
{
    /* With input and output port.

    digraph graphname{
        graph[rankdir = "LR"];
        node[shape = "record"];
        edge[];

        "sum_a"[label = "sum-a | <f0> | <f1>"];

        "sum_a":f0->int_a[id = 1];
        sum_b->int_b[label = "2-10"];
        prod->sum_b[label = "3-4"];
        prod -> "sum_a":f0[label = "3-2"];
        int_a->qua_a[label = "4-11"];
        int_a->prod[label = "4-5"];
        int_a -> "sum_a":f1[label = "4-1"];
        int_b->qua_b[label = "5-12"];
        int_b->prod[label = "5-6"];
        int_b->sum_b[label = "5-3"];
        qua_a->int_a[label = "6-7"];
        qua_b->int_b[label = "7-9"];
    }
    */

    !boost::ut::expect(os != nullptr);

    std::fputs("digraph graphname {\n", os);

    irt::output_port* output_port = nullptr;
    while (sim.output_ports.next(output_port)) {
        for (const irt::input_port_id dst : output_port->connections) {
            if (auto* input_port = sim.input_ports.try_to_get(dst);
                input_port) {
                auto* mdl_src = sim.models.try_to_get(output_port->model);
                auto* mdl_dst = sim.models.try_to_get(input_port->model);

                if (!(mdl_src && mdl_dst))

                    continue;
                if (mdl_src->name.empty())
                    fmt::print(os, "{} -> ", irt::get_key(output_port->model));
                else
                    fmt::print(os, "{} -> ", mdl_src->name.c_str());

                if (mdl_dst->name.empty())
                    fmt::print(os, "{}", irt::get_key(input_port->model));
                else
                    fmt::print(os, "{}", mdl_dst->name.c_str());

                std::fputs(" [label=\"", os);

                if (output_port->name.empty())
                    fmt::print(
                      os,
                      "{}",
                      irt::get_key(sim.output_ports.get_id(*output_port)));
                else
                    fmt::print(os, "{}", output_port->name.c_str());

                std::fputs("-", os);

                if (input_port->name.empty())
                    fmt::print(
                      os,
                      "{}",
                      irt::get_key(sim.input_ports.get_id(*input_port)));
                else
                    fmt::print(os, "{}", input_port->name.c_str());

                std::fputs("\"];\n", os);
            }
        }
    }
}

struct neuron {
  irt::dynamics_id  sum;
  irt::dynamics_id  prod;
  irt::dynamics_id  integrator;
  irt::dynamics_id  quantifier;
  irt::dynamics_id  constant;
  irt::dynamics_id  cross;
  irt::dynamics_id  constant_cross;
};

struct synapse {
  irt::dynamics_id  sum_pre;
  irt::dynamics_id  prod_pre;
  irt::dynamics_id  integrator_pre;
  irt::dynamics_id  quantifier_pre;
  irt::dynamics_id  cross_pre;

  irt::dynamics_id  sum_post;
  irt::dynamics_id  prod_post;
  irt::dynamics_id  integrator_post;
  irt::dynamics_id  quantifier_post;
  irt::dynamics_id  cross_post;

  irt::dynamics_id  constant_syn;
  irt::dynamics_id  accumulator_syn;
};

struct neuron
make_neuron(irt::simulation* sim, long unsigned int i) noexcept
{
    using namespace boost::ut;
  double tau_lif =  1.0 + static_cast <float> (rand()) /( static_cast <float> (RAND_MAX/(2.0-1.0)));
  double Vr_lif = 0.0;
  double Vt_lif = 1.0;

  auto& sum_lif = sim->adder_2_models.alloc();
  auto& prod_lif = sim->adder_2_models.alloc();
  auto& integrator_lif = sim->integrator_models.alloc();
  auto& quantifier_lif = sim->quantifier_models.alloc();
  auto& constant_lif = sim->constant_models.alloc();
  auto& constant_cross_lif = sim->constant_models.alloc();
  auto& cross_lif = sim->cross_models.alloc();

  sum_lif.default_input_coeffs[0] = -1.0;
  sum_lif.default_input_coeffs[1] = 2*Vt_lif;

  prod_lif.default_input_coeffs[0] = 1.0/tau_lif;
  prod_lif.default_input_coeffs[1] = 0.0;

  constant_lif.default_value = 1.0;
  constant_cross_lif.default_value = Vr_lif;

  integrator_lif.default_current_value = 0.0;

  quantifier_lif.default_adapt_state =
    irt::quantifier::adapt_state::possible;
  quantifier_lif.default_zero_init_offset = true;
  quantifier_lif.default_step_size = 0.1;
  quantifier_lif.default_past_length = 3;

  cross_lif.default_threshold = Vt_lif;

  sim->alloc(sum_lif, sim->adder_2_models.get_id(sum_lif));
  sim->alloc(prod_lif, sim->adder_2_models.get_id(prod_lif));
  sim->alloc(integrator_lif, sim->integrator_models.get_id(integrator_lif));
  sim->alloc(quantifier_lif, sim->quantifier_models.get_id(quantifier_lif));
  sim->alloc(constant_lif, sim->constant_models.get_id(constant_lif));
  sim->alloc(cross_lif, sim->cross_models.get_id(cross_lif));
  sim->alloc(constant_cross_lif, sim->constant_models.get_id(constant_cross_lif));

  struct neuron neuron_model = {sim->adder_2_models.get_id(sum_lif),
                                sim->adder_2_models.get_id(prod_lif),
                                sim->integrator_models.get_id(integrator_lif),
                                sim->quantifier_models.get_id(quantifier_lif),
                                sim->constant_models.get_id(constant_lif),
                                sim->cross_models.get_id(cross_lif),
                                sim->constant_models.get_id(constant_cross_lif),
                                };

  // Connections
  expect(sim->connect(quantifier_lif.y[0], integrator_lif.x[0]) ==
          irt::status::success);
  expect(sim->connect(prod_lif.y[0], integrator_lif.x[1]) ==
          irt::status::success);
  expect(sim->connect(cross_lif.y[0], integrator_lif.x[2]) ==
          irt::status::success);
  expect(sim->connect(cross_lif.y[0], quantifier_lif.x[0]) ==
          irt::status::success);
  expect(sim->connect(cross_lif.y[0], sum_lif.x[0]) ==
          irt::status::success);
  expect(sim->connect(integrator_lif.y[0],cross_lif.x[0]) ==
          irt::status::success);
  expect(sim->connect(integrator_lif.y[0],cross_lif.x[2]) ==
          irt::status::success);
  expect(sim->connect(constant_cross_lif.y[0],cross_lif.x[1]) ==
          irt::status::success);
  expect(sim->connect(constant_lif.y[0], sum_lif.x[1]) ==
          irt::status::success);
  expect(sim->connect(sum_lif.y[0],prod_lif.x[0]) ==
          irt::status::success);
  expect(sim->connect(constant_lif.y[0],prod_lif.x[1]) ==
          irt::status::success);
  return neuron_model;
}

struct synapse make_synapse(irt::simulation* sim, long unsigned int source, long unsigned int target,
                              irt::output_port_id presynaptic,irt::output_port_id postsynaptic)
{
  using namespace boost::ut;
  double taupre = 20.0*1;
  double taupost = taupre;
  double gamax = 0.015;
  double dApre = 0.01;
  double dApost = -dApre * taupre / taupost * 1.05;
  dApost = dApost * gamax;
  dApre = dApre * gamax;

  auto& int_pre = sim->integrator_models.alloc();
  auto& quant_pre = sim->quantifier_models.alloc();
  auto& sum_pre = sim->adder_2_models.alloc();
  auto& mult_pre = sim->adder_2_models.alloc();
  auto& cross_pre = sim->cross_models.alloc();

  auto& int_post = sim->integrator_models.alloc();
  auto& quant_post = sim->quantifier_models.alloc();
  auto& sum_post = sim->adder_2_models.alloc();
  auto& mult_post = sim->adder_2_models.alloc();
  auto& cross_post = sim->cross_models.alloc();

  auto& const_syn = sim->constant_models.alloc();
  auto& accumulator_syn = sim->accumulator_2_models.alloc();

  cross_pre.default_threshold = 1.0;
  int_pre.default_current_value = 0.0;
  quant_pre.default_adapt_state =
    irt::quantifier::adapt_state::possible;
  quant_pre.default_zero_init_offset = true;
  quant_pre.default_step_size = 1e-6;
  quant_pre.default_past_length = 3;
  sum_pre.default_input_coeffs[0] = 1.0;
  sum_pre.default_input_coeffs[1] = dApre;
  mult_pre.default_input_coeffs[0] = -1.0/taupre;
  mult_pre.default_input_coeffs[1] = 0.0;

  cross_post.default_threshold = 1.0;
  int_post.default_current_value = 0.0;
  quant_post.default_adapt_state =
    irt::quantifier::adapt_state::possible;
  quant_post.default_zero_init_offset = true;
  quant_post.default_step_size = 1e-6;
  quant_post.default_past_length = 3;
  sum_post.default_input_coeffs[0] = 1.0;
  sum_post.default_input_coeffs[1] = dApost;
  mult_post.default_input_coeffs[0] = -1.0/taupost;
  mult_post.default_input_coeffs[1] = 0.0;

  const_syn.default_value = 1.0;

  char intpre[7];char quapre[7];char addpre[7];char propre[7];char crosspre[7];
  char intpost[7];char quapost[7];char addpost[7];char propost[7];char crosspost[7];
  char ctesyn[7];char accumsyn[7];

  snprintf(intpre, 7,"int%ld%ld-",source,target);snprintf(quapre, 7,"qua%ld%ld-",source,target);
  snprintf(addpre, 7,"add%ld%ld-",source,target);snprintf(propre, 7,"pro%ld%ld-",source,target);snprintf(crosspre, 7,"cro%ld%ld-",source,target);

  snprintf(intpost, 7,"int%ld%ld+",source,target);snprintf(quapost, 7,"qua%ld%ld+",source,target);snprintf(addpost, 7,"add%ld%ld+",source,target);
  snprintf(propost, 7,"pro%ld%ld+",source,target);snprintf(crosspost, 7,"cro%ld%ld+",source,target);

  snprintf(ctesyn, 7,"cte%ld%ld",source,target);snprintf(accumsyn, 7,"acc%ld%ld",source,target);

  !expect(irt::is_success(sim->alloc(int_pre, sim->integrator_models.get_id(int_pre), intpre)));
  !expect(irt::is_success(sim->alloc(quant_pre, sim->quantifier_models.get_id(quant_pre), quapre)));
  !expect(irt::is_success(sim->alloc(sum_pre, sim->adder_2_models.get_id(sum_pre), addpre)));
  !expect(irt::is_success(sim->alloc(mult_pre, sim->adder_2_models.get_id(mult_pre), propre)));
  !expect(irt::is_success(sim->alloc(cross_pre, sim->cross_models.get_id(cross_pre), crosspre)));

  !expect(irt::is_success(sim->alloc(int_post, sim->integrator_models.get_id(int_post), intpost)));
  !expect(irt::is_success(sim->alloc(quant_post, sim->quantifier_models.get_id(quant_post), quapost)));
  !expect(irt::is_success(sim->alloc(sum_post, sim->adder_2_models.get_id(sum_post), addpost)));
  !expect(irt::is_success(sim->alloc(mult_post, sim->adder_2_models.get_id(mult_post), propost)));
  !expect(irt::is_success(sim->alloc(cross_post, sim->cross_models.get_id(cross_post), crosspost)));

  !expect(irt::is_success(sim->alloc(const_syn, sim->constant_models.get_id(const_syn), ctesyn)));
  !expect(irt::is_success(sim->alloc(accumulator_syn, sim->accumulator_2_models.get_id(accumulator_syn), accumsyn)));

  struct synapse synapse_model = {sim->adder_2_models.get_id(sum_pre),
                                  sim->adder_2_models.get_id(mult_pre),
                                  sim->integrator_models.get_id(int_pre),
                                  sim->quantifier_models.get_id(quant_pre),
                                  sim->cross_models.get_id(cross_pre),

                                  sim->adder_2_models.get_id(sum_post),
                                  sim->adder_2_models.get_id(mult_post),
                                  sim->integrator_models.get_id(int_post),
                                  sim->quantifier_models.get_id(quant_post),
                                  sim->cross_models.get_id(cross_post),

                                  sim->constant_models.get_id(const_syn),
                                  sim->accumulator_2_models.get_id(accumulator_syn),
                                  };

  // Connections
  expect(sim->connect(quant_pre.y[0],
                      int_pre.x[0]) ==
                        irt::status::success);
  expect(sim->connect(mult_pre.y[0],
                      int_pre.x[1]) ==
                        irt::status::success);
  expect(sim->connect(cross_pre.y[0],
                      int_pre.x[2]) ==
                        irt::status::success);
  expect(sim->connect(int_pre.y[0],
                      cross_pre.x[2]) ==
                        irt::status::success);
  expect(sim->connect(cross_pre.y[0],
                      quant_pre.x[0]) ==
                        irt::status::success);
  expect(sim->connect(cross_pre.y[0],
                      mult_pre.x[0]) ==
                        irt::status::success);
  expect(sim->connect(const_syn.y[0],
                      mult_pre.x[1]) ==
                        irt::status::success);
  expect(sim->connect(int_pre.y[0],
                      sum_pre.x[0]) ==
                        irt::status::success);
  expect(sim->connect(const_syn.y[0],
                      sum_pre.x[1]) ==
                        irt::status::success);
  expect(sim->connect(sum_pre.y[0],
                      cross_pre.x[1]) ==
                        irt::status::success);
  expect(sim->connect(presynaptic,
                      cross_pre.x[0]) ==
                        irt::status::success);

  expect(sim->connect(quant_post.y[0],
                      int_post.x[0]) ==
                        irt::status::success);
  expect(sim->connect(mult_post.y[0],
                      int_post.x[1]) ==
                        irt::status::success);
  expect(sim->connect(cross_post.y[0],
                      int_post.x[2]) ==
                        irt::status::success);
  expect(sim->connect(int_post.y[0],
                      cross_post.x[2]) ==
                        irt::status::success);
  expect(sim->connect(cross_post.y[0],
                      quant_post.x[0]) ==
                        irt::status::success);
  expect(sim->connect(cross_post.y[0],
                      mult_post.x[0]) ==
                        irt::status::success);
  expect(sim->connect(const_syn.y[0],
                      mult_post.x[1]) ==
                        irt::status::success);
  expect(sim->connect(int_post.y[0],
                      sum_post.x[0]) ==
                        irt::status::success);
  expect(sim->connect(const_syn.y[0],
                      sum_post.x[1]) ==
                        irt::status::success);
  expect(sim->connect(sum_post.y[0],
                      cross_post.x[1]) ==
                        irt::status::success);
  expect(sim->connect(postsynaptic,
                      cross_post.x[0]) ==
                        irt::status::success);

  expect(sim->connect(presynaptic,
                      accumulator_syn.x[0]) ==
                        irt::status::success);
  expect(sim->connect(postsynaptic,
                      accumulator_syn.x[1]) ==
                        irt::status::success);
  expect(sim->connect(cross_post.y[0],
                      accumulator_syn.x[2]) ==
                        irt::status::success);
  expect(sim->connect(cross_pre.y[0],
                      accumulator_syn.x[3]) ==
                        irt::status::success);

  return synapse_model;
}

int
main()
{
    using namespace boost::ut;

    "song_1_simulation"_test = [] {
        irt::simulation sim;
        // Neuron constants
        long unsigned int N = 4;
        /*double F = 15.0;

        double Eex = 0.0;
        double Ein = -70*0.001;
        double tauex = 5*0.001;
        double tauin = tauex;*/

        // Synapse constants

        //double ginbar = 0.05;

        expect(irt::is_success(sim.init(10000lu, 10000lu)));

        expect(sim.generator_models.can_alloc(N));
        expect(sim.integrator_models.can_alloc(2*N*N));
        expect(sim.quantifier_models.can_alloc(2*N*N));
        expect(sim.adder_2_models.can_alloc(4*N*N));
        expect(sim.constant_models.can_alloc(N));
        expect(sim.cross_models.can_alloc(2*N*N));

        std::vector<struct neuron> neurons;
        for (long unsigned int i = 0 ; i < N; i++) {
          struct neuron neuron_model = make_neuron(&sim,i);
          neurons.emplace_back(neuron_model);
        }

        // Neurons
        /*std::vector<irt::dynamics_id> generators;
        for (long unsigned int i = 0 ; i < N; i++) {
          auto& gen = sim.generator_models.alloc();
          gen.default_value = 3.0;
          gen.default_offset = i+1;
          gen.default_period = 10.0;

          char genstr[5];
          snprintf(genstr, 5,"gen%ld", i);
          !expect(irt::is_success(sim.alloc(gen, sim.generator_models.get_id(gen), genstr)));

          generators.emplace_back(sim.generator_models.get_id(gen));
        } */

        std::vector<struct synapse> synapses;
        for (long unsigned int i = 0 ; i < N; i++) {
          for (long unsigned int j = 0 ; j < N; j++) {

            struct synapse synapse_model = make_synapse(&sim,i,j,
                                            sim.cross_models.get(neurons[i].cross).y[1],
                                              sim.cross_models.get(neurons[j].cross).y[1]);
            synapses.emplace_back(synapse_model);
          }
        }

        dot_graph_save(sim, stdout);

        irt::time t = 0.0;
        std::FILE* os = std::fopen("output_song.csv", "w");
        !expect(os != nullptr);

        std::string s = "t,";
        for (long unsigned int i = 0; i < N*N; i++)
        {
          s =  s + "Apre" + std::to_string(i)
                + ",Apost" + std::to_string(i)
                + ",";
        }
        for (long unsigned int i = 0; i < N*N; i++)
        s =  s + "W" + std::to_string(i) + ",";
        fmt::print(os, s + "\n");
        expect(irt::status::success == sim.initialize(t));

        do {

            irt::status st = sim.run(t);
            expect(st == irt::status::success);

            std::string s = std::to_string(t)+",";
            for (long unsigned int i = 0; i < N*N; i++)
            {
              s =  s + std::to_string(sim.integrator_models.get(synapses[i].integrator_pre).last_output_value)
                    + ","
                    + std::to_string(sim.integrator_models.get(synapses[i].integrator_post).last_output_value)
                    + ",";
            }
            for (long unsigned int i = 0; i < N*N; i++)
            s =  s + std::to_string(sim.accumulator_2_models.get(synapses[i].accumulator_syn).number) + ",";
            fmt::print(os, s + "\n");

        } while (t < 30);

        std::fclose(os);
      };

}


#include <cstdlib>
#include <cmath>
#include <cstdio>

#include <NTL/config.h>

using namespace std;

#ifndef NTL_GMP_LIP

int main()
{
   fprintf(stderr, "NTL_GMP_LIP flag not set\n");

   return 0;
}

#else

#include <gmp.h>
#include <NTL/mach_desc.h>

void print2k(FILE *f, long k, long bpl)
{
   long m, l;
   long first;

   if (k <= 0) {
      fprintf(f, "((double) 1.0)");
      return;
   }

   m = bpl - 2;
   first = 1;

   fprintf(f, "(");

   while (k > 0) {
      if (k > m)
         l = m;
      else
         l = k;

      k = k - l;

      if (first)
         first = 0;
      else
         fprintf(f, "*");

      fprintf(f, "((double)(1L<<%ld))", l);
   }

   fprintf(f, ")");
}

void Error(const char *s)
{
   fprintf(stderr, "%s\n", s);
   abort();
}

int main()
{
   long bpl;
   long ntl_zz_nbits;
   long nail_bits;

   fprintf(stderr, "NTL_GMP_LIP flag set\n");

   bpl = NTL_BITS_PER_LONG;

   /*
    * We require that the number of bits per limb quantity correspond to the
    * number of bits of a long, or possibly a "long long" that is twice as wide
    * as a long.  These restrictions will almost certainly be satisfied.
    */

   ntl_zz_nbits = 0;

   nail_bits = 0;
#ifdef GMP_NAIL_BITS
   nail_bits = GMP_NAIL_BITS;
#endif

   if (nail_bits > 0)
      fprintf(stderr, "WARNING: GMP_NAIL_BITS > 0: this has not been well tested\n");

   // NOTE: newer versions of GMP define mp_bits_per_limb as
   // GMP_LIMB_BITS

   if (sizeof(mp_limb_t) == sizeof(long) && mp_bits_per_limb == bpl)
      ntl_zz_nbits = bpl-nail_bits;
   else if (sizeof(mp_limb_t) == 2*sizeof(long) && mp_bits_per_limb == 2*bpl)
      ntl_zz_nbits = 2*bpl-nail_bits;
   else
      Error("sorry...this is a funny gmp");

   if (ntl_zz_nbits % 2 != 0 || ntl_zz_nbits < 30)
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) != sizeof(long) && sizeof(mp_size_t) != sizeof(int))
      Error("sorry...this is a funny gmp");

   if (sizeof(mp_size_t) < sizeof(long)) {
      printf("#define NTL_SMALL_MP_SIZE_T\n");
      fprintf(stderr, "setting NTL_SMALL_MP_SIZE_T\n");
   }

   fprintf(stderr, "NTL_ZZ_NBITS = %ld\n", ntl_zz_nbits);
   printf("#define NTL_ZZ_NBITS (%ld)\n",  ntl_zz_nbits);

   fprintf(stderr, "NTL_BITS_PER_LIMB_T = %ld\n", ntl_zz_nbits+nail_bits);
   printf("#define NTL_BITS_PER_LIMB_T (%ld)\n", ntl_zz_nbits+nail_bits);

   printf("#define NTL_ZZ_FRADIX ");
   print2k(stdout, ntl_zz_nbits, bpl);
   printf("\n");

   return 0;
}

#endif


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

#ifndef SOURCES_SAMPLER_METROPOLISHASTINGSPT_HPP
#define SOURCES_SAMPLER_METROPOLISHASTINGSPT_HPP

#include <Eigen/Core>
#include <functional>
#include <limits>
#include <map>
#include <string>
#include <vector>

#include "Sampler/abstract_sampler.hpp"
#include "Utils/exceptions.hpp"
#include "Utils/random_utils.hpp"

namespace netket {

// Generic Metropolis-Hastings MCMC with parallel tempering
// a.k.a. replica exchange Monte Carlo
class MetropolisHastingsPt : public AbstractSampler {
 public:
  using TransitionKernel = std::function<void(
      Eigen::Ref<const RowMatrix<double>>, Eigen::Ref<RowMatrix<double>>,
      Eigen::Ref<Eigen::ArrayXd>)>;

 private:
  RowMatrix<double> current_X_;
  RowMatrix<double> proposed_X_;

  Eigen::ArrayXcd current_Y_;
  Eigen::ArrayXcd proposed_Y_;

  Eigen::ArrayXcd quotient_Y_;
  Eigen::ArrayXd probability_;

  Eigen::ArrayXd beta_;
  Eigen::ArrayXd proposed_beta_;

  Eigen::ArrayXd log_acceptance_correction_;

  Eigen::Array<bool, Eigen::Dynamic, 1> accept_;

  TransitionKernel transition_kernel_;

  Index n_replicas_;
  Index sweep_size_;

  // Index holding the position of beta=1
  Index beta1_ind_;

  Eigen::ArrayXd accepted_samples_;
  Index total_samples_;
  Index total_exchange_steps_;
  double beta1_av_;
  double beta1_av_sq_;

 public:
  MetropolisHastingsPt(AbstractMachine& ma, TransitionKernel tk,
                       Index n_replicas, Index sweep_size);

  Index BatchSize() const noexcept override;
  Index NChains() const noexcept override;
  Index SweepSize() const noexcept;
  void SweepSize(Index sweep_size);

  /// Returns a batch of current visible states and corresponding log values.
  std::pair<Eigen::Ref<const RowMatrix<double>>,
            Eigen::Ref<const Eigen::VectorXcd>>
  CurrentState() const override;

  void SetVisible(Eigen::Ref<const RowMatrix<double>> x) override;

  void OneStep();

  void ExchangeStep();

  void Sweep() override;

  /// Resets the sampler.
  void Reset(bool init_random) override;

  void ProposePairwiseSwap(Eigen::Ref<const Eigen::ArrayXd>,
                           Eigen::Ref<Eigen::ArrayXd>, int);

  NETKET_SAMPLER_ACCEPTANCE_DEFAULT(accepted_samples_.mean(), total_samples_);

  std::map<std::string, double> Stats() const;
};

namespace detail {
inline Index CheckReplicasSize(const char* func, const Index n_replicas) {
  if (n_replicas < 2) {
    std::ostringstream msg;
    msg << func << ": invalid number of replicas : " << n_replicas
        << "; expected at least two replicas";
    throw InvalidInputError{msg.str()};
  }
  if (n_replicas % 2 != 0) {
    std::ostringstream msg;
    msg << func << ": invalid number of replicas : " << n_replicas
        << "; expected an even number of replicas";
    throw InvalidInputError{msg.str()};
  }

  return n_replicas;
}

}  // namespace detail

}  // namespace netket

#endif


/****************************************************************************
**
** Copyright (C) 2018 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

#include <cstdio>
#include <fstream>
#include <iostream>

#include <boost/algorithm/string/predicate.hpp>

#include <glog/logging.h>

#include <opencv2/opencv.hpp>

#include <v4r/common/intrinsics.h>

namespace v4r {

void Intrinsics::adjustToSize(unsigned int new_width, unsigned int new_height) {
  if (new_width == w && new_height == h)
    return;

  const float downsampling_ratio = static_cast<float>(new_width) / w;  // assume that always
  // all pixel horizontally are used for creating image
  fx *= downsampling_ratio;
  fy *= downsampling_ratio;
  cx *= downsampling_ratio;
  cy *= downsampling_ratio;

  // check if aspect ratio is kept the same
  if (w * new_height != new_width * h) {
    LOG(WARNING) << "New aspect ratio (" << new_width << "x" << new_height
                 << ") is different than the current aspect ratio (" << w << "x" << h
                 << ")in the RGB calibration file. Will assume only vertical FOV has changed. The center of the FOV "
                    "and the horizontal FOV are assumed to stay constant.";

    // assume that only top and bottom pixel are turned off.
    int new_height_according_to_old_aspect_ratio = new_width * h / static_cast<float>(w);
    int difference = new_height_according_to_old_aspect_ratio - new_height;
    cy -= difference / 2.f;
  }
  w = new_width;
  h = new_height;
}

cv::Mat Intrinsics::getCameraMatrix() const {
  return cv::Mat_<float>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1;
}

Intrinsics Intrinsics::fromCameraMatrixAndResolution(cv::InputArray _camera_matrix, unsigned int w, unsigned int h) {
  if (_camera_matrix.size() != cv::Size(3, 3))
    throw std::runtime_error("Camera matrix should have 3x3 size");
  if (_camera_matrix.type() != CV_32FC1 && _camera_matrix.type() != CV_64FC1)
    throw std::runtime_error("Camera matrix should be single- or double-precision floating point");

  cv::Mat_<float> m;
  _camera_matrix.getMat().convertTo(m, CV_32FC1);

  return {m(0, 0), m(1, 1), m(0, 2), m(1, 2), w, h};
}

Intrinsics Intrinsics::load(const std::string& filename) {
  Intrinsics intr;
  if (boost::algorithm::ends_with(filename, ".yaml") || boost::algorithm::ends_with(filename, ".yml") ||
      boost::algorithm::ends_with(filename, ".xml")) {
    // OpenCV storage format
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    cv::Mat m;
    for (const auto& name : {"cameraMatrix", "camera_matrix", "matrix"}) {
      if (!fs[name].empty()) {
        fs[name] >> m;
        break;
      }
    }
    if (m.empty())
      throw std::runtime_error("intrinsics file does not contain camera matrix");
    intr.fx = m.at<double>(0, 0);
    intr.fy = m.at<double>(1, 1);
    intr.cx = m.at<double>(0, 2);
    intr.cy = m.at<double>(1, 2);
    int width = 0, height = 0;
    if (!fs["image_width"].empty())
      fs["image_width"] >> width;
    if (!fs["image_height"].empty())
      fs["image_height"] >> height;
    if (!fs["resolution"].empty()) {
      std::cout << "got resolution" << std::endl;
      auto node = fs["resolution"].begin();
      (*node++)["width"] >> width;
      (*node)["height"] >> height;
    }
    if (width == 0 || height == 0)
      throw std::runtime_error("intrinsics file does not contain image width/height");
    else
      intr.w = width, intr.h = height;
    return intr;
  } else {
    // V4R or COLMAP format
    std::ifstream file(filename);
    if (file.is_open()) {
      std::string line;
      int camera_id;
      while (std::getline(file, line)) {
        if (line.size() == 0 || line[0] == '#')
          continue;
        if (sscanf(line.c_str(), "%f %f %f %f %d %d", &intr.fx, &intr.fy, &intr.cx, &intr.cy, &intr.w, &intr.h) == 6)
          return intr;
        if (sscanf(line.c_str(), "%d OPENCV %d %d %f %f %f %f", &camera_id, &intr.w, &intr.h, &intr.fx, &intr.fy,
                   &intr.cx, &intr.cy) == 7)
          return intr;
        throw std::runtime_error("invalid intrinsics file format");
      }
    }
  }
  throw std::runtime_error("unable to read intrinsics file");
  return intr;
}

Intrinsics Intrinsics::PrimeSense() {
  return {525.0f, 525.0f, 319.5f, 239.0f, 640, 480};
}

std::ostream& operator<<(std::ostream& os, const Intrinsics& obj) {
  os << obj.fx << ", " << obj.fy << "  " << obj.cx << ", " << obj.cy << " " << obj.w << "x" << obj.h;
  return os;
}
}  // namespace v4r


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MINUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_MINUS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( minus_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_sub_pd(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( minus_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                          , bs::pack_<bd::ints16_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_sub_epi16(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( minus_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints8_<A0>, bs::sse_>
                          , bs::pack_<bd::ints8_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_sub_epi8(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( minus_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          , bs::pack_<bd::ints32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_sub_epi32(a0,a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( minus_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::ints64_<A0>, bs::sse_>
                          , bs::pack_<bd::ints64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0
                                    , const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm_sub_epi64(a0,a1);
    }
  };
} } }

#include <boost/simd/arch/x86/sse2/simd/function/minus_s.hpp>

#endif


// IGNORE: refer to motor_controller.py

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>

// cstring is probably safer choice for serial data
#include <cstring>
// For serial communication
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
// C++ library for ports and hex handling
#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/asio/serial_port_base.hpp>
#include <boost/lexical_cast.hpp>
#include <iomanip>
#include <sstream>
// For info on serial comm for Roboteq AX3500, see pg. 138 in the manual (In drive or Google it)
// Contains node that publishes encoder node, as well as motor controlling node

// Device port is set to be accessed globally
boost::asio::io_service io;
boost::asio::serial_port port(io);

using namespace std;

// In the future, considering putting globals into structs and enum ...

const string COM_FOR_CH1 = "!A"; // Add two string nn for speed in Hex, with 7F being max
const string COM_BAC_CH1 = "!a";
const string COM_FOR_CH2 = "!B"; // As above
const string COM_BAC_CH2 = "!b";
const string QUE_SPEED = "?K";
const string QUE_BAT = "?E";
const string QUE_VOLT = "?V";

const int LEFT = 0;
const int RIGHT = 1;
const float wheel_diameter = 0.42;
const float wheel_circum = M_PI * wheel_diameter;
const float wheel_separation = 0.69;
const float max_rpm = 3000.0 / 26.0; // wheel_rpm = motor_rpm / gear_ratio
//Refer https://mycontraption.com/calculating-robot-speed-and-motor-torque/

const int max_effort = 127; // 7F in int; 00-7F is the range of effort for motor message

//  Max speed comes to be about 5.63 mph ... TODO: Check on motor_rpm

//! The node handle we'll be using
ros::NodeHandle n;

// Refer to pg. 160 for specs
// 0 is the left, 1 is right

inline void readPort(char * data, size_t size) {
	port.read_some(boost::asio::buffer(data, size));
}

inline void sendMessage(string msg) {
	port.write_some(boost::asio::buffer(msg.c_str(), msg.length()));
}

double getSpeed(int wheel_side) {
	sendMessage(QUE_SPEED); // QUE_SPEED is cstring

	char * buffer_data = new char[3];

	int effort = 0;

	try {
		readPort(buffer_data, 2);
		effort = boost::lexical_cast<int>(std::string(buffer_data));
	} catch(boost::bad_lexical_cast&) {
		ROS_ERROR("Bad hex handling!");
	}

	float speed = wheel_circum * max_rpm * (effort * 1.0 / max_effort);

	delete[] buffer_data;

	return speed;
}

// The direction is accounted by the sign of speed
// Will need to figure out the corresponding speed for the
void changeSpeed(int wheel_side, double speed) {
	// TODO: Find equation to change speed -> wheel effort
	// INFO: 26:1, effort: 00-7F, forward or backward

	int needed_effort = (int) ((speed * max_effort) / (wheel_circum * max_rpm));

	// Changing int to hex
	// Accomodate for negative int (reverse direction ...)

	string message("");
	string effort_hex_str("");

	if (needed_effort < 0) {
		if (wheel_side == LEFT) message = COM_BAC_CH1;
		else if (wheel_side == RIGHT) message = COM_BAC_CH2;
	}
	else {
		if (wheel_side == LEFT) message = COM_FOR_CH1;
		if (wheel_side == RIGHT) message = COM_FOR_CH2;
	}

	if (needed_effort < -127 || needed_effort > 127) {
		ROS_ERROR("Exceeded possible motor effort!");
		string effort_hex_str = "7F";
	}

	else {
		stringstream stream;
		stream << hex << needed_effort;
		effort_hex_str = stream.str();
	}

	sendMessage(message + effort_hex_str);
}

// Called whenever the mux_cmdvel is received
void velCallback(const geometry_msgs::Twist::ConstPtr& cmd_vel) {
	// TODO: Put asserts for min and max speed if roslaunch does not properly handle

	double linear_vel = cmd_vel->linear.x;
	double angular_vel = cmd_vel->angular.x;

	// TODO: Compensation if there is signficant motor speed differences

	double left_speed = linear_vel + angular_vel * wheel_separation / 2.0;
	double right_speed = linear_vel - angular_vel * wheel_separation / 2.0;

	changeSpeed(LEFT, left_speed);
	changeSpeed(RIGHT, right_speed);
}

int main(int argc, char** argv)
{
	string port_name;

	//init the ROS node
	ros::init(argc, argv, "motor_controller");
	ros::NodeHandle n;

	if (!n.getParam("port", port_name))
	{
		ROS_ERROR("Failed to get param port");
		exit(-1);
	}

	// Checks if we can open the port, else return error
	try {
		port.open(port_name.c_str());
	}
	catch(...) {
		ROS_ERROR("Could not open port");
		exit(-1);
	}

	// Refer to this link: http://www.cmrr.umn.edu/~strupp/serial.html#2_5_3
	// as well as the manual
	// The port info is baud rate 9600, 7 bit data, 1 start bit, 1 stop bit, even parity
	// Setting up port options
	port.set_option(boost::asio::serial_port_base::baud_rate(9600));
	port.set_option(boost::asio::serial_port_base::parity(
				boost::asio::serial_port_base::parity::even));
	port.set_option(boost::asio::serial_port_base::stop_bits(
				boost::asio::serial_port_base::stop_bits::one));

	//ros::Publisher encoder_pub = n.advertise<nav_msgs::Odometry>("motor_odom", 1000);

	// Odom and Transforms are handled at diff_ty.py
	ros::Publisher left_whl_pub = n.advertise<std_msgs::Float32>("lwheel", 1000);
	ros::Publisher right_whl_pub = n.advertise<std_msgs::Float32>("rwheel", 1000);

	ros::Subscriber cmd_vel_sub = n.subscribe("mux_cmdvel", 1000, velCallback);

	ros::Rate loop_rate(10);

	ros::Time current_time = ros::Time::now();
	ros::Time last_time = ros::Time::now();

	std_msgs::Float32 left_wheel_speed, right_wheel_speed;
	while (ros::ok()) {
		current_time = ros::Time::now();

		left_wheel_speed.data = getSpeed(LEFT);
		right_wheel_speed.data = getSpeed(RIGHT);

		left_whl_pub.publish(left_wheel_speed);
		right_whl_pub.publish(right_wheel_speed);

		ros::spinOnce();
		loop_rate.sleep();
	}

	port.close();

	return 0;
}


// SPDX-License-Identifier: Apache-2.0
// Copyright 2021 - 2021, the Anboto author and contributors
#include <Core/Core.h>
#include <Functions4U/Functions4U.h>
#include <Eigen/Eigen.h>
#include "Utility.h"

namespace Upp {

using namespace Eigen;

}


#include <boost/msm/mpl_graph/depth_first_search.hpp>