//
// Created by Hamza El-Kebir on 6/18/21.
//

#ifndef LODESTAR_BUTCHERTABLEAU_HPP
#define LODESTAR_BUTCHERTABLEAU_HPP

#include <type_traits>
#include <tuple>
#include <array>
#include <string>
#include <string_view>
#include <functional>
#include "Lodestar/aux/Indices.hpp"
#include "Lodestar/aux/Conjunction.hpp"
#include <iostream>
#include <Eigen/Dense>

namespace ls {
    namespace primitives {
        namespace detail {
            /**
             * @brief A Butcher tableau row.
             *
             * @details There exist two partial specializations for this type: one for a regular row, which includes
             * a node and several Runge-Kutta coefficients (the exact number being depending on \c (TStage + 1)); the
             * other is a weight row, which only includes an array of \c TStages for the weights.
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<typename TScalarType, size_t TStages, size_t TStage, bool TIsWeights>
            struct ButcherRowImpl {
            };

            /**
             * @brief Butcher tableau specialization for regular row.
             *
             * @sa ButcherRowImpl
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             */
            template<typename TScalarType, size_t TStages, size_t TStage>
            struct ButcherRowImpl<TScalarType, TStages, TStage, false> {
                static_assert(TStage >= 0, "Butcher tableau row number must be non-negative.");
                static_assert(TStage < (TStages - 1),
                              "Butcher tableau row number must be smaller than the number of stages minus one.");

                TScalarType node;
                std::array<TScalarType, TStage + 1> rkCoefficients;
            };

            /**
             * @brief Butcher tableau specialization for weight row.
             *
             * @sa ButcherRowImpl
             *
             * @tparam TScalarType Type of the scalar coefficients.
             * @tparam TStages Number of stages in the integration scheme.
             * @tparam TStage Index of the current stage.
             */
            template<typename TScalarType, size_t TStages, size_t TStage>
            struct ButcherRowImpl<TScalarType, TStages, TStage, true> {
                std::array<TScalarType, TStages> weights;
            };
        }

        /**
         * @brief Implements a compile-time structure for simple/extended Butcher tableaus.
         *
         * @details Explicit methods in the Runge-Kutta family of numerical integrators can be described by the
         * following coefficient:
         * <ul>
         *      <li> Runge-Kutta coefficients \c a_ij;
         *      <li> Weights \c b_i;
         *      <li> Nodes \c c_i.
         * </ul>
         *
         *
         * @tparam TStages Number of stages in the integration scheme.
         * @tparam TExtended If true, an extended Butcher tableau is given.
         * @tparam TScalarType Type of the scalar coefficients.
         */
        template<size_t TStages, bool TExtended = true, typename TScalarType = double>
        class ButcherTableau {
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");
        };

        template<size_t TStages, typename TScalarType>
        class ButcherTableau<TStages, false, TScalarType> {
        public:
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");

            static const size_t stages = TStages; //! Number of stages.
            using type = TScalarType; //! Coefficient type.

            /**
             * @brief Alias template for \c ButcherRow.
             *
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<size_t TStage, bool TIsWeights = (TStage >= (TStages - 1))>
            using ButcherRow = detail::ButcherRowImpl<TScalarType, TStages, TStage, TIsWeights>;

            /**
             * @brief Returns the Runge-Kutta coefficient for the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), TScalarType>::type
            inline getCoefficient()
            {
                return std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients);
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient getter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), TScalarType>::type
            inline getCoefficient()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");

                return TScalarType{};
            }

            /**
             * @brief Sets the Runge-Kutta coefficient at the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients) = coeff;
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient setter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");
            }

            /**
             * @brief Returns the node value for the given row number.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), TScalarType>::type
            inline getNode()
            {
                return std::get<TRow>(rows_).node;
            }

            /**
             * @brief Degenerate node getter.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), TScalarType>::type
            inline getNode()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");

                return TScalarType{};
            }

            /**
             * @brief Sets the node value for the given row number.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                std::get<TRow>(rows_).node = node;
            }

            /**
             * @brief Degenerate node setter.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @tparam TIdx Weight index.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx < TStages, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages - 1>(rows_).weights);
            }

            /**
             * @brief Degenerate weight getter.
             *
             * @tparam TIdx Weight index.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx >= TStages, TScalarType>::type
            inline getWeight()
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");

                return TScalarType{};
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @tparam TIdx Weight index.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx < TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages - 1>(rows_).weights) = weight;
            }

            /**
             * @brief Degenerate weight setter.
             *
             * @tparam TIdx Weight index.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx>
            typename std::enable_if<TIdx >= TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0>
            typename std::enable_if<TStage == 0, TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
//                TType kCurr;
//                std::cout << "f(t,y) in Butcher tableau: " << f(t, y) << std::endl;
//                kCurr = f(t, y);
                auto kCurr = f(t,y);

                return execute<TType, TStage + 1>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1>(f, y, t, h, vars..., kCurr);
//                return execute<TType, TStage + 1>(f, y, t, h, vars..., f(t + getNode<TStage - 1>() * h, y + h * sumCoefficients<TType, TStage>(vars...)));
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                yFinal += h * sumWeights<TType>(vars...);

                return yFinal;
            }

        protected:
            /**
             * Helper struct that will define a tuple of specialized Butcher rows.
             *
             * @sa Repeat
             * @sa Indices
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices An \c IndexSequence type.
             */
            template<int TTimes, typename TIndices = typename Indices<TTimes>::type>
            struct Rows;

