/* boost random/discrete_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2009-2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: discrete_distribution.hpp 80996 2012-10-16 03:11:52Z marshall $
 */

#ifndef BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_DISCRETE_DISTRIBUTION_HPP_INCLUDED

#include <vector>
#include <limits>
#include <numeric>
#include <utility>
#include <iterator>
#include <boost/assert.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/detail/vector_io.hpp>

#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

/**
 * The class @c discrete_distribution models a \random_distribution.
 * It produces integers in the range [0, n) with the probability
 * of producing each value is specified by the parameters of the
 * distribution.
 */
template<class IntType = int, class WeightType = double>
class discrete_distribution {
public:
    typedef WeightType input_type;
    typedef IntType result_type;

    class param_type {
    public:

        typedef discrete_distribution distribution_type;

        /**
         * Constructs a @c param_type object, representing a distribution
         * with \f$p(0) = 1\f$ and \f$p(k|k>0) = 0\f$.
         */
        param_type() : _probabilities(1, static_cast<WeightType>(1)) {}
        /**
         * If @c first == @c last, equivalent to the default constructor.
         * Otherwise, the values of the range represent weights for the
         * possible values of the distribution.
         */
        template<class Iter>
        param_type(Iter first, Iter last) : _probabilities(first, last)
        {
            normalize();
        }
#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
        /**
         * If wl.size() == 0, equivalent to the default constructor.
         * Otherwise, the values of the @c initializer_list represent
         * weights for the possible values of the distribution.
         */
        param_type(const std::initializer_list<WeightType>& wl)
          : _probabilities(wl)
        {
            normalize();
        }
#endif
        /**
         * If the range is empty, equivalent to the default constructor.
         * Otherwise, the elements of the range represent
         * weights for the possible values of the distribution.
         */
        template<class Range>
        explicit param_type(const Range& range)
          : _probabilities(boost::begin(range), boost::end(range))
        {
            normalize();
        }

        /**
         * If nw is zero, equivalent to the default constructor.
         * Otherwise, the range of the distribution is [0, nw),
         * and the weights are found by  calling fw with values
         * evenly distributed between \f$\mbox{xmin} + \delta/2\f$ and
         * \f$\mbox{xmax} - \delta/2\f$, where
         * \f$\delta = (\mbox{xmax} - \mbox{xmin})/\mbox{nw}\f$.
         */
        template<class Func>
        param_type(std::size_t nw, double xmin, double xmax, Func fw)
        {
            std::size_t n = (nw == 0) ? 1 : nw;
            double delta = (xmax - xmin) / n;
            BOOST_ASSERT(delta > 0);
            for(std::size_t k = 0; k < n; ++k) {
                _probabilities.push_back(fw(xmin + k*delta + delta/2));
            }
            normalize();
        }

        /**
         * Returns a vector containing the probabilities of each possible
         * value of the distribution.
         */
        std::vector<WeightType> probabilities() const
        {
            return _probabilities;
        }

        /** Writes the parameters to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            detail::print_vector(os, parm._probabilities);
            return os;
        }

        /** Reads the parameters from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            std::vector<WeightType> temp;
            detail::read_vector(is, temp);
            if(is) {
                parm._probabilities.swap(temp);
            }
            return is;
        }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        {
            return lhs._probabilities == rhs._probabilities;
        }
        /** Returns true if the two sets of parameters are different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)
    private:
        /// @cond show_private
        friend class discrete_distribution;
        explicit param_type(const discrete_distribution& dist)
          : _probabilities(dist.probabilities())
        {}
        void normalize()
        {
            WeightType sum =
                std::accumulate(_probabilities.begin(), _probabilities.end(),
                                static_cast<WeightType>(0));
            for(typename std::vector<WeightType>::iterator
                    iter = _probabilities.begin(),
                    end = _probabilities.end();
                    iter != end; ++iter)
            {
                *iter /= sum;
            }
        }
        std::vector<WeightType> _probabilities;
        /// @endcond
    };

    /**
     * Creates a new @c discrete_distribution object that has
     * \f$p(0) = 1\f$ and \f$p(i|i>0) = 0\f$.
     */
    discrete_distribution()
    {
        _alias_table.push_back(std::make_pair(static_cast<WeightType>(1),
                                              static_cast<IntType>(0)));
    }
    /**
     * Constructs a discrete_distribution from an iterator range.
     * If @c first == @c last, equivalent to the default constructor.
     * Otherwise, the values of the range represent weights for the
     * possible values of the distribution.
     */
    template<class Iter>
    discrete_distribution(Iter first, Iter last)
    {
        init(first, last);
    }
#ifndef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    /**
     * Constructs a @c discrete_distribution from a @c std::initializer_list.
     * If the @c initializer_list is empty, equivalent to the default
     * constructor.  Otherwise, the values of the @c initializer_list
     * represent weights for the possible values of the distribution.
     * For example, given the distribution
     *
     * @code
     * discrete_distribution<> dist{1, 4, 5};
     * @endcode
     *
     * The probability of a 0 is 1/10, the probability of a 1 is 2/5,
     * the probability of a 2 is 1/2, and no other values are possible.
     */
    discrete_distribution(std::initializer_list<WeightType> wl)
    {
        init(wl.begin(), wl.end());
    }
#endif
    /**
     * Constructs a discrete_distribution from a Boost.Range range.
     * If the range is empty, equivalent to the default constructor.
     * Otherwise, the values of the range represent weights for the
     * possible values of the distribution.
     */
    template<class Range>
    explicit discrete_distribution(const Range& range)
    {
        init(boost::begin(range), boost::end(range));
    }
    /**
     * Constructs a discrete_distribution that approximates a function.
     * If nw is zero, equivalent to the default constructor.
     * Otherwise, the range of the distribution is [0, nw),
     * and the weights are found by  calling fw with values
     * evenly distributed between \f$\mbox{xmin} + \delta/2\f$ and
     * \f$\mbox{xmax} - \delta/2\f$, where
     * \f$\delta = (\mbox{xmax} - \mbox{xmin})/\mbox{nw}\f$.
     */
    template<class Func>
    discrete_distribution(std::size_t nw, double xmin, double xmax, Func fw)
    {
        std::size_t n = (nw == 0) ? 1 : nw;
        double delta = (xmax - xmin) / n;
        BOOST_ASSERT(delta > 0);
        std::vector<WeightType> weights;
        for(std::size_t k = 0; k < n; ++k) {
            weights.push_back(fw(xmin + k*delta + delta/2));
        }
        init(weights.begin(), weights.end());
    }
    /**
     * Constructs a discrete_distribution from its parameters.
     */
    explicit discrete_distribution(const param_type& parm)
    {
        param(parm);
    }

