/*******
edit_distance: STL and Boost compatible edit distance functions for C++

Copyright (c) 2013 Erik Erlandson

Author:  Erik Erlandson <erikerlandson@yahoo.com>

Distributed under the Boost Software License, Version 1.0.
See accompanying file LICENSE or copy at
http://www.boost.org/LICENSE_1_0.txt
*******/

#if !defined(BOOST_ALGORITHM_SEQUENCE_DETAIL_EDIT_DISTANCE_TYPES_HPP)
#define BOOST_ALGORITHM_SEQUENCE_DETAIL_EDIT_DISTANCE_TYPES_HPP

#include <cstddef>
#include <iterator>

#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>

#include <boost/exception/exception.hpp>

#include <boost/type_traits.hpp>
#include <boost/type_traits/is_arithmetic.hpp>

#include <boost/utility/enable_if.hpp>
#include <boost/typeof/std/utility.hpp>

#include <boost/function_types/result_type.hpp>
#include <boost/function_types/function_arity.hpp>
#include <boost/function_types/parameter_types.hpp>

#include <boost/tti/has_type.hpp>
#include <boost/tti/has_member_function.hpp>

#include <boost/range/metafunctions.hpp>
#include <boost/range/as_literal.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/mpl/equal_to.hpp>

#include <boost/concept/assert.hpp>
#include <boost/concept/usage.hpp>

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

#include <boost/parameter/name.hpp>
#include <boost/parameter/preprocessor.hpp>

namespace boost {
namespace algorithm {
namespace sequence {

struct unit_cost {
    typedef size_t cost_type;

    template <typename value_type> inline
    cost_type insertion(value_type const&) const {
        return cost_type(1);
    }

    template <typename value_type> inline
    cost_type deletion(value_type const&) const {
        return cost_type(1);
    }

    template <typename value_type_1, typename value_type_2> inline
    cost_type substitution(value_type_1 const&, value_type_2 const&) const {
        return cost_type(1);
    }
};

struct max_edit_cost_exception : public boost::exception {};

namespace parameter {
    BOOST_PARAMETER_NAME(sequence1)
    BOOST_PARAMETER_NAME(sequence2)
    BOOST_PARAMETER_NAME(cost)
    BOOST_PARAMETER_NAME(equal)
    BOOST_PARAMETER_NAME(script)
    BOOST_PARAMETER_NAME(substitution)
    BOOST_PARAMETER_NAME(max_cost)
    BOOST_PARAMETER_NAME(max_cost_exception)
}

namespace detail {

using boost::is_same;
using boost::is_member_function_pointer;
using boost::integral_constant;
using boost::false_type;
using boost::true_type;
using boost::function_types::result_type;
using boost::function_types::parameter_types;
using boost::function_types::function_arity;
using boost::mpl::at_c;
using boost::mpl::and_;
using boost::mpl::not_;
using boost::mpl::equal_to;
using boost::enable_if;

template <typename X> struct invoke : public true_type {};

struct none {};

template <typename T> inline T& nonconst_default() {
    static T r;
    return r;
}

template <typename Range1, typename Range2, typename Tag> struct range_category : public
    and_<is_same<typename std::iterator_traits<typename boost::range_iterator<Range1>::type>::iterator_category, Tag>,
         is_same<typename std::iterator_traits<typename boost::range_iterator<Range2>::type>::iterator_category, Tag> > {};

template <typename MaxCost, typename CostT, typename Node, typename Enable = void> struct max_cost_checker {};

template <typename MaxCost, typename CostT, typename Node>
struct max_cost_checker<MaxCost, CostT, Node, typename enable_if<is_same<MaxCost, none> >::type> {
    typedef typename Node::pos1_type pos1_type;
    typedef typename Node::pos2_type pos2_type;
    max_cost_checker(const MaxCost&, const pos1_type&, const pos2_type&) {}
    inline bool operator()(const CostT&) const { return false; }
    inline void update(const Node*) const {}
    inline void get(Node*&) const {}
};

template <typename MaxCost, typename CostT, typename Node>
struct max_cost_checker<MaxCost, CostT, Node, typename enable_if<is_arithmetic<MaxCost> >::type> {
    typedef typename Node::pos1_type pos1_type;
    typedef typename Node::pos2_type pos2_type;
    typedef typename pos1_type::difference_type diff_type;

    CostT max_cost;
    pos1_type beg1;
    pos2_type beg2;
    diff_type mcmin;
    diff_type mctec;
    Node* mcnode;

    max_cost_checker(const MaxCost& max_cost_, const pos1_type& pos1_, const pos2_type& pos2_) : max_cost(CostT(std::abs(max_cost_))), beg1(pos1_), beg2(pos2_), mcmin(-1), mctec(-1), mcnode(NULL) {}
    inline bool operator()(const CostT& c) const { return c > max_cost; }
    inline void update(Node* node) {
        // primary criteria:  position that consumes most sequence elements
        diff_type ttec = (node->pos1 - beg1) + (node->pos2 - beg2);
        if (ttec < mctec) return;

        // secondary criteria: favor positions closest to diagonal
        diff_type tmin = std::min(node->pos1 - beg1, node->pos2 - beg2);
        if (ttec > mctec  ||  tmin > mcmin) {
            mcnode = node;
            mctec = ttec;
            mcmin = tmin;
        }
    }
    inline void get(Node*& node) const {
        node = mcnode;
    }
};

struct remainder {
    enum kind { none, forward, reverse, bidirectional };
};

template <typename MaxCost, typename CostT, typename Pos, typename Enable = void> struct max_cost_checker_myers {};

template <typename MaxCost, typename CostT, typename Pos>
struct max_cost_checker_myers<MaxCost, CostT, Pos, typename enable_if<is_same<MaxCost, none> >::type> {
    max_cost_checker_myers(const MaxCost&) {}
    inline bool operator()(const CostT&) const { return false; }
    inline void update(const Pos&, const Pos&, const CostT&) const {}
    inline void get(Pos&, Pos&, CostT&) const {}
    template <typename Itr>
    inline void update(const Pos&, const Itr&, const Itr&, const Pos&, const Pos&, const Pos&, const Pos&) const {}
    inline void get(Pos&, remainder::kind&) const {}
};

template <typename MaxCost, typename CostT, typename Pos>
struct max_cost_checker_myers<MaxCost, CostT, Pos, typename enable_if<is_arithmetic<MaxCost> >::type> {
    CostT max_cost;
    Pos mcmin;
    Pos mctec;
    Pos mck;
    remainder::kind kind;

    max_cost_checker_myers(const MaxCost& max_cost_) : max_cost(CostT(std::abs(max_cost_))), mcmin(-1), mctec(-1), mck(0), kind(remainder::none) {}
    inline bool operator()(const CostT& c) const { return c > max_cost; }
    template <typename Itr>
    inline void update(const Pos& k, const Itr& Vf, const Itr& Vr, const Pos& delta, const Pos& L1, const Pos& L2, const Pos& D) {
        Pos j1f = Vf[k];
        Pos j2f = Vf[k]-k;
        Pos cf = j1f+j2f;

        // test bi-directional path
        if ((k-delta) >= -D  &&  (k-delta) <= D) {
            Pos j1r = Vr[k];
            Pos j2r = Vr[k]-k;
            Pos cr = (L1-j1r)+(L2-j2r);

            Pos ttec = cf+cr;
            if (ttec < mctec) return;

            Pos tmin = std::min(j1f, j2f);
            if (ttec > mctec  ||  tmin > mcmin) {
                mctec = ttec;
                mcmin = tmin;
                mck = k;
                kind = remainder::bidirectional;
            }

            // if a bidirectional path is available, that will be the best possible for this (k),
            // so we do not have to test forward/reverse individually:
            return;
        }

        // test forward path
        if (cf >= mctec) {
            Pos tmin = std::min(j1f, j2f);
            if (cf > mctec || tmin > mcmin) {
                mctec = cf;
                mcmin = tmin;
                mck = k;
                kind = remainder::forward;
            }
        }

        // test reverse path
        Pos j1r = Vr[k+delta];
        Pos j2r = Vr[k+delta]-(k+delta);
        Pos cr = (L1-j1r)+(L2-j2r);
        if (cr >= mctec) {
            Pos tmin = std::min((L1-j1r), (L2-j2r));
            if (cr > mctec  ||  tmin > mcmin) {
                mctec = cr;
                mcmin = tmin;
                mck = k+delta;
                kind = remainder::reverse;
            }
        }
    }

    inline void get(Pos& k_, remainder::kind& kind_) const {
        k_ = mck;
        kind_ = kind;
    }
};

struct default_equal {
    template <typename T1, typename T2>
    inline bool operator()(const T1& a, const T2& b) const { return a == b; }
};

template <typename AllowSub, typename Cost, typename CostT, typename Output, typename Enable=void>
struct sub_checker {
    // informative compile error here
};

template <typename AllowSub, typename Cost, typename CostT, typename Output>
struct sub_checker<AllowSub, Cost, CostT, Output, typename enable_if<is_same<AllowSub, bool> >::type> {
    bool allow;
    sub_checker(const bool& allow_) : allow(allow_) {}
    inline bool operator()() const { return allow; }
    template <typename V1, typename V2> inline CostT substitution(const Cost& cost, const V1& v1, const V2& v2) const { return cost.substitution(v1, v2); }
    template <typename V1, typename V2> inline void substitution(Output& out, const V1& v1, const V2& v2, const CostT& csub) const { out.substitution(v1, v2, csub); }
};

template <typename AllowSub, typename Cost, typename CostT, typename Output>
struct sub_checker<AllowSub, Cost, CostT, Output, typename enable_if<is_same<AllowSub, true_type> >::type> {
    sub_checker(const AllowSub&) {}
    inline bool operator()() const { return true; }
    template <typename V1, typename V2> inline CostT substitution(const Cost& cost, const V1& v1, const V2& v2) const { return cost.substitution(v1, v2); }
    template <typename V1, typename V2> inline void substitution(Output& out, const V1& v1, const V2& v2, const CostT& csub) const { out.substitution(v1, v2, csub); }
};

template <typename AllowSub, typename Cost, typename CostT, typename Output>
struct sub_checker<AllowSub, Cost, CostT, Output, typename enable_if<is_same<AllowSub, false_type> >::type> {
    sub_checker(const AllowSub&) {}
    inline bool operator()() const { return false; }
    template <typename V1, typename V2> inline CostT substitution(const Cost&, const V1&, const V2&) const { return 0; }
    template <typename V1, typename V2> inline void substitution(Output&, const V1&, const V2&, const CostT&) const {}
};

template <typename Itr, typename Enabled=void>
struct position {
    typedef Itr itr_type;
    typedef typename std::iterator_traits<Itr>::difference_type idx_type;
    typedef typename std::iterator_traits<Itr>::difference_type difference_type;
    typedef typename std::iterator_traits<Itr>::value_type value_type;
    typedef typename std::iterator_traits<Itr>::reference reference;

    itr_type j;
    idx_type idx;

    inline void beg(const itr_type& src) { j = src;  idx = 0; }

