// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#define BOOST_PYTHON_MAX_ARITY 40
#include <boost/python.hpp>
#include <cmath>
#include <iostream>

#include "numpy_bind.hh"

#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

#include "graph_filtering.hh"

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_properties.hh"
#include "graph_util.hh"

#include "random.hh"

#include "config.h"

#include "graph_blockmodel_overlap.hh"
#include "graph_blockmodel.hh"

using namespace boost;
using namespace graph_tool;

// ====================
// Entropy calculation
// ====================

// Repeated computation of x*log(x) and log(x) actually adds up to a lot of
// time. A significant speedup can be made by caching pre-computed values. This
// is doable since the values of mrse are bounded in [0, 2E], where E is the
// total number of edges in the network. Here we simply grow the cache as
// needed.

namespace graph_tool
{

vector<double> __safelog_cache;
vector<double> __xlogx_cache;
vector<double> __lgamma_cache;

void init_safelog(size_t x)
{
    size_t old_size = __safelog_cache.size();
    if (x >= old_size)
    {
        __safelog_cache.resize(x + 1);
        for (size_t i = old_size; i < __safelog_cache.size(); ++i)
            __safelog_cache[i] = safelog(double(i));
    }
}

void clear_safelog()
{
    vector<double>().swap(__safelog_cache);
}

void init_xlogx(size_t x)
{
    size_t old_size = __xlogx_cache.size();
    if (x >= old_size)
    {
        __xlogx_cache.resize(x + 1);
        for (size_t i = old_size; i < __xlogx_cache.size(); ++i)
            __xlogx_cache[i] = i * safelog(i);
    }
}

void clear_xlogx()
{
    vector<double>().swap(__xlogx_cache);
}

void init_lgamma(size_t x)
{
    size_t old_size = __lgamma_cache.size();
    if (x >= old_size)
    {
        __lgamma_cache.resize(x + 1);
        for (size_t i = old_size; i < __lgamma_cache.size(); ++i)
            __lgamma_cache[i] = lgamma(i);
    }
}

void clear_lgamma()
{
    vector<double>().swap(__lgamma_cache);
}

}

double do_get_ent(GraphInterface& gi, boost::any omrs, boost::any omrp,
                  boost::any omrm, boost::any owr, bool deg_corr)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t mrs = any_cast<emap_t>(omrs);
    vmap_t mrp = any_cast<vmap_t>(omrp);
    vmap_t mrm = any_cast<vmap_t>(omrm);
    vmap_t wr = any_cast<vmap_t>(owr);

    double S = 0;
    run_action<>()
        (gi, std::bind(entropy(), mrs, mrp, mrm, wr, deg_corr,
                       placeholders::_1,
                       std::ref(S)))();
    return S;
}

double do_get_ent_dense(GraphInterface& gi, boost::any omrs, boost::any owr,
                        bool multigraph)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t mrs = any_cast<emap_t>(omrs);
    vmap_t wr = any_cast<vmap_t>(owr);

    double S = 0;
    run_action<>()
        (gi, std::bind(entropy_dense(), mrs, wr, multigraph,
                       placeholders::_1, std::ref(S)))();
    return S;
}

double do_get_ent_parallel(GraphInterface& gi, boost::any oweight)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t weight = any_cast<emap_t>(oweight);

    double S = 0;
    run_action<>()
        (gi, std::bind(entropy_parallel_edges(),
                       placeholders::_1, weight, std::ref(S)))();
    return S;
}

boost::any do_create_emat(GraphInterface& gi)
{
    boost::any emat;
    run_action<>()(gi, std::bind<void>(create_emat(), placeholders::_1,
                                       std::ref(emat)))();
    return emat;
}

boost::any do_create_ehash(GraphInterface& gi)
{
    boost::any emat;
    run_action<>()(gi, std::bind<void>(create_ehash(), placeholders::_1,
                                       std::ref(emat)))();
    return emat;
}

//============
// Main loops
//============

template <class Eprop, class Vprop, class VEprop>
struct move_sweep_dispatch
{
    move_sweep_dispatch(Eprop eweight, Vprop vweight, boost::any egroups,
                        VEprop esrcpos, VEprop etgtpos, Vprop label,
                        vector<int>& vlist, bool deg_corr, bool dense,
                        bool multigraph, double beta, bool sequential,
                        bool parallel, bool random_move, double c, bool verbose,
                        size_t max_edge_index, size_t nmerges, size_t niter,
                        Vprop merge_map, partition_stats_t& partition_stats,
                        rng_t& rng, double& S, size_t& nmoves,
                        GraphInterface& bgi)

        : eweight(eweight), vweight(vweight), oegroups(egroups), esrcpos(esrcpos),
          etgtpos(etgtpos), label(label), vlist(vlist),
          deg_corr(deg_corr), dense(dense), multigraph(multigraph), beta(beta),
          sequential(sequential), parallel(parallel), random_move(random_move),
          c(c), verbose(verbose), max_edge_index(max_edge_index),
          nmerges(nmerges), niter(niter), merge_map(merge_map),
          partition_stats(partition_stats), rng(rng), S(S),
          nmoves(nmoves), bgi(bgi)
    {}

    Eprop eweight;
    Vprop vweight;
    boost::any oegroups;
    VEprop esrcpos;
    VEprop etgtpos;
    Vprop label;
    size_t n;
    vector<int>& vlist;
    bool deg_corr;
    bool dense;
    bool multigraph;
    double beta;
    bool sequential;
    bool parallel;
    bool random_move;
    double c;
    bool verbose;
    size_t max_edge_index;
    size_t nmerges;
    size_t niter;
    Vprop merge_map;
    partition_stats_t& partition_stats;
    rng_t& rng;
    double& S;
    size_t& nmoves;
    GraphInterface& bgi;

    template <class Graph>
    void operator()(Eprop mrs, Vprop mrp, Vprop mrm, Vprop wr, Vprop b,
                    Graph& g, boost::any& emat, boost::any sampler,
                    boost::any cavity_sampler, bool weighted) const
    {
        if (is_directed::apply<Graph>::type::value)
        {
            dispatch(mrs, mrp, mrm, wr, b, g, emat, sampler, cavity_sampler,
                     bgi.GetGraph(), weighted);
        }
        else
        {
            UndirectedAdaptor<GraphInterface::multigraph_t> ug(bgi.GetGraph());
            dispatch(mrs, mrp, mrm, wr, b, g, emat, sampler, cavity_sampler, ug,
                     weighted);
        }
    }

    template <class Graph, class BGraph>
    void dispatch(Eprop mrs, Vprop mrp, Vprop mrm, Vprop wr, Vprop b, Graph& g,
                  boost::any& aemat, boost::any asampler,
                  boost::any acavity_sampler, BGraph& bg, bool weighted) const
    {
        if (weighted)
        {
            typedef typename property_map_type::apply<DynamicSampler<std::tuple<typename graph_traits<Graph>::edge_descriptor, bool> >,
                                                      GraphInterface::vertex_index_map_t>::type vemap_t;
            vemap_t egroups = any_cast<vemap_t>(oegroups);

            try
            {
                typedef typename get_emat_t::apply<BGraph>::type emat_t;
                emat_t& emat = any_cast<emat_t&>(aemat);
                size_t B = num_vertices(bg);
                size_t max_BE = is_directed::apply<Graph>::type::value ?
                    B * B : (B * (B + 1)) / 2;
                dispatch(mrs.get_unchecked(max_BE), mrp, mrm, wr, b, g,
                         asampler, acavity_sampler, bg, egroups, emat);
            }
            catch (bad_any_cast&)
            {
                typedef typename get_ehash_t::apply<BGraph>::type emat_t;
                emat_t& emat = any_cast<emat_t&>(aemat);

                dispatch(mrs, mrp, mrm, wr, b, g, asampler, acavity_sampler, bg,
                         egroups, emat);
            }
        }
        else
        {
            typedef typename property_map_type::apply<vector<std::tuple<typename graph_traits<Graph>::edge_descriptor, bool> >,
                                                      GraphInterface::vertex_index_map_t>::type vemap_t;
            vemap_t egroups = any_cast<vemap_t>(oegroups);

            try
            {
                typedef typename get_emat_t::apply<BGraph>::type emat_t;
                emat_t& emat = any_cast<emat_t&>(aemat);
                size_t B = num_vertices(bg);
                size_t max_BE = is_directed::apply<Graph>::type::value ?
                    B * B : (B * (B + 1)) / 2;
                dispatch(mrs.get_unchecked(max_BE), mrp, mrm, wr, b, g,
                         asampler, acavity_sampler, bg, egroups, emat);
            }
            catch (bad_any_cast&)
            {
                typedef typename get_ehash_t::apply<BGraph>::type emat_t;
                emat_t& emat = any_cast<emat_t&>(aemat);

                dispatch(mrs, mrp, mrm, wr, b, g, asampler, acavity_sampler, bg,
                         egroups, emat);
            }
        }
    }

    template <class Graph, class BGraph, class Egroups, class Emat, class MEprop>
    void dispatch(MEprop mrs, Vprop mrp, Vprop mrm, Vprop wr, Vprop b, Graph& g,
                  boost::any asampler, boost::any acavity_sampler, BGraph& bg,
                  Egroups egroups, Emat& emat) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

        size_t eidx = random_move ? 1 : max_edge_index;

        typedef typename property_map<Graph, vertex_index_t>::type vindex_map_t;
        typedef typename property_map_type::apply<Sampler<vertex_t, boost::mpl::false_>,
                                                  vindex_map_t>::type::unchecked_t
            sampler_map_t;
        sampler_map_t sampler = any_cast<sampler_map_t>(asampler);
        sampler_map_t cavity_sampler = any_cast<sampler_map_t>(acavity_sampler);

        ConstantPropertyMap<std::array<int, 1>, typename graph_traits<Graph>::vertex_descriptor> cv({-1});
        IdentityArrayPropertyMap<typename graph_traits<Graph>::vertex_descriptor> vmap;
        boost::typed_identity_property_map<int> identity;