    /**
     * Returns a value distributed according to the parameters of the
     * discrete_distribution.
     */
    template<class URNG>
    IntType operator()(URNG& urng) const
    {
        BOOST_ASSERT(!_alias_table.empty());
        WeightType test = uniform_01<WeightType>()(urng);
        IntType result = uniform_int<IntType>((min)(), (max)())(urng);
        if(test < _alias_table[result].first) {
            return result;
        } else {
            return(_alias_table[result].second);
        }
    }

    /**
     * Returns a value distributed according to the parameters
     * specified by param.
     */
    template<class URNG>
    IntType operator()(URNG& urng, const param_type& parm) const
    {
        while(true) {
            WeightType val = uniform_01<WeightType>()(urng);
            WeightType sum = 0;
            std::size_t result = 0;
            for(typename std::vector<WeightType>::const_iterator
                iter = parm._probabilities.begin(),
                end = parm._probabilities.end();
                iter != end; ++iter, ++result)
            {
                sum += *iter;
                if(sum > val) {
                    return result;
                }
            }
        }
    }

    /** Returns the smallest value that the distribution can produce. */
    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
    /** Returns the largest value that the distribution can produce. */
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return static_cast<result_type>(_alias_table.size() - 1); }

    /**
     * Returns a vector containing the probabilities of each
     * value of the distribution.  For example, given
     *
     * @code
     * discrete_distribution<> dist = { 1, 4, 5 };
     * std::vector<double> p = dist.param();
     * @endcode
     *
     * the vector, p will contain {0.1, 0.4, 0.5}.
     */
    std::vector<WeightType> probabilities() const
    {
        std::vector<WeightType> result(_alias_table.size());
        const WeightType mean =
            static_cast<WeightType>(1) / _alias_table.size();
        std::size_t i = 0;
        for(typename alias_table_t::const_iterator
                iter = _alias_table.begin(),
                end = _alias_table.end();
                iter != end; ++iter, ++i)
        {
            WeightType val = iter->first * mean;
            result[i] += val;
            result[iter->second] += mean - val;
        }
        return(result);
    }

    /** Returns the parameters of the distribution. */
    param_type param() const
    {
        return param_type(*this);
    }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        init(parm._probabilities.begin(), parm._probabilities.end());
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() {}

    /** Writes a distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, discrete_distribution, dd)
    {
        os << dd.param();
        return os;
    }

    /** Reads a distribution from a @c std::istream */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, discrete_distribution, dd)
    {
        param_type parm;
        if(is >> parm) {
            dd.param(parm);
        }
        return is;
    }

    /**
     * Returns true if the two distributions will return the
     * same sequence of values, when passed equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(discrete_distribution, lhs, rhs)
    {
        return lhs._alias_table == rhs._alias_table;
    }
    /**
     * Returns true if the two distributions may return different
     * sequences of values, when passed equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(discrete_distribution)

private:

    /// @cond show_private

    template<class Iter>
    void init(Iter first, Iter last, std::input_iterator_tag)
    {
        std::vector<WeightType> temp(first, last);
        init(temp.begin(), temp.end());
    }
    template<class Iter>
    void init(Iter first, Iter last, std::forward_iterator_tag)
    {
        std::vector<std::pair<WeightType, IntType> > below_average;
        std::vector<std::pair<WeightType, IntType> > above_average;
        std::size_t size = std::distance(first, last);
        WeightType weight_sum =
            std::accumulate(first, last, static_cast<WeightType>(0));
        WeightType weight_average = weight_sum / size;
        std::size_t i = 0;
        for(; first != last; ++first, ++i) {
            WeightType val = *first / weight_average;
            std::pair<WeightType, IntType> elem(val, static_cast<IntType>(i));
            if(val < static_cast<WeightType>(1)) {
                below_average.push_back(elem);
            } else {
                above_average.push_back(elem);
            }
        }

        _alias_table.resize(size);
        typename alias_table_t::iterator
            b_iter = below_average.begin(),
            b_end = below_average.end(),
            a_iter = above_average.begin(),
            a_end = above_average.end()
            ;
        while(b_iter != b_end && a_iter != a_end) {
            _alias_table[b_iter->second] =
                std::make_pair(b_iter->first, a_iter->second);
            a_iter->first -= (static_cast<WeightType>(1) - b_iter->first);
            if(a_iter->first < static_cast<WeightType>(1)) {
                *b_iter = *a_iter++;
            } else {
                ++b_iter;
            }
        }
        for(; b_iter != b_end; ++b_iter) {
            _alias_table[b_iter->second].first = static_cast<WeightType>(1);
        }
        for(; a_iter != a_end; ++a_iter) {
            _alias_table[a_iter->second].first = static_cast<WeightType>(1);
        }
    }
    template<class Iter>
    void init(Iter first, Iter last)
    {
        if(first == last) {
            _alias_table.clear();
            _alias_table.push_back(std::make_pair(static_cast<WeightType>(1),
                                                  static_cast<IntType>(0)));
        } else {
            typename std::iterator_traits<Iter>::iterator_category category;
            init(first, last, category);
        }
    }
    typedef std::vector<std::pair<WeightType, IntType> > alias_table_t;
    alias_table_t _alias_table;
    /// @endcond
};

}
}

#include <boost/random/detail/enable_warnings.hpp>

#endif


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__OPTIM_HPP_
#define SMOOTH__OPTIM_HPP_

/**
 * @file
 * @brief Non-linear least squares optimization on Manifolds.
 */

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <iostream>
#include <numeric>

#include "concepts.hpp"
#include "diff.hpp"
#include "internal/lmpar.hpp"
#include "internal/lmpar_sparse.hpp"
#include "internal/utils.hpp"