            /**
             * @brief Partial specialization of the \c Rows struct, which defines a tuple of specialized Butcher rows.
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices Template pack of integers (from 0 to TTimes - 1 by default).
             */
            template<int TTimes, int... TIndices>
            struct Rows<TTimes, IndexSequence<TIndices...>> {
                using type = std::tuple<ButcherRow<TIndices>...>;
            };

            typename Rows<TStages>::type rows_; //! Rows of the Butcher tableau.

            /**
             * @brief Sums Runge-Kutta coefficients multiplied with results from previous stages.
             *
             * @details This function computes sum_(j=0)^i a_ij * k_i, where i is \c TStage.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<
                    (TStage > 0) && (TStage < TStages) &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumCoefficients(TArgs... vars)
            {
                return sumCoefficientsImpl<TType, TStage - 1>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumCoefficients.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, int... TIndices>
            TType
            inline sumCoefficientsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getCoefficient<TStage, (size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Sums weight coefficients multiplied with results from previous stages.
             *
             * @tparam TType State type.
             * @tparam TArgs Parameter pack of argument (previous stage result) types
             * @tparam TIndex TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<(Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumWeights(TArgs... vars)
            {
                return sumWeightsImpl<TType>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, typename... TArgs, int... TIndices>
            TType
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @details This particular function is the base case, which simply returns the value that it was given.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             *
             * @return Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                return var;
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @details This function checks convertibility of the argument type to \c TType.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<!std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                static_assert(std::is_convertible<TArg, TType>::value, "Summed values must be convertible.");
                return var;
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             *
             * @return Sum.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<std::is_convertible<TArg, TType>::value &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                return var + sum<TType>(vars...);
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<!(std::is_convertible<TArg, TType>::value &&
                                      (Conjunction<std::is_convertible<TArgs, TType>...>::value)), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                static_assert(std::is_convertible<TArg, TType>::value &&
                              (Conjunction<std::is_convertible<TArgs, TType>...>::value),
                              "Summed values must be convertible.");

                return var + sum<TType>(vars...);
            }
        };

// --------------- Extended Butcher tableau -----------------------

        /**
         * @brief Implements a compile-time structure for extended Butcher tableaus.
         *
         * @details Explicit methods in the Runge-Kutta family of numerical integrators can be described by the
         * following coefficient:
         * <ul>
         *      <li> Runge-Kutta coefficients \c a_ij;
         *      <li> Weights \c b_i;
         *      <li> Nodes \c c_i.
         * </ul>
         *
         *
         * @tparam TStages Number of stages in the integration scheme.
         * @tparam TScalarType Type of the scalar coefficients.
         */
        template<size_t TStages, typename TScalarType>
        class ButcherTableau<TStages, true, TScalarType> {
        public:
            static_assert(TStages > 1, "Butcher tableau must have more than one stage.");

            static const size_t stages = TStages; //! Number of stages.
            using type = TScalarType; //! Coefficient type.

            /**
             * @brief Alias template for \c ButcherRow.
             *
             * @tparam TStage Index of the current stage.
             * @tparam TIsWeights If true, the current row is a weight row.
             */
            template<size_t TStage, bool TIsWeights = (TStage >= (TStages - 1))>
            using ButcherRow = detail::ButcherRowImpl<TScalarType, TStages, TStage, TIsWeights>;

            /**
             * @brief Returns the Runge-Kutta coefficient for the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), TScalarType>::type
            inline getCoefficient()
            {
                return std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients);
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient getter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @return Runge-Kutta coefficient at (\c TRow, \c TCoeffIdx).
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), TScalarType>::type
            inline getCoefficient()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");

                return TScalarType{};
            }

            /**
             * @brief Sets the Runge-Kutta coefficient at the given row number and coefficient index.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<(TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1)), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                std::get<TCoeffIdx>(std::get<TRow>(rows_).rkCoefficients) = coeff;
            }

            /**
             * @brief Degenerate Runge-Kutta coefficient setter.
             *
             * @tparam TRow Row number (related to stage number).
             * @tparam TCoeffIdx Index of the Runge-Kutta coefficient.
             *
             * @param coeff Coefficient value to be set.
             */
            template<size_t TRow, size_t TCoeffIdx>
            typename std::enable_if<!((TRow < (TStages - 1)) && (TCoeffIdx < (TRow + 1))), void>::type
            inline setCoefficient(TScalarType coeff)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
                static_assert(TCoeffIdx < (TRow + 1), "Coefficient index must be less than or equal to the row index.");
            }

            /**
             * @brief Returns the node value for the given row number.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), TScalarType>::type
            inline getNode()
            {
                return std::get<TRow>(rows_).node;
            }

            /**
             * @brief Degenerate node getter.
             *
             * @tparam TRow Row number (related to stage number).
             *
             * @return Node value of \c TRow.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), TScalarType>::type
            inline getNode()
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");

                return TScalarType{};
            }

            /**
             * @brief Sets the node value for the given row number.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow < (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                std::get<TRow>(rows_).node = node;
            }

            /**
             * @brief Degenerate node setter.
             *
             * @tparam TRow  Row number (related to stage number).
             *
             * @param node Node value to be set.
             */
            template<size_t TRow>
            typename std::enable_if<TRow >= (TStages - 1), void>::type
            inline setNode(TScalarType node)
            {
                static_assert(TRow < (TStages - 1),
                              "Row index must be smaller than the number of stages minus one to access coefficients.");
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @details This function gets the higher order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && THigherOrder, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages - 1>(rows_).weights);
            }