    inline reference operator*() { return *j; }
    inline bool operator==(const itr_type& rhs) const { return j == rhs; }
    inline bool operator!=(const itr_type& rhs) const { return j != rhs; }
    inline bool operator<(const position& rhs) const { return idx < rhs.idx; }
    inline position& operator++() { ++j; ++idx; return *this; }
    inline difference_type operator-(const position& rhs) const { return idx - rhs.idx; }
};

template <typename Itr>
struct position<Itr, typename enable_if<is_same<typename std::iterator_traits<Itr>::iterator_category, std::random_access_iterator_tag> >::type> {
    typedef Itr itr_type;
    typedef typename std::iterator_traits<Itr>::difference_type idx_type;
    typedef typename std::iterator_traits<Itr>::difference_type difference_type;
    typedef typename std::iterator_traits<Itr>::value_type value_type;
    typedef typename std::iterator_traits<Itr>::reference reference;

    itr_type j;

    inline void beg(const itr_type& src) { j = src; }

    inline reference operator*() { return *j; }
    inline bool operator==(const itr_type& rhs) const { return j == rhs; }
    inline bool operator!=(const itr_type& rhs) const { return j != rhs; }
    inline bool operator<(const position& rhs) const { return j < rhs.j; }
    inline position& operator++() { ++j; return *this; }
    inline difference_type operator-(const position& rhs) const { return j - rhs.j; }
};

template <typename Itr1, typename Itr2, typename Cost>
struct path_head {
    typedef position<Itr1> pos1_type;
    typedef position<Itr2> pos2_type;

    pos1_type pos1;
    pos2_type pos2;
    Cost cost;
};

template <typename Itr1, typename Itr2, typename Cost>
struct path_node : public path_head<Itr1, Itr2, Cost> {
    struct path_node* edge;
};

template <typename Pool, typename Visited, typename Pos1, typename Pos2, typename Cost>
inline
path_head<typename Pos1::itr_type, typename Pos2::itr_type, Cost>*
construct(Pool& pool, Visited& visited, const Pos1& pos1_, const Pos2& pos2_, const Cost& cost_) {
    typedef path_head<typename Pos1::itr_type, typename Pos2::itr_type, Cost> head_t;
    head_t w;
    w.pos1 = pos1_;
    w.pos2 = pos2_;
    typename Visited::iterator f(visited.find(&w));
    if (visited.end() != f   &&   cost_ >= (*f)->cost) return static_cast<head_t*>(NULL);
    head_t* r = pool.construct();
    r->pos1 = pos1_;
    r->pos2 = pos2_;
    r->cost = cost_;
    if (visited.end() != f) visited.erase(f);
    visited.insert(r);
    return r;
}

template <typename Pool, typename Visited, typename Pos1, typename Pos2, typename Cost>
inline
path_node<typename Pos1::itr_type, typename Pos2::itr_type, Cost>*
construct(Pool& pool, Visited& visited, const Pos1& pos1_, const Pos2& pos2_, const Cost& cost_, path_node<typename Pos1::itr_type, typename Pos2::itr_type, Cost>* const& edge_) {
    typedef path_node<typename Pos1::itr_type, typename Pos2::itr_type, Cost> head_t;
    head_t w;
    w.pos1 = pos1_;
    w.pos2 = pos2_;
    typename Visited::iterator f(visited.find(&w));
    if (visited.end() != f   &&   cost_ >= (*f)->cost) return static_cast<head_t*>(NULL);
    head_t* r = pool.construct();
    r->pos1 = pos1_;
    r->pos2 = pos2_;
    r->cost = cost_;
    r->edge = edge_;
    if (visited.end() != f) visited.erase(f);
    visited.insert(r);
    return r;
}

template <typename Pos1, typename Pos2>
struct heap_lessthan {
    Pos1 beg1;
    Pos2 beg2;
    heap_lessthan(const Pos1& pos1_, const Pos2& pos2_) : beg1(pos1_), beg2(pos2_) {}
    template <typename T> inline bool operator()(T const* a, T const* b) const {
        return a->cost > b->cost;
    }
};

template <typename Pos1, typename Pos2>
struct visited_hash {
    Pos1 beg1;
    Pos2 beg2;
    visited_hash(const Pos1& pos1_, const Pos2& pos2_) : beg1(pos1_), beg2(pos2_) {}
    template<typename T> inline
    size_t operator()(T const* e) const {
        size_t h = 0;
        boost::hash_combine(h, e->pos1-beg1);
        boost::hash_combine(h, e->pos2-beg2);
        return h;
    }
};

struct visited_equal {
    template<typename T> inline
    bool operator()(T const* a, T const* b) const {
        return (a->pos1.j == b->pos1.j) && (a->pos2.j == b->pos2.j);
    }
};

template <typename X>
struct ForwardRangeConvertible {
    BOOST_CONCEPT_USAGE(ForwardRangeConvertible) {
        // all I really want to capture here is that any sequence argument to edit_distance()
        // and friends can be treated as a ForwardRange -- currently I'm doing this by
        // applying as_literal() to all incoming arguments, which seems to allow me to send in
        // null-terminated strings, ranges, sequence containers, etc, which is what I want.
        boost::as_literal(x);
    }
    X x;
};

// I'm a little surprised this doesn't exist already
template <typename X>
struct Arithmetic {
    BOOST_CONCEPT_USAGE(Arithmetic) {
        BOOST_STATIC_ASSERT(boost::is_arithmetic<X>::value);
    }
    X x;
};

BOOST_TTI_HAS_TYPE(cost_type)

template <typename X, typename V, typename Enable=void>
struct cost_type {
    static X x;
    static V v;
    // by default, we infer cost_type from the return value of cost functions
    typedef BOOST_TYPEOF(x.insertion(v)) type;
};

template <typename X, typename V>
struct cost_type<X, V, typename enable_if<has_type_cost_type<X> >::type> {
    // if the class explicitly defines a cost_type, then that is what we use
    typedef typename X::cost_type type;
};

template <typename X, typename Sequence, typename Enabled=void> struct TestCostSub {};

template <typename X, typename Sequence>
struct TestCostSub<X, Sequence, typename enable_if<invoke<BOOST_TYPEOF_TPL(&X::substitution)> >::type> {
    typedef typename boost::range_value<Sequence>::type value_type;
    typedef typename cost_type<X, value_type>::type cost_type;
    BOOST_CONCEPT_USAGE(TestCostSub) {
        c = x.substitution(v,v);
    }

    X x;
    cost_type c;
    value_type v;
};

template <typename X, typename Sequence> struct SequenceAlignmentCost {
    BOOST_CONCEPT_ASSERT((ForwardRangeConvertible<Sequence>));

    typedef typename boost::range_value<Sequence>::type value_type;
    typedef typename cost_type<X, value_type>::type cost_type;
    BOOST_CONCEPT_ASSERT((Arithmetic<cost_type>));

    BOOST_CONCEPT_USAGE(SequenceAlignmentCost) {
        c = x.insertion(v);
        c = x.deletion(v);
    }

    // test x.substitution() method, if it is defined
    BOOST_CONCEPT_ASSERT((TestCostSub<X, Sequence>));

    X x;
    cost_type c;
    value_type v;
};

}}}}

#endif


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

#ifndef BOOST_UNITS_CODATA_ELECTRON_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_ELECTRON_CONSTANTS_HPP

#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/amount.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/magnetic_flux_density.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/wavenumber.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

/// \file
/// CODATA recommended values of fundamental atomic and nuclear constants
/// CODATA 2006 values as of 2007/03/30

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

/// CODATA recommended values of the fundamental physical constants: NIST SP 961

/// electron mass
BOOST_UNITS_PHYSICAL_CONSTANT(m_e,quantity<mass>,9.10938215e-31*kilograms,4.5e-38*kilograms);
/// electron-muon mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_mu,quantity<dimensionless>,4.83633171e-3*dimensionless(),1.2e-10*dimensionless());
/// electron-tau mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_tau,quantity<dimensionless>,2.87564e-4*dimensionless(),4.7e-8*dimensionless());
/// electron-proton mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_p,quantity<dimensionless>,5.4461702177e-4*dimensionless(),2.4e-13*dimensionless());
/// electron-neutron mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_n,quantity<dimensionless>,5.4386734459e-4*dimensionless(),3.3e-13*dimensionless());
/// electron-deuteron mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_d,quantity<dimensionless>,2.7244371093e-4*dimensionless(),1.2e-13*dimensionless());
/// electron-alpha particle mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(m_e_over_m_alpha,quantity<dimensionless>,1.37093355570e-4*dimensionless(),5.8e-14*dimensionless());
/// electron charge to mass ratio
BOOST_UNITS_PHYSICAL_CONSTANT(e_over_m_e,quantity<electric_charge_over_mass>,1.758820150e11*coulombs/kilogram,4.4e3*coulombs/kilogram);
/// electron molar mass
BOOST_UNITS_PHYSICAL_CONSTANT(M_e,quantity<mass_over_amount>,5.4857990943e-7*kilograms/mole,2.3e-16*kilograms/mole);
/// Compton wavelength
BOOST_UNITS_PHYSICAL_CONSTANT(lambda_C,quantity<length>,2.4263102175e-12*meters,3.3e-21*meters);
/// classical electron radius
BOOST_UNITS_PHYSICAL_CONSTANT(r_e,quantity<length>,2.8179402894e-15*meters,5.8e-24*meters);
/// Thompson cross section
BOOST_UNITS_PHYSICAL_CONSTANT(sigma_e,quantity<area>,0.6652458558e-28*square_meters,2.7e-37*square_meters);
/// electron magnetic moment
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e,quantity<energy_over_magnetic_flux_density>,-928.476377e-26*joules/tesla,2.3e-31*joules/tesla);
/// electron-Bohr magenton moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_B,quantity<dimensionless>,-1.00115965218111*dimensionless(),7.4e-13*dimensionless());
/// electron-nuclear magneton moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_N,quantity<dimensionless>,-183.28197092*dimensionless(),8.0e-7*dimensionless());
/// electron magnetic moment anomaly
BOOST_UNITS_PHYSICAL_CONSTANT(a_e,quantity<dimensionless>,1.15965218111e-3*dimensionless(),7.4e-13*dimensionless());
/// electron g-factor
BOOST_UNITS_PHYSICAL_CONSTANT(g_e,quantity<dimensionless>,-2.0023193043622*dimensionless(),1.5e-12*dimensionless());
/// electron-muon magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_mu,quantity<dimensionless>,206.7669877*dimensionless(),5.2e-6*dimensionless());
/// electron-proton magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_p,quantity<dimensionless>,-658.2106848*dimensionless(),5.4e-6*dimensionless());
/// electron-shielded proton magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_p_prime,quantity<dimensionless>,-658.2275971*dimensionless(),7.2e-6*dimensionless());
/// electron-neutron magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_n,quantity<dimensionless>,960.92050*dimensionless(),2.3e-4*dimensionless());
/// electron-deuteron magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_d,quantity<dimensionless>,-2143.923498*dimensionless(),1.8e-5*dimensionless());
/// electron-shielded helion magnetic moment ratio
BOOST_UNITS_PHYSICAL_CONSTANT(mu_e_over_mu_h_prime,quantity<dimensionless>,864.058257*dimensionless(),1.0e-5*dimensionless());
/// electron gyromagnetic ratio
BOOST_UNITS_PHYSICAL_CONSTANT(gamma_e,quantity<frequency_over_magnetic_flux_density>,1.760859770e11/second/tesla,4.4e3/second/tesla);

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_ELECTRON_CONSTANTS_HPP