        // make sure the properties are _unchecked_, since otherwise it
        // affects performance

        overlap_stats_t ostats;
        vector<size_t> free_blocks;
        auto state = make_block_state(g, eweight.get_unchecked(max_edge_index),
                                      vweight.get_unchecked(num_vertices(g)),
                                      b.get_unchecked(num_vertices(g)), bg,
                                      emat, mrs,
                                      mrp.get_unchecked(num_vertices(bg)),
                                      mrm.get_unchecked(num_vertices(bg)),
                                      wr.get_unchecked(num_vertices(bg)),
                                      egroups.get_unchecked(num_vertices(bg)),
                                      esrcpos.get_unchecked(eidx),
                                      etgtpos.get_unchecked(eidx), sampler,
                                      cavity_sampler, partition_stats, ostats,
                                      identity, identity, free_blocks,
                                      false, false, true);

        vector<decltype(state)> states = {state};
        vector<EntrySet<Graph>> m_entries = {EntrySet<Graph>(num_vertices(bg))};

        move_sweep(states, m_entries,
                   wr.get_unchecked(num_vertices(bg)),
                   b.get_unchecked(num_vertices(g)),
                   cv, vmap,
                   label.get_unchecked(num_vertices(bg)), vlist, deg_corr,
                   dense, multigraph, beta,
                   eweight.get_unchecked(max_edge_index),
                   vweight.get_unchecked(num_vertices(g)),
                   g, sequential, parallel, random_move, c,
                   nmerges,
                   merge_map.get_unchecked(num_vertices(g)),
                   niter, num_vertices(bg),
                   verbose, rng, S, nmoves);
    }
};

boost::python::object do_move_sweep(GraphInterface& gi, GraphInterface& bgi,
                                    boost::any& emat, boost::any sampler,
                                    boost::any cavity_sampler, boost::any omrs,
                                    boost::any omrp, boost::any omrm,
                                    boost::any owr, boost::any ob,
                                    boost::any olabel, vector<int>& vlist,
                                    bool deg_corr, bool dense, bool multigraph,
                                    boost::any oeweight, boost::any ovweight,
                                    boost::any oegroups, boost::any oesrcpos,
                                    boost::any oetgtpos, double beta,
                                    bool sequential, bool parallel,
                                    bool random_move, double c, bool weighted,
                                    size_t nmerges, boost::any omerge_map,
                                    size_t niter,
                                    partition_stats_t& partition_stats,
                                    bool verbose, rng_t& rng)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        vemap_t;
    emap_t mrs = any_cast<emap_t>(omrs);
    vmap_t mrp = any_cast<vmap_t>(omrp);
    vmap_t mrm = any_cast<vmap_t>(omrm);
    vmap_t wr = any_cast<vmap_t>(owr);
    vmap_t b = any_cast<vmap_t>(ob);
    vmap_t label = any_cast<vmap_t>(olabel);
    emap_t eweight = any_cast<emap_t>(oeweight);
    vmap_t vweight = any_cast<vmap_t>(ovweight);

    vemap_t esrcpos = any_cast<vemap_t>(oesrcpos);
    vemap_t etgtpos = any_cast<vemap_t>(oetgtpos);

    double S = 0;
    size_t nmoves = 0;

    vmap_t merge_map = any_cast<vmap_t>(omerge_map);

    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind(move_sweep_dispatch<emap_t, vmap_t, vemap_t>
                       (eweight, vweight, oegroups, esrcpos, etgtpos,
                        label, vlist, deg_corr, dense, multigraph, beta,
                        sequential, parallel, random_move, c, verbose,
                        gi.GetMaxEdgeIndex(), nmerges, niter, merge_map,
                        partition_stats, rng, S, nmoves, bgi),
                       mrs, mrp, mrm, wr, b, placeholders::_1,
                       std::ref(emat), sampler, cavity_sampler, weighted))();
    return boost::python::make_tuple(S, nmoves);
}

struct build_egroups
{
    template <class Eprop, class Vprop, class VEprop, class Graph, class VertexIndex>
    void operator()(Vprop b, boost::any& oegroups, VEprop esrcpos,
                    VEprop etgtpos, Eprop eweight, Graph& g,
                    VertexIndex vertex_index, size_t B, bool weighted, bool empty) const
    {
        egroups_manage::build(b, oegroups, esrcpos, etgtpos, eweight, g,
                              vertex_index, B, weighted, empty);
    }
};

boost::any do_build_egroups(GraphInterface& gi, GraphInterface& bgi,
                            boost::any ob, boost::any oeweights,
                            boost::any oesrcpos, boost::any oetgtpos,
                            bool weighted, bool empty)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        vemap_t;
    vmap_t b = any_cast<vmap_t>(ob);

    vemap_t esrcpos = any_cast<vemap_t>(oesrcpos);
    vemap_t etgtpos = any_cast<vemap_t>(oetgtpos);
    emap_t eweights = any_cast<emap_t>(oeweights);

    boost::any oegroups;
    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind<void>(build_egroups(), b, std::ref(oegroups),
                             esrcpos.get_unchecked(gi.GetMaxEdgeIndex()),
                             etgtpos.get_unchecked(gi.GetMaxEdgeIndex()),
                             eweights.get_unchecked(gi.GetMaxEdgeIndex()),
                             placeholders::_1, bgi.GetVertexIndex(),
                             bgi.GetNumberOfVertices(), weighted, empty))();
    return oegroups;
}

boost::any do_init_neighbour_sampler(GraphInterface& gi, boost::any oeweights,
                                     bool self_loops, bool empty)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t eweights = any_cast<emap_t>(oeweights);

    boost::any osampler;
    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind(init_neighbour_sampler(), placeholders::_1, eweights,
                       self_loops, empty, std::ref(osampler)))();
    return osampler;
}

struct collect_edge_marginals_dispatch
{
    template <class Graph, class Vprop, class MEprop>
    void operator()(Graph& g, size_t B, Vprop cb, MEprop p,
                    std::tuple<boost::any, GraphInterface&> abg) const
    {
        Graph& bg = *any_cast<Graph*>(get<0>(abg));
        collect_edge_marginals(B, cb.get_unchecked(num_vertices(bg)), p, g, bg);
    }
};

void do_collect_edge_marginals(GraphInterface& gi, GraphInterface& gbi,
                               size_t B, boost::any ob, boost::any op)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<vector<int32_t>,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    vmap_t b = any_cast<vmap_t>(ob);
    emap_t p = any_cast<emap_t>(op);

    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind<void>(collect_edge_marginals_dispatch(),
                             placeholders::_1, B, b, p,
                             std::tuple<boost::any, GraphInterface&>(gbi.GetGraphView(), gbi)))();
}

boost::python::tuple do_bethe_entropy(GraphInterface& gi, size_t B, boost::any op,
                                      boost::any opv)
{
    typedef property_map_type::apply<vector<double>,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<vector<int32_t>,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;
    emap_t p = any_cast<emap_t>(op);
    vmap_t pv = any_cast<vmap_t>(opv);

    double H=0, sH=0, Hmf=0, sHmf=0;
    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind<void>(bethe_entropy(),
                             placeholders::_1, B, p, pv, std::ref(H), std::ref(sH),
                             std::ref(Hmf), std::ref(sHmf)))();
    return boost::python::make_tuple(H, sH, Hmf, sHmf);
}

struct collect_vertex_marginals_dispatch
{
    template <class Graph, class Vprop, class MEprop>
    void operator()(Graph& g, Vprop cb, MEprop p) const
    {
        collect_vertex_marginals(cb.get_unchecked(num_vertices(g)),
                                 p.get_unchecked(num_vertices(g)), g);
    }
};

void do_collect_vertex_marginals(GraphInterface& gi, boost::any ob,
                                 boost::any op)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    vmap_t b = any_cast<vmap_t>(ob);

    run_action<graph_tool::detail::all_graph_views, boost::mpl::true_>()
        (gi, std::bind(collect_vertex_marginals_dispatch(),
                       placeholders::_1, b, placeholders::_2),
         vertex_scalar_vector_properties())(op);
}

struct get_deg_entropy_term
{
    template <class Graph>
    void operator()(Graph& g, double& S) const
    {
        for(auto v : vertices_range(g))
        {
            S -= lgamma_fast(out_degree(v, g) + 1);
            S -= lgamma_fast(in_degreeS()(v, g) + 1);
        }
    }
};

struct get_deg_entropy_term_overlap
{

    template <class Graph, class Vprop>
    void operator()(Graph& g, Vprop b, overlap_stats_t& overlap_stats, size_t N,
                    double& S) const
    {
#ifdef HAVE_SPARSEHASH
        typedef dense_hash_map<int, int, std::hash<int>> map_t;
#else
        typedef unordered_map<int, int> map_t;
#endif

        map_t in_hist, out_hist;

#ifdef HAVE_SPARSEHASH
        in_hist.set_empty_key(numeric_limits<int>::max());
        out_hist.set_empty_key(numeric_limits<int>::max());
#endif

        for (size_t v = 0; v < N; ++v)
        {
            in_hist.clear();
            out_hist.clear();

            auto& half_edges = overlap_stats.get_half_edges(v);
            for (size_t u : half_edges)
            {
                in_hist[b[u]] += in_degreeS()(u, g);
                out_hist[b[u]] += out_degree(u, g);
            }

            for (auto& k_c : in_hist)
                S -= lgamma_fast(k_c.second + 1);
            for (auto& k_c : out_hist)
                S -= lgamma_fast(k_c.second + 1);
        }
    }
};

double do_get_deg_entropy_term(GraphInterface& gi, boost::any ob,
                               overlap_stats_t& overlap_stats, size_t N)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;

    double S = 0;

    if (!ob.empty())
    {
        vmap_t b = any_cast<vmap_t>(ob);
        run_action<>()
            (gi, std::bind(get_deg_entropy_term_overlap(),
                           placeholders::_1, b, std::ref(overlap_stats), N,
                           std::ref(S)))();
    }
    else
    {
        run_action<>()
            (gi, std::bind(get_deg_entropy_term(),
                           placeholders::_1, std::ref(S)))();
    }

    return S;
}