namespace smooth {

/**
 * @brief Optimization options.
 */
struct MinimizeOptions
{
  /// relative parameter tolerance for convergence
  double ptol{1e-6};
  /// relative function tolerance for convergence
  double ftol{1e-6};
  /// maximum number of iterations
  std::size_t max_iter{1000};
  /// solver verbosity level
  int verbosity{0};
};

/**
 * @brief Find a minimum of the non-linear least-squares problem
 *
 * \f[
 *  \min_{x} \sum_i \| f(x)_i \|^2
 * \f]
 *
 * @tparam Diff differentiation method to use in solver (see diff::Type in diff.hpp)
 * @param f residuals to minimize
 * @param x reference tuple of arguments to f
 * @param opts solver options
 *
 * All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<diff::Type Diff, typename _F, typename _Wrt>
void minimize(_F && f, _Wrt && x, const MinimizeOptions & opts = MinimizeOptions{})
{
  // evaluate residuals and jacobian at initial point
  auto [r, J] = diff::dr<Diff>(f, x);

  // extract some properties from jacobian
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<decltype(J)>, decltype(J)>;
  static constexpr int Nx = decltype(J)::ColsAtCompileTime;
  const int nx            = J.cols();

  // scaling parameters
  Eigen::Matrix<double, Nx, 1> d(nx);
  if constexpr (is_sparse) {
    d = (Eigen::Matrix<double, 1, -1>::Ones(J.rows()) * J.cwiseProduct(J)).cwiseSqrt().transpose();
  } else {
    d = J.colwise().stableNorm().transpose();
  }

  // ensure scaling parameters are non-zero
  for (auto i = 0u; i != d.size(); ++i) {
    if (d[i] == 0) { d[i] = 1; }
  }

  double r_norm = r.stableNorm();
  double Delta = 100. * d.stableNorm();  // TODO for Rn arguments we should multiply with norm(x)

  for (auto i = 0u; i != opts.max_iter; ++i) {
    // calculate step a via LM parameter algorithm
    Eigen::Matrix<double, Nx, 1> a(nx);
    double lambda;
    if constexpr (is_sparse) {
      std::tie(lambda, a) = detail::lmpar_sparse(J, d, r, Delta);
    } else {
      std::tie(lambda, a) = detail::lmpar(J, d, r, Delta);
    }

    // evaluate function and jacobian at x + a
    auto x_plus_a               = utils::tuple_plus(x, a);
    const auto [r_cand, J_cand] = diff::dr<Diff>(f, x_plus_a);

    const double r_cand_norm = r_cand.stableNorm();
    const double Da_norm     = d.cwiseProduct(a).stableNorm();

    // calculate actual to predicted reduction
    const double act_red  = 1. - Eigen::numext::abs2(r_cand_norm / r_norm);
    const double fra2     = Eigen::numext::abs2((J * a).stableNorm() / r_norm);
    const double fra3     = Eigen::numext::abs2(std::sqrt(lambda) * Da_norm / r_norm);
    const double pred_red = fra2 + 2. * fra3;
    const double rho      = act_red / pred_red;

    // update trust region following Moré (1978)
    if (rho < 0.25) {
      double mu;
      if (r_cand_norm <= r_norm) {
        mu = 0.5;
      } else if (r_cand_norm <= 10 * r_norm) {
        const double gamma = -fra2 - fra3;
        mu                 = std::clamp(gamma / (2. * gamma + act_red), 0.1, 0.5);
      } else {
        mu = 0.1;
      }
      Delta *= mu;
    } else if ((lambda == 0 && rho < 0.75) || rho > 0.75) {
      Delta = 2 * Da_norm;
    }

    //// TAKE STEP IF SUCCESSFUL ////

    if (rho > 1e-4) {
      x      = x_plus_a;
      r      = r_cand;
      J      = J_cand;
      r_norm = r_cand_norm;

      // update scaling
      if constexpr (is_sparse) {
        d = d.cwiseMax((Eigen::Matrix<double, 1, -1>::Ones(J.rows()) * J.cwiseProduct(J))
                         .cwiseSqrt()
                         .transpose());
      } else {
        d = d.cwiseMax(J.colwise().stableNorm().transpose());
      }
    }

    //// PRINT STATUS ////

    // TODO Pretty-print solver steps
    if (opts.verbosity > 0) { std::cout << "Step " << i << ": " << r.sum() << std::endl; }

    //// CHECK FOR CONVERGENCE ////

    // function tolerance
    if (std::abs(act_red) < opts.ftol && pred_red < opts.ftol && rho <= 2.) { break; }

    // parameter tolerance
    // TODO a.size() should be norm(x) for non-angle states
    if (Da_norm < opts.ptol * a.size()) { break; }
  }
}

/**
 * @brief Find a minimum of the non-linear least-squares problem
 *
 * \f[
 *  \min_{x} \sum_i \| f(x)_i \|^2
 * \f]
 *
 * @param f residuals to minimize
 * @param x reference tuple of arguments to f
 * @param opts solver options
 *
 * All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<typename _F, typename _Wrt>
void minimize(_F && f, _Wrt && x, const MinimizeOptions & opts = MinimizeOptions{})
{
  minimize<diff::Type::DEFAULT>(std::forward<_F>(f), std::forward<_Wrt>(x), opts);
}

}  // namespace smooth

#endif  // SMOOTH__OPTIM_HPP_


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <iostream>

using namespace boost::gil;

int main() {
  gray8_image_t img;
  read_image("test_adaptive.png", img, png_tag{});
  gray8_image_t img_out(img.dimensions());

  boost::gil::threshold_adaptive(const_view(img), view(img_out), 11,
                                 threshold_adaptive_method::mean,
                                 threshold_direction::regular, 2);
  write_view("out-threshold-adaptive-mean.png", view(img_out), png_tag{});

  boost::gil::threshold_adaptive(const_view(img), view(img_out), 11,
                                 threshold_adaptive_method::mean,
                                 threshold_direction::inverse, 2);
  write_view("out-threshold-adaptive-mean-inv.png", view(img_out), png_tag{});

  boost::gil::threshold_adaptive(const_view(img), view(img_out), 7,
                                 threshold_adaptive_method::gaussian,
                                 threshold_direction::regular, 2);
  write_view("out-threshold-adaptive-gaussian.png", view(img_out), png_tag{});

  boost::gil::threshold_adaptive(const_view(img), view(img_out), 11,
                                 threshold_adaptive_method::gaussian,
                                 threshold_direction::inverse, 2);
  write_view("out-threshold-adaptive-gaussian-inv.png", view(img_out),
             png_tag{});

  return 0;
}


#pragma once

#include <boost/intrusive/list.hpp>
#include <boost/intrusive/slist.hpp>
#include <boost/intrusive/options.hpp>
#include "litiv/3rdparty/sospd/submodular-functions.hpp"

namespace sospd {

    enum class UBfn {
        chen,
        cvpr14,
    };

    enum class FlowAlgorithm {
        bidirectional,
        source,
        parametric
    };

    struct SubmodularIBFSParams {
        SubmodularIBFSParams() = default;
        SubmodularIBFSParams(sospd::FlowAlgorithm _alg) : alg(_alg) {}
        sospd::FlowAlgorithm alg = sospd::FlowAlgorithm::bidirectional;
        sospd::UBfn ub = sospd::UBfn::cvpr14;
        std::vector<bool> fixedVars;
    };

    template<typename ValueType, typename IndexType>
    class SubmodularIBFS;

    template<typename ValueType, typename IndexType>
    class FlowSolver {
    public:
        FlowSolver() = default;
        virtual ~FlowSolver() = default;
        virtual void Solve(SubmodularIBFS<ValueType,IndexType>* energy) = 0;
        FlowSolver(const FlowSolver&) = delete;
        FlowSolver(FlowSolver&&) = delete;
        FlowSolver& operator=(const FlowSolver&) = delete;
        FlowSolver& operator=(FlowSolver&&) = delete;
    };

    template<typename ValueType, typename IndexType>
    std::unique_ptr<FlowSolver<ValueType,IndexType>> GetSolver(const SubmodularIBFSParams& params);

    /** Graph structure and algorithm for sum-of-submodular IBFS
     */
    template<typename ValueType, typename IndexType>
    class SoSGraph {
        static_assert(std::is_arithmetic<ValueType>::value,"value type must be arithmetic");
        static_assert(std::is_integral<IndexType>::value,"index type must be integral");
        public:
            typedef ValueType REAL;
            typedef IndexType NodeId;
            typedef IndexType CliqueId;
            typedef std::vector<CliqueId> NeighborList;
            enum class NodeState : char {
                S, T, S_orphan, T_orphan, N
            };
            class IBFSEnergyTableClique;
            typedef std::tuple<sospd::UBfn,std::string,sospd::UpperBoundFunction<ValueType,IndexType>> UBParam;
            static const std::vector<UBParam> ubParamList;