#ifndef FAST_GICP_FAST_VGICP_HPP
#define FAST_GICP_FAST_VGICP_HPP

#include <unordered_map>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/registration/registration.h>

#include <fast_gicp/gicp/gicp_settings.hpp>
#include <fast_gicp/gicp/fast_gicp.hpp>
#include <fast_gicp/gicp/fast_vgicp_voxel.hpp>

namespace fast_gicp {

/**
 * @brief Fast Voxelized GICP algorithm boosted with OpenMP
 */
template<typename PointSource, typename PointTarget>
class FastVGICP : public FastGICP<PointSource, PointTarget> {
public:
  using Scalar = float;
  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;

  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
  using Ptr = pcl::shared_ptr<FastVGICP<PointSource, PointTarget>>;
  using ConstPtr = pcl::shared_ptr<const FastVGICP<PointSource, PointTarget>>;
#else
  using Ptr = boost::shared_ptr<FastVGICP<PointSource, PointTarget>>;
  using ConstPtr = boost::shared_ptr<const FastVGICP<PointSource, PointTarget>>;
#endif

protected:
  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;

  using FastGICP<PointSource, PointTarget>::num_threads_;
  using FastGICP<PointSource, PointTarget>::source_kdtree_;
  using FastGICP<PointSource, PointTarget>::target_kdtree_;
  using FastGICP<PointSource, PointTarget>::source_covs_;
  using FastGICP<PointSource, PointTarget>::target_covs_;

public:
  FastVGICP();
  virtual ~FastVGICP() override;

  void setResolution(double resolution);
  void setVoxelAccumulationMode(VoxelAccumulationMode mode);
  void setNeighborSearchMethod(NeighborSearchMethod method);

  virtual void swapSourceAndTarget() override;
  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;

protected:
  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
  virtual void update_correspondences(const Eigen::Isometry3d& trans) override;
  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) override;
  virtual double compute_error(const Eigen::Isometry3d& trans) override;

protected:
  double voxel_resolution_;
  NeighborSearchMethod search_method_;
  VoxelAccumulationMode voxel_mode_;

  std::unique_ptr<GaussianVoxelMap<PointTarget>> voxelmap_;

  std::vector<std::pair<int, GaussianVoxel::Ptr>> voxel_correspondences_;
  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> voxel_mahalanobis_;
};
}  // namespace fast_gicp

#endif


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2014 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2013 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_VMX_ALTIVEC_TRUNC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_VMX_ALTIVEC_TRUNC_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/arithmetic/functions/trunc.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( trunc_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_< single_<A0>
                                            , boost::simd::tag::vmx_
                                            >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return vec_trunc( a0() );
    }
  };
} } }

#endif
#endif


//
// GraphTools library
// Copyright 2017-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Roman Petrovski <RPetrovski@illumina.com>
//
// 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.
//

#pragma once

#include <algorithm>
#include <iostream>
#include <string>

#include <boost/algorithm/string/join.hpp>
#include <boost/assert.hpp>

#include "dagAligner/AffineAlignMatrix.hh"
#include "dagAligner/AffineAlignMatrixVectorized.hh"
#include "dagAligner/PenaltyMatrix.hh"

namespace graphalign
{

namespace dagAligner
{
    /**
     * Performs global alignment of query against DAG of target nodes.
     * \param clipFront true instructs to represent insertions at the start of CIGAR as soft clips
     */
    template <typename AlignMatrix, bool clipFront = true> class Aligner
    {
        AlignMatrix alignMatrix_;

        // max number of best paths to backtrack
        const std::size_t maxRepeats_;

    public:
        Aligner(
            const typename AlignMatrix::PenaltyMatrix& penaltyMatrix, Score gapOpen, Score gapExt,
            std::size_t maxRepeats = 10)
            : alignMatrix_(penaltyMatrix, gapOpen, gapExt)
            , maxRepeats_(maxRepeats)
        {
        }

        template <typename QueryIt, typename TargetIt>
        void __attribute((noinline))
        align(QueryIt queryBegin, QueryIt queryEnd, TargetIt targetBegin, TargetIt targetEnd, const EdgeMap& edgeMap)
        {
            alignMatrix_.init(queryBegin, queryEnd, targetBegin, targetEnd, edgeMap);
        }

        struct Step
        {
            Cigar::OpCode operation_;
            int q_;
            int t_;
        };

        static bool removeDuplicateCigars(std::vector<Cigar>& cigars)
        {
            std::sort(cigars.begin(), cigars.end());
            std::vector<Cigar>::iterator uniq = std::unique(cigars.begin(), cigars.end());
            if (cigars.end() != uniq)
            {
                cigars.erase(uniq, cigars.end());
                return true;
            }

            return false;
        }
        template <bool localAlign>
        Score __attribute((noinline))
        backtrackAllPaths(const EdgeMap& edgeMap, std::vector<Cigar>& cigars, Score& secondBestScore) const
        {
            Score bestScore = SCORE_MIN;
            secondBestScore = SCORE_MIN;
            typename AlignMatrix::const_iterator bestCell
                = alignMatrix_.template nextBestAlign<localAlign>(alignMatrix_.alignBegin(), secondBestScore);
            for (; alignMatrix_.alignEnd() != bestCell && bestScore <= secondBestScore;
                 bestCell = alignMatrix_.template nextBestAlign<localAlign>(bestCell + 1, secondBestScore))
            {
                bestScore = secondBestScore;
                const int t = alignMatrix_.targetOffset(bestCell);
                const int q = alignMatrix_.queryOffset(bestCell);
                const int softClip = alignMatrix_.queryLen() - 1 - q;

                std::size_t firstNodeId = edgeMap.getNodeId(t);
                Cigar start;
                start.push_back(Cigar::Operation(Cigar::NODE_START, firstNodeId));
                if (softClip)
                {
                    start.push_back(Cigar::Operation(Cigar::SOFT_CLIP, softClip));
                }

                if (!backtrackPath<true>(edgeMap, start, firstNodeId, q, t, cigars))
                {
                    // ran out of cigars buffer
                    return bestScore;
                }
            }

            removeDuplicateCigars(cigars);
            if (1 < cigars.size())
            {
                // at least one duplicate, reset secondBestScore
                secondBestScore = bestScore;
            }
            else if (alignMatrix_.alignEnd() == bestCell)
            {
                // one candidate only, no second best. Might had some duplicates, reset secondBestScore
                secondBestScore = SCORE_MIN;
            }
            // else scondBest is set properly
            return bestScore;
        }

        template <bool localAlign>
        Cigar __attribute((noinline))
        backtrackBestPath(const EdgeMap& edgeMap, Score& bestScore, Score& secondBestScore) const
        {
            bestScore = SCORE_MIN;
            secondBestScore = SCORE_MIN;
            typename AlignMatrix::const_iterator bestCell
                = alignMatrix_.template nextBestAlign<localAlign>(alignMatrix_.alignBegin(), bestScore);
            if (alignMatrix_.alignEnd() == bestCell)
            {
                throw std::logic_error("No best path available");
            }
            std::vector<Cigar> ret;
            const int t = alignMatrix_.targetOffset(bestCell);
            const int q = alignMatrix_.queryOffset(bestCell);
            const int softClip = alignMatrix_.queryLen() - 1 - q;

            std::size_t firstNodeId = edgeMap.getNodeId(t);
            Cigar start;
            start.push_back(Cigar::Operation(Cigar::NODE_START, firstNodeId));
            if (softClip)
            {
                start.push_back(Cigar::Operation(Cigar::SOFT_CLIP, softClip));
            }

            backtrackPath<false>(edgeMap, start, firstNodeId, q, t, ret);
            alignMatrix_.template nextBestAlign<localAlign>(bestCell + 1, secondBestScore);
            return ret.front();
        }

    private:
        template <bool exploreAllPaths>
        Step __attribute((noinline)) stepBack(
            const EdgeMap& edgeMap, const Cigar& base, std::size_t lastNodeId, int q, int t,
            std::vector<Cigar>& cigars) const
        {
            Step ret = { Cigar::UNKNOWN, q, t };

            if (alignMatrix_.isInsertion(q, t))
            {
                const Cigar::OpCode code
                    = (1 == base.length() && Cigar::NODE_START == base.lastOp()) || Cigar::SOFT_CLIP == base.lastOp()
                    ? Cigar::SOFT_CLIP
                    : Cigar::INSERT;
                ret = Step{ code, q - 1, t };
            }

            EdgeMap::OffsetEdges::const_iterator prevNodeIndexIt = edgeMap.prevNodesBegin(t);
            while (prevNodeIndexIt != edgeMap.prevNodesEnd(t))
            {
                const int p = *prevNodeIndexIt;
                if (alignMatrix_.isDeletion(q, t, p))
                {
                    if (Cigar::UNKNOWN != ret.operation_ && exploreAllPaths)
                    {
                        // recurse if more than one path is possible
                        if (!backtrackPath<true>(edgeMap, base + Cigar::DELETE, lastNodeId, q, p, cigars))
                        {
                            return Step{ Cigar::UNKNOWN, q, t };
                        }
                    }
                    else
                    {
                        ret = Step{ Cigar::DELETE, q, p };
                    }
                }

                if (alignMatrix_.isMatch(q, t, p))
                {
                    if (Cigar::UNKNOWN != ret.operation_ && exploreAllPaths)
                    {
                        // recurse if more than one path is possible
                        if (!backtrackPath<true>(edgeMap, base + Cigar::MATCH, lastNodeId, q - 1, p, cigars))
                        {
                            return Step{ Cigar::UNKNOWN, q, t };
                        }
                    }
                    else
                    {
                        ret = Step{ Cigar::MATCH, q - 1, p };
                    }
                }
                else if (alignMatrix_.isMismatch(q, t, p))
                {
                    const Cigar::OpCode code = (1 == base.length() && Cigar::NODE_START == base.lastOp())
                            || Cigar::SOFT_CLIP == base.lastOp()
                        ? Cigar::SOFT_CLIP
                        : Cigar::MISMATCH;
                    if (Cigar::UNKNOWN != ret.operation_ && exploreAllPaths)
                    {
                        // recurse if more than one path is possible
                        if (!backtrackPath<true>(edgeMap, base + code, lastNodeId, q - 1, p, cigars))
                        {
                            return Step{ Cigar::UNKNOWN, q, t };
                        }
                    }
                    else
                    {
                        ret = Step{ code, q - 1, p };
                    }
                }

                ++prevNodeIndexIt;
            }

            if (Cigar::UNKNOWN == ret.operation_)
            {
                throw std::logic_error("backtracking failure: no nodes found on best path!");
            }

            // Return the one we have not recursed for. If only one path is possible, no recursion occurs
            return ret;
        }

        template <bool exploreAllPaths>
        bool backtrackPath(
            const EdgeMap& edgeMap, const Cigar& base, std::size_t lastNodeId, int q, int t,
            std::vector<Cigar>& cigars) const
        {
            Cigar ret = base;
            while (-1 != t)
            {
                const std::size_t curNodeId = edgeMap.getNodeId(t);
                if (lastNodeId != curNodeId)
                {
                    ret.push_back(Cigar::Operation(Cigar::NODE_END, lastNodeId));
                    ret.push_back(Cigar::Operation(Cigar::NODE_START, curNodeId));
                    lastNodeId = curNodeId;
                }

                Step step = stepBack<exploreAllPaths>(edgeMap, ret, lastNodeId, q, t, cigars);
                if (Cigar::UNKNOWN == step.operation_)
                {
                    // ran out of cigars buffer
                    return false;
                }

                ret.append(step.operation_);
                q = step.q_;
                t = step.t_;
            }

            if (-1 != q)
            {
                if (Cigar::DELETE == ret.lastOp())
                {
                    const Cigar::Operation del = ret.pop_back();
                    ret.push_back(Cigar::Operation(Cigar::INSERT, q + 1));
                    ret.push_back(del);
                }
                else
                {
                    ret.push_back(Cigar::Operation(Cigar::INSERT, q + 1));
                }
            }
            // TODO: according to while loop above, this cannot happen. Code below is legacy for
            // the original termination condition of while loop
            else if (-1 != t) // we're either on -1 row or -1 column
            {
                // count number of bases to the start of the last node
                const std::size_t nodeId = edgeMap.getNodeId(t + 1);
                int cur = t + 1;
                while (cur && edgeMap.getNodeId(cur - 1) == nodeId)
                {
                    --cur;
                }
                if (t + 1 != cur)
                {
                    ret.push_back(Cigar::Operation(Cigar::DELETE, t + 1 - cur));
                }
            }

            if (clipFront && Cigar::INSERT == ret.lastOp())
            {
                ret.lastOp() = Cigar::SOFT_CLIP;
            }
            ret.push_back(Cigar::Operation(Cigar::NODE_END, lastNodeId));

            ret.collapseLastEmptyNode();
            ret.reverse();
            if (cigars.size() == maxRepeats_ && !removeDuplicateCigars(cigars))
            {
                return false;
            }
            cigars.push_back(ret);
            return true;
        }

        friend std::ostream& operator<<(std::ostream& os, const Aligner& aligner)
        {
            return os << "Aligner(" << aligner.alignMatrix_ << ")";
        }
    };

} // namespace dagAligner

// template <bool penalizeMove>
// class DagAligner
//     : public dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FixedPenaltyMatrix, penalizeMove>>
// {
// public:
// DagAligner(const dagAligner::FixedPenaltyMatrix& penaltyMatrix, dagAligner::Score gapOpen, dagAligner::Score gapExt)
// : dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FixedPenaltyMatrix, penalizeMove>>(
// penaltyMatrix, gapOpen, gapExt)
// {
// }
//
// DagAligner(dagAligner::Score match, dagAligner::Score mismatch, dagAligner::Score gapOpen, dagAligner::Score gapExt)
// : dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FixedPenaltyMatrix, penalizeMove>>(
// dagAligner::FixedPenaltyMatrix(match, mismatch), gapOpen, gapExt)
// {
// }
// };

template <bool penalizeMove, bool clipFront = true, bool matchQueryN = true, bool matchTargetN = true>
class DagAligner : public dagAligner::Aligner<
                       dagAligner::AffineAlignMatrixVectorized<
                           dagAligner::FixedPenaltyMatrix<matchQueryN, matchTargetN>, penalizeMove>,
                       clipFront>
{
    typedef dagAligner::FixedPenaltyMatrix<matchQueryN, matchTargetN> PenaltyMatrix;

public:
    DagAligner(const PenaltyMatrix& penaltyMatrix, dagAligner::Score gapOpen, dagAligner::Score gapExt)
        : dagAligner::Aligner<dagAligner::AffineAlignMatrixVectorized<PenaltyMatrix, penalizeMove>, clipFront>(
              penaltyMatrix, gapOpen, gapExt)
    {
    }

    DagAligner(dagAligner::Score match, dagAligner::Score mismatch, dagAligner::Score gapOpen, dagAligner::Score gapExt)
        : dagAligner::Aligner<dagAligner::AffineAlignMatrixVectorized<PenaltyMatrix, penalizeMove>, clipFront>(
              PenaltyMatrix(match, mismatch), gapOpen, gapExt)
    {
    }
};

// template <bool penalizeMove>
// class DagAligner
//     : public dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FreePenaltyMatrix, penalizeMove>>
// {
// public:
//    DagAligner(const dagAligner::FreePenaltyMatrix& penaltyMatrix, dagAligner::Score gapOpen, dagAligner::Score
//    gapExt)
//        : dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FreePenaltyMatrix, penalizeMove>>(
//              penaltyMatrix, gapOpen, gapExt)
//    {
//    }
//
//    DagAligner(dagAligner::Score match, dagAligner::Score mismatch, dagAligner::Score gapOpen, dagAligner::Score
//    gapExt)
//        : dagAligner::Aligner<dagAligner::AffineAlignMatrix<dagAligner::FreePenaltyMatrix, penalizeMove>>(
//              dagAligner::FreePenaltyMatrix(match, mismatch), gapOpen, gapExt)
//    {
//    }
// };

} // namespace graphalign