            /**
             * @brief Returns the weight value at the given index.
             *
             * @details This function gets the lower order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && !THigherOrder, TScalarType>::type
            inline getWeight()
            {
                return std::get<TIdx>(std::get<TStages>(rows_).weights);
            }

            /**
             * @brief Degenerate weight getter.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are retrieved.
             *
             * @return Weight value at \c TIdx.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<TIdx >= TStages, TScalarType>::type
            inline getWeight()
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");

                return TScalarType{};
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @details This function sets the higher order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && THigherOrder, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages - 1>(rows_).weights) = weight;
            }

            /**
             * @brief Sets the weight value for the given index.
             *
             * @details This function sets the lower order weight.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<(TIdx < TStages) && !THigherOrder, void>::type
            inline setWeight(TScalarType weight)
            {
                std::get<TIdx>(std::get<TStages>(rows_).weights) = weight;
            }

            /**
             * @brief Degenerate weight setter.
             *
             * @tparam TIdx Weight index.
             * @tparam THigherOrder If true, the higher order weights are changed.
             *
             * @param weight Weight value to be set.
             */
            template<size_t TIdx, bool THigherOrder = true>
            typename std::enable_if<TIdx >= TStages, void>::type
            inline setWeight(TScalarType weight)
            {
                static_assert(TIdx < TStages, "Weight index must be smaller than the number of stages.");
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers and embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false>
            typename std::enable_if<(TStage == 0) && !TIsEmbedded, TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
                TType kCurr = f(t, y);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function is the entry point to run a single iteration of the Runge-Kutta scheme
             * encoded in the Butcher tableau.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false>
            typename std::enable_if<(TStage == 0) && TIsEmbedded, std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h)
            {
                TType kCurr = f(t, y);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) && !TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                    const TScalarType h,
                    TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, vars..., kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the regular stages of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage < TStages) && TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType kCurr{}, yCurr = y;

                yCurr += h * sumCoefficients<TType, TStage>(vars...);
                TScalarType tCurr = t + getNode<TStage - 1>() * h;

                kCurr = f(tCurr, yCurr);

                return execute<TType, TStage + 1, TIsEmbedded>(f, y, t, h, vars..., kCurr);
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Integrated state.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) && !TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                yFinal += h * sumWeights<TType>(vars...);

                return yFinal;
            }

            /**
             * @brief Executes the explicit Runge-Kutta scheme defined in the Butcher tableau.
             *
             * @details The integration scheme integrates y'(t) = f(t, y(t)) from \c t to \c (t + h), where \c y(t) = y.
             *
             * This particular function deals with the final stage of the integration scheme.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TIsEmbedded If true, the execution considers an embedded scheme.
             *
             * @param f Function to be integrated.
             * @param y Initial state.
             * @param t Initial time.
             * @param h Integration step.
             *
             * @return Pair of integrated state (with higher-order scheme) and error.
             */
            template<typename TType, size_t TStage = 0, bool TIsEmbedded = false, typename... TArgs>
            typename std::enable_if<(TStage > 0) && (TStage == TStages) && TIsEmbedded &&
                                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), std::pair<TType, TType>>::type
            inline execute(const std::function<TType(TScalarType, TType)> &f, const TType &y, const TScalarType t,
                           const TScalarType h,
                           TArgs... vars)
            {
                TType yFinal = y;
                TType summedWeightsHigher = sumWeights<TType, true>(vars...);
                TType summedWeightsLower = sumWeights<TType, false>(vars...);
                yFinal += h * summedWeightsHigher;

                std::pair<TType, TType> pair{yFinal, h * (summedWeightsHigher - summedWeightsLower)};

                return pair;
            }

        protected:
            /**
             * Helper struct that will define a tuple of specialized Butcher rows.
             *
             * @sa Repeat
             * @sa Indices
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices An \c IndexSequence type.
             */
            template<int TTimes, typename TIndices = typename Indices<TTimes>::type>
            struct Rows;

            /**
             * @brief Partial specialization of the \c Rows struct, which defines a tuple of specialized Butcher rows.
             *
             * @tparam TTimes Number of times the generator must be ran.
             * @tparam TIndices Template pack of integers (from 0 to TTimes - 1 by default).
             */
            template<int TTimes, int... TIndices>
            struct Rows<TTimes, IndexSequence<TIndices...>> {
                using type = std::tuple<ButcherRow<TIndices>...>;
            };

            typename Rows<TStages + 1>::type rows_; //! Rows of the Butcher tableau.

            /**
             * @brief Sums Runge-Kutta coefficients multiplied with results from previous stages.
             *
             * @details This function computes sum_(j=0)^i a_ij * k_i, where i is \c TStage.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<
                    (TStage > 0) && (TStage < TStages) &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumCoefficients(TArgs... vars)
            {
                return sumCoefficientsImpl<TType, TStage - 1>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumCoefficients.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam TStage Index of the current stage.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, size_t TStage = 0, typename... TArgs, int... TIndices>
            TType
            inline sumCoefficientsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getCoefficient<TStage, (size_t) TIndices>() * vars)...);
            }

            /**
             * @brief Sums weight coefficients multiplied with results from previous stages.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types
             * @tparam TIndex TIndex Typename of the \c Indices object.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder = true, typename... TArgs, typename TIndex = typename Indices<sizeof...(TArgs)>::type>
            typename std::enable_if<(Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sumWeights(TArgs... vars)
            {
                return sumWeightsImpl<TType, THigherOrder>(TIndex{}, vars...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder, typename... TArgs, int... TIndices>
            typename std::enable_if<THigherOrder, TType>::type
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices, true>() * vars)...);
            }

            /**
             * @brief Implementation of \c sumWeights.
             *
             * @note This separate implementation is required to extract the list of indices at compile time.
             *
             * @tparam TType State type.
             * @tparam THigherOrder If true, higher order weights are used.
             * @tparam TArgs Parameter pack of argument (previous stage result) types.
             * @tparam TIndices Parameter pack of indices.
             *
             * @param vars Variadic argument list of results of previous stages.
             *
             * @return Sum.
             */
            template<typename TType, bool THigherOrder, typename... TArgs, int... TIndices>
            typename std::enable_if<!THigherOrder, TType>::type
            inline sumWeightsImpl(IndexSequence<TIndices...>, TArgs... vars)
            {
                return sum<TType>((getWeight<(size_t) TIndices, false>() * vars)...);
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @details This particular function is the base case, which simply returns the value that it was given.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             *
             * @return Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                return var;
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @details This function checks convertibility of the argument type to \c TType.
             *
             * @tparam TType State type.
             * @tparam TArg Argument type.
             *
             * @param var Argument.
             */
            template<typename TType, typename TArg>
            typename std::enable_if<!std::is_convertible<TArg, TType>::value, TType>::type
            inline sum(TArg var)
            {
                static_assert(std::is_convertible<TArg, TType>::value, "Summed values must be convertible.");
                return var;
            }

            /**
             * @brief Helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             *
             * @return Sum.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<std::is_convertible<TArg, TType>::value &&
                    (Conjunction<std::is_convertible<TArgs, TType>...>::value), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                return var + sum<TType>(vars...);
            }

            /**
             * @brief Degenerate case of helper function that sums arguments.
             *
             * @tparam TType State type.
             * @tparam TArg First argument type.
             * @tparam TArgs Template pack of variadic argument list typenames.
             *
             * @param var First argument.
             * @param vars Variadic arguments.
             */
            template<typename TType, typename TArg, typename... TArgs>
            typename std::enable_if<!(std::is_convertible<TArg, TType>::value &&
                                      (Conjunction<std::is_convertible<TArgs, TType>...>::value)), TType>::type
            inline sum(TArg var, TArgs... vars)
            {
                static_assert(std::is_convertible<TArg, TType>::value &&
                              (Conjunction<std::is_convertible<TArgs, TType>...>::value),
                              "Summed values must be convertible.");

                return var + sum<TType>(vars...);
            }
        };
    }
}