            SoSGraph()
                : m_num_nodes(0),
                s(NodeId(-1)),
                t(NodeId(-1)),
                m_num_cliques(0)
            { }

            /** Add n new nodes to the base set V
             *
             * \return Index of first created node
             */
            NodeId AddNode(IndexType n = 1);

            /** Add weights to s-i and i-t edges, respectively
             */
            void AddTerminalWeights(NodeId n, REAL sCap, REAL tCap);

            /** Zero the capacities on the s-i and i-t edges
             */
            void ClearTerminals();

            // Add Clique defined by nodes and energy table given
            IBFSEnergyTableClique& AddClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energyTable);

            /* Clique: abstract base class for user-defined clique functions
             *
             * Clique stores the list of nodes associated with a clique.
             * Actual functionality is provided by the user writing a derived
             * class with Clique as the base, and which implements the
             * ComputeEnergy and ExchangeCapacity functions
             */
            class Clique {
                public:
                typedef std::vector<NodeId> NodeVec;
                Clique() : m_nodes(), m_alpha_Ci() { }
                Clique(const NodeVec& nodes)
                    : m_nodes(nodes),
                    m_alpha_Ci(nodes.size(), 0)
                { }
                ~Clique() = default;

                // Returns the energy of the given labeling for this clique function
                virtual REAL ComputeEnergy(const std::vector<int>& labels) const = 0;

                const NodeVec& Nodes() const { return m_nodes; }
                IndexType Size() const { return IndexType(m_nodes.size()); }
                std::vector<REAL>& AlphaCi() { return m_alpha_Ci; }
                const std::vector<REAL>& AlphaCi() const { return m_alpha_Ci; }
                IndexType GetIndex(NodeId i) const {
                    return IndexType(std::find(this->m_nodes.begin(), this->m_nodes.end(), i) - this->m_nodes.begin());
                }

                protected:
                NodeVec m_nodes; // The list of nodes in the clique
                std::vector<REAL> m_alpha_Ci; // The reparameterization variables for this clique

            };
            /*
             * IBFSEnergyTableClique: stores energy as a list of 2^k values for each subset
             */
            class IBFSEnergyTableClique : public Clique {
                public:
                    typedef uint32_t Assignment;

                    IBFSEnergyTableClique() : Clique(), m_energy(), m_alpha_energy(), m_min_tight_set() { }
                    IBFSEnergyTableClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energy)
                        : Clique(nodes),
                        m_energy(energy),
                        m_alpha_energy(energy),
                        m_min_tight_set(nodes.size(), (1u << nodes.size()) - 1)
                    {
                        ASSERT(nodes.size() <= 31);
                    }

                    virtual REAL ComputeEnergy(const std::vector<int>& labels) const;
                    REAL ComputeAlphaEnergy(const std::vector<int>& labels) const;
                    REAL ExchangeCapacity(IndexType u_idx, IndexType v_idx) const;
                    bool NonzeroCapacity(IndexType u_idx, IndexType v_idx) const;
                    void NormalizeEnergy(std::vector<REAL>& psi, REAL& constantTerm);

                    void Push(IndexType u_idx, IndexType v_idx, REAL delta);
                    void ComputeMinTightSets();
                    std::vector<REAL>& EnergyTable() { return m_energy; }
                    const std::vector<REAL>& EnergyTable() const { return m_energy; }
                    std::vector<REAL>& AlphaEnergy() { return m_alpha_energy; }
                    const std::vector<REAL>& AlphaEnergy() const { return m_alpha_energy; }

                    void ResetAlpha();