vector<int32_t> get_vector(size_t n)
{
    return vector<int32_t>(n);
}

template <class Value>
void vector_map(boost::python::object ovals, boost::python::object omap)
{

    multi_array_ref<Value,1> vals = get_array<Value,1>(ovals);
    multi_array_ref<Value,1> map = get_array<Value,1>(omap);

    size_t pos = 0;
    for (size_t i = 0; i < vals.size(); ++i)
    {
        Value v = vals[i];
        if (map[v] == -1)
            map[v] = pos++;
        vals[i] = map[v];
    }
}

template <class Value>
void vector_continuous_map(boost::python::object ovals)
{

    multi_array_ref<Value,1> vals = get_array<Value,1>(ovals);
    unordered_map<Value, size_t> map;

    for (size_t i = 0; i < vals.size(); ++i)
    {
        Value v = vals[i];
        auto iter = map.find(v);
        if (iter == map.end())
            iter = map.insert(make_pair(v, map.size())).first;
        vals[i] = iter->second;
    }
}

template <class Value>
void vector_rmap(boost::python::object ovals, boost::python::object omap)
{

    multi_array_ref<Value,1> vals = get_array<Value,1>(ovals);
    multi_array_ref<Value,1> map = get_array<Value,1>(omap);

    for (size_t i = 0; i < vals.size(); ++i)
    {
        map[vals[i]] = i;
    }
}

python::tuple python_get_mu_l(double N, double E, double epsilon=1e-8)
{
    double mu, l;
    get_mu_l(N, E, mu, l, epsilon);
    return python::make_tuple(mu, l);
}

struct get_partition_stats
{
    template <class Graph, class Vprop, class Eprop>
    void operator()(Graph& g, Vprop b, Eprop eweight, size_t N, size_t B,
                    bool edges_dl, partition_stats_t& partition_stats) const
    {
        partition_stats = partition_stats_t(g, b, eweight, N, B, edges_dl);
    }
};

partition_stats_t
do_get_partition_stats(GraphInterface& gi, boost::any ob, boost::any aeweight,
                       size_t N, size_t B, bool edges_dl)
{
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        vmap_t;
    typedef property_map_type::apply<int32_t,
                                     GraphInterface::edge_index_map_t>::type
        emap_t;

    partition_stats_t partition_stats;

    vmap_t b = any_cast<vmap_t>(ob);
    emap_t eweight = any_cast<emap_t>(aeweight);

    run_action<>()(gi, std::bind(get_partition_stats(),
                                 placeholders::_1, b, eweight, N, B, edges_dl,
                                 std::ref(partition_stats)))();
    return partition_stats;
}

void export_blockmodel()
{
    using namespace boost::python;

    class_<partition_stats_t>("partition_stats")
        .def("is_enabled", &partition_stats_t::is_enabled)
        .def("get_partition_dl", &partition_stats_t::get_partition_dl)
        .def("get_deg_dl", &partition_stats_t::get_deg_dl);

    def("init_partition_stats", do_get_partition_stats);

    def("init_safelog", init_safelog);
    def("clear_safelog", clear_safelog);
    def("init_xlogx", init_xlogx);
    def("clear_xlogx", clear_xlogx);
    def("init_lgamma", init_lgamma);
    def("clear_lgamma", clear_lgamma);
    def("get_xi", get_xi<double,double>);
    def("get_xi_fast", get_xi_fast<double,double>);
    def("get_mu_l", python_get_mu_l);
    def("polylog", polylog<double>);
    def("lbinom_careful", lbinom_careful);
    def("lbinom_fast", lbinom_fast);
    def("lbinom", lbinom);

    def("get_vector", get_vector);
    def("vector_map", vector_map<int32_t>);
    def("vector_map64", vector_map<int64_t>);
    def("vector_rmap", vector_rmap<int32_t>);
    def("vector_rmap64", vector_rmap<int64_t>);
    def("vector_continuous_map", vector_continuous_map<int32_t>);
    def("vector_continuous_map64", vector_continuous_map<int64_t>);

    def("create_emat", do_create_emat);
    def("create_ehash", do_create_ehash);
    def("build_egroups", do_build_egroups);
    def("init_neighbour_sampler", do_init_neighbour_sampler);

    def("move_sweep", do_move_sweep);

    def("entropy", do_get_ent);
    def("entropy_dense", do_get_ent_dense);
    def("entropy_parallel", do_get_ent_parallel);
    def("deg_entropy_term", do_get_deg_entropy_term);

    def("edge_marginals", do_collect_edge_marginals);
    def("bethe_entropy", do_bethe_entropy);
    def("vertex_marginals", do_collect_vertex_marginals);
}


#pragma once

#include <algorithm>
#include <fstream>
#include <iostream>
#include <list>
#include <stdio.h>

#include <Eigen/Dense>
#include <Eigen/QR>

#include "configuration.hpp"
#include "third_party/yaml/include/yaml/yaml.h"

namespace color {
constexpr int kRed = 0;
constexpr int kBoldRed = 1;
constexpr int kGreen = 2;
constexpr int kBoldGreen = 3;
constexpr int kYellow = 4;
constexpr int kBoldYellow = 5;
constexpr int kBlue = 6;
constexpr int kBoldBlue = 7;
constexpr int kMagneta = 8;
constexpr int kBoldMagneta = 9;
constexpr int kCyan = 10;
constexpr int kBoldCyan = 11;
}; // namespace color

namespace util {
void SaveVector(const Eigen::VectorXd &vec_, std::string name_,
                bool b_param = false);
void SaveVector(double *_vec, std::string _name, int size,
                bool b_param = false);
void SaveVector(const std::vector<double> &_vec, std::string _name,
                bool b_param = false);
void SaveValue(double _value, std::string _name, bool b_param = false);
void CleaningFile(std::string file_name_, std::string &ret_file_, bool b_param);
static std::list<std::string> gs_fileName_string; // global & static

template <typename YamlType>
YamlType ReadParameter(const YAML::Node &node, const std::string &name) {
  try {
    return node[name.c_str()].as<YamlType>();
  } catch (...) {
    throw std::runtime_error(name);
  }
};

template <typename YamlType>
void ReadParameter(const YAML::Node &node, const std::string &name,
                   YamlType &parameter) {
  try {
    parameter = ReadParameter<YamlType>(node, name);
  } catch (...) {
    throw std::runtime_error(name);
  }
};

void PrettyConstructor(const int &_num_tab, const std::string &_name);
void ColorPrint(const int &_color, const std::string &_name,
                bool line_change = true);

Eigen::MatrixXd hStack(const Eigen::MatrixXd &a_, const Eigen::MatrixXd &b_);
Eigen::MatrixXd vStack(const Eigen::MatrixXd &a_, const Eigen::MatrixXd &b_);
Eigen::MatrixXd block_diag(const Eigen::MatrixXd &a, const Eigen::MatrixXd &b);

Eigen::Matrix3d SkewSymmetric(const Eigen::Vector3d &omg);
Eigen::MatrixXd Adjoint(const Eigen::MatrixXd &R, const Eigen::Vector3d &p);

Eigen::Vector3d QuatToExp(const Eigen::Quaternion<double> &quat);
Eigen::Quaternion<double> ExpToQuat(const Eigen::Vector3d &exp);

// Euler ZYX
//     Represents either:
//     extrinsic XYZ rotations: Fixed-frame roll, then fixed-frame pitch, then
//     fixed-frame yaw.
//     or intrinsic ZYX rotations: Body-frame yaw, body-frame pitch, then
//     body-frame roll
//
//     The equation is similar, but the values for fixed and body frame
//     rotations are different.
// World Orientation is R = Rz*Ry*Rx
Eigen::Quaterniond EulerZYXtoQuat(const double roll, const double pitch,
                                  const double yaw);

// Quaternion to Euler ZYX
Eigen::Vector3d QuatToEulerZYX(const Eigen::Quaterniond &quat_in);

// ZYX extrinsic rotation rates to world angular velocity
// angular vel = [wx, wy, wz]
Eigen::Vector3d EulerZYXRatestoAngVel(const double roll, const double pitch,
                                      const double yaw, const double roll_rate,
                                      const double pitch_rate,
                                      const double yaw_rate);

void AvoidQuatJump(const Eigen::Quaternion<double> &des_ori,
                   Eigen::Quaternion<double> &act_ori);

double Clamp(const double &s_in, double lo = 0.0, double hi = 1.0);

void PseudoInverse(Eigen::MatrixXd const &matrix, double sigmaThreshold,
                   Eigen::MatrixXd &invMatrix,
                   Eigen::VectorXd *opt_sigmaOut = 0);

Eigen::MatrixXd PseudoInverse(const Eigen::MatrixXd &matrix,
                              const double &threshold);

Eigen::MatrixXd NullSpace(const Eigen::MatrixXd &J,
                          const double threshold = 0.00001);

Eigen::MatrixXd WeightedPseudoInverse(const Eigen::MatrixXd &J,
                                      const Eigen::MatrixXd &W,
                                      const double sigma_threshold = 0.0001);
} // namespace util


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file math.hpp
 * @author Vaclav Blazek <vaclav.blazek@citationtech.net>
 *
 * 3D mesh representation and operations.
 */

#ifndef geometry_mesh_hpp_included_
#define geometry_mesh_hpp_included_

#include <memory>
#include <vector>
#include <string>

#include <boost/noncopyable.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>

#include "math/geometry.hpp"
#include "geometry/parse-obj.hpp"

namespace geometry {

/** Face in a mesh.
 */
struct Face {
    typedef std::vector<Face> list;
    typedef unsigned int index_type;

    unsigned int imageId;