#endif //LODESTAR_BUTCHERTABLEAU_HPP


#include "set_dual_objective_b.hxx"
#include "../../read_vector.hxx"

#include <El.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>

void read_objectives(const boost::filesystem::path &sdp_directory,
                     const El::Grid &grid, El::BigFloat &objective_const,
                     El::DistMatrix<El::BigFloat> &dual_objective_b)
{
  const boost::filesystem::path objectives_path(sdp_directory / "objectives");
  boost::filesystem::ifstream objectives_stream(objectives_path);
  if(!objectives_stream.good())
    {
      throw std::runtime_error("Could not open '" + objectives_path.string()
                               + "'");
    }
  objectives_stream >> objective_const;
  if(!objectives_stream.good())
    {
      throw std::runtime_error("Corrupted file: " + objectives_path.string());
    }

  std::vector<El::BigFloat> temp;
  read_vector(objectives_stream, temp);
  set_dual_objective_b(temp, grid, dual_objective_b);
}


/*
 * Twist.hpp
 *
 *  Created on: Apr 26, 2021
 *      Author: jelavice
 */

#pragma once
#include <Eigen/Dense>
#include "icp_localization/common/time.hpp"

#include <geometry_msgs/Twist.h>

namespace icp_loco {

template<typename FloatType>
class Twist3
{
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Vector = Eigen::Matrix<FloatType, 3, 1>;

  Twist3()
      : linear_(Vector::Zero()),
        angular_(Vector::Zero())
  {
  }
  Twist3(const Vector& lin, const Vector& ang)
      : linear_(lin),
        angular_(ang)
  {
  }

  template<typename OtherType>
  Twist3<OtherType> cast() const
  {
    return Twist3<OtherType>(linear_.template cast<OtherType>(), angular_.template cast<OtherType>());
  }

  const Vector& linear() const
  {
    return linear_;
  }
  const Vector& angular() const
  {
    return angular_;
  }

 private:
  Vector linear_;
  Vector angular_;
};

using Twist3d = Twist3<double>;
using Twist3f = Twist3<float>;

struct TimestampedTwist {
  Time time_;
  Twist3d twist_;
};

TimestampedTwist interpolate(const TimestampedTwist& start,
                                 const TimestampedTwist& end,
                                 const Time &time);

Twist3d fromRos(const geometry_msgs::Twist &msg);

}  // namespace icp_loco


/**
 *
 * Copyright (C) 2021 Mohammad Javad Dousti, Qing Xie, Mahdi Nazemi,
 * and Massoud Pedram. All rights reserved.
 *
 * Please refer to the LICENSE file for terms of use.
 *
 */

#include <algorithm>
#include <boost/functional/hash.hpp>
#include <tuple>
#include <unordered_set>
#include <utility>

#include "headers/general.hpp"
#include "headers/neighbor_components.hpp"
#include "headers/utils.hpp"