#pragma once
#include <EigenLinearSolvers/EigenDirectSparseSolver[CRS].h>

#include <sofa/helper/ScopedAdvancedTimer.h>

namespace EigenLinearSolvers
{
template <class TBlockType, class EigenSolver>
void EigenDirectSparseSolver<
    sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>,
    sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver>
    ::init()
{
    updateSolverOderingMethod();
}

template <class TBlockType, class EigenSolver>
void EigenDirectSparseSolver<
    sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>,
    sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver>
    ::reinit()
{
    updateSolverOderingMethod();
}

template <class TBlockType, class EigenSolver>
void EigenDirectSparseSolver<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver >
    ::solve(Matrix& A, Vector& x, Vector& b)
{
    SOFA_UNUSED(A);

    EigenVectorXdMap xMap(x.ptr(), x.size());
    EigenVectorXdMap bMap(b.ptr(), b.size());

    std::visit([&bMap, &xMap](auto&& solver)
    {
        xMap = solver.solve(bMap);
    }, m_solver);
}

template <class TBlockType, class EigenSolver>
void EigenDirectSparseSolver<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver >
    ::invert(Matrix& A)
{
    {
        sofa::helper::ScopedAdvancedTimer copyTimer("copyMatrixData");
        Mfiltered.copyNonZeros(A);
        Mfiltered.compress();
    }

    if (!m_map)
    {
        m_map = std::make_unique<EigenSparseMatrixMap>(Mfiltered.rows(), Mfiltered.cols(), Mfiltered.getColsValue().size(),
            (typename EigenSparseMatrixMap::StorageIndex*)Mfiltered.rowBegin.data(), (typename EigenSparseMatrixMap::StorageIndex*)Mfiltered.colsIndex.data(), Mfiltered.colsValue.data());
    }

    const bool analyzePattern = (MfilteredrowBegin != Mfiltered.rowBegin) || (MfilteredcolsIndex != Mfiltered.colsIndex);

    if (analyzePattern)
    {
        sofa::helper::ScopedAdvancedTimer patternAnalysisTimer("patternAnalysis");
        std::visit([this](auto&& solver)
        {
            solver.analyzePattern(*m_map);
        }, m_solver);

        MfilteredrowBegin = Mfiltered.rowBegin;
        MfilteredcolsIndex = Mfiltered.colsIndex;
    }

    {
        sofa::helper::ScopedAdvancedTimer factorizeTimer("factorization");
        std::visit([this](auto&& solver)
        {
            solver.factorize(*m_map);
        }, m_solver);
    }

    msg_error_when(getSolverInfo() == Eigen::ComputationInfo::InvalidInput) << "Solver cannot factorize: invalid input";
    msg_error_when(getSolverInfo() == Eigen::ComputationInfo::NoConvergence) << "Solver cannot factorize: no convergence";
    msg_error_when(getSolverInfo() == Eigen::ComputationInfo::NumericalIssue) << "Solver cannot factorize: numerical issue";
}

template <class TBlockType, class EigenSolver>
Eigen::ComputationInfo EigenDirectSparseSolver<
    sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>,
    sofa::linearalgebra::FullVector<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver>
::getSolverInfo() const
{
    Eigen::ComputationInfo info;
    std::visit([&info](auto&& solver)
    {
        info = solver.info();
    }, m_solver);
    return info;
}

template <class TBlockType, class EigenSolver>
void EigenDirectSparseSolver<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector
<typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver>::updateSolverOderingMethod()
{
    if (m_selectedOrderingMethod != d_orderingMethod.getValue().getSelectedId())
    {
        switch(d_orderingMethod.getValue().getSelectedId())
        {
        case 0:  m_solver.template emplace<std::variant_alternative_t<0, decltype(m_solver)> >(); break;
        case 1:  m_solver.template emplace<std::variant_alternative_t<1, decltype(m_solver)> >(); break;
        case 2:  m_solver.template emplace<std::variant_alternative_t<2, decltype(m_solver)> >(); break;
#if EIGENLINEARSOLVERS_HAS_METIS_INCLUDE == 1
        case 3:  m_solver.template emplace<std::variant_alternative_t<3, decltype(m_solver)> >(); break;
#endif
        default: m_solver.template emplace<std::variant_alternative_t<1, decltype(m_solver)> >(); break;
        }
        m_selectedOrderingMethod = d_orderingMethod.getValue().getSelectedId();
        if (m_selectedOrderingMethod >= std::variant_size_v<decltype(m_solver)>)
            m_selectedOrderingMethod = 1;

        MfilteredrowBegin.clear();
        MfilteredcolsIndex.clear();
        m_map.reset();
    }
}

template <class TBlockType, class EigenSolver>
EigenDirectSparseSolver<sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>, sofa::linearalgebra::FullVector<
typename sofa::linearalgebra::CompressedRowSparseMatrix<TBlockType>::Real>, EigenSolver>::EigenDirectSparseSolver()
    : Inherit1()
    , d_orderingMethod(initData(&d_orderingMethod, "ordering", "Ordering method"))
{
#if EIGENLINEARSOLVERS_HAS_METIS_INCLUDE == 1
    sofa::helper::OptionsGroup d_orderingMethodOptions(4,"Natural", "AMD", "COLAMD", "Metis");
#else
    sofa::helper::OptionsGroup d_orderingMethodOptions(3,"Natural", "AMD", "COLAMD");
#endif

    d_orderingMethodOptions.setSelectedItem(1); // default None
    d_orderingMethod.setValue(d_orderingMethodOptions);
}

}


/**************************************************************************
** Copyright (C) 2020 MS-Cheminformatics LLC
*
** Contact: info@ms-cheminfo.com
**
** Commercial Usage
**
** Licensees holding valid MS-Cheminformatics commercial licenses may use this file in
** accordance with the MS-Cheminformatics Commercial License Agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and MS-Cheminformatics.
**
** GNU Lesser General Public License Usage
**
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.TXT included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
**************************************************************************/

#pragma once

// #include "adportable_global.h"
#include <date/date.h>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/format.hpp>
#include <chrono>
#include <ctime>
#include <iostream>
#include <string>
#include <type_traits>

namespace adportable {

    struct /* ADPORTABLESHARED_EXPORT */ subseconds {
        static std::string to_str( int64_t, size_t n );
    };

    template< bool is_system_clock = true >
    struct date_time_t {
        template< typename duration_t, typename time_point_t >
        std::pair< std::time_t, duration_t > to_time_t( time_point_t tp ) {
            // std::cout << "-- system_clock -->";
            auto utc = std::chrono::system_clock::to_time_t( std::chrono::time_point_cast< std::chrono::system_clock::duration >( tp ) );
            return { utc, duration_t( std::chrono::time_point_cast< duration_t >( tp ) - date::floor< std::chrono::seconds >( tp ) ) };
        }
    };