                protected:
                    std::vector<REAL> m_energy;
                    std::vector<REAL> m_alpha_energy;
                    std::vector<Assignment> m_min_tight_set;

            };
            struct ArcIterator {
                NodeId source;
                typename NeighborList::iterator cIter;
                IndexType cliqueIdx;
                IndexType cliqueSize;
                SoSGraph* graph;

                bool operator!=(const ArcIterator& a) {
                    return (cIter != a.cIter) || (cliqueIdx != a.cliqueIdx);
                }
                bool operator==(const ArcIterator& a) {
                    return !(*this != a);
                }
                bool operator<(const ArcIterator& a) {
                    ASSERT(source == a.source);
                    return (cIter == a.cIter) ? (source < a.source) : (cIter < a.cIter);
                }

                ArcIterator& operator++() {
                    //ASSERT(*cIter < static_cast<int>(graph->m_cliques.size()));
                    cliqueIdx++;
                    if (cliqueIdx == cliqueSize) {
                        cliqueIdx = IndexType(0);
                        cIter++;
                        if (cIter != graph->m_neighbors[source].end())
                            cliqueSize = graph->m_cliques[*cIter].Size();
                        else
                            cliqueSize = IndexType(0);
                    }
                    //ASSERT(cIter == graph->m_neighbors[source].end() || *cIter < static_cast<int>(graph->m_cliques.size()));
                    //ASSERT(cIter == graph->m_neighbors[source].end() || cliqueIdx < static_cast<int>(graph->m_cliques[*cIter].Nodes().size()));
                    return *this;
                }
                NodeId Source() const {
                    return source;
                }
                NodeId Target() const {
                    //ASSERT(*cIter < static_cast<int>(graph->m_cliques.size()));
                    //ASSERT(cliqueIdx < static_cast<int>(graph->m_cliques[*cIter].Nodes().size()));
                    return graph->m_cliques[*cIter].Nodes()[cliqueIdx];
                }
                IndexType SourceIdx() const { return graph->m_cliques[*cIter].GetIndex(source); }
                IndexType TargetIdx() const { return cliqueIdx; }
                CliqueId cliqueId() const { return *cIter; }
                ArcIterator Reverse() const {
                    auto newSource = Target();
                    auto newCIter = std::find(graph->m_neighbors[newSource].begin(), graph->m_neighbors[newSource].end(), *cIter);
                    auto newCliqueIdx = graph->GetCliques()[*newCIter].GetIndex(source);
                    return {newSource, newCIter, newCliqueIdx, graph->m_cliques[*newCIter].Size(), graph};
                }
            };

            typedef boost::intrusive::list_base_hook<boost::intrusive::link_mode<boost::intrusive::normal_link>> ListHook;
            typedef boost::intrusive::slist_base_hook<boost::intrusive::link_mode<boost::intrusive::normal_link>> OrphanListHook;
            struct Node : public ListHook, OrphanListHook {
                NodeId id;
                NodeState state;
                IndexType dis;
                ArcIterator parent_arc;
                NodeId parent;
                NeighborList cliques;
                explicit Node(NodeId _id)
                    : id(_id)
                    , state(NodeState::N)
                    , dis(std::numeric_limits<IndexType>::max())
                    , parent_arc()
                    , parent()
                    , cliques() { }
            };

            typedef boost::intrusive::list<Node> NodeQueue;
            typedef boost::intrusive::slist<Node, boost::intrusive::base_hook<OrphanListHook>, boost::intrusive::cache_last<true>> OrphanList;

            ArcIterator ArcsBegin(NodeId i) {
                auto cIter = m_neighbors[i].begin();
                if (cIter == m_neighbors[i].end())
                    return ArcsEnd(i);
                return {i, cIter, 0, m_cliques[*cIter].Size(), this};
            }
            ArcIterator ArcsEnd(NodeId i) {
                auto& neighborList = m_neighbors[i];
                return {i, neighborList.end(), 0, 0, this};
            }

            typedef std::vector<IBFSEnergyTableClique> CliqueVec;

            NodeId NumNodes() const { return m_num_nodes; }
            NodeId GetS() const { return s; }
            NodeId GetT() const { return t; }
            Node& node(NodeId i) { return m_nodes[i]; }
            const Node& node(NodeId i) const { return m_nodes[i]; }
            IBFSEnergyTableClique& clique(CliqueId c) { return m_cliques[c]; }
            const IBFSEnergyTableClique& clique(CliqueId c) const { return m_cliques[c]; }
            const std::vector<REAL>& GetC_si() const { return m_c_si; }
            const std::vector<REAL>& GetC_it() const { return m_c_it; }
            const std::vector<REAL>& GetPhi_si() const { return m_phi_si; }
            const std::vector<REAL>& GetPhi_it() const { return m_phi_it; }
            CliqueId GetNumCliques() const { return m_num_cliques; }
            const CliqueVec& GetCliques() const { return m_cliques; }
            CliqueVec& GetCliques() { return m_cliques; }
            const std::vector<NeighborList>& GetNeighbors() const { return m_neighbors; }
            std::vector<Node>& GetNodes() { return m_nodes; }
            const std::vector<Node>& GetNodes() const { return m_nodes; }

            REAL ResCap(const ArcIterator& arc, bool forwardArc);
            bool NonzeroCap(const ArcIterator& arc, bool forwardArc);
            void Push(ArcIterator& arc, bool forwardArc, REAL delta);

            void ResetFlow();
            typedef void(*BoundFn)(int, const std::vector<REAL>&, std::vector<REAL>&);
            struct NormStats {
                double L1 = 0;
                double L2 = 0;
                double LInfty = 0;
            };
            template<BoundFn fn>
            void UpperBoundCliques(const std::vector<bool>& fixedVars, NormStats* stats);
            void UpperBoundCliques(sospd::UBfn ub, NormStats* stats = 0);
            void UpperBoundCliques(sospd::UBfn ub, const std::vector<bool>& fixedVars, const std::vector<int>& labels, NormStats* stats = 0);

            NodeId m_num_nodes;
            NodeId s,t;
            std::vector<REAL> m_c_si;
            std::vector<REAL> m_c_it;
            std::vector<REAL> m_phi_si;
            std::vector<REAL> m_phi_it;

            CliqueId m_num_cliques;
            CliqueVec m_cliques;
            std::vector<NeighborList> m_neighbors;

        protected:
            std::vector<Node> m_nodes;
    };

} // namespace sospd