// Open intervals should be sorted and assessed before the close intervals.
enum class IntervalType { open = 0, close = 1 };

namespace neighbor_components {

std::unordered_set<subcomponent_pair, boost::hash<subcomponent_pair>>
getOverlaps(const std::vector<std::tuple<SubComponent *, VALUE, IntervalType>>
                &subcomponents) {

  std::unordered_set<subcomponent_pair, boost::hash<subcomponent_pair>>
      overlaps;
  std::unordered_set<SubComponent *> current_open;

  for (auto &tuple_component : subcomponents) {
    auto [c, val, type] = tuple_component;

    if (type == IntervalType::open) {
      for (auto overlap : current_open) {
        overlaps.insert(std::make_pair(min(c, overlap), max(c, overlap)));
      }
      current_open.insert(c);
    } else {
      current_open.erase(c);
    }
  }
  return overlaps;
}

bool compareTuples(std::tuple<SubComponent *, VALUE, IntervalType> a,
                   std::tuple<SubComponent *, VALUE, IntervalType> b) {
  VALUE a_val = std::get<1>(a);
  VALUE b_val = std::get<1>(b);
  // First place the open interval to capture the tangential
  // overlap.
  return utils::neq(a_val, b_val) ? a_val < b_val
                                  // first open, then close
                                  : std::get<2>(a) < std::get<2>(b);
}

std::vector<std::tuple<SubComponent *, VALUE, IntervalType>>
sortSubcomponents(std::vector<SubComponent *> subComponents,
                  VALUE func1(SubComponent *), VALUE func2(SubComponent *)) {
  std::vector<std::tuple<SubComponent *, VALUE, IntervalType>>
      subComponents_sorted;

  for (auto c : subComponents) {
    subComponents_sorted.push_back(
        std::tuple<SubComponent *, VALUE, IntervalType>(c, func1(c),
                                                        IntervalType::open));
    subComponents_sorted.push_back(
        std::tuple<SubComponent *, VALUE, IntervalType>(c, func1(c) + func2(c),
                                                        IntervalType::close));
  }
  std::sort(subComponents_sorted.begin(), subComponents_sorted.end(),
            compareTuples);
  return subComponents_sorted;
}

std::vector<subcomponent_pair> getNeighbors(Device *device) {
  auto subComponents = device->getSubComponents();
  auto subComponents_sorted_x = sortSubcomponents(
      subComponents, [](SubComponent *c) -> VALUE { return c->getX(); },
      [](SubComponent *c) -> VALUE { return c->getLength(); }

  );
  auto subComponents_sorted_y = sortSubcomponents(
      subComponents, [](SubComponent *c) -> VALUE { return c->getY(); },
      [](SubComponent *c) -> VALUE { return c->getWidth(); }

  );
  auto subComponents_sorted_z = sortSubcomponents(
      subComponents, [](SubComponent *c) -> VALUE { return c->getZ(); },
      [](SubComponent *c) -> VALUE { return c->getHeight(); }

  );

  auto overlaps_x = getOverlaps(subComponents_sorted_x);
  auto overlaps_y = getOverlaps(subComponents_sorted_y);
  auto overlaps_z = getOverlaps(subComponents_sorted_z);

  std::vector<std::pair<SubComponent *, SubComponent *>> common_overlaps;

  for (auto &c : overlaps_x) {
    if (overlaps_y.find(c) != overlaps_y.end() &&
        overlaps_z.find(c) != overlaps_z.end()) {
      common_overlaps.push_back(c);
    }
  }
  for (auto &c : overlaps_y) {
    if (overlaps_x.find(c) != overlaps_x.end() &&
        overlaps_z.find(c) != overlaps_z.end()) {
      common_overlaps.push_back(c);
    }
  }
  for (auto &c : overlaps_z) {
    if (overlaps_x.find(c) != overlaps_x.end() &&
        overlaps_y.find(c) != overlaps_y.end()) {
      common_overlaps.push_back(c);
    }
  }
  return common_overlaps;
}

} // namespace neighbor_components


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/type_list.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(find(list(1.0, 2, '3'), trait_<std::is_integral>) == just(2));
    BOOST_HANA_CONSTANT_ASSERT(find(list(1.0, 2, '3'), trait_<std::is_class>) == nothing);

    constexpr auto types = type_list<char, int, unsigned, long, unsigned long>;
    BOOST_HANA_CONSTANT_ASSERT(find(types, _ == type<unsigned>) == just(type<unsigned>));
    BOOST_HANA_CONSTANT_ASSERT(find(types, _ == type<void>) == nothing);
    //! [main]
}