    template<>
    template< typename duration_t, typename time_point_t >
    std::pair< std::time_t, duration_t >
    date_time_t< false >::to_time_t( time_point_t tp ) {
        typedef typename decltype( tp )::clock clock_t;
        // std::cout << "-- steady_clock -->";
        auto tt = std::chrono::time_point_cast< duration_t >( std::chrono::system_clock::now() ) + ( tp - clock_t::now() );
        auto utc = std::chrono::system_clock::to_time_t( std::chrono::time_point_cast< std::chrono::system_clock::duration >( tt ) );
        return { utc, std::chrono::duration_cast< duration_t >( tt - date::floor< std::chrono::seconds >( tt ) ) };
    }

    template< uint64_t N > struct num_digits {
        enum { value = 1 + num_digits< N / 10 >::value };
    };

    template<> struct num_digits<0> {
        enum { value = 0 };
    };

    struct date_time {

        template< typename duration_t, typename time_point_t >
        static std::string to_iso( time_point_t tp, bool utc_offset = true ) {
            typedef typename decltype( tp )::clock clock_t;
#if __cplusplus < 201703L
            std::time_t utc; duration_t subseconds;
            std::tie( utc, subseconds )
#else
            auto [utc, subseconds]
#endif
                  = date_time_t< std::is_same< clock_t, std::chrono::system_clock >::value >(). template to_time_t< duration_t >( tp );

            // subseconds_t< num_digits< duration_t::period::den >::value - 1 > subseconds_to_string;
            constexpr size_t N = num_digits< duration_t::period::den >::value - 1;

            std::ostringstream o;
            if ( utc_offset ) {
                auto tz( boost::posix_time::second_clock::local_time() - boost::posix_time::second_clock::universal_time() );
                o << std::put_time( std::localtime(&utc), "%FT%T" ) << subseconds::to_str( subseconds.count(), N )
                  << boost::format( "%c%02d%02d" )
                    % (tz.is_negative() ? '-' : '+')
                    % boost::date_time::absolute_value( tz.hours() )
                    % boost::date_time::absolute_value( tz.minutes() );
            } else {
                auto syst = std::chrono::time_point_cast< duration_t >( std::chrono::system_clock::from_time_t( utc ) ) + duration_t( subseconds );
                o << date::format("%FT%TZ", syst );
            }
            return o.str();
        }
    };
}


#include <complex>
#include <exception>
#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <vector>
#include <range/v3/all.hpp>
#include <cstdint>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/interval_set.hpp>
namespace icl = boost::icl;
using gint = std::int64_t;

int main(int argc, char **argv)
{
  if (argc > 1) {
    gint l, h;
    icl::interval_set<gint> ips;
    std::ifstream ifs(argv[1]);
    while (ifs >> l >> h) {
      icl::interval<gint>::type p(l, h);
      ips.add(p);
    }
    std::cout << "A\n";
    int n = 0;
    icl::interval_set<gint> ipsb = ips;
    auto j = ips.begin();
    ++j;
    for (auto i = ips.begin(); i != ips.end() && (j != ips.end()); ++i, ++j) {
      if (j->lower() - i->upper() == 1) {
        icl::interval<gint>::type p(i->upper(), j->lower());
        ipsb.add(p);
      }
    }
    for (auto i = ipsb.begin(); i != ipsb.end(); ++i) {
      std::cout << "i:" << *i << std::endl;
      if (n == 1) {
        break;
      }
    }
  }
  return 0;
}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>,
//         Mitch Bekritsky <mbekritsky@illumina.com>, Richard Shaw
// Concept: Michael Eberle <meberle@illumina.com>
//
// 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.
//
//

#include "genotyping/RepeatGenotyper.hh"

#include <algorithm>
#include <cassert>
#include <iostream>

#include <boost/math/distributions/poisson.hpp>

#include "genotyping/RegionLengthEstimation.hh"
#include "genotyping/ShortRepeatGenotyper.hh"