template<typename V, typename I>
const std::vector<typename sospd::SoSGraph<V,I>::UBParam> sospd::SoSGraph<V,I>::ubParamList = {
    UBParam{ sospd::UBfn::chen, "chen", sospd::ChenUpperBound<REAL> },
    UBParam{ sospd::UBfn::cvpr14, "cvpr14", sospd::UpperBoundCVPR14<REAL> },
};

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::NodeId sospd::SoSGraph<V,I>::AddNode(I n) {
    ASSERT(n >= I(1));
    ASSERT(s == I(-1));
    NodeId first_node = m_num_nodes;
    for(I i = 0; i < n; ++i) {
        m_nodes.push_back(Node(m_num_nodes));
        m_c_si.push_back(0);
        m_c_it.push_back(0);
        m_phi_si.push_back(0);
        m_phi_it.push_back(0);
        m_neighbors.push_back(NeighborList());
        m_num_nodes++;
    }
    return first_node;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::AddTerminalWeights(NodeId n, REAL sCap, REAL tCap) {
    m_c_si[n] += sCap;
    m_c_it[n] += tCap;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::ClearTerminals() {
    for (NodeId i = 0; i < m_num_nodes; ++i) {
        m_c_si[i] = m_c_it[i] = 0;
        m_phi_si[i] = m_phi_it[i] = 0;
    }
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::IBFSEnergyTableClique& sospd::SoSGraph<V,I>::AddClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energyTable) {
    ASSERT(s == I(-1));
    m_cliques.emplace_back(nodes, energyTable);
    for (NodeId i : nodes) {
        ASSERT(0 <= i && i < m_num_nodes);
        m_neighbors[i].push_back(m_num_cliques);
    }
    return m_cliques[m_num_cliques++];
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::ResetFlow() {
    // Initialize source, sink (only do once)
    if (s == I(-1)) {
        s = m_num_nodes; t = m_num_nodes + 1;
        m_nodes.push_back(Node(s));
        m_nodes.push_back(Node(t));
    }
    // reset distance, state and parent
    for (I i = 0; i < m_num_nodes + 2; ++i) {
        Node& node = m_nodes[i];
        node.dis = std::numeric_limits<I>::max();
        node.state = NodeState::N;
        node.parent = i;
        m_phi_si[i] = m_phi_it[i] = 0;
    }

    // Reset Clique parameters
    for (I cid = 0; cid < m_num_cliques; ++cid) {
        auto& c = m_cliques[cid];
        c.ResetAlpha();
        c.ComputeMinTightSets();
    }

}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::ResCap(const ArcIterator& arc, bool forwardArc) {
    ASSERT(arc.cliqueId() >= 0 && arc.cliqueId() < I(m_cliques.size()));
    if (forwardArc)
        return m_cliques[arc.cliqueId()].ExchangeCapacity(arc.SourceIdx(), arc.TargetIdx());
    else
        return m_cliques[arc.cliqueId()].ExchangeCapacity(arc.TargetIdx(), arc.SourceIdx());
}

template<typename V, typename I>
inline bool sospd::SoSGraph<V,I>::NonzeroCap(const ArcIterator& arc, bool forwardArc) {
    if (forwardArc)
        return m_cliques[arc.cliqueId()].NonzeroCapacity(arc.SourceIdx(), arc.TargetIdx());
    else
        return m_cliques[arc.cliqueId()].NonzeroCapacity(arc.TargetIdx(), arc.SourceIdx());
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::NormalizeEnergy(std::vector<REAL>& psi, REAL& constantTerm) {
    ASSERT(false /* Should not be calling this function*/);
    const I n = I(this->m_nodes.size());
    ASSERT(sospd::CheckSubmodular((int)n,m_energy));
    const Assignment num_assignments = 1u << n;
    REAL allOnes = m_energy[num_assignments - 1];
    constantTerm += allOnes;
    psi.resize(n);
    Assignment assgn = num_assignments - 1; // The all 1 assignment
    for (I i = 0; i < n; ++i) {
        Assignment next_assgn = assgn ^ (1u << i);
        psi[i] = (m_energy[assgn] - m_energy[next_assgn]);
        assgn = next_assgn;
    }

    for (Assignment a = 0; a < num_assignments; ++a) {
        m_energy[a] -= allOnes;
        for (I i = 0; i < n; ++i) {
            if (!(a & (1u << i))) m_energy[a] += psi[i];
        }
        ASSERT(m_energy[a] >= V(0));
        m_alpha_energy[a] = m_energy[a];
    }
    ComputeMinTightSets();
    ASSERT(sospd::CheckSubmodular((int)n,m_energy));
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeEnergy(const std::vector<int>& labels) const {
    Assignment assgn = 0;
    for (I i = 0; i < I(this->m_nodes.size()); ++i) {
        NodeId n = this->m_nodes[i];
        if (labels[n] == 1) {
            assgn |= 1u << i;
        }
    }
    return m_energy[assgn];
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeAlphaEnergy(const std::vector<int>& labels) const {
    Assignment assgn = 0;
    for (I i = 0; i < I(this->m_nodes.size()); ++i) {
        NodeId n = this->m_nodes[i];
        if (labels[n] == 1) {
            assgn |= 1u << i;
        }
    }
    return m_alpha_energy[assgn];
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ExchangeCapacity(I u_idx, I v_idx) const {
    const I n = I(this->m_nodes.size());
    ASSERT(u_idx < n);
    ASSERT(v_idx < n);
    REAL min_energy = std::numeric_limits<REAL>::max();
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    const Assignment u_mask = 1u << u_idx;
    const Assignment v_mask = 1u << v_idx;
    const Assignment uv_mask = u_mask | v_mask;
    const Assignment subset_mask = bound & ~uv_mask;
    // Terrible bit-hacks to optimize the living hell out of this function
    // Iterate over all assignments without u_idx or v_idx set
    Assignment assgn = subset_mask;
    do {
        Assignment u_sep = assgn | u_mask;
        REAL energy = m_alpha_energy[u_sep];
        if (energy < min_energy) min_energy = energy;
        assgn = ((assgn - 1) & subset_mask);
    } while (assgn != subset_mask);

    return min_energy;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::Push(I u_idx, I v_idx, REAL delta) {
    ASSERT(u_idx < this->m_nodes.size());
    ASSERT(v_idx < this->m_nodes.size());
    Clique::m_alpha_Ci[u_idx] += delta;
    Clique::m_alpha_Ci[v_idx] -= delta;
    const I n = I(this->m_nodes.size());
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    const Assignment u_mask = 1u << u_idx;
    const Assignment v_mask = 1u << v_idx;
    const Assignment uv_mask = u_mask | v_mask;
    const Assignment subset_mask = bound & ~uv_mask;
    // Terrible bit-hacks to optimize the living hell out of this function
    // Iterate over all assignments without u_idx or v_idx set
    Assignment assgn = subset_mask;
    do {
        Assignment u_sep = assgn | u_mask;
        Assignment v_sep = assgn | v_mask;
        m_alpha_energy[u_sep] -= delta;
        m_alpha_energy[v_sep] += delta;
        assgn = ((assgn - 1) & subset_mask);
    } while (assgn != subset_mask);

    ComputeMinTightSets();
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeMinTightSets() {
    I n = I(this->m_nodes.size());
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    for (auto& a : m_min_tight_set)
        a = bound;
    for (Assignment assgn = bound-1; assgn >= 1; --assgn) {
        if (m_alpha_energy[assgn] == 0) {
            for (I i = 0; i < n; ++i) {
                //ASSERT(m_alpha_energy[m_min_tight_set[i] & assgn] == 0);
                //ASSERT(m_alpha_energy[m_min_tight_set[i] | assgn] == 0);
                if ((assgn & (1u << i)) != 0)
                    m_min_tight_set[i] = assgn;
            }
        }
    }
}

template<typename V, typename I>
inline bool sospd::SoSGraph<V,I>::IBFSEnergyTableClique::NonzeroCapacity(I u_idx, I v_idx) const {
    Assignment min_set = m_min_tight_set[u_idx];
    return (min_set & (1u << v_idx)) != 0;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ResetAlpha() {
    for (auto& a : this->m_alpha_Ci) {
        a = 0;
    }
    const I n = I(this->m_nodes.size());
    const Assignment num_assignments = 1u << n;
    for (Assignment a = 0; a < num_assignments; ++a) {
        m_alpha_energy[a] = m_energy[a];
    }
}

template<typename V, typename I>
template<typename sospd::SoSGraph<V,I>::BoundFn UB>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(const std::vector<bool>& fixedVars, NormStats* stats) {
    std::vector<REAL> psi;
    //int nCliques = m_cliques.size();
    I cliquesDone = 0;
    /*
     *std::cout << "Upper Bounding Cliques: ";
     *std::cout.flush();
     */
    for (auto& c : m_cliques) {
        /*
         *if (cliquesDone % (nCliques/10) == 0) {
         *    std::cout << ".";
         *    std::cout.flush();
         *}
         */
        cliquesDone++;
        auto& newEnergy = c.AlphaEnergy();
        I k = I(c.Size());
        psi.resize(k);
        // Compute upper bound g of clique energy
        UB(k, c.EnergyTable(), newEnergy);

        if (!fixedVars.empty()) {
            I fixedSet = 0;
            for (I i = 0; i < k; ++i)
                fixedSet |= (fixedVars[c.Nodes()[i]] << i);
            sospd::ZeroMarginalSet(k, newEnergy, fixedSet);
        }

        if (stats) {
            stats->L1 += sospd::DiffL1(c.EnergyTable(), newEnergy);
            stats->L2 += sospd::DiffL2(c.EnergyTable(), newEnergy);
            stats->LInfty += sospd::DiffLInfty(c.EnergyTable(), newEnergy);
        }
        // Modify g, find psi so that g'(S) = g(S) + psi(S) >= 0
        sospd::Normalize(k, newEnergy, psi);
        /*
         *AddLinear(k, c.EnergyTable(), psi);
         */

        auto& alpha_Ci = c.AlphaCi();
        for (I i = 0; i < k; ++i) {
            alpha_Ci[i] = -psi[i];
            m_phi_it[c.Nodes()[i]] += psi[i];
        }
        c.ComputeMinTightSets();
    }
    /*
     *std::cout << "\n";
     *std::cout << "L1: " << diffL1 << "\tL2: " << diffL2 << "\tLInfty: " << diffLInfty << "\n";
     */
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(sospd::UBfn ub, NormStats* stats) {
    UpperBoundCliques(ub, std::vector<bool>{}, std::vector<int>{}, stats);
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(sospd::UBfn ub, const std::vector<bool>& fixedVars, const std::vector<int>& /*labels*/, NormStats* stats) {
    switch(ub) {
        case sospd::UBfn::chen:
            UpperBoundCliques<sospd::ChenUpperBound>(fixedVars, stats);
            break;
        case sospd::UBfn::cvpr14:
            UpperBoundCliques<sospd::UpperBoundCVPR14>(fixedVars, stats);
            break;
    }
}


#include <tuple>
#include <Eigen/Core>
#include <npe.h>

// This defines a function named 'test_binding' which will be exposed to python
npe_function(test_binding)

// This says test binding takes as input a dense Eigen matrix, 'a', which can have scalar
// type dense_f32 (float) or dense_f64 (double).
// The variable 'a' has type Eigen::Map with scalar matching one of the input types. This means
// the corresponding Python function takes a numpy array whose dtype is float32 or float64
npe_arg(a, dense_float, dense_double)

// This is another input variable, 'b'. It's type is a npe_matches() statement. In this case, the npe_matches() says that the
// type of 'b' must match the type of 'a'. npe_matches() statements can only match to numpy overloaded variables.
npe_arg(b, npe_matches(a))

// Here is another variable 'c' and two variables whose types have to match it.
npe_arg(c, dense_int, dense_long)
npe_arg(d, npe_matches(c))
npe_arg(e, npe_matches(d))

// This is a non-numpy overloaded input parameter of type int.
npe_arg(f, int)

// After this directive is the actual source code of the bound function
npe_begin_code()

// We compute some return values using the types specified in the input. So, for example,
// if you pass in matrices of doubles, you get the result as a matrix of doubles
npe_Matrix_a ret1 = a + b;
npe_Matrix_c ret2 = c + d + e * f;

// MOVE_TO_NP returns a pybind11::array which takes ownership of the Eigen::Matrix passed as a second argument.
// The first argument is the type of the Eigen::Matrix
// return std::make_tuple(NPE_MOVE(npe::Matrix_a, ret1), NPE_MOVE(npe::Matrix_c, ret2));
return std::make_tuple(npe::move(ret1), npe::move(ret2));
npe_end_code()


#include <algorithm>
#include <array>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/functional/hash.hpp>
#include <iostream>
#include <sstream>
#include <string>
#include <list>
#include <unordered_map>

struct Position
{
    int x = 0;
    int y = 0;

    bool operator== (const Position& o) const {
        return std::tie(x, y) == std::tie(o.x, o.y);
    }

    void move(const std::string& dir)
    {
        if (y & 1) {
            // lichy radek
            if (dir == "n") {
                y -= 2;
            } else if (dir == "s") {
                y += 2;
            } else if (dir == "nw") {
                y -= 1;
            } else if (dir == "ne") {
                y -= 1;
                x += 1;
            } else if (dir == "sw") {
                y += 1;
            } else if (dir == "se") {
                y += 1;
                x += 1;
            } else {
                std::cerr << "WRONG MOVE!\n";
                exit(1);
            }
        } else {
            // sudy radek
            if (dir == "n") {
                y -= 2;
            } else if (dir == "s") {
                y += 2;
            } else if (dir == "nw") {
                y -= 1;
                x -= 1;
            } else if (dir == "ne") {
                y -= 1;
            } else if (dir == "sw") {
                y += 1;
                x -= 1;
            } else if (dir == "se") {
                y += 1;
            } else {
                std::cerr << "WRONG MOVE!\n";
                exit(1);
            }
        }
    }
};

namespace std {

template<>
struct hash<Position>
{
    using argument_type = Position;
    using result_type = std::size_t;

    result_type operator()(argument_type const& pos) const noexcept
    {
        size_t seed = 0;
        boost::hash_combine(seed, pos.x);
        boost::hash_combine(seed, pos.y);
        return seed;
    }
};

}  // namespace std

int main()
{
    std::string line;
    std::getline(std::cin, line);

    std::vector<std::string> moves;
    boost::algorithm::split(
        moves, line, boost::is_any_of(", "), boost::algorithm::token_compress_on);

    Position child;
    for (const std::string& move : moves) {
        child.move(move);
    }

    std::unordered_map<Position, int> positions;
    std::list<const Position*> queue;

    auto x = positions.insert({Position{}, 0});
    queue.emplace_back(&(x.first->first));

    for (;!queue.empty();) {
        const auto cur = positions.find(*queue.front());
        assert(cur != positions.end());

        if (cur->first == child) {
            std::cout << "STEPS: " << cur->second << "\n";
            break;
        }
        queue.pop_front();

        for (const auto& dir : {"n", "s", "nw", "ne", "sw", "se"}) {
            Position _p = cur->first;
            _p.move(dir);
            int nextStep = cur->second + 1;
            auto it = positions.insert({_p, nextStep});
            if (it.second) {
                queue.emplace_back(&(it.first->first));
            } else if (it.first->second > nextStep) {
                it.first->second = nextStep;
            }
        }

    }

    return 0;
}


/* NormalModeMeanSquareFluctuationCalculator.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

// system includes =============================================================

#include <string>
#include <Eigen/Dense>

// local includes ==============================================================

#include "../src/Reach.hpp"
#include "../src/FileFactory.hpp"
#include "../src/TrajectoryFile.hpp"
#include "../src/ProteinFile.hpp"
#include "../src/SecondaryStructureFile.hpp"
#include "../src/cli/ConfigParser.hpp"

#include "utils/file.hpp"
#include "utils/log.hpp"
#include "utils/definitions.hpp"

BOOST_AUTO_TEST_SUITE(calnmodemsfss)

    BOOST_AUTO_TEST_CASE(nmodem_test_all) {
        TEST_MESSAGE("nmodem_test_all");

        std::string test_dir = "test/resources/calnmodemsfss/";
        std::string test_prefix = "mb_ca";

        NormalModeMeanSquareFluctuationCalculator nmmsfc
                = get_nmmsfc_for_testing("tools/REACH/test/mb_ca.pdb","tools/REACH/test/1a6g.dssp","mb_ca");

        Eigen::MatrixXd hessian = nmmsfc.get_hessian_matrix();
        Eigen::VectorXd xxcom = nmmsfc.get_mean_square_fluctuation();
        Eigen::VectorXd eig = nmmsfc.get_weighted_eigenvalues();
        Eigen::VectorXd vec = nmmsfc.get_weighted_eigenvector();

        // read reach result
        Eigen::MatrixXd reach_hessian = get_sym_matrix_from_file(test_dir + test_prefix + "_calnmodemsfss_hessian.csv",453, ',', true);
        Eigen::VectorXd xxcom_reach = get_vector_from_csv(test_dir + test_prefix + "_calnmodemsfss_xxcom.csv","xxcom");
        Eigen::VectorXd eig_reach = get_vector_from_csv(test_dir + test_prefix + "_calnmodemsfss_eigvec.csv","eig");
        Eigen::VectorXd vec_reach = get_vector_from_csv(test_dir + test_prefix + "_calnmodemsfss_eigvec.csv","vec");

        // compare hessian, eig, vec, xxcom
        for (size_t i=0; i<453; ++i) {
            for (size_t j=0; j<453; ++j) {
                BOOST_REQUIRE_CLOSE_FRACTION(hessian(i,j), reach_hessian(i,j), PRECISION);
            }
            BOOST_REQUIRE_CLOSE_FRACTION(vec(i), vec_reach(i), PRECISION);
        }
        // check eig, ignoring the first six trivial eigenvalues
        for (size_t i=6; i<453; ++i) {
            BOOST_REQUIRE_CLOSE_FRACTION(eig(i), eig_reach(i), PRECISION);
        }

        for (size_t i=0; i<151; ++i) {
            BOOST_REQUIRE_CLOSE_FRACTION(xxcom(i), xxcom_reach(i), PRECISION);
        }
}

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8