/* Copyright (C) 2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <iostream>
#include <fstream>
#include <cstdio>
#include <cstdlib> // ldiv, system

#include <helib/helib.h>
#include <helib/ArgMap.h>

#include "Writer.h"
#include "common.h"

#include <NTL/BasicThreadPool.h>

struct CmdLineOpts
{
  std::string dataFilePath;
  std::string pkFilePath;
  std::string outFilePath;
  long batchSize = 0;
  long nthreads = 0; // Default is 0 for number of cpus.
  long offset = 0;
};

std::pair<long, long> parseDimsHeader(const std::string& s)
{
  std::stringstream iss(s);
  std::istream_iterator<long> issit(iss);
  std::vector<long> vl(issit, {});

  switch (vl.size()) {
  case 1:
    return {vl[0], 1};
  case 2:
    return {vl[0], vl[1]};
  default:
    std::ostringstream oss;
    oss << "Dimensions in header is wrong.\n";
    for (const auto& l : vl)
      oss << l << " ";
    throw std::runtime_error(oss.str());
  }
}

template <typename SCHEME>
void encryptFromTo(const CmdLineOpts& cmdLineOpts,
                   const helib::Context& context,
                   const helib::PubKey& pk)
{

  // Read in data file header
  std::fstream dataFile(cmdLineOpts.dataFilePath);
  if (!dataFile.is_open()) {
    throw std::runtime_error("Could not open file '" +
                             cmdLineOpts.dataFilePath + "'.");
  }

  std::pair<long, long> dims = parseDimsHeader(readline(dataFile));

  // Here we 'batch'. Read in the batch size into memory.
  // Then, process with n threads. Repeat.
  // Write the header to file
  Writer<helib::Ctxt> writer(cmdLineOpts.outFilePath,
                             dims.first,
                             dims.second,
                             estimateCtxtSize(context, cmdLineOpts.offset));

  // Setting the stage
  std::vector<helib::Ptxt<SCHEME>> ptxts;
  std::vector<helib::Ctxt> ctxts;
  helib::Ctxt zero_ctxt(pk);
  helib::Ptxt<SCHEME> zero_ptxt(context);

  // Read in a batch
  for (long remaining = dims.first * dims.second, writtenBatches = 0;
       remaining > 0;
       remaining -= cmdLineOpts.batchSize, ++writtenBatches) {

    // Local batchSize
    long bsz =
        (remaining > cmdLineOpts.batchSize) ? cmdLineOpts.batchSize : remaining;
    ptxts.resize(bsz, zero_ptxt);
    ctxts.resize(bsz, zero_ctxt);

    std::vector<std::string> ptxt_strings(ptxts.size());
    for (std::size_t j = 0; j < ptxts.size(); j++) {
      std::getline(dataFile, ptxt_strings[j], '\n');
    }

    // Spread across n threads
    NTL_EXEC_RANGE(ctxts.size(), first, last)
    Writer<helib::Ctxt> threadWriter(writer);
    for (long i = first; i < last; ++i) {
      std::istringstream istr(ptxt_strings[i]);
      istr >> ptxts[i];
      pk.Encrypt(ctxts[i], ptxts[i]);
    }

    // Write to file
    for (long i = first; i < last; ++i) {
      if (dims.second == 1) {
        threadWriter.writeByLocation(ctxts[i],
                                     writtenBatches * cmdLineOpts.batchSize + i,
                                     0);
      } else {
        ldiv_t qr =
            ldiv(writtenBatches * cmdLineOpts.batchSize + i, dims.second);
        threadWriter.writeByLocation(ctxts[i], qr.quot, qr.rem);
      }
    }
    NTL_EXEC_RANGE_END
  }
}

int main(int argc, char* argv[])
{
  CmdLineOpts cmdLineOpts;

  // clang-format off
  helib::ArgMap()
    .required()
    .positional()
      .arg("<pk-file>", cmdLineOpts.pkFilePath,
           "the public key file.", nullptr)
      .arg("<input-file>", cmdLineOpts.dataFilePath,
           "the input file.", nullptr)
    .separator(helib::ArgMap::Separator::WHITESPACE)
    .named()
    .optional()
      .arg("-o", cmdLineOpts.outFilePath,
           "the output file name.", nullptr)
      .arg("-b", cmdLineOpts.batchSize,
           "batch size, how many ctxts in memory. If not set or 0 defaults to the number of threads used.")
      .arg("-n", cmdLineOpts.nthreads,
           "number of threads to use. If not set or 0 defaults to the number of concurrent threads supported.", "num. of cores")
      .arg("--offset", cmdLineOpts.offset,
           "byte packing offset in output file.")
    .parse(argc, argv);
  // clang-format on

  // Set NTL nthreads
  if (cmdLineOpts.nthreads == 0) {
    cmdLineOpts.nthreads = std::thread::hardware_concurrency();
    // hardware_concurrency may still return 0 if not supported on
    // implementation.
    if (cmdLineOpts.nthreads == 0) {
      std::cerr << "C++ `hardware_concurrency` call not available on this"
                   "platform.\nnthreads must be explicitly provided."
                << std::endl;
      return EXIT_FAILURE;
    }
  }

  if (cmdLineOpts.nthreads > 0) {
    NTL::SetNumThreads(cmdLineOpts.nthreads);
  } else {
    std::cerr << "Number of threads must be a positive integer." << std::endl;
    return EXIT_FAILURE;
  }

  // Set default batch size.
  if (cmdLineOpts.batchSize == 0) {
    cmdLineOpts.batchSize = cmdLineOpts.nthreads;
  }

  // Check batch size above 1.
  if (cmdLineOpts.batchSize < 1) {
    std::cerr << "Batch size must be a positive integer." << std::endl;
    return EXIT_FAILURE;
  }

  // Warn if batch size is less than thread count
  if (cmdLineOpts.batchSize < cmdLineOpts.nthreads) {
    std::cerr << "WARNING: Not enough elements in the batch ("
              << cmdLineOpts.batchSize << ") to run with "
              << cmdLineOpts.nthreads << " threads." << std::endl;
    std::cerr << "To achieve better performance set batch size (-b) to be "
                 "equal to or more than the number of threads (-n). Ideally "
                 "the batch size should be a multiple of the number of threads."
              << std::endl;
  }

  // Set default outFilePath
  if (cmdLineOpts.outFilePath.empty()) {
    cmdLineOpts.outFilePath =
        stripExtension(cmdLineOpts.dataFilePath) + ".ctxt";
  }

  // Load Context and PubKey
  std::unique_ptr<helib::Context> contextp;
  std::unique_ptr<helib::PubKey> pkp;

  std::tie(contextp, pkp) =
      loadContextAndKey<helib::PubKey>(cmdLineOpts.pkFilePath);

  try {
    // Read in, encrypt, output.
    if (contextp->getP() == -1) { // CKKS
      encryptFromTo<helib::CKKS>(cmdLineOpts, *contextp, *pkp);
    } else if (contextp->getP() > 0) { // BGV
      encryptFromTo<helib::BGV>(cmdLineOpts, *contextp, *pkp);
    } else {
      std::cerr << "Unrecognized scheme from context." << std::endl;
      return EXIT_FAILURE;
    }
  } catch (const std::invalid_argument& e) {
    std::cerr << "Exit due to invalid argument thrown:\n"
              << e.what() << std::endl;
    std::remove(cmdLineOpts.outFilePath.c_str());
    return EXIT_FAILURE;
  } catch (const helib::IOError& e) {
    std::cerr << "Exit due to IOError thrown:\n" << e.what() << std::endl;
    std::remove(cmdLineOpts.outFilePath.c_str());
    return EXIT_FAILURE;
  } catch (const std::runtime_error& e) {
    std::cerr << "Exit due to runtime error thrown:\n" << e.what() << std::endl;
    std::remove(cmdLineOpts.outFilePath.c_str());
    return EXIT_FAILURE;
  } catch (const std::logic_error& e) {
    std::cerr << "Exit due to logic error thrown:\n" << e.what() << std::endl;
    std::remove(cmdLineOpts.outFilePath.c_str());
    return EXIT_FAILURE;
  } catch (const std::exception& e) {
    std::cerr << "Exit due to unknown exception thrown:\n"
              << e.what() << std::endl;
    std::remove(cmdLineOpts.outFilePath.c_str());
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/range.hpp>
using namespace boost::hana;

//! [main]
BOOST_HANA_CONSTANT_ASSERT(head(range_c<int, 0, 5>) == int_<0>);
BOOST_HANA_CONSTANT_ASSERT(last(range_c<unsigned long, 0, 5>) == ulong<4>);
BOOST_HANA_CONSTANT_ASSERT(tail(range_c<int, 0, 5>) == range(int_<1>, int_<5>));
//! [main]

int main() { }


#include <functional>
#include <iostream>
#include <map>
#include <utility>

#include <boost/program_options.hpp>

#include "LinearProgram.h"
#include "Pivot.h"

namespace po = boost::program_options;

std::map<std::string, std::function<PivotFunctionReturnType(Tableau &tableau)>> pivotFunctions;
std::string allowedPivotFunctions;

/*
Take the pivot function callables from Pivot.h (or elsewhere), and register them with the command-line options parser
*/
void registerPivotFunctions()
{
	pivotFunctions = decltype(pivotFunctions){
		{"bland", Bland()},
		{"random", Random()},
		{"maxincrease", MaxIncrease()},
		{"maxcoef", MaxCoef()}};
	allowedPivotFunctions.clear();
	for (auto &it : pivotFunctions)
	{
		allowedPivotFunctions += it.first;
		allowedPivotFunctions += ',';
	}
	allowedPivotFunctions.pop_back();
}