namespace ehunter
{

using boost::optional;
using std::map;
using std::string;
using std::vector;

static CountTable combineFlankingAndInrepeatReads(
    int maxNumUnitsInRead, const CountTable& flankingCounts, const CountTable& inrepeatCounts)
{
    int maxNumReadsToTransfer = 5;
    CountTable updatedFlankingCounts = flankingCounts;
    for (int numUnits = maxNumUnitsInRead; numUnits != 0; --numUnits)
    {
        const int count = inrepeatCounts.countOf(numUnits);
        const int countToTransfer = std::min(count, maxNumReadsToTransfer);

        for (int counter = 0; counter != countToTransfer; ++counter)
        {
            updatedFlankingCounts.incrementCountOf(numUnits);
        }

        maxNumReadsToTransfer -= countToTransfer;
        if (maxNumReadsToTransfer == 0)
        {
            break;
        }
    }

    return updatedFlankingCounts;
}

optional<RepeatGenotype> RepeatGenotyper::genotypeRepeat(const vector<int32_t>& alleleSizeCandidates) const
{
    if (alleleSizeCandidates.empty())
    {
        return optional<RepeatGenotype>();
    }

    const CountTable countsOfFlankingReadsForShortRepeatGenotyper
        = combineFlankingAndInrepeatReads(maxNumUnitsInRead_, countsOfFlankingReads_, countsOfInrepeatReads_);

    ShortRepeatGenotyper shortRepeatGenotyper(repeatUnitLen_, maxNumUnitsInRead_, propCorrectMolecules_);

    int repeatReadCount
        = ::ehunter::countFullLengthRepeatReads(maxNumUnitsInRead_, countsOfFlankingReads_, countsOfInrepeatReads_);

    const int minInrepeatReadsInLongExpansion = 1;
    if (repeatReadCount >= minInrepeatReadsInLongExpansion)
    {
        repeatReadCount += 2 * countOfInrepeatReadPairs_;
    }

    if (expectedAlleleCount_ == AlleleCount::kOne)
    {
        RepeatGenotype genotype = shortRepeatGenotyper.genotypeRepeatWithOneAllele(
            countsOfFlankingReadsForShortRepeatGenotyper, countsOfSpanningReads_, alleleSizeCandidates);

        const bool isSpanningAllele = countsOfSpanningReads_.countOf(genotype.longAlleleSizeInUnits()) != 0;

        if (!isSpanningAllele && repeatReadCount != 0)
        {
            extendGenotypeWhenOneAlleleIsRepeat(genotype, repeatReadCount);
        }
        else if (!isSpanningAllele)
        {
            extendGenotypeWhenOneAlleleIsFlanking(genotype);
        }
        else
        {
            assert(countsOfSpanningReads_.countOf(genotype.longAlleleSizeInUnits()));
        }

        return genotype;
    }

    assert(expectedAlleleCount_ == AlleleCount::kTwo);

    RepeatGenotype genotype = shortRepeatGenotyper.genotypeRepeatWithTwoAlleles(
        countsOfFlankingReadsForShortRepeatGenotyper, countsOfSpanningReads_, alleleSizeCandidates);

    const bool shortAlleleIsSpanning = countsOfSpanningReads_.countOf(genotype.shortAlleleSizeInUnits()) != 0;
    const bool longAlleleIsSpanning = countsOfSpanningReads_.countOf(genotype.longAlleleSizeInUnits()) != 0;
    // const int repeatReadCount = countFullLengthRepeatReads();

    if (!longAlleleIsSpanning && !shortAlleleIsSpanning && repeatReadCount != 0)
    {
        extendGenotypeWhenBothAllelesAreRepeat(genotype, repeatReadCount);
    }
    else if (!longAlleleIsSpanning && repeatReadCount != 0)
    {
        extendGenotypeWhenOneAlleleIsRepeat(genotype, repeatReadCount);
    }
    else if (shortAlleleIsSpanning && longAlleleIsSpanning)
    {
        // Nothing needs to be done.
    }
    else if (shortAlleleIsSpanning)
    {
        // assert(countsOfFlankingReads_.countOf(genotype.longAlleleSizeInUnits()));
        extendGenotypeWhenOneAlleleIsFlanking(genotype);
    }
    else
    {
        // Both alleles must be flanking.
        // assert(countsOfFlankingReads_.countOf(genotype.shortAlleleSizeInUnits()));
        // assert(countsOfFlankingReads_.countOf(genotype.longAlleleSizeInUnits()));

        extendGenotypeWhenBothAllelesAreFlanking(genotype);
    }

    return genotype;
}

void RepeatGenotyper::extendGenotypeWhenBothAllelesAreFlanking(RepeatGenotype& genotype) const
{
    // In some exceptional situations flanking reads might be filtered out before the extension step; if this happens,
    // genotype extension is aborted.
    if (countFlankingReadsLongerThanSpanning() == 0)
    {
        return;
    }

    int32_t flankingAlleleSize, flankingAlleleCiLower, flankingAlleleCiUpper;
    estimateFlankingAlleleSize(flankingAlleleSize, flankingAlleleCiLower, flankingAlleleCiUpper);

    // genotype.setLongAlleleSizeInUnits(flankingAlleleSize);
    flankingAlleleCiLower = std::min(genotype.shortAlleleSizeInUnits(), flankingAlleleCiLower);
    flankingAlleleCiUpper = std::max(genotype.longAlleleSizeInUnits(), flankingAlleleCiUpper);

    genotype.setLongAlleleSizeInUnitsCi(flankingAlleleCiLower, flankingAlleleCiUpper);

    // genotype.setShortAlleleSizeInUnits(flankingAlleleSize);
    genotype.setShortAlleleSizeInUnitsCi(flankingAlleleCiLower, flankingAlleleCiUpper);
}

void RepeatGenotyper::extendGenotypeWhenOneAlleleIsFlanking(RepeatGenotype& genotype) const
{
    // In some exceptional situations flanking reads might be filtered out before the extension step; if this happens,
    // genotype extension is aborted.
    if (countFlankingReadsLongerThanSpanning() == 0)
    {
        return;
    }

    int32_t flankingAlleleSize, flankingAlleleCiLower, flankingAlleleCiUpper;
    estimateFlankingAlleleSize(flankingAlleleSize, flankingAlleleCiLower, flankingAlleleCiUpper);

    // genotype.setLongAlleleSizeInUnits(flankingAlleleSize);
    flankingAlleleCiLower = std::min(genotype.shortAlleleSizeInUnits(), flankingAlleleCiLower);
    flankingAlleleCiUpper = std::max(genotype.longAlleleSizeInUnits(), flankingAlleleCiUpper);

    genotype.setLongAlleleSizeInUnitsCi(flankingAlleleCiLower, flankingAlleleCiUpper);
}

void RepeatGenotyper::extendGenotypeWhenOneAlleleIsRepeat(RepeatGenotype& genotype, int numRepeatReads) const
{
    assert(numRepeatReads);

    int32_t longAlleleSize, longAlleleSizeLowerBound, longAlleleSizeUpperBound;
    estimateRepeatAlleleSize(numRepeatReads, longAlleleSize, longAlleleSizeLowerBound, longAlleleSizeUpperBound);
    genotype.setLongAlleleSizeInUnits(longAlleleSize);
    genotype.setLongAlleleSizeInUnitsCi(longAlleleSizeLowerBound, longAlleleSizeUpperBound);
}

void RepeatGenotyper::extendGenotypeWhenBothAllelesAreRepeat(RepeatGenotype& genotype, int numRepeatReads) const
{
    assert(numRepeatReads);

    int32_t allIrrSize, allIrrSizeLowerBound, allIrrlongSizeUpperBound;
    estimateRepeatAlleleSize(numRepeatReads, allIrrSize, allIrrSizeLowerBound, allIrrlongSizeUpperBound);

    int32_t halfIrrSize, halfIrrSizeLowerBound, halfIrrSizeUpperBound;
    estimateRepeatAlleleSize(numRepeatReads / 2, halfIrrSize, halfIrrSizeLowerBound, halfIrrSizeUpperBound);

    const int longAlleleSizeLowerBound = halfIrrSizeLowerBound;
    const int longAlleleSizeUpperBound = allIrrlongSizeUpperBound;
    const int longAlleleSize = (longAlleleSizeLowerBound + longAlleleSizeUpperBound) / 2;

    genotype.setLongAlleleSizeInUnits(longAlleleSize);
    genotype.setLongAlleleSizeInUnitsCi(longAlleleSizeLowerBound, longAlleleSizeUpperBound);

    const int shortAlleleSizeLowerBound = maxNumUnitsInRead_;
    const int shortAlleleSizeUpperBound = halfIrrSizeUpperBound;
    const int shortAlleleSize = (shortAlleleSizeLowerBound + shortAlleleSizeUpperBound) / 2;

    genotype.setShortAlleleSizeInUnits(shortAlleleSize);
    genotype.setShortAlleleSizeInUnitsCi(shortAlleleSizeLowerBound, shortAlleleSizeUpperBound);
}

void RepeatGenotyper::estimateRepeatAlleleSize(
    int32_t numIrrs, int32_t& size, int32_t& sizeLowerBound, int32_t& sizeUpperBound) const
{
    const int32_t readLength = repeatUnitLen_ * maxNumUnitsInRead_;
    estimateRegionLength(numIrrs, readLength, haplotypeDepth_, size, sizeLowerBound, sizeUpperBound);

    size /= repeatUnitLen_;
    sizeLowerBound /= repeatUnitLen_;
    sizeUpperBound /= repeatUnitLen_;
}

void RepeatGenotyper::estimateFlankingAlleleSize(
    int32_t& flankingAlleleSize, int32_t& flankingAlleleCiLower, int32_t& flankingAlleleCiUpper) const
{
    const int32_t readLength = repeatUnitLen_ * maxNumUnitsInRead_;

    const int longestSpanning = calculateLongestSpanning();
    const int numFlankingReadsLongerThanSpanning = countFlankingReadsLongerThanSpanning();

    // Haplotype depth should be twice as high because flanking reads come from both flanks of the repeat.
    estimateRegionLength(
        numFlankingReadsLongerThanSpanning, readLength, 2 * haplotypeDepth_, flankingAlleleSize, flankingAlleleCiLower,
        flankingAlleleCiUpper);

    // estimateRegionLength adds read length to size estimates so we need to subtract it out.
    flankingAlleleSize -= readLength;
    flankingAlleleCiLower -= readLength;
    flankingAlleleCiUpper -= readLength;

    flankingAlleleSize = flankingAlleleSize / repeatUnitLen_ + longestSpanning + 1;
    flankingAlleleCiLower = flankingAlleleCiLower / repeatUnitLen_ + longestSpanning + 1;
    flankingAlleleCiUpper = flankingAlleleCiUpper / repeatUnitLen_ + longestSpanning + 1;

    // Repeat must be at least at long as the longest flanking read.
    const vector<int32_t>& flankingSizes = countsOfFlankingReads_.getElementsWithNonzeroCounts();
    const int32_t longestFlanking = *std::max_element(flankingSizes.begin(), flankingSizes.end());

    flankingAlleleCiLower = std::max(flankingAlleleCiLower, longestFlanking);
    flankingAlleleSize = std::max(flankingAlleleSize, longestFlanking);
    flankingAlleleCiUpper = std::max(flankingAlleleCiUpper, longestFlanking);

    // Repeat estimated from flanking reads cannot be longer than the read length.
    flankingAlleleCiLower = std::min(flankingAlleleCiLower, maxNumUnitsInRead_);
    flankingAlleleSize = std::min(flankingAlleleSize, maxNumUnitsInRead_);
    flankingAlleleCiUpper = std::min(flankingAlleleCiUpper, maxNumUnitsInRead_);
}

int RepeatGenotyper::countFullLengthRepeatReads() const
{
    const double kMinProportionAlignedBases = 0.93;
    const int fullLengthSizeCutoff = static_cast<int>(std::round(maxNumUnitsInRead_ * kMinProportionAlignedBases));

    int repeatReadCount = 0;
    for (int numUnits : countsOfInrepeatReads_.getElementsWithNonzeroCounts())
    {
        if (numUnits >= fullLengthSizeCutoff)
        {
            repeatReadCount += countsOfInrepeatReads_.countOf(numUnits);
        }
    }

    return repeatReadCount;
}

int RepeatGenotyper::calculateLongestSpanning() const
{
    const vector<int32_t>& spanningSizes = countsOfSpanningReads_.getElementsWithNonzeroCounts();
    const int longestSpanning
        = spanningSizes.empty() ? 0 : *std::max_element(spanningSizes.begin(), spanningSizes.end());
    return longestSpanning;
}

int RepeatGenotyper::countFlankingReadsLongerThanSpanning() const
{
    const int longestSpanning = calculateLongestSpanning();
    int32_t numFlankingReadsLongerThanSpanning = 0;
    for (int32_t repeatSize : countsOfFlankingReads_.getElementsWithNonzeroCounts())
    {
        if (repeatSize > longestSpanning)
        {
            numFlankingReadsLongerThanSpanning += countsOfFlankingReads_.countOf(repeatSize);
        }
    }
    return numFlankingReadsLongerThanSpanning;
}

static int depthBasedCountOfInrepeatReads(
    int maxNumUnitsInRead, const CountTable& countsOfFlankingReads, const CountTable& countsOfInrepeatReads)
{
    const int kNumFlanks = 2;
    const double kPropLowConfidenceFlank = 0.1;
    const double kPropHighConfidenceFlank = 1.0 - kPropLowConfidenceFlank;

    const int maxUnitsFromLowConfidenceFlank
        = static_cast<int>(std::round(maxNumUnitsInRead * kPropHighConfidenceFlank));

    int numPutativeIrrs = 0;
    for (const auto& numUnitsSpannedAndCount : countsOfInrepeatReads)
    {
        int numUnitsSpanned = numUnitsSpannedAndCount.first;
        int readCount = numUnitsSpannedAndCount.second;

        if (numUnitsSpanned >= maxUnitsFromLowConfidenceFlank)
        {
            numPutativeIrrs += readCount;
        }
    }

    int numFlankingReads = 0;
    for (const auto& numUnitsSpannedAndCount : countsOfFlankingReads)
    {
        int numUnitsSpanned = numUnitsSpannedAndCount.first;
        int readCount = numUnitsSpannedAndCount.second;

        if (numUnitsSpanned < maxUnitsFromLowConfidenceFlank)
        {
            numFlankingReads += readCount;
        }
    }
    if (numFlankingReads == 0)
        return 0;

    const double estimatedDepth = numFlankingReads / (kNumFlanks * kPropHighConfidenceFlank);
    const double expectedNumLowconfidenceFlankingReads = kNumFlanks * kPropLowConfidenceFlank * estimatedDepth;

    boost::math::poisson_distribution<> lowconfidenceFlankingDistro(expectedNumLowconfidenceFlankingReads);
    const double probability = boost::math::cdf(lowconfidenceFlankingDistro, numPutativeIrrs);

    const double kProbabilityCutoff = 0.95;
    return (probability >= kProbabilityCutoff ? numPutativeIrrs : 0);
}

static int lengthBasedCountOfInrepeatReads(int maxNumUnitsInRead, const CountTable& countsOfInrepeatReads)
{
    const double kPropForFullLength = 0.96;
    const int minNumUnitsForFullLength = static_cast<int>(std::round(maxNumUnitsInRead * kPropForFullLength));

    int numIrrs = 0;
    for (const auto& numUnitsSpannedAndCount : countsOfInrepeatReads)
    {
        int numUnitsSpanned = numUnitsSpannedAndCount.first;
        int readCount = numUnitsSpannedAndCount.second;

        if (numUnitsSpanned >= minNumUnitsForFullLength)
        {
            numIrrs += readCount;
        }
    }

    return numIrrs;
}

int countFullLengthRepeatReads(
    int maxNumUnitsInRead, const CountTable& countsOfFlankingReads, const CountTable& countsOfInrepeatReads)
{
    const int lengthBasedCount = lengthBasedCountOfInrepeatReads(maxNumUnitsInRead, countsOfInrepeatReads);
    const int depthBasedCount
        = depthBasedCountOfInrepeatReads(maxNumUnitsInRead, countsOfFlankingReads, countsOfInrepeatReads);
    return std::max(lengthBasedCount, depthBasedCount);
}

}


#include <stdint.h>
#include <cstdlib>

#include <iostream>
#include <memory>
#include <random>
#include <string>
#include <vector>

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-register"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/stuff/sampler.h>
#pragma clang diagnostic pop

#include <cpp_mpl.hpp>

#include "sampler.hpp"

using cppmpl::NumpyArray;

cppmpl::CppMatplotlib MplConnect (void);

// Need to define types for
// - vertices in the graph
// - edges that link the vertex types

// Example for a vertex.  UNDERLYING_TYPE is the type of the _estimate field.
// class VertexType : public g2o::BaseVertex<DIMENSION, UNDERLYING_TYPE> {
// public:
//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
//     VertexType() {
//       _estimate.setZero();
//     }
//
//   | kind of a "reset" method
//   virtual void setToOriginImpl() {
//     _estimate.setZero();
//   }
//
//   | has oPlusImpl that describes how a step in the minimal representation
//   | updates the state represented by this vertex
//   virtual void oplusImpl(const double* update) {
//     _estimate = ApplyUpdate(update);
//   }
//
//   | (optional) serialization
//   virtual bool read(std::istream& /*is*/) { return false; }
//   virtual bool write(std::ostream& /*os*/) const { return false; }
// };

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

  return 0;
}

cppmpl::CppMatplotlib MplConnect (void) {
  auto config_path = std::getenv("IPYTHON_KERNEL");
  if (config_path == nullptr) {
    std::cerr << "Please export IPYTHON_KERNEL=/path/to/kernel-NNN.json"
      << std::endl;
    std::exit(-1);
  }

  cppmpl::CppMatplotlib mpl{config_path};
  mpl.Connect();

  return mpl;
}


/****************************************************************************
*
*    Copyright (c) 2019 Vivante Corporation
*
*    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.
*
*****************************************************************************/

#pragma once

#include <backendsCommon/CpuTensorHandle.hpp>
#include <backendsCommon/Workload.hpp>
#include <backendsCommon/WorkloadData.hpp>
#include <boost/log/trivial.hpp>
#include "TNpuWorkloads.hpp"