    // Indices of vertices.
    index_type a, b, c;
    // Indices of texture coordinates.
    index_type ta, tb, tc;

    Face() : imageId(), a(), b(), c(), ta(), tb(), tc() {}

    Face(index_type a, index_type b, index_type c, unsigned int imageId = 0)
        : imageId(imageId), a(a), b(b), c(c), ta(), tb(), tc()
    {}

    Face(index_type a, index_type b, index_type c
         , index_type ta, index_type tb, index_type tc
         , unsigned int imageId = 0)
        : imageId(imageId), a(a), b(b), c(c), ta(ta), tb(tb), tc(tc)
    {}

    index_type vertex(const int idx) const {
        const index_type abc[] = { a, b, c };
        return abc[idx];
    }

    /** Calculate normal of this face.
     */
    math::Point3 normal(const math::Points3 &vertices) const {
        return math::normalize
            (math::crossProduct(vertices[b] - vertices[a]
                                , vertices[c] - vertices[b]));
    }

    void clear() {
        a = b = c = 0;
        ta = tb = tc = 0;
    }

    /** Is this face not a triangle?
     */
    bool degenerate() const {
        return ((a == b) || (b == c) || (c == a));
    }

    // needed by python bindings
    bool operator==(const Face &f) const;
    bool operator<(const Face &f) const;
};

/** Textured 3D mesh representation.
 *
 * Vertices and texture coordinates are held in two arrays. Vertex at index I
 * has vertex coordinates at vertices[I] and texture coordinates at tCoords[I].
 *
 * Faces are defined as 3 indices to vertices and 3 indices to tCoords.
 */
struct Mesh {
    typedef std::shared_ptr<Mesh> pointer;
    typedef std::vector<Mesh> list;

    /** vertices */
    std::vector <int> vertecesClass;
    math::Points3 vertices;

    /** per-vertex texture coordinates */
    math::Points2 tCoords;

    /** Faces (triplets of indices to vertices and texture coordinates) */
    Face::list faces;

    /** Face normal. */
    math::Point3 normal(const Face &face) const {
        return face.normal(vertices);
    }

    /** Add new face.
     */
    void addFace(math::Points3::size_type a, math::Points3::size_type b
                 , math::Points3::size_type c);

    void addFace(math::Points3::size_type a, math::Points3::size_type b
                 , math::Points3::size_type c, unsigned int imageId);

    void addFace(math::Points3::size_type a, math::Points3::size_type b
                 , math::Points3::size_type c, math::Points2::size_type ta
                 , math::Points2::size_type tb, math::Points2::size_type tc );

    void addFace(math::Points3::size_type a, math::Points3::size_type b
                 , math::Points3::size_type c, math::Points2::size_type ta
                 , math::Points2::size_type tb, math::Points2::size_type tc
                 , unsigned int imageId);

    /** First face point.
    */
    const math::Point3& a(const Face &face) const {
        return vertices[face.a];
    }

    /** Second face point.
     */
    const math::Point3& b(const Face &face) const {
        return vertices[face.b];
    }

    /** Third face point.
     */
    const math::Point3& c(const Face &face) const {
        return vertices[face.c];
    }

    /** First face texture point.
    */
    const math::Point2& ta(const Face &face) const {
        return tCoords[face.ta];
    }

    /** Second face texture point.
     */
    const math::Point2& tb(const Face &face) const {
        return tCoords[face.tb];
    }

    /** Third face texture point.
     */
    const math::Point2& tc(const Face &face) const {
        return tCoords[face.tc];
    }

    /** Is given face not a triangle?
     */
    bool degenerate(const Face &face) const {
        return (face.degenerate() || (a(face) == b(face))
                || (b(face) == c(face)) || (c(face) == a(face)));
    }

    /** Are face indices ok?
     */
    bool good(const Face &face) const {
        return face.a < vertices.size() &&
               face.b < vertices.size() &&
               face.c < vertices.size();
    }

    /**
     * @brief provide mesh with skirt
     * @details skirt is a set quads pointing in the direction of the given
     * vector and attached to odd edges (edges adjacent to a single face).
     */
    void skirt( const math::Point3 & down = math::Point3( 0.0, 0.0, -1.0 ) );

    void sortFacesByImageId();

    /** Iterator that iterates over face points (a->b->c->end)
     */
    struct FaceVertexConstIterator;
    struct FaceVertexConstIteratorRange;

    FaceVertexConstIterator begin(const Face &face) const;

    FaceVertexConstIterator end(const Face&) const;

    FaceVertexConstIteratorRange face(const Face &face) const;

    /** Calculate face area (in 3D space).
     */
    double area(const Face &face) const;

    /** Calculate face area (in UV space).;
     */
    double txArea(const Face &face) const;

    /** Calculate face barycenter.
     */
    math::Point3 barycenter(const Face &face) const;
};

// inlines

inline void Mesh::addFace(math::Points3::size_type a
                          , math::Points3::size_type b
                          , math::Points3::size_type c
                          , unsigned int imageId)
{
    faces.emplace_back(a, b, c, imageId);
}

inline void Mesh::addFace(math::Points3::size_type a
                          , math::Points3::size_type b
                          , math::Points3::size_type c)
{
    faces.emplace_back(a, b, c);
}

inline void
Mesh::addFace(math::Points3::size_type a, math::Points3::size_type b
              , math::Points3::size_type c, math::Points2::size_type ta
              , math::Points2::size_type tb, math::Points2::size_type tc)
{
    faces.emplace_back(a, b, c, ta, tb, tc);
}

inline void
Mesh::addFace(math::Points3::size_type a, math::Points3::size_type b
              , math::Points3::size_type c, math::Points2::size_type ta
              , math::Points2::size_type tb, math::Points2::size_type tc
              , unsigned int imageId)
{
    faces.emplace_back(a, b, c, ta, tb, tc, imageId);
}

inline void Mesh::sortFacesByImageId()
{
    std::sort(faces.begin(), faces.end()
              , [](const Face &l, const Face &r) {
                  return l.imageId < r.imageId;
              });
}

struct Mesh::FaceVertexConstIterator {
    FaceVertexConstIterator() : face_(), vertices_(), index_() {}

    FaceVertexConstIterator(const Mesh &mesh, const Face &face)
        : face_(&face), vertices_(&mesh.vertices), index_()
    {}

    const math::Point3& operator*() const {
        switch (index_) {
        case 0: return (*vertices_)[face_->a];
        case 1: return (*vertices_)[face_->b];
        default: return (*vertices_)[face_->c];
        }
    }

    const math::Point3* operator->() const {
        switch (index_) {
        case 0: return &(*vertices_)[face_->a];
        case 1: return &(*vertices_)[face_->b];
        default: return &(*vertices_)[face_->c];
        }
    }

    FaceVertexConstIterator operator++() {
        ++index_;
        return *this;
    }

    bool operator==(const FaceVertexConstIterator &o) {
        if (((index_ > 2) || !face_) && !o.face_) {
            return true;
        }

        if (!face_ && ((o.index_ > 2) || !o.face_)) {
            return true;
        }

        return ((index_ == o.index_) && (face_ == o.face_)
                && (vertices_ == o.vertices_));
    }

    bool operator!=(const FaceVertexConstIterator &o) {
        return !operator==(o);
    }

private:
    const Face *face_;
    const math::Points3 *vertices_;
    unsigned int index_;
};

inline Mesh::FaceVertexConstIterator Mesh::begin(const Face &face) const {
    return FaceVertexConstIterator(*this, face);
}

inline Mesh::FaceVertexConstIterator Mesh::end(const Face&) const {
    return FaceVertexConstIterator();
}

class Mesh::FaceVertexConstIteratorRange {
public:
    FaceVertexConstIteratorRange(const Mesh &mesh, const Face &face)
        : begin_(mesh, face), end_()
    {}

    const FaceVertexConstIterator& begin() const { return begin_; }
    const FaceVertexConstIterator& end() const { return end_; }

private:
    FaceVertexConstIterator begin_;
    FaceVertexConstIterator end_;
};

inline Mesh::FaceVertexConstIteratorRange Mesh::face(const Face &face) const
{
    return { *this, face };
}

// inlines

inline bool Face::operator==(const Face &f) const {
    return
        (a == f.a) && (b == f.b) && (c == f.c)
        && (ta == f.ta) && (tb == f.tb) && (tc == f.tc)
        && (imageId == f.imageId)
        ;
}

inline bool Face::operator<(const Face &f) const
{
    if (a < f.a) { return true; }
    if (f.a < a) { return false; }
    if (b < f.b) { return true; }
    if (f.b < b) { return false; }
    if (c < f.c) { return true; }
    if (f.c < c) { return false; }

    if (ta < f.ta) { return true; }
    if (f.ta < ta) { return false; }
    if (tb < f.tb) { return true; }
    if (f.tb < tb) { return false; }
    if (tc < f.tc) { return true; }
    if (f.tc < tc) { return false; }

    return imageId < f.imageId;
}

} // namespace geometry

#endif // geometry_mesh_hpp_included_


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.
//Copyright (c) 2019 agate-pris

//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_QVM_384AFF3AD23A11DFA80B754FE0D72085
#define BOOST_QVM_384AFF3AD23A11DFA80B754FE0D72085

#include <boost/qvm/detail/vec_assign.hpp>
#include <boost/qvm/vec_operations2.hpp>
#include <boost/qvm/vec_operations3.hpp>
#include <boost/qvm/vec_operations4.hpp>
#include <boost/qvm/assert.hpp>
#include <boost/qvm/scalar_traits.hpp>
#include <string>