/*
Parse the command-line options, and return them as a tuple.
*/
auto parseOptions(int argc, char **argv)
{
	po::options_description optionsDescription("Allowed options");
	std::string pivotHelpText = "the pivot rule that is used. Can be one of {" + allowedPivotFunctions + "}";
	optionsDescription.add_options()("verbose", po::value<bool>()->implicit_value(true)->default_value(false), "verbose output")("pivot", po::value<std::string>()->default_value("maxincrease"), pivotHelpText.c_str())("input", po::value<std::string>(), "input linear program");
	po::positional_options_description positionalOptionsDescription;
	positionalOptionsDescription.add("input", -1);

	po::variables_map variablesMap;
	po::store(
		po::command_line_parser(argc, argv)
			.options(optionsDescription)
			.positional(positionalOptionsDescription)
			.run(),
		variablesMap);
	po::notify(variablesMap);

	std::string inputPath;
	if (variablesMap.count("input"))
	{
		inputPath = variablesMap["input"].as<std::string>();
	}
	else
	{
		std::cout << "You must provide a valid path\n"
				  << optionsDescription;
		std::exit(1);
	}

	auto verboseOutput = variablesMap["verbose"].as<bool>();
	auto pivotAlgorithm = variablesMap["pivot"].as<std::string>();

	if (pivotFunctions.find(pivotAlgorithm) == pivotFunctions.end())
	{
		std::cout << "Invalid pivot algorithm \"" << pivotAlgorithm << "\"\n";
		std::cout << "Allowed values are {" << allowedPivotFunctions << "}\n";
		std::exit(1);
	}

	return std::make_tuple(inputPath, verboseOutput, pivotAlgorithm);
}