namespace armnn {
template <typename ParentDescriptor, nnrt::OperationType operationType,
         typename armnn::DataType... DataTypes>
class NpuElementwiseWorkload : public TNpuWorkload<ParentDescriptor, DataTypes...> {
   public:
    using base_type = TNpuWorkload<ParentDescriptor, DataTypes...>;
    explicit NpuElementwiseWorkload(const ParentDescriptor& descriptor, const WorkloadInfo& info)
        : TNpuWorkload<ParentDescriptor, DataTypes...>(descriptor, info) {
        // Add inputs operand
        assert(2 == descriptor.m_Inputs.size());
        std::vector<uint32_t> inOperandIds = this->AddOperandWithTensorHandle(
                descriptor.m_Inputs);

        std::vector<uint32_t> outOperandIds;
        NpuTensorHandler* outputTensorHandle =
            dynamic_cast<NpuTensorHandler*>(descriptor.m_Outputs[0]);
        uint32_t outputTensorId = this->AddOperandAndSetValue(
            outputTensorHandle->GetTensorInfo(), outputTensorHandle->GetShape(), nullptr);
        outOperandIds.push_back(outputTensorId);

        this->AddOperation(operationType,
                           inOperandIds.size(),
                           inOperandIds.data(),
                           outOperandIds.size(),
                           outOperandIds.data());
    }
};
using NpuAdditionFloat32Workload = NpuElementwiseWorkload<AdditionQueueDescriptor,
      nnrt::OperationType::ADD, armnn::DataType::Float32>;
using NpuAdditionFloat16Workload = NpuElementwiseWorkload<AdditionQueueDescriptor,
      nnrt::OperationType::ADD, armnn::DataType::Float16>;
using NpuAdditionUint8Workload = NpuElementwiseWorkload<AdditionQueueDescriptor,
      nnrt::OperationType::ADD, armnn::DataType::QuantisedAsymm8>;

using NpuMinimumFloat32Workload = NpuElementwiseWorkload<MinimumQueueDescriptor,
      nnrt::OperationType::MINIMUM, armnn::DataType::Float32>;
using NpuMinimumFloat16Workload = NpuElementwiseWorkload<MinimumQueueDescriptor,
      nnrt::OperationType::MINIMUM, armnn::DataType::Float16>;
using NpuMinimumUint8Workload = NpuElementwiseWorkload<MinimumQueueDescriptor,
      nnrt::OperationType::MINIMUM, armnn::DataType::QuantisedAsymm8>;

using NpuMaximumFloat32Workload = NpuElementwiseWorkload<MaximumQueueDescriptor,
      nnrt::OperationType::MAXIMUM, armnn::DataType::Float32>;
using NpuMaximumFloat16Workload = NpuElementwiseWorkload<MaximumQueueDescriptor,
      nnrt::OperationType::MAXIMUM, armnn::DataType::Float16>;
using NpuMaximumUint8Workload = NpuElementwiseWorkload<MaximumQueueDescriptor,
      nnrt::OperationType::MAXIMUM, armnn::DataType::QuantisedAsymm8>;

using NpuSubtractionFloat32Workload = NpuElementwiseWorkload<SubtractionQueueDescriptor,
      nnrt::OperationType::SUB, armnn::DataType::Float32>;
using NpuSubtractionFloat16Workload = NpuElementwiseWorkload<SubtractionQueueDescriptor,
      nnrt::OperationType::SUB, armnn::DataType::Float16>;
using NpuSubtractionUint8Workload = NpuElementwiseWorkload<SubtractionQueueDescriptor,
      nnrt::OperationType::SUB, armnn::DataType::QuantisedAsymm8>;

using NpuDivisionFloat32Workload = NpuElementwiseWorkload<DivisionQueueDescriptor,
      nnrt::OperationType::DIV, armnn::DataType::Float32>;
using NpuDivisionFloat16Workload = NpuElementwiseWorkload<DivisionQueueDescriptor,
      nnrt::OperationType::DIV, armnn::DataType::Float16>;
using NpuDivisionUint8Workload = NpuElementwiseWorkload<DivisionQueueDescriptor,
      nnrt::OperationType::DIV, armnn::DataType::QuantisedAsymm8>;

using NpuMultiplicationFloat32Workload = NpuElementwiseWorkload<MultiplicationQueueDescriptor,
      nnrt::OperationType::MUL, armnn::DataType::Float32>;
using NpuMultiplicationFloat16Workload = NpuElementwiseWorkload<MultiplicationQueueDescriptor,
      nnrt::OperationType::MUL, armnn::DataType::Float16>;
using NpuMultiplicationUint8Workload = NpuElementwiseWorkload<MultiplicationQueueDescriptor,
      nnrt::OperationType::MUL, armnn::DataType::QuantisedAsymm8>;
}  // namespace armnn


// Copyright 2020 Oscar Higgott

// 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.

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/connected_components.hpp>
#include "matching_graph.h"
#include <memory>
#include <set>
#include <utility>
#include <stdexcept>
#include <cstdint>
#include <iostream>
#include <limits>
#include <cmath>
#include "rand_gen.h"

WeightedEdgeData::WeightedEdgeData() {}

WeightedEdgeData::WeightedEdgeData(
    std::set<int> fault_ids,
    double weight,
    double error_probability,
    bool has_error_probability,
    bool weight_is_negative
): fault_ids(fault_ids), weight(weight),
error_probability(error_probability), has_error_probability(has_error_probability),
weight_is_negative(weight_is_negative) {}

std::string set_repr(std::set<int> x) {
    std::stringstream ss;
    ss << "{";
    bool first = true;
    for (auto i : x){
        if (first){
            first = false;
        } else {
            ss << ", ";
        }
        ss << i;
    }
    ss << "}";
    return ss.str();
}

std::string WeightedEdgeData::repr() const {
    std::stringstream ss;
    ss << "pymatching._cpp_mwpm.WeightedEdgeData(";
    ss << set_repr(fault_ids) << ", " << weight << ", " << error_probability << ", "
    << has_error_probability << ")";
    return ss.str();
}

MatchingGraph::MatchingGraph()
    : all_edges_have_error_probabilities(true),
     connected_components_need_updating(true),
     negative_weight_sum(0.0)  {
    wgraph_t sgraph = wgraph_t();
    this->matching_graph = sgraph;
}

MatchingGraph::MatchingGraph(
    int num_detectors,
    std::set<int>& boundary)
    : all_edges_have_error_probabilities(true),
    boundary(boundary),
    connected_components_need_updating(true),
     negative_weight_sum(0.0) {
    wgraph_t sgraph = wgraph_t(num_detectors+boundary.size());
    this->matching_graph = sgraph;
}

void MatchingGraph::AddEdge(
    int node1,
    int node2,
    std::set<int> fault_ids,
    double weight,
    double error_probability,
    bool has_error_probability){
        if (has_error_probability && (error_probability > 1 || error_probability < 0)){
            throw std::invalid_argument("error_probability must be between 0 and 1");
        }
        if (node1 < 0 || node2 < 0){
            throw std::invalid_argument("Node IDs must be non-negative");
        }
        auto n1 = boost::vertex(node1, matching_graph);
        auto n2 = boost::vertex(node2, matching_graph);
        int num_nodes = GetNumNodes();
        bool nodes_in_graph = (n1 < num_nodes) && (n2 < num_nodes);
        if (nodes_in_graph && boost::edge(n1, n2, matching_graph).second){
            throw std::invalid_argument("This edge already exists in the graph. "
                                        "Parallel edges are not supported.");
        }
        if (std::signbit(weight)){
            HandleNewNegativeWeightEdge(node1, node2, weight, fault_ids);
        }
        if (!has_error_probability){
            all_edges_have_error_probabilities = false;
            error_probability = -1;
        }
        connected_components_need_updating = true;
        WeightedEdgeData data;
        data.fault_ids = fault_ids;
        data.weight = std::abs(weight);
        data.error_probability = error_probability;
        data.has_error_probability = has_error_probability;
        data.weight_is_negative = std::signbit(weight);
        boost::add_edge(
            n1,
            n2,
            data,
            matching_graph);
}

void MatchingGraph::HandleNewNegativeWeightEdge(int u, int v, double weight, std::set<int> &fault_ids){
    assert(std::signbit(weight));
    negative_weight_sum += weight;

    for (auto fid : fault_ids){
        if (negative_edge_fault_ids.find(fid) != negative_edge_fault_ids.end()){
            negative_edge_fault_ids.erase(fid);
        } else {
            negative_edge_fault_ids.insert(fid);
        }
    }

    for (auto node : {u, v}){
        if (negative_edge_syndrome.find(node) != negative_edge_syndrome.end()){
            negative_edge_syndrome.erase(node);
        } else {
            negative_edge_syndrome.insert(node);
        }
    }

}

void MatchingGraph::ComputeAllPairsShortestPaths(){
    int n = boost::num_vertices(matching_graph);
    all_distances.clear();
    all_predecessors.clear();
    for (int i=0; i<n; i++){
        std::vector<double> distances(n);
        std::vector<vertex_descriptor> p(n);
        vertex_descriptor from = boost::vertex(i, matching_graph);
        boost::dijkstra_shortest_paths(matching_graph, from,
            boost::weight_map(boost::get(&WeightedEdgeData::weight, matching_graph))
            .distance_map(boost::make_iterator_property_map(distances.begin(),
                            boost::get(boost::vertex_index, matching_graph)))
            .predecessor_map(&p[0]));
        all_distances.push_back(distances);
        all_predecessors.push_back(p);
    }
}

class exit_search{};

class DijkstraNeighbourVisitor : public boost::default_dijkstra_visitor
{
    public:
        DijkstraNeighbourVisitor(std::vector<int>& defect_id,
            int num_defects, std::vector<int>& examined_defects,
            std::vector<int>& discovered_nodes) :
        defect_id(defect_id), num_defects(num_defects),
        examined_defects(examined_defects), num_found(0),
        discovered_nodes(discovered_nodes) {}

        void examine_vertex(wgraph_t::vertex_descriptor v, const wgraph_t &g)
        {
            if (defect_id[v] > -1){
                num_found++;
                examined_defects.push_back(v);
                if (num_found >= num_defects) {
                    throw exit_search();
                }
            }
        }
        void discover_vertex(wgraph_t::vertex_descriptor v, const wgraph_t &g){
            discovered_nodes.push_back(v);
        }
        std::vector<int>& defect_id;
        int num_defects;
        std::vector<int>& examined_defects;
        std::vector<int>& discovered_nodes;
        int num_found;
};

void MatchingGraph::ResetDijkstraNeighbours(){
    int n = boost::num_vertices(matching_graph);
    double inf = std::numeric_limits<double>::max();
    if (_distances.size() < n){
        _distances.resize(n, inf);
    }
    if (_predecessors.size() < n){
        _predecessors.resize(n);
        for (int i=0; i<_predecessors.size(); i++){
            _predecessors[i] = i;
        }
    }
}