namespace
boost
    {
    namespace
    qvm
        {
        namespace
        qvm_detail
            {
            BOOST_QVM_INLINE_CRITICAL
            void const *
            get_valid_ptr_vec_operations()
                {
                static int const obj=0;
                return &obj;
                }
            }

        ////////////////////////////////////////////////

        namespace
        qvm_to_string_detail
            {
            template <class T>
            std::string to_string( T const & x );
            }

        namespace
        qvm_detail
            {
            template <int D>
            struct
            to_string_v_defined
                {
                static bool const value=false;
                };

            template <int I,int DimMinusOne>
            struct
            to_string_vector_elements
                {
                template <class A>
                static
                std::string
                f( A const & a )
                    {
                    using namespace qvm_to_string_detail;
                    return to_string(vec_traits<A>::template read_element<I>(a))+','+to_string_vector_elements<I+1,DimMinusOne>::f(a);
                    }
                };

            template <int DimMinusOne>
            struct
            to_string_vector_elements<DimMinusOne,DimMinusOne>
                {
                template <class A>
                static
                std::string
                f( A const & a )
                    {
                    using namespace qvm_to_string_detail;
                    return to_string(vec_traits<A>::template read_element<DimMinusOne>(a));
                    }
                };
            }

        template <class A>
        inline
        typename boost::enable_if_c<
            is_vec<A>::value  &&
            !qvm_detail::to_string_v_defined<vec_traits<A>::dim>::value,
            std::string>::type
        to_string( A const & a )
            {
            return '('+qvm_detail::to_string_vector_elements<0,vec_traits<A>::dim-1>::f(a)+')';
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            convert_to_v_defined
                {
                static bool const value=false;
                };
            }

        template <class R,class A>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_vec<R>::value && is_vec<A>::value &&
            vec_traits<R>::dim==vec_traits<A>::dim &&
            !qvm_detail::convert_to_v_defined<vec_traits<R>::dim>::value,
            R>::type
        convert_to( A const & a )
            {
            R r; assign(r,a);
            return r;
            }

        ////////////////////////////////////////////////

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==3 && vec_traits<B>::dim==3,
            deduce_vec2<A,B,3> >::type
        cross( A const & a, B const & b )
            {
            typedef typename deduce_vec2<A,B,3>::type R;
            R r;
            vec_traits<R>::template write_element<0>(r)=
                vec_traits<A>::template read_element<1>(a)*vec_traits<B>::template read_element<2>(b)-
                vec_traits<A>::template read_element<2>(a)*vec_traits<B>::template read_element<1>(b);
            vec_traits<R>::template write_element<1>(r)=
                vec_traits<A>::template read_element<2>(a)*vec_traits<B>::template read_element<0>(b)-
                vec_traits<A>::template read_element<0>(a)*vec_traits<B>::template read_element<2>(b);
            vec_traits<R>::template write_element<2>(r)=
                vec_traits<A>::template read_element<0>(a)*vec_traits<B>::template read_element<1>(b)-
                vec_traits<A>::template read_element<1>(a)*vec_traits<B>::template read_element<0>(b);
            return r;
            }

        ////////////////////////////////////////////////

        template <class A,class B,class Cmp>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim,
            bool>::type
        cmp( A const & a, B const & b, Cmp f )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                if( !f(
                    vec_traits<A>::read_element_idx(i,a),
                    vec_traits<B>::read_element_idx(i,b)) )
                    return false;
            return true;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T,int Dim>
            class
            zero_vec_
                {
                zero_vec_( zero_vec_ const & );
                zero_vec_ & operator=( zero_vec_ const & );
                ~zero_vec_();

                public:

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class V>
        struct vec_traits;

        template <class T,int Dim>
        struct
        vec_traits< qvm_detail::zero_vec_<T,Dim> >
            {
            typedef qvm_detail::zero_vec_<T,Dim> this_vector;
            typedef T scalar_type;
            static int const dim=Dim;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<Dim);
                return scalar_traits<scalar_type>::value(0);
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<Dim);
                return scalar_traits<scalar_type>::value(0);
                }
            };

        template <class T,int Dim,int D>
        struct
        deduce_vec<qvm_detail::zero_vec_<T,Dim>,D>
            {
            typedef vec<T,D> type;
            };

        template <class T,int Dim>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::zero_vec_<T,Dim> const &
        zero_vec()
            {
            return *(qvm_detail::zero_vec_<T,Dim> const *)qvm_detail::get_valid_ptr_vec_operations();
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value,
            void>::type
        set_zero( A & a )
            {
            assign(a,zero_vec<typename vec_traits<A>::scalar_type,vec_traits<A>::dim>());
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class OriginalType,class Scalar>
            class
            vector_scalar_cast_
                {
                vector_scalar_cast_( vector_scalar_cast_ const & );
                vector_scalar_cast_ & operator=( vector_scalar_cast_ const & );
                ~vector_scalar_cast_();

                public:

                template <class T>
                BOOST_QVM_INLINE_TRIVIAL
                vector_scalar_cast_ &
                operator=( T const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };

            template <bool> struct scalar_cast_vector_filter { };
            template <> struct scalar_cast_vector_filter<true> { typedef int type; };
            }

        template <class OriginalType,class Scalar>
        struct
        vec_traits< qvm_detail::vector_scalar_cast_<OriginalType,Scalar> >
            {
            typedef Scalar scalar_type;
            typedef qvm_detail::vector_scalar_cast_<OriginalType,Scalar> this_vector;
            static int const dim=vec_traits<OriginalType>::dim;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return scalar_type(vec_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x)));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return scalar_type(vec_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x)));
                }
            };

        template <class OriginalType,class Scalar,int D>
        struct
        deduce_vec<qvm_detail::vector_scalar_cast_<OriginalType,Scalar>,D>
            {
            typedef vec<Scalar,D> type;
            };

        template <class Scalar,class T>
        BOOST_QVM_INLINE_TRIVIAL
        qvm_detail::vector_scalar_cast_<T,Scalar> const &
        scalar_cast( T const & x, typename qvm_detail::scalar_cast_vector_filter<is_vec<T>::value>::type=0 )
            {
            return reinterpret_cast<qvm_detail::vector_scalar_cast_<T,Scalar> const &>(x);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            div_eq_vs_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_scalar<B>::value &&
            !qvm_detail::div_eq_vs_defined<vec_traits<A>::dim>::value,
            A &>::type
        operator/=( A & a, B b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<A>::write_element_idx(i,a)/=b;
            return a;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            div_vs_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_scalar<B>::value &&
            !qvm_detail::div_vs_defined<vec_traits<A>::dim>::value,
            deduce_vec<A> >::type
        operator/( A const & a, B b )
            {
            typedef typename deduce_vec<A>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=vec_traits<A>::read_element_idx(i,a)/b;
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            dot_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::dot_vv_defined<vec_traits<A>::dim>::value,
            deduce_scalar<typename vec_traits<A>::scalar_type,typename vec_traits<B>::scalar_type> >::type
        dot( A const & a, B const & b )
            {
            typedef typename deduce_scalar<typename vec_traits<A>::scalar_type,typename vec_traits<B>::scalar_type>::type T;
            T m(scalar_traits<T>::value(0));
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                m+=vec_traits<A>::read_element_idx(i,a)*vec_traits<B>::read_element_idx(i,b);
            return m;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            eq_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::eq_vv_defined<vec_traits<A>::dim>::value,
            bool>::type
        operator==( A const & a, B const & b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                if( vec_traits<A>::read_element_idx(i,a)!=vec_traits<B>::read_element_idx(i,b) )
                    return false;
            return true;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            mag_sqr_v_defined
                {
                static bool const value=false;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value &&
            !qvm_detail::mag_sqr_v_defined<vec_traits<A>::dim>::value,
            typename vec_traits<A>::scalar_type>::type
        mag_sqr( A const & a )
            {
            typedef typename vec_traits<A>::scalar_type T;
            T m(scalar_traits<T>::value(0));
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                {
                T x=vec_traits<A>::read_element_idx(i,a);
                m+=x*x;
                }
            return m;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            mag_v_defined
                {
                static bool const value=false;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value &&
            !qvm_detail::mag_v_defined<vec_traits<A>::dim>::value,
            typename vec_traits<A>::scalar_type>::type
        mag( A const & a )
            {
            typedef typename vec_traits<A>::scalar_type T;
            T m(scalar_traits<T>::value(0));
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                {
                T x=vec_traits<A>::read_element_idx(i,a);
                m+=x*x;
                }
            return sqrt<T>(m);
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            minus_eq_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::minus_eq_vv_defined<vec_traits<A>::dim>::value,
            A &>::type
        operator-=( A & a, B const & b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<A>::write_element_idx(i,a)-=vec_traits<B>::read_element_idx(i,b);
            return a;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            minus_v_defined
                {
                static bool const value=false;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value &&
            !qvm_detail::minus_v_defined<vec_traits<A>::dim>::value,
            deduce_vec<A> >::type
        operator-( A const & a )
            {
            typedef typename deduce_vec<A>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=-vec_traits<A>::read_element_idx(i,a);
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            minus_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::minus_vv_defined<vec_traits<A>::dim>::value,
            deduce_vec2<A,B,vec_traits<A>::dim> >::type
        operator-( A const & a, B const & b )
            {
            typedef typename deduce_vec2<A,B,vec_traits<A>::dim>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=vec_traits<A>::read_element_idx(i,a)-vec_traits<B>::read_element_idx(i,b);
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            mul_eq_vs_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_scalar<B>::value &&
            !qvm_detail::mul_eq_vs_defined<vec_traits<A>::dim>::value,
            A &>::type
        operator*=( A & a, B b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<A>::write_element_idx(i,a)*=b;
            return a;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            mul_vs_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_scalar<B>::value &&
            !qvm_detail::mul_vs_defined<vec_traits<A>::dim>::value,
            deduce_vec<A> >::type
        operator*( A const & a, B b )
            {
            typedef typename deduce_vec<A>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=vec_traits<A>::read_element_idx(i,a)*b;
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            mul_sv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_scalar<A>::value && is_vec<B>::value &&
            !qvm_detail::mul_sv_defined<vec_traits<B>::dim>::value,
            deduce_vec<B> >::type
        operator*( A a, B const & b )
            {
            typedef typename deduce_vec<B>::type R;
            R r;
            for( int i=0; i!=vec_traits<B>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=a*vec_traits<B>::read_element_idx(i,b);
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            neq_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::neq_vv_defined<vec_traits<A>::dim>::value,
            bool>::type
        operator!=( A const & a, B const & b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                if( vec_traits<A>::read_element_idx(i,a)!=vec_traits<B>::read_element_idx(i,b) )
                    return true;
            return false;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            normalize_v_defined
                {
                static bool const value=false;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value &&
            !qvm_detail::normalize_v_defined<vec_traits<A>::dim>::value,
            deduce_vec<A> >::type
        normalized( A const & a )
            {
            typedef typename vec_traits<A>::scalar_type T;
            T m(scalar_traits<T>::value(0));
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                {
                T x=vec_traits<A>::read_element_idx(i,a);
                m+=x*x;
                }
            if( m==scalar_traits<T>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
            T rm=scalar_traits<T>::value(1)/sqrt<T>(m);
            typedef typename deduce_vec<A>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=vec_traits<A>::read_element_idx(i,a)*rm;
            return r;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value &&
            !qvm_detail::normalize_v_defined<vec_traits<A>::dim>::value,
            void>::type
        normalize( A & a )
            {
            typedef typename vec_traits<A>::scalar_type T;
            T m(scalar_traits<T>::value(0));
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                {
                T x=vec_traits<A>::read_element_idx(i,a);
                m+=x*x;
                }
            if( m==scalar_traits<T>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
            T rm=scalar_traits<T>::value(1)/sqrt<T>(m);
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<A>::write_element_idx(i,a)*=rm;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            plus_eq_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::plus_eq_vv_defined<vec_traits<A>::dim>::value,
            A &>::type
        operator+=( A & a, B const & b )
            {
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<A>::write_element_idx(i,a)+=vec_traits<B>::read_element_idx(i,b);
            return a;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <int D>
            struct
            plus_vv_defined
                {
                static bool const value=false;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            is_vec<A>::value && is_vec<B>::value &&
            vec_traits<A>::dim==vec_traits<B>::dim &&
            !qvm_detail::plus_vv_defined<vec_traits<A>::dim>::value,
            deduce_vec2<A,B,vec_traits<A>::dim> >::type
        operator+( A const & a, B const & b )
            {
            typedef typename deduce_vec2<A,B,vec_traits<A>::dim>::type R;
            R r;
            for( int i=0; i!=vec_traits<A>::dim; ++i )
                vec_traits<R>::write_element_idx(i,r)=vec_traits<A>::read_element_idx(i,a)+vec_traits<B>::read_element_idx(i,b);
            return r;
            }

        ////////////////////////////////////////////////

        namespace
        qvm_detail
            {
            template <class T>
            class
            vref_
                {
                vref_( vref_ const & );
                vref_ & operator=( vref_ const & );
                ~vref_();

                public:

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                vref_ &
                operator=( R const & x )
                    {
                    assign(*this,x);
                    return *this;
                    }

                template <class R>
                BOOST_QVM_INLINE_TRIVIAL
                operator R() const
                    {
                    R r;
                    assign(r,*this);
                    return r;
                    }
                };
            }

        template <class V>
        struct
        vec_traits< qvm_detail::vref_<V> >
            {
            typedef typename vec_traits<V>::scalar_type scalar_type;
            typedef qvm_detail::vref_<V> this_vector;
            static int const dim=vec_traits<V>::dim;

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element( this_vector const & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return vec_traits<V>::template read_element<I>(reinterpret_cast<V const &>(x));
                }

            template <int I>
            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element( this_vector & x )
                {
                BOOST_QVM_STATIC_ASSERT(I>=0);
                BOOST_QVM_STATIC_ASSERT(I<dim);
                return vec_traits<V>::template write_element<I>(reinterpret_cast<V &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type
            read_element_idx( int i, this_vector const & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return vec_traits<V>::read_element_idx(i,reinterpret_cast<V const &>(x));
                }

            static
            BOOST_QVM_INLINE_CRITICAL
            scalar_type &
            write_element_idx( int i, this_vector & x )
                {
                BOOST_QVM_ASSERT(i>=0);
                BOOST_QVM_ASSERT(i<dim);
                return vec_traits<V>::write_element_idx(i,reinterpret_cast<V &>(x));
                }
            };

        template <class V,int D>
        struct
        deduce_vec<qvm_detail::vref_<V>,D>
            {
            typedef vec<typename vec_traits<V>::scalar_type,D> type;
            };

        template <class V>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_vec<V>::value,
            qvm_detail::vref_<V> const &>::type
        vref( V const & a )
            {
            return reinterpret_cast<qvm_detail::vref_<V> const &>(a);
            }

        template <class V>
        BOOST_QVM_INLINE_TRIVIAL
        typename enable_if_c<
            is_vec<V>::value,
            qvm_detail::vref_<V> &>::type
        vref( V & a )
            {
            return reinterpret_cast<qvm_detail::vref_<V> &>(a);
            }

        ////////////////////////////////////////////////

        namespace
        sfinae
            {
            using ::boost::qvm::to_string;
            using ::boost::qvm::assign;
            using ::boost::qvm::convert_to;
            using ::boost::qvm::cross;
            using ::boost::qvm::cmp;
            using ::boost::qvm::set_zero;
            using ::boost::qvm::scalar_cast;
            using ::boost::qvm::operator/=;
            using ::boost::qvm::operator/;
            using ::boost::qvm::dot;
            using ::boost::qvm::operator==;
            using ::boost::qvm::mag_sqr;
            using ::boost::qvm::mag;
            using ::boost::qvm::operator-=;
            using ::boost::qvm::operator-;
            using ::boost::qvm::operator*=;
            using ::boost::qvm::operator*;
            using ::boost::qvm::operator!=;
            using ::boost::qvm::normalized;
            using ::boost::qvm::normalize;
            using ::boost::qvm::operator+=;
            using ::boost::qvm::operator+;
            using ::boost::qvm::vref;
            }

        ////////////////////////////////////////////////
        }
    }

#endif


#include "conex.h"

#include <iostream>
#include <memory>
#include <vector>

#include <Eigen/Dense>

#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/dense_lmi_constraint.h"
#include "conex/equality_constraint.h"
#include "conex/hermitian_psd.h"
#include "conex/linear_constraint.h"
#include "conex/soc_constraint.h"

#include "conex/error_checking_macros.h"

// TODO(FrankPermenter): check for null pointers.
#define SAFER_CAST_TO_Program(x, prog)                                     \
  CONEX_DEMAND(x, "Program pointer is null.");                             \
  prog = static_cast<Program*>(x);                                         \
  if (prog->is_initialized) {                                              \
    if (prog->NumberOfConstraints() + 2 !=                                 \
        static_cast<int>(prog->workspaces.size())) {                       \
      CONEX_DEMAND(false, "Program corrupted or invalid pointer.");        \
    }                                                                      \
  } else {                                                                 \
    if (prog->workspaces.size() != 0 || prog->NumberOfConstraints() < 0) { \
      CONEX_DEMAND(false, "Program corrupted or invalid pointer.");        \
    }                                                                      \
  }                                                                        \
  CONEX_DEMAND(prog, "Program corrupted or invalid pointer.");

using DenseMatrix = Eigen::MatrixXd;
using conex::DenseLMIConstraint;
using conex::HermitianPsdConstraint;
using conex::LinearConstraint;
using conex::Program;
using conex::SolverConfiguration;

namespace {

void NonZeroSubMat(const Eigen::MatrixXd& M, std::vector<int>* vars,
                   Eigen::MatrixXd* Y) {
  vars->clear();
  for (int i = 0; i < M.rows(); i++) {
    if (M(i, i) > 0) {
      vars->push_back(i);
    }
  }

  int row = 0;
  Y->resize(vars->size(), vars->size());
  for (auto& i : *vars) {
    int col = 0;
    for (auto& j : *vars) {
      (*Y)(row, col) = (M(i, j) + M(j, i)) / 2.0;
      (*Y)(col, row) = (*Y)(row, col);
      col++;
    }
    row++;
  }
}
SolverConfiguration APIConvertSolverConfiguration(
    const CONEX_SolverConfiguration* config) {
  SolverConfiguration c;
  c.prepare_dual_variables = config->prepare_dual_variables;
  c.initialization_mode = config->initialization_mode;
  c.inv_sqrt_mu_max = config->inv_sqrt_mu_max;
  c.minimum_mu = config->minimum_mu;
  c.maximum_mu = config->maximum_mu;
  c.divergence_upper_bound = config->divergence_upper_bound;
  c.enable_line_search = config->enable_line_search;
  c.dinf_upper_bound = config->dinf_upper_bound;
  c.final_centering_steps = config->final_centering_steps;
  c.final_centering_tolerance = config->final_centering_tolerance;
  c.initial_centering_steps_warmstart =
      config->initial_centering_steps_warmstart;
  c.initial_centering_steps_coldstart =
      config->initial_centering_steps_coldstart;
  c.warmstart_abort_threshold = config->warmstart_abort_threshold;
  c.max_iterations = config->max_iterations;
  c.iterative_refinement_iterations = config->iterative_refinement_iterations;
  c.infeasibility_threshold = config->infeasibility_threshold;
  c.kkt_error_tolerance = config->kkt_error_tolerance;
  c.enable_rescaling = config->enable_rescaling;
  c.kkt_solver = config->kkt_solver;
  return c;
}
}  // namespace

int CONEX_Maximize(void* prog_ptr, const double* b, int br,
                   const CONEX_SolverConfiguration* config_input, double* y,
                   int yr) {
  using InputMatrix = Eigen::Map<const DenseMatrix>;
  InputMatrix bmap(b, br, 1);
  DenseMatrix blinear = bmap;

  SolverConfiguration config = APIConvertSolverConfiguration(config_input);

  Program& prog = *reinterpret_cast<Program*>(prog_ptr);

  return Solve(blinear, prog, config, y);
}

int CONEX_Solve(void* prog_ptr, const CONEX_SolverConfiguration* config_input,
                double* y, int yr) {
  SolverConfiguration config = APIConvertSolverConfiguration(config_input);
  Program& prog = *reinterpret_cast<Program*>(prog_ptr);
  return Solve(prog, config, y);
}

void CONEX_GetDualVariable(void* prog_ptr, int i, double* x, int xr, int xc) {
  Program& prog = *reinterpret_cast<Program*>(prog_ptr);
  assert(prog.GetDualVariableSize(i) == xr * xc);

  using InputMatrix = Eigen::Map<DenseMatrix>;

  InputMatrix xmap(x, xr, xc);
  prog.GetDualVariable(i, &xmap);
}

int CONEX_GetDualVariableSize(void* prog_ptr, int i) {
  Program& prog = *reinterpret_cast<Program*>(prog_ptr);
  return prog.GetDualVariableSize(i);
}

void* CONEX_CreateConeProgram() {
  return reinterpret_cast<void*>(new Program(0));
}

void CONEX_DeleteConeProgram(void* prog) {
  delete reinterpret_cast<Program*>(prog);
}

int CONEX_AddDenseLMIConstraint(void* prog, const double* A, int Ar, int Ac,
                                int m, const double* c, int cr, int cc) {
  assert(Ar == Ac);
  assert(Ar == cr);
  assert(cc == cr);

  using InputMatrix = Eigen::Map<const DenseMatrix>;
  auto offset = A;
  std::vector<DenseMatrix> Avect;
  for (int i = 0; i < m; i++) {
    InputMatrix Amap(offset, Ar, Ac);
    Avect.push_back(Amap);
    offset += Ar * Ac;
  }
  InputMatrix Cmap(c, cr, cc);

  int n = cc;

  DenseLMIConstraint T3{n, Avect, Cmap};
  auto& program = *reinterpret_cast<Program*>(prog);
  int constraint_id = program.NumberOfConstraints();
  program.AddConstraint(T3);
  return constraint_id;
}

int CONEX_AddSparseLMIConstraint(void* prog, const double* A, int Ar, int Ac,
                                 int num_vars, const double* c, int cr, int cc,
                                 const long* vars, int vars_rows) {
  assert(Ar == Ac);
  assert(Ar == cr);
  assert(cc == cr);
  assert(vars_rows == num_vars);

  // TODO(FrankPermenter): Remove these copies.
  using InputMatrix = Eigen::Map<const DenseMatrix>;
  auto offset = A;
  std::vector<DenseMatrix> Avect;
  std::vector<int> variables(num_vars);
  for (int i = 0; i < num_vars; i++) {
    InputMatrix Amap(offset, Ar, Ac);
    Avect.push_back(Amap);
    offset += Ar * Ac;
    variables.at(i) = *(vars + i);
  }
  InputMatrix Cmap(c, cr, cc);

  conex::DenseLMIConstraint T3{Avect, Cmap};
  auto& program = *reinterpret_cast<Program*>(prog);
  int constraint_id = program.NumberOfConstraints();
  program.AddConstraint(T3, variables);
  return constraint_id;
}

int CONEX_AddLinearInequalities(void* prog, const double* A, int Ar, int Ac,
                                const double* lb, int num_lb, const double* ub,
                                int num_ub) {
  assert(Ar == num_lb);
  assert(Ar == num_ub);
  Eigen::MatrixXd Ain = Eigen::Map<const Eigen::MatrixXd>(A, Ar, Ac);
  Eigen::MatrixXd lbin = Eigen::Map<const Eigen::MatrixXd>(lb, Ar, 1);
  Eigen::MatrixXd ubin = Eigen::Map<const Eigen::MatrixXd>(ub, Ar, 1);

  Eigen::MatrixXd Aeq;
  Eigen::MatrixXd beq;
  Eigen::MatrixXd Aineq;
  Eigen::MatrixXd bineq;
  conex::PreprocessLinearInequality(Ain, lbin, ubin, &Aineq, &bineq, &Aeq,
                                    &beq);

  auto& program = *reinterpret_cast<Program*>(prog);
  if (Aineq.rows() > 0) {
    program.AddConstraint(conex::LinearConstraint(Aineq, bineq));
  }
  if (Aeq.rows() > 0) {
    program.AddConstraint(conex::EqualityConstraints(Aeq, beq));
  }
  // TODO(FrankPermenter): Return the correct ID.
  return -1;
}
int CONEX_AddDenseLinearConstraint(void* prog, const double* A, int Ar, int Ac,
                                   const double* c, int cr) {
  assert(Ar == cr);

  int n = Ar;
  int m = Ac;

  conex::LinearConstraint T3{n, m, A, c};
  auto& program = *reinterpret_cast<Program*>(prog);

  int constraint_id = program.NumberOfConstraints();
  program.AddConstraint(T3);
  return constraint_id;
}

void CONEX_SetDefaultOptions(CONEX_SolverConfiguration* c) {
  if (c == NULL) {
    std::cerr << "Received null pointer.";
    return;
  }
  SolverConfiguration config;

  c->prepare_dual_variables = config.prepare_dual_variables;
  c->initialization_mode = config.initialization_mode;
  c->inv_sqrt_mu_max = config.inv_sqrt_mu_max;
  c->minimum_mu = config.minimum_mu;
  c->maximum_mu = config.maximum_mu;
  c->divergence_upper_bound = config.divergence_upper_bound;
  c->enable_line_search = config.enable_line_search;
  c->dinf_upper_bound = config.dinf_upper_bound;
  c->final_centering_steps = config.final_centering_steps;
  c->final_centering_tolerance = config.final_centering_tolerance;
  c->initial_centering_steps_warmstart =
      config.initial_centering_steps_warmstart;
  c->initial_centering_steps_coldstart =
      config.initial_centering_steps_coldstart;
  c->warmstart_abort_threshold = config.warmstart_abort_threshold;
  c->max_iterations = config.max_iterations;
  c->infeasibility_threshold = config.infeasibility_threshold;
  c->kkt_error_tolerance = config.kkt_error_tolerance;
  c->enable_rescaling = config.enable_rescaling;
}

void CONEX_GetIterationStats(void* prog, CONEX_IterationStats* stats,
                             int iter_num_circular) {
  if ((prog == NULL) || (stats == NULL)) {
    std::cerr << "Received null pointer.";
    return;
  }

  auto& program = *reinterpret_cast<Program*>(prog);

  if (!program.stats->IsInitialized()) {
    std::cerr << "No statistics available.";
    return;
  }

  int iter_num = iter_num_circular;
  if (iter_num_circular < 0) {
    iter_num = program.stats->num_iter + iter_num_circular;
  }

  if ((program.stats->num_iter <= iter_num) || (iter_num < 0)) {
    std::cerr << "Specified iteration is out of bounds.";
    return;
  }
  stats->mu = 1.0 / (program.stats->sqrt_inv_mu[iter_num] *
                     program.stats->sqrt_inv_mu[iter_num]);
  stats->iteration_number = iter_num;
}

CONEX_STATUS CONEX_NewLinearMatrixInequality(void* p, int order,
                                             int hyper_complex_dim,
                                             int* constraint_id) {
  CONEX_DEMAND(order >= 1, "Invalid LMI dimensions.");
  CONEX_DEMAND(constraint_id, "Received output null pointer.");
  CONEX_DEMAND(hyper_complex_dim == 1 || hyper_complex_dim == 2 ||
                   hyper_complex_dim == 4 || hyper_complex_dim == 8,
               "Hypercomplex dimension must be 1, 2, 4, or 8.");

  Program* prg;
  SAFER_CAST_TO_Program(p, prg);

  switch (hyper_complex_dim) {
    case 1:
      prg->AddConstraint(HermitianPsdConstraint<conex::Real>(order));
      break;
    case 2:
      prg->AddConstraint(HermitianPsdConstraint<conex::Complex>(order));
      break;
    case 4:
      prg->AddConstraint(HermitianPsdConstraint<conex::Quaternions>(order));
      break;
    case 8:
      CONEX_DEMAND(order <= 3,
                   "Order of octonion algebra cannot be greater than 3.");
      prg->AddConstraint(HermitianPsdConstraint<conex::Octonions>(order));
  }
  *constraint_id = prg->NumberOfConstraints() - 1;
  return CONEX_SUCCESS;
}

CONEX_STATUS CONEX_NewLinearInequality(void* program, int num_rows,
                                       int* constraint_id) {
  CONEX_DEMAND(constraint_id, "Received output null pointer.");
  Program* prg;
  SAFER_CAST_TO_Program(program, prg);
  int n = prg->GetNumberOfVariables();
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_rows, n);
  Eigen::MatrixXd b = Eigen::MatrixXd::Zero(num_rows, 1);
  bool status = prg->AddConstraint(LinearConstraint{A, b});
  *constraint_id = prg->NumberOfConstraints() - 1;
  return status;
}

CONEX_STATUS CONEX_NewQuadraticCost(void* p, int* constraint_id) {
  CONEX_DEMAND(constraint_id, "Received output null pointer.");

  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  int n = prg->GetNumberOfVariables();
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(n, n);
  bool status = prg->AddQuadraticCost(Q);
  *constraint_id = prg->NumberOfConstraints() - 1;
  return status;
}

CONEX_STATUS CONEX_AddQuadraticCost(void* p, const double* A, int Ar, int Ac) {
  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  int n = prg->GetNumberOfVariables();
  Eigen::MatrixXd Q;
  std::vector<int> vars;

  Eigen::Map<const DenseMatrix> input(A, Ar, Ac);
  NonZeroSubMat(input, &vars, &Q);
  bool status = prg->AddQuadraticCost(Q, vars);
  return status;
}

CONEX_STATUS CONEX_UpdateQuadraticCostMatrix(void* p, int constraint,
                                             double value, int row, int col) {
  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  CONEX_DEMAND(constraint < prg->NumberOfConstraints(), "Invalid Constraint.");
  return prg->UpdateAffineTermOfConstraint(constraint, value, row, col,
                                           0 /*hyper complex dimension*/);
}

CONEX_STATUS CONEX_UpdateLinearOperator(void* p, int constraint, double value,
                                        int variable, int row, int col,
                                        int hyper_complex_dim) {
  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  CONEX_DEMAND(constraint < prg->NumberOfConstraints(), "Invalid Constraint.");
  return prg->UpdateLinearOperatorOfConstraint(constraint, value, variable, row,
                                               col, hyper_complex_dim);
}

CONEX_STATUS CONEX_UpdateAffineTerm(void* p, int constraint, double value,
                                    int row, int col, int hyper_complex_dim) {
  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  CONEX_DEMAND(constraint < prg->NumberOfConstraints(), "Invalid Constraint.");
  return prg->UpdateAffineTermOfConstraint(constraint, value, row, col,
                                           hyper_complex_dim);
}

CONEX_STATUS CONEX_NewLorentzConeConstraint(void* p, int order,
                                            int* constraint_id) {
  CONEX_DEMAND(
      order >= 1,
      "Received invalid n. Second order cone must have order (n + 1) >= 2.");
  CONEX_DEMAND(constraint_id, "Received output null pointer.");

  Program* prg;
  SAFER_CAST_TO_Program(p, prg);

  prg->AddConstraint(conex::SOCConstraint(order));
  *constraint_id = prg->NumberOfConstraints() - 1;
  return CONEX_SUCCESS;
}

CONEX_STATUS CONEX_SetNumberOfVariables(void* p, int number_of_variables) {
  CONEX_DEMAND(number_of_variables >= 1, "Number of variables must be > 0.");
  Program* prg;
  SAFER_CAST_TO_Program(p, prg);
  CONEX_DEMAND(prg->GetNumberOfVariables() == 0,
               "Number of variables already set.");
  prg->SetNumberOfVariables(number_of_variables);
  return CONEX_SUCCESS;
}


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018, 2019.
// Modifications copyright (c) 2017-2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_LABRD_HPP
#define BOOST_GEOMETRY_PROJECTIONS_LABRD_HPP

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_param.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace labrd
    {
            static const double epsilon = 1.e-10;

            template <typename T>
            struct par_labrd
            {
                T    Az, kRg, p0s, A, C, Ca, Cb, Cc, Cd;
            };

            template <typename T, typename Parameters>
            struct base_labrd_ellipsoid
            {
                par_labrd<T> m_proj_parm;

                // FORWARD(e_forward)
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(Parameters const& par, T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T fourth_pi = detail::fourth_pi<T>();

                    T V1, V2, ps, sinps, cosps, sinps2, cosps2;
                    T I1, I2, I3, I4, I5, I6, x2, y2, t;

                    V1 = this->m_proj_parm.A * log( tan(fourth_pi + .5 * lp_lat) );
                    t = par.e * sin(lp_lat);
                    V2 = .5 * par.e * this->m_proj_parm.A * log ((1. + t)/(1. - t));
                    ps = 2. * (atan(exp(V1 - V2 + this->m_proj_parm.C)) - fourth_pi);
                    I1 = ps - this->m_proj_parm.p0s;
                    cosps = cos(ps);    cosps2 = cosps * cosps;
                    sinps = sin(ps);    sinps2 = sinps * sinps;
                    I4 = this->m_proj_parm.A * cosps;
                    I2 = .5 * this->m_proj_parm.A * I4 * sinps;
                    I3 = I2 * this->m_proj_parm.A * this->m_proj_parm.A * (5. * cosps2 - sinps2) / 12.;
                    I6 = I4 * this->m_proj_parm.A * this->m_proj_parm.A;
                    I5 = I6 * (cosps2 - sinps2) / 6.;
                    I6 *= this->m_proj_parm.A * this->m_proj_parm.A *
                        (5. * cosps2 * cosps2 + sinps2 * (sinps2 - 18. * cosps2)) / 120.;
                    t = lp_lon * lp_lon;
                    xy_x = this->m_proj_parm.kRg * lp_lon * (I4 + t * (I5 + t * I6));
                    xy_y = this->m_proj_parm.kRg * (I1 + t * (I2 + t * I3));
                    x2 = xy_x * xy_x;
                    y2 = xy_y * xy_y;
                    V1 = 3. * xy_x * y2 - xy_x * x2;
                    V2 = xy_y * y2 - 3. * x2 * xy_y;
                    xy_x += this->m_proj_parm.Ca * V1 + this->m_proj_parm.Cb * V2;
                    xy_y += this->m_proj_parm.Ca * V2 - this->m_proj_parm.Cb * V1;
                }

                // INVERSE(e_inverse)  ellipsoid & spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(Parameters const& par, T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T fourth_pi = detail::fourth_pi<T>();

                    /* t = 0.0 optimization is to avoid a false positive cppcheck warning */
                    /* (cppcheck git beaf29c15867984aa3c2a15cf15bd7576ccde2b3). Might no */
                    /* longer be necessary with later versions. */
                    T x2, y2, V1, V2, V3, V4, t = 0.0, t2, ps, pe, tpe, s;
                    T I7, I8, I9, I10, I11, d, Re;
                    int i;

                    x2 = xy_x * xy_x;
                    y2 = xy_y * xy_y;
                    V1 = 3. * xy_x * y2 - xy_x * x2;
                    V2 = xy_y * y2 - 3. * x2 * xy_y;
                    V3 = xy_x * (5. * y2 * y2 + x2 * (-10. * y2 + x2 ));
                    V4 = xy_y * (5. * x2 * x2 + y2 * (-10. * x2 + y2 ));
                    xy_x += - this->m_proj_parm.Ca * V1 - this->m_proj_parm.Cb * V2 + this->m_proj_parm.Cc * V3 + this->m_proj_parm.Cd * V4;
                    xy_y +=   this->m_proj_parm.Cb * V1 - this->m_proj_parm.Ca * V2 - this->m_proj_parm.Cd * V3 + this->m_proj_parm.Cc * V4;
                    ps = this->m_proj_parm.p0s + xy_y / this->m_proj_parm.kRg;
                    pe = ps + par.phi0 - this->m_proj_parm.p0s;

                    for ( i = 20; i; --i) {
                        V1 = this->m_proj_parm.A * log(tan(fourth_pi + .5 * pe));
                        tpe = par.e * sin(pe);
                        V2 = .5 * par.e * this->m_proj_parm.A * log((1. + tpe)/(1. - tpe));
                        t = ps - 2. * (atan(exp(V1 - V2 + this->m_proj_parm.C)) - fourth_pi);
                        pe += t;
                        if (fabs(t) < epsilon)
                            break;
                    }

                    t = par.e * sin(pe);
                    t = 1. - t * t;
                    Re = par.one_es / ( t * sqrt(t) );
                    t = tan(ps);
                    t2 = t * t;
                    s = this->m_proj_parm.kRg * this->m_proj_parm.kRg;
                    d = Re * par.k0 * this->m_proj_parm.kRg;
                    I7 = t / (2. * d);
                    I8 = t * (5. + 3. * t2) / (24. * d * s);
                    d = cos(ps) * this->m_proj_parm.kRg * this->m_proj_parm.A;
                    I9 = 1. / d;
                    d *= s;
                    I10 = (1. + 2. * t2) / (6. * d);
                    I11 = (5. + t2 * (28. + 24. * t2)) / (120. * d * s);
                    x2 = xy_x * xy_x;
                    lp_lat = pe + x2 * (-I7 + I8 * x2);
                    lp_lon = xy_x * (I9 + x2 * (-I10 + x2 * I11));
                }

                static inline std::string get_name()
                {
                    return "labrd_ellipsoid";
                }

            };

            // Laborde
            template <typename Params, typename Parameters, typename T>
            inline void setup_labrd(Params const& params, Parameters const& par, par_labrd<T>& proj_parm)
            {
                static const T fourth_pi = detail::fourth_pi<T>();

                T Az, sinp, R, N, t;

                Az = pj_get_param_r<T, srs::spar::azi>(params, "azi", srs::dpar::azi);
                sinp = sin(par.phi0);
                t = 1. - par.es * sinp * sinp;
                N = 1. / sqrt(t);
                R = par.one_es * N / t;
                proj_parm.kRg = par.k0 * sqrt( N * R );
                proj_parm.p0s = atan( sqrt(R / N) * tan(par.phi0) );
                proj_parm.A = sinp / sin(proj_parm.p0s);
                t = par.e * sinp;
                proj_parm.C = .5 * par.e * proj_parm.A * log((1. + t)/(1. - t)) +
                    - proj_parm.A * log( tan(fourth_pi + .5 * par.phi0))
                    + log( tan(fourth_pi + .5 * proj_parm.p0s));
                t = Az + Az;
                proj_parm.Ca = (1. - cos(t)) * ( proj_parm.Cb = 1. / (12. * proj_parm.kRg * proj_parm.kRg) );
                proj_parm.Cb *= sin(t);
                proj_parm.Cc = 3. * (proj_parm.Ca * proj_parm.Ca - proj_parm.Cb * proj_parm.Cb);
                proj_parm.Cd = 6. * proj_parm.Ca * proj_parm.Cb;
            }

    }} // namespace detail::labrd
    #endif // doxygen

    /*!
        \brief Laborde projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Special for Madagascar
        \par Projection parameters
         - no_rot: No rotation (boolean)
         - azi: Azimuth (or Gamma) (degrees)
        \par Example
        \image html ex_labrd.gif
    */
    template <typename T, typename Parameters>
    struct labrd_ellipsoid : public detail::labrd::base_labrd_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline labrd_ellipsoid(Params const& params, Parameters const& par)
        {
            detail::labrd::setup_labrd(params, par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION_FI(srs::spar::proj_labrd, labrd_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(labrd_entry, labrd_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(labrd_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(labrd, labrd_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_LABRD_HPP