int main(int argc, char **argv)
{
	registerPivotFunctions();

	std::string inputPath, pivotAlgorithm;
	bool verboseOutput;

	std::tie(inputPath, verboseOutput, pivotAlgorithm) = parseOptions(argc, argv);

	LinearProgram lp(inputPath, verboseOutput);
	lp.printFancyStatement();

	auto result = lp.solve(pivotFunctions[pivotAlgorithm]);

	switch (result)
	{
	case LinearProgram::Result::INFEASIBLE:
		std::cout << "The linear program is infeasible\n";
		break;
	case LinearProgram::Result::FEASIBLE_UNBOUNDED:
		std::cout << "The linear program is unbounded\n";
		break;
	case LinearProgram::Result::FEASIBLE_BOUNDED:
		std::cout << "An optimal solution is: ";
		lp.printFancySolution();
		std::cout << "\nThe value of the objective function is: " << lp.tableau.value();
		std::cout << "\nThe number of pivots is: " << lp.numPivots;
		std::cout << "\nThe pivot rule used: " << pivotAlgorithm;
	}
	return 0;
}


#include "common.h"

#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

size_t randomPick(size_t n)
{
    // add the random seed,
    struct timeval time;
    gettimeofday(&time, NULL);
    std::default_random_engine generator(time.tv_usec);
    std::uniform_int_distribution<size_t> distribution(0, n - 1);

    return distribution(generator);
}

std::mt19937 create_random_engine()
{
    std::random_device random_device;
    std::vector<std::uint_least32_t> v(10);
    std::generate(v.begin(), v.end(), std::ref(random_device));
    std::seed_seq seed(v.begin(), v.end());
    return std::mt19937(seed);
}

unsigned int computeIterationsNumberAdaptively(float inlierProbability, int sample_num)
{
    if (inlierProbability < 0.01)
    {
        return (unsigned int)1000000;
    }
    // const float stable_p = 0.99;
    const float unstable_p = 1 - 0.99;
    const float unstable_p_log = std::log10(unstable_p);
    unsigned int N = 0;
    int s = sample_num;

    float outlier_pro = 1 - std::pow(inlierProbability, s);
    N = unstable_p_log / std::log10(outlier_pro);
    return N;
}

std::vector<Eigen::Vector3d> resampleByIndices(const std::vector<unsigned int> &sample_indices,
                                               const std::vector<Eigen::Vector3d> &data_set)
{
    if (sample_indices.empty() || data_set.empty())
    {
        ERROR_STREAM("[resampleByIndices] Empty Input Parameters!");
        return std::vector<Eigen::Vector3d>();
    }
    if (sample_indices.size() > data_set.size())
    {
        ERROR_STREAM("[resampleByIndice] Sample size is greater than dataset!");
        return std::vector<Eigen::Vector3d>();
    }

    std::vector<Eigen::Vector3d> sampled_data;
    sampled_data.reserve(data_set.size());

    for (size_t i = 0; i < sample_indices.size(); ++i)
    {
        auto indice = sample_indices[i];
        if (indice < data_set.size())
        {
            sampled_data.emplace_back(data_set[indice]);
        }
        else
        {
            ERROR_STREAM("[resampleByIndices] Invalid indice!");
            continue;
        }
    }

    if (sampled_data.empty())
    {
        ERROR_STREAM("[resampledByIndice] Empty output!");
    }
    return sampled_data;
}

std::vector<cv::Point2d> resampleByIndices(const std::vector<unsigned int> &sample_indices,
                                           const std::vector<cv::Point2d> &data_set)
{
    if (sample_indices.empty() || data_set.empty())
    {
        ERROR_STREAM("[resampleByIndices] Empty Input Parameters!");
        return std::vector<cv::Point2d>();
    }
    if (sample_indices.size() > data_set.size())
    {
        ERROR_STREAM("[resampleByIndice] Sample size is greater than dataset!");
        return std::vector<cv::Point2d>();
    }

    std::vector<cv::Point2d> sampled_data;
    sampled_data.reserve(data_set.size());
    for (size_t i = 0; i < sample_indices.size(); ++i)
    {
        auto indice = sample_indices[i];
        if (indice < data_set.size())
        {
            sampled_data.emplace_back(data_set[indice]);
        }
        else
        {
            ERROR_STREAM("[resampleByIndices] Invalid indice!");
            continue;
        }
    }

    if (sampled_data.empty())
    {
        ERROR_STREAM("[resampledByIndice] Empty output!");
    }
    return sampled_data;
}

template <typename T>
std::vector<T> create_random_array(const size_t size, const T rand_min, const T rand_max)
{
    assert(rand_min <= rand_max);
    assert(size <= static_cast<size_t>(rand_max - rand_min + 1));

    auto random_engine = create_random_engine();
    std::uniform_int_distribution<T> uniform_int_distribution(rand_min, rand_max);

    // 制作比size稍微大一点的随机数列(有重复)
    const auto make_size = static_cast<size_t>(size * 1.2);

    // 重复v到size为止
    std::vector<T> v;
    v.reserve(size);
    while (v.size() != size)
    {
        // 依次添加随机整数列(可能有重复)
        while (v.size() < make_size)
        {
            v.push_back(uniform_int_distribution(random_engine));
        }

        // 排序去除重复->去除重复的数列末尾的数组进入unique_end
        std::sort(v.begin(), v.end());
        auto unique_end = std::unique(v.begin(), v.end());

        // 如果v的尺寸太大，unique_end就换成size的长度
        if (size < static_cast<size_t>(std::distance(v.begin(), unique_end)))
        {
            unique_end = std::next(v.begin(), size);
        }

        // 删除重复部分
        v.erase(unique_end, v.end());
    }

    // 因为是升序，所以洗牌
    std::shuffle(v.begin(), v.end(), random_engine);

    return v;
}

template std::vector<int> create_random_array(size_t, int, int);
template std::vector<unsigned int> create_random_array(size_t, unsigned int, unsigned int);