std::vector<std::pair<int, double>> MatchingGraph::GetNearestNeighbours(
    int source, int num_neighbours, std::vector<int>& defect_id){
    int n = boost::num_vertices(matching_graph);
    if (source < 0 || source >= n) {
        throw std::invalid_argument("source must be non-negative and less than the number of nodes "
                                    "in the matching graph");
    }
    if (defect_id.size() != n) {
        throw std::invalid_argument("defect_id must have the same number of elements as the number "
                                    "of nodes in the matching graph");
    }
    if (num_neighbours < 0) {
        throw std::invalid_argument("num_neighbours must be a positive integer");
    }
    double inf = std::numeric_limits<double>::max();
    ResetDijkstraNeighbours();
    _distances[source] = 0;
    std::vector<int> examined_defects;
    std::vector<int> discovered_nodes;
    int source_is_defect = defect_id[source] > -1;
    DijkstraNeighbourVisitor vis = DijkstraNeighbourVisitor(
        defect_id, num_neighbours + source_is_defect, examined_defects, discovered_nodes);
    vertex_descriptor from = boost::vertex(source, matching_graph);
    try {
        boost::dijkstra_shortest_paths_no_color_map_no_init(matching_graph, from,
                &_predecessors[0], boost::make_iterator_property_map(_distances.begin(),
                boost::get(boost::vertex_index, matching_graph)),
                boost::get(&WeightedEdgeData::weight, matching_graph),
                boost::get(boost::vertex_index, matching_graph),
                std::less<double>(),
                boost::closed_plus<double>(),
                inf,
                0,
                vis);
    } catch (exit_search e) {}

    std::vector<std::pair<int, double>> neighbours;
    for (auto d : examined_defects){
        if (d != source){
            neighbours.push_back({d, _distances[d]});
        }
    }

    for (auto n : discovered_nodes){
        _distances[n] = inf;
        _predecessors[n] = n;
    }
    return neighbours;
}

class DijkstraPathVisitor : public boost::default_dijkstra_visitor
{
    public:
        DijkstraPathVisitor(int target, std::vector<int>& discovered_nodes) :
        target(target),
        discovered_nodes(discovered_nodes) {}

        void examine_vertex(wgraph_t::vertex_descriptor v, const wgraph_t &g)
        {
            if (v == target){
                throw exit_search();
            }
        }
        void discover_vertex(wgraph_t::vertex_descriptor v, const wgraph_t &g){
            discovered_nodes.push_back(v);
        }
        int target;
        std::vector<int>& discovered_nodes;
};

std::vector<int> MatchingGraph::GetPath(
    int source, int target){
    int n = boost::num_vertices(matching_graph);
    if (source >= n || target >= n
        || source < 0 || target < 0){
        throw std::invalid_argument("source and target must non-negative and less "
                                    "than the number of nodes");
    }
    double inf = std::numeric_limits<double>::max();
    ResetDijkstraNeighbours();
    _distances[source] = 0;
    std::vector<int> discovered_nodes;
    DijkstraPathVisitor vis = DijkstraPathVisitor(
        target, discovered_nodes);
    vertex_descriptor from = boost::vertex(source, matching_graph);
    try {
        boost::dijkstra_shortest_paths_no_color_map_no_init(matching_graph, from,
                &_predecessors[0], boost::make_iterator_property_map(_distances.begin(),
                boost::get(boost::vertex_index, matching_graph)),
                boost::get(&WeightedEdgeData::weight, matching_graph),
                boost::get(boost::vertex_index, matching_graph),
                std::less<double>(),
                boost::closed_plus<double>(),
                inf,
                0,
                vis);
    } catch (exit_search e) {}

    std::vector<int> path;
    path.push_back(target);
    while (_predecessors[target] != target){
        target = _predecessors[target];
        path.push_back(target);
    }
    std::reverse(path.begin(),path.end());

    for (auto n : discovered_nodes){
        _distances[n] = inf;
        _predecessors[n] = n;
    }
    return path;
}

double MatchingGraph::Distance(int node1, int node2) {
    int num_nodes = GetNumNodes();
    if (node1 >= num_nodes || node2 >= num_nodes
        || node1 < 0 || node2 < 0){
        throw std::invalid_argument("node1 and node2 must non-negative and less "
                                    "than the number of nodes");
    }
    if (!HasComputedAllPairsShortestPaths()){
        ComputeAllPairsShortestPaths();
    }
    vertex_descriptor n2 = boost::vertex(node2, matching_graph);
    return all_distances[node1][n2];
}

std::vector<int> MatchingGraph::ShortestPath(int node1, int node2) {
    int num_nodes = GetNumNodes();
    if (node1 >= num_nodes || node2 >= num_nodes
        || node1 < 0 || node2 < 0){
        throw std::invalid_argument("node1 and node2 must non-negative and less "
                                    "than the number of nodes");
    }
    if (!HasComputedAllPairsShortestPaths()){
        ComputeAllPairsShortestPaths();
    }
    std::vector<vertex_descriptor> parent = all_predecessors[node2];
    auto index = boost::get(boost::vertex_index, matching_graph);
    int c = boost::vertex(node1, matching_graph);
    std::vector<int> path;
    path.push_back(index[c]);
    while (parent[c]!=c){
        c = parent[c];
        path.push_back(index[c]);
    }
    return path;
}

int MatchingGraph::GetNumEdges() const {
    return boost::num_edges(matching_graph);
}

int MatchingGraph::GetNumFaultIDs() const {
    auto qid = boost::get(&WeightedEdgeData::fault_ids, matching_graph);
    int num_edges = boost::num_edges(matching_graph);
    int maxid = -1;
    std::set<int> edge_fault_ids;
    auto es = boost::edges(matching_graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
        edge_fault_ids = qid[*eit];
        for (auto fault_id : edge_fault_ids){
            if (fault_id >= maxid){
                maxid = fault_id;
            }
            if (fault_id < 0 && fault_id != -1){
                throw std::runtime_error("Fault ids must be non-negative, or -1 if no fault IDs are associated with the edge.");
            }
        }
    }
    return maxid + 1;
}

int MatchingGraph::GetNumNodes() const {
    return boost::num_vertices(matching_graph);
};

std::set<int> MatchingGraph::FaultIDs(int node1, int node2) const {
    int num_nodes = GetNumNodes();
    if (node1 >= num_nodes || node2 >= num_nodes
        || node1 < 0 || node2 < 0){
        throw std::invalid_argument("node1 and node2 must non-negative and less "
                                    "than the number of nodes");
    }
    auto e = boost::edge(node1, node2, matching_graph);
    if (!e.second){
        throw std::invalid_argument("Graph does not contain edge ("
                        + std::to_string((int)node1) + ", "
                        + std::to_string((int)node2) + ").");
    }
    return matching_graph[e.first].fault_ids;
}

std::pair<py::array_t<std::uint8_t>,py::array_t<std::uint8_t>> MatchingGraph::AddNoise() const {
    auto syndrome = new std::vector<int>(GetNumNodes(), 0);
    auto error = new std::vector<int>(GetNumFaultIDs(), 0);
    double p;
    std::set<int> qids;
    vertex_descriptor s, t;
    bool to_flip;
    auto es = boost::edges(matching_graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
        p = matching_graph[*eit].error_probability;
        if ((p >= 0) && (rand_float(0.0, 1.0) < p)){
            s = boost::source(*eit, matching_graph);
            t = boost::target(*eit, matching_graph);
            (*syndrome)[s] = ((*syndrome)[s] + 1) % 2;
            (*syndrome)[t] = ((*syndrome)[t] + 1) % 2;
            qids = matching_graph[*eit].fault_ids;
            for (auto qid : qids){
                if (qid >= 0){
                    (*error)[qid] = ((*error)[qid] + 1) % 2;
                }
            }
        }
    }
    for (auto b : boundary){
        (*syndrome)[b] = 0;
    }

    auto capsule = py::capsule(syndrome, [](void *syndrome) { delete reinterpret_cast<std::vector<int>*>(syndrome); });
    py::array_t<int> syndrome_arr = py::array_t<int>(syndrome->size(), syndrome->data(), capsule);
    auto err_capsule = py::capsule(error, [](void *error) { delete reinterpret_cast<std::vector<int>*>(error); });
    py::array_t<int> error_arr = py::array_t<int>(error->size(), error->data(), err_capsule);
    return {error_arr, syndrome_arr};
}

std::set<int> MatchingGraph::GetBoundary() const {
    return boundary;
}

void MatchingGraph::SetBoundary(std::set<int>& boundary) {
    for (auto b: boundary){
        if (b < 0){
            throw std::invalid_argument("Boundary nodes must be non-negative.");
        }
    }
    this->boundary = boundary;
    connected_components_need_updating = true;
    return;
}

std::vector<std::tuple<int,int,WeightedEdgeData>> MatchingGraph::GetEdges() const {
    std::vector<std::tuple<int,int,WeightedEdgeData>> edges;
    auto es = boost::edges(matching_graph);
    for (auto eit = es.first; eit != es.second; ++eit) {
        WeightedEdgeData edata = matching_graph[*eit];
        int s = boost::source(*eit, matching_graph);
        int t = boost::target(*eit, matching_graph);
        if (edata.weight_is_negative) {
            edata.weight = -1 * edata.weight;
        }
        std::tuple<int,int,WeightedEdgeData> edge = std::make_tuple(s, t, edata);
        edges.push_back(edge);
    }
    return edges;
}

bool MatchingGraph::HasComputedAllPairsShortestPaths() const {
    int n = boost::num_vertices(matching_graph);
    bool has_distances = all_distances.size() == n;
    bool has_preds = all_predecessors.size() == n;
    return has_distances && has_preds;
}

int MatchingGraph::GetNumConnectedComponents() {
    if (connected_components_need_updating){
        component.resize(GetNumNodes());
        num_components = boost::connected_components(matching_graph, &component[0]);
        component_boundary.resize(num_components, -1);
        for (auto b : boundary){
            if (b >= GetNumNodes() || b < 0){
                throw std::invalid_argument(
                    "Boundary node ID " + std::to_string(b)
                    + " does not correspond to a node in the graph, which "
                    "has " + std::to_string(GetNumNodes()) + " nodes."
                );
            }
            int c = component[b];
            if (component_boundary[c] == -1){
                component_boundary[c] = b;
            }
        }
    }
    connected_components_need_updating = false;
    return num_components;
}

void MatchingGraph::FlipBoundaryNodesIfNeeded(std::set<int> &defects){
    int num_comps = GetNumConnectedComponents();
    if (num_comps == 1){
        if ((defects.size() % 2) == 0){
            return;
        } else if ((defects.size() % 2) == 1 && boundary.size() == 0){
            throw std::invalid_argument(
            "The syndrome has an odd number of defects, but no boundary nodes were provided"
            );
        }
    }
    std::vector<std::uint8_t> component_parities(num_comps, 0);
    for (auto df : defects){
        if (df >= component.size()){
            throw std::invalid_argument(
            "Defect id should not exceed the number of vertices in the graph"
            );
        }
        component_parities[component[df]] ^= 1;
    }

    for (int i=0; i<component_parities.size(); i++){
        if (component_parities[i] == 1){
            int b = component_boundary[i];
            if (b == -1){
                throw std::invalid_argument(
                    "The syndrome has an odd number of defects in a component of the matching graph "
                    "that does not have a boundary node"
                    );
            }
            bool is_in_defects = defects.find(b) != defects.end();
            if (is_in_defects){
                defects.erase(b);
            } else {
                defects.insert(b);
            }
        }
    }
}

bool MatchingGraph::AllEdgesHaveErrorProbabilities() const {
    return all_edges_have_error_probabilities;
}

std::string MatchingGraph::repr() const {
    std::stringstream ss;
    ss << "<pymatching._cpp_mwpm.MatchingGraph object with ";
    ss << GetNumNodes() << " nodes, ";
    ss << GetNumEdges() << " edges and " << GetBoundary().size() << " boundary nodes>";
    return ss.str();
}