// Copyright Michael Drexl 2005, 2006.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(disable : 4267)
#endif

#include <boost/graph/adjacency_list.hpp>
//#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <boost/graph/r_c_shortest_paths.hpp>
#include <iostream>
#include <boost/core/lightweight_test.hpp>

using namespace boost;

struct SPPRC_Example_Graph_Vert_Prop
{
    SPPRC_Example_Graph_Vert_Prop(int n = 0, int e = 0, int l = 0)
    : num(n), eat(e), lat(l)
    {
    }
    int num;
    // earliest arrival time
    int eat;
    // latest arrival time
    int lat;
};

struct SPPRC_Example_Graph_Arc_Prop
{
    SPPRC_Example_Graph_Arc_Prop(int n = 0, int c = 0, int t = 0)
    : num(n), cost(c), time(t)
    {
    }
    int num;
    // traversal cost
    int cost;
    // traversal time
    int time;
};

typedef adjacency_list< vecS, vecS, directedS, SPPRC_Example_Graph_Vert_Prop,
    SPPRC_Example_Graph_Arc_Prop >
    SPPRC_Example_Graph;

// data structures for spp without resource constraints:
// ResourceContainer model
struct spp_no_rc_res_cont
{
    spp_no_rc_res_cont(int c = 0) : cost(c) {};
    spp_no_rc_res_cont& operator=(const spp_no_rc_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_no_rc_res_cont();
        new (this) spp_no_rc_res_cont(other);
        return *this;
    }
    int cost;
};

bool operator==(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost);
}

bool operator<(
    const spp_no_rc_res_cont& res_cont_1, const spp_no_rc_res_cont& res_cont_2)
{
    return (res_cont_1.cost < res_cont_2.cost);
}

// ResourceExtensionFunction model
class ref_no_res_cont
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_no_rc_res_cont& new_cont, const spp_no_rc_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        new_cont.cost = old_cont.cost + g[ed].cost;
        return true;
    }
};

// DominanceFunction model
class dominance_no_res_cont
{
public:
    inline bool operator()(const spp_no_rc_res_cont& res_cont_1,
        const spp_no_rc_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for spp without resource constraints:

// data structures for shortest path problem with time windows (spptw)
// ResourceContainer model
struct spp_spptw_res_cont
{
    spp_spptw_res_cont(int c = 0, int t = 0) : cost(c), time(t) {}
    spp_spptw_res_cont& operator=(const spp_spptw_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_spptw_res_cont();
        new (this) spp_spptw_res_cont(other);
        return *this;
    }
    int cost;
    int time;
};

bool operator==(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    return (res_cont_1.cost == res_cont_2.cost
        && res_cont_1.time == res_cont_2.time);
}

bool operator<(
    const spp_spptw_res_cont& res_cont_1, const spp_spptw_res_cont& res_cont_2)
{
    if (res_cont_1.cost > res_cont_2.cost)
        return false;
    if (res_cont_1.cost == res_cont_2.cost)
        return res_cont_1.time < res_cont_2.time;
    return true;
}

// ResourceExtensionFunction model
class ref_spptw
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_spptw_res_cont& new_cont, const spp_spptw_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        const SPPRC_Example_Graph_Arc_Prop& arc_prop = get(edge_bundle, g)[ed];
        const SPPRC_Example_Graph_Vert_Prop& vert_prop
            = get(vertex_bundle, g)[target(ed, g)];
        new_cont.cost = old_cont.cost + arc_prop.cost;
        int& i_time = new_cont.time;
        i_time = old_cont.time + arc_prop.time;
        i_time < vert_prop.eat ? i_time = vert_prop.eat : 0;
        return i_time <= vert_prop.lat ? true : false;
    }
};

// DominanceFunction model
class dominance_spptw
{
public:
    inline bool operator()(const spp_spptw_res_cont& res_cont_1,
        const spp_spptw_res_cont& res_cont_2) const
    {
        // must be "<=" here!!!
        // must NOT be "<"!!!
        return res_cont_1.cost <= res_cont_2.cost
            && res_cont_1.time <= res_cont_2.time;
        // this is not a contradiction to the documentation
        // the documentation says:
        // "A label $l_1$ dominates a label $l_2$ if and only if both are
        // resident at the same vertex, and if, for each resource, the resource
        // consumption of $l_1$ is less than or equal to the resource
        // consumption of $l_2$, and if there is at least one resource where
        // $l_1$ has a lower resource consumption than $l_2$." one can think of
        // a new label with a resource consumption equal to that of an old label
        // as being dominated by that old label, because the new one will have a
        // higher number and is created at a later point in time, so one can
        // implicitly use the number or the creation time as a resource for
        // tie-breaking
    }
};
// end data structures for shortest path problem with time windows (spptw)

struct spp_spptw_marked_res_cont
{
    spp_spptw_marked_res_cont(
        SPPRC_Example_Graph::vertex_descriptor v, int c = 0, int t = 0)
    : cost(c), time(t), marked()
    {
        marked.insert(v);
    }
    spp_spptw_marked_res_cont& operator=(const spp_spptw_marked_res_cont& other)
    {
        if (this == &other)
            return *this;
        this->~spp_spptw_marked_res_cont();
        new (this) spp_spptw_marked_res_cont(other);
        return *this;
    }
    int cost;
    int time;
    std::set< SPPRC_Example_Graph::vertex_descriptor > marked;
};

bool operator==(const spp_spptw_marked_res_cont& res_cont_1,
    const spp_spptw_marked_res_cont& res_cont_2)
{
    return res_cont_1.cost == res_cont_2.cost
        && res_cont_1.time == res_cont_2.time
        && res_cont_1.marked == res_cont_2.marked;
}

bool operator<(const spp_spptw_marked_res_cont& res_cont_1,
    const spp_spptw_marked_res_cont& res_cont_2)
{
    if (res_cont_1.cost > res_cont_2.cost || res_cont_1.time > res_cont_2.time)
    {
        return false;
    }

    if (!std::includes(res_cont_2.marked.begin(), res_cont_2.marked.end(),
            res_cont_1.marked.begin(), res_cont_1.marked.end()))
    {
        return false;
    }

    if (res_cont_1.cost == res_cont_2.cost)
    {
        return res_cont_1.time < res_cont_2.time;
    }
    return true;
}

class ref_spptw_marked
{
public:
    inline bool operator()(const SPPRC_Example_Graph& g,
        spp_spptw_marked_res_cont& new_cont,
        const spp_spptw_marked_res_cont& old_cont,
        graph_traits< SPPRC_Example_Graph >::edge_descriptor ed) const
    {
        const graph_traits< SPPRC_Example_Graph >::vertex_descriptor dest
            = target(ed, g);

        if (old_cont.marked.find(dest) != old_cont.marked.end())
        {
            return false;
        }

        const SPPRC_Example_Graph_Arc_Prop& arc_prop = get(edge_bundle, g)[ed];
        const SPPRC_Example_Graph_Vert_Prop& vert_prop
            = get(vertex_bundle, g)[dest];
        new_cont.cost = old_cont.cost + arc_prop.cost;
        new_cont.marked = old_cont.marked;
        new_cont.marked.insert(dest);
        int& i_time = new_cont.time;
        i_time = old_cont.time + arc_prop.time;
        i_time < vert_prop.eat ? i_time = vert_prop.eat : 0;
        return i_time <= vert_prop.lat;
    }
};

class dominance_spptw_marked
{
public:
    inline bool operator()(const spp_spptw_marked_res_cont& res_cont_1,
        const spp_spptw_marked_res_cont& res_cont_2) const
    {
        return res_cont_1.time <= res_cont_2.time
            && res_cont_1.cost <= res_cont_2.cost
            && std::includes(res_cont_1.marked.begin(), res_cont_1.marked.end(),
                res_cont_2.marked.begin(), res_cont_2.marked.end());
    }
};

int main(int, char*[])
{
    SPPRC_Example_Graph g;
    add_vertex(SPPRC_Example_Graph_Vert_Prop(0, 0, 1000000000), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(1, 56, 142), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(2, 0, 1000000000), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(3, 89, 178), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(4, 0, 1000000000), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(5, 49, 76), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(6, 0, 1000000000), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(7, 98, 160), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(8, 0, 1000000000), g);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(9, 90, 158), g);
    add_edge(0, 7, SPPRC_Example_Graph_Arc_Prop(6, 33, 2), g);
    add_edge(0, 6, SPPRC_Example_Graph_Arc_Prop(5, 31, 6), g);
    add_edge(0, 4, SPPRC_Example_Graph_Arc_Prop(3, 14, 4), g);
    add_edge(0, 1, SPPRC_Example_Graph_Arc_Prop(0, 43, 8), g);
    add_edge(0, 4, SPPRC_Example_Graph_Arc_Prop(4, 28, 10), g);
    add_edge(0, 3, SPPRC_Example_Graph_Arc_Prop(1, 31, 10), g);
    add_edge(0, 3, SPPRC_Example_Graph_Arc_Prop(2, 1, 7), g);
    add_edge(0, 9, SPPRC_Example_Graph_Arc_Prop(7, 25, 9), g);
    add_edge(1, 0, SPPRC_Example_Graph_Arc_Prop(8, 37, 4), g);
    add_edge(1, 6, SPPRC_Example_Graph_Arc_Prop(9, 7, 3), g);
    add_edge(2, 6, SPPRC_Example_Graph_Arc_Prop(12, 6, 7), g);
    add_edge(2, 3, SPPRC_Example_Graph_Arc_Prop(10, 13, 7), g);
    add_edge(2, 3, SPPRC_Example_Graph_Arc_Prop(11, 49, 9), g);
    add_edge(2, 8, SPPRC_Example_Graph_Arc_Prop(13, 47, 5), g);
    add_edge(3, 4, SPPRC_Example_Graph_Arc_Prop(17, 5, 10), g);
    add_edge(3, 1, SPPRC_Example_Graph_Arc_Prop(15, 47, 1), g);
    add_edge(3, 2, SPPRC_Example_Graph_Arc_Prop(16, 26, 9), g);
    add_edge(3, 9, SPPRC_Example_Graph_Arc_Prop(21, 24, 10), g);
    add_edge(3, 7, SPPRC_Example_Graph_Arc_Prop(20, 50, 10), g);
    add_edge(3, 0, SPPRC_Example_Graph_Arc_Prop(14, 41, 4), g);
    add_edge(3, 6, SPPRC_Example_Graph_Arc_Prop(19, 6, 1), g);
    add_edge(3, 4, SPPRC_Example_Graph_Arc_Prop(18, 8, 1), g);
    add_edge(4, 5, SPPRC_Example_Graph_Arc_Prop(26, 38, 4), g);
    add_edge(4, 9, SPPRC_Example_Graph_Arc_Prop(27, 32, 10), g);
    add_edge(4, 3, SPPRC_Example_Graph_Arc_Prop(24, 40, 3), g);
    add_edge(4, 0, SPPRC_Example_Graph_Arc_Prop(22, 7, 3), g);
    add_edge(4, 3, SPPRC_Example_Graph_Arc_Prop(25, 28, 9), g);
    add_edge(4, 2, SPPRC_Example_Graph_Arc_Prop(23, 39, 6), g);
    add_edge(5, 8, SPPRC_Example_Graph_Arc_Prop(32, 6, 2), g);
    add_edge(5, 2, SPPRC_Example_Graph_Arc_Prop(30, 26, 10), g);
    add_edge(5, 0, SPPRC_Example_Graph_Arc_Prop(28, 38, 9), g);
    add_edge(5, 2, SPPRC_Example_Graph_Arc_Prop(31, 48, 10), g);
    add_edge(5, 9, SPPRC_Example_Graph_Arc_Prop(33, 49, 2), g);
    add_edge(5, 1, SPPRC_Example_Graph_Arc_Prop(29, 22, 7), g);
    add_edge(6, 1, SPPRC_Example_Graph_Arc_Prop(34, 15, 7), g);
    add_edge(6, 7, SPPRC_Example_Graph_Arc_Prop(35, 20, 3), g);
    add_edge(7, 9, SPPRC_Example_Graph_Arc_Prop(40, 1, 3), g);
    add_edge(7, 0, SPPRC_Example_Graph_Arc_Prop(36, 23, 5), g);
    add_edge(7, 6, SPPRC_Example_Graph_Arc_Prop(38, 36, 2), g);
    add_edge(7, 6, SPPRC_Example_Graph_Arc_Prop(39, 18, 10), g);
    add_edge(7, 2, SPPRC_Example_Graph_Arc_Prop(37, 2, 1), g);
    add_edge(8, 5, SPPRC_Example_Graph_Arc_Prop(46, 36, 5), g);
    add_edge(8, 1, SPPRC_Example_Graph_Arc_Prop(42, 13, 10), g);
    add_edge(8, 0, SPPRC_Example_Graph_Arc_Prop(41, 40, 5), g);
    add_edge(8, 1, SPPRC_Example_Graph_Arc_Prop(43, 32, 8), g);
    add_edge(8, 6, SPPRC_Example_Graph_Arc_Prop(47, 25, 1), g);
    add_edge(8, 2, SPPRC_Example_Graph_Arc_Prop(44, 44, 3), g);
    add_edge(8, 3, SPPRC_Example_Graph_Arc_Prop(45, 11, 9), g);
    add_edge(9, 0, SPPRC_Example_Graph_Arc_Prop(48, 41, 5), g);
    add_edge(9, 1, SPPRC_Example_Graph_Arc_Prop(49, 44, 7), g);

    // spp without resource constraints

    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions;
    std::vector< spp_no_rc_res_cont > pareto_opt_rcs_no_rc;
    std::vector< int > i_vec_opt_solutions_spp_no_rc;
    // std::cout << "r_c_shortest_paths:" << std::endl;
    for (int s = 0; s < 10; ++s)
    {
        for (int t = 0; t < 10; ++t)
        {
            r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
                get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t, opt_solutions,
                pareto_opt_rcs_no_rc, spp_no_rc_res_cont(0), ref_no_res_cont(),
                dominance_no_res_cont(),
                std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
                    spp_no_rc_res_cont > >(),
                default_r_c_shortest_paths_visitor());
            i_vec_opt_solutions_spp_no_rc.push_back(
                pareto_opt_rcs_no_rc[0].cost);
            // std::cout << "From " << s << " to " << t << ": ";
            // std::cout << pareto_opt_rcs_no_rc[0].cost << std::endl;
        }
    }

    // std::vector<graph_traits<SPPRC_Example_Graph>::vertex_descriptor>
    //  p( num_vertices( g ) );
    // std::vector<int> d( num_vertices( g ) );
    // std::vector<int> i_vec_dijkstra_distances;
    // std::cout << "Dijkstra:" << std::endl;
    // for( int s = 0; s < 10; ++s )
    //{
    //  dijkstra_shortest_paths( g,
    //                           s,
    //                           &p[0],
    //                           &d[0],
    //                           get( &SPPRC_Example_Graph_Arc_Prop::cost, g ),
    //                           get( &SPPRC_Example_Graph_Vert_Prop::num, g ),
    //                           std::less<int>(),
    //                           closed_plus<int>(),
    //                           (std::numeric_limits<int>::max)(),
    //                           0,
    //                           default_dijkstra_visitor() );
    //  for( int t = 0; t < 10; ++t )
    //  {
    //    i_vec_dijkstra_distances.push_back( d[t] );
    //    std::cout << "From " << s << " to " << t << ": " << d[t] << std::endl;
    //  }
    //}

    std::vector< int > i_vec_correct_solutions;
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(22);
    i_vec_correct_solutions.push_back(27);
    i_vec_correct_solutions.push_back(1);
    i_vec_correct_solutions.push_back(6);
    i_vec_correct_solutions.push_back(44);
    i_vec_correct_solutions.push_back(7);
    i_vec_correct_solutions.push_back(27);
    i_vec_correct_solutions.push_back(50);
    i_vec_correct_solutions.push_back(25);
    i_vec_correct_solutions.push_back(37);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(29);
    i_vec_correct_solutions.push_back(38);
    i_vec_correct_solutions.push_back(43);
    i_vec_correct_solutions.push_back(81);
    i_vec_correct_solutions.push_back(7);
    i_vec_correct_solutions.push_back(27);
    i_vec_correct_solutions.push_back(76);
    i_vec_correct_solutions.push_back(28);
    i_vec_correct_solutions.push_back(25);
    i_vec_correct_solutions.push_back(21);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(13);
    i_vec_correct_solutions.push_back(18);
    i_vec_correct_solutions.push_back(56);
    i_vec_correct_solutions.push_back(6);
    i_vec_correct_solutions.push_back(26);
    i_vec_correct_solutions.push_back(47);
    i_vec_correct_solutions.push_back(27);
    i_vec_correct_solutions.push_back(12);
    i_vec_correct_solutions.push_back(21);
    i_vec_correct_solutions.push_back(26);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(5);
    i_vec_correct_solutions.push_back(43);
    i_vec_correct_solutions.push_back(6);
    i_vec_correct_solutions.push_back(26);
    i_vec_correct_solutions.push_back(49);
    i_vec_correct_solutions.push_back(24);
    i_vec_correct_solutions.push_back(7);
    i_vec_correct_solutions.push_back(29);
    i_vec_correct_solutions.push_back(34);
    i_vec_correct_solutions.push_back(8);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(38);
    i_vec_correct_solutions.push_back(14);
    i_vec_correct_solutions.push_back(34);
    i_vec_correct_solutions.push_back(44);
    i_vec_correct_solutions.push_back(32);
    i_vec_correct_solutions.push_back(29);
    i_vec_correct_solutions.push_back(19);
    i_vec_correct_solutions.push_back(26);
    i_vec_correct_solutions.push_back(17);
    i_vec_correct_solutions.push_back(22);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(23);
    i_vec_correct_solutions.push_back(43);
    i_vec_correct_solutions.push_back(6);
    i_vec_correct_solutions.push_back(41);
    i_vec_correct_solutions.push_back(43);
    i_vec_correct_solutions.push_back(15);
    i_vec_correct_solutions.push_back(22);
    i_vec_correct_solutions.push_back(35);
    i_vec_correct_solutions.push_back(40);
    i_vec_correct_solutions.push_back(78);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(20);
    i_vec_correct_solutions.push_back(69);
    i_vec_correct_solutions.push_back(21);
    i_vec_correct_solutions.push_back(23);
    i_vec_correct_solutions.push_back(23);
    i_vec_correct_solutions.push_back(2);
    i_vec_correct_solutions.push_back(15);
    i_vec_correct_solutions.push_back(20);
    i_vec_correct_solutions.push_back(58);
    i_vec_correct_solutions.push_back(8);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(49);
    i_vec_correct_solutions.push_back(1);
    i_vec_correct_solutions.push_back(23);
    i_vec_correct_solutions.push_back(13);
    i_vec_correct_solutions.push_back(37);
    i_vec_correct_solutions.push_back(11);
    i_vec_correct_solutions.push_back(16);
    i_vec_correct_solutions.push_back(36);
    i_vec_correct_solutions.push_back(17);
    i_vec_correct_solutions.push_back(37);
    i_vec_correct_solutions.push_back(0);
    i_vec_correct_solutions.push_back(35);
    i_vec_correct_solutions.push_back(41);
    i_vec_correct_solutions.push_back(44);
    i_vec_correct_solutions.push_back(68);
    i_vec_correct_solutions.push_back(42);
    i_vec_correct_solutions.push_back(47);
    i_vec_correct_solutions.push_back(85);
    i_vec_correct_solutions.push_back(48);
    i_vec_correct_solutions.push_back(68);
    i_vec_correct_solutions.push_back(91);
    i_vec_correct_solutions.push_back(0);
    BOOST_TEST(
        i_vec_opt_solutions_spp_no_rc.size() == i_vec_correct_solutions.size());
    for (int i = 0; i < static_cast< int >(i_vec_correct_solutions.size()); ++i)
        BOOST_TEST(
            i_vec_opt_solutions_spp_no_rc[i] == i_vec_correct_solutions[i]);

    // spptw
    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        opt_solutions_spptw;
    std::vector< spp_spptw_res_cont > pareto_opt_rcs_spptw;
    std::vector< std::vector< std::vector< std::vector<
        graph_traits< SPPRC_Example_Graph >::edge_descriptor > > > >
        vec_vec_vec_vec_opt_solutions_spptw(10);

    for (int s = 0; s < 10; ++s)
    {
        for (int t = 0; t < 10; ++t)
        {
            r_c_shortest_paths(g, get(&SPPRC_Example_Graph_Vert_Prop::num, g),
                get(&SPPRC_Example_Graph_Arc_Prop::num, g), s, t,
                opt_solutions_spptw, pareto_opt_rcs_spptw,
                // be careful, do not simply take 0 as initial value for time
                spp_spptw_res_cont(0, g[s].eat), ref_spptw(), dominance_spptw(),
                std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
                    spp_spptw_res_cont > >(),
                default_r_c_shortest_paths_visitor());
            vec_vec_vec_vec_opt_solutions_spptw[s].push_back(
                opt_solutions_spptw);
            if (opt_solutions_spptw.size())
            {
                bool b_is_a_path_at_all = false;
                bool b_feasible = false;
                bool b_correctly_extended = false;
                spp_spptw_res_cont actual_final_resource_levels(0, 0);
                graph_traits< SPPRC_Example_Graph >::edge_descriptor
                    ed_last_extended_arc;
                check_r_c_path(g, opt_solutions_spptw[0],
                    spp_spptw_res_cont(0, g[s].eat), true,
                    pareto_opt_rcs_spptw[0], actual_final_resource_levels,
                    ref_spptw(), b_is_a_path_at_all, b_feasible,
                    b_correctly_extended, ed_last_extended_arc);
                BOOST_TEST(
                    b_is_a_path_at_all && b_feasible && b_correctly_extended);
                b_is_a_path_at_all = false;
                b_feasible = false;
                b_correctly_extended = false;
                spp_spptw_res_cont actual_final_resource_levels2(0, 0);
                graph_traits< SPPRC_Example_Graph >::edge_descriptor
                    ed_last_extended_arc2;
                check_r_c_path(g, opt_solutions_spptw[0],
                    spp_spptw_res_cont(0, g[s].eat), false,
                    pareto_opt_rcs_spptw[0], actual_final_resource_levels2,
                    ref_spptw(), b_is_a_path_at_all, b_feasible,
                    b_correctly_extended, ed_last_extended_arc2);
                BOOST_TEST(
                    b_is_a_path_at_all && b_feasible && b_correctly_extended);
            }
        }
    }

    std::vector< int > i_vec_correct_num_solutions_spptw;
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(0);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(5);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(0);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(0);
    i_vec_correct_num_solutions_spptw.push_back(2);
    i_vec_correct_num_solutions_spptw.push_back(3);
    i_vec_correct_num_solutions_spptw.push_back(4);
    i_vec_correct_num_solutions_spptw.push_back(1);
    for (int s = 0; s < 10; ++s)
        for (int t = 0; t < 10; ++t)
            BOOST_TEST(static_cast< int >(
                            vec_vec_vec_vec_opt_solutions_spptw[s][t].size())
                == i_vec_correct_num_solutions_spptw[10 * s + t]);

    // one pareto-optimal solution
    SPPRC_Example_Graph g2;
    add_vertex(SPPRC_Example_Graph_Vert_Prop(0, 0, 1000000000), g2);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(1, 0, 1000000000), g2);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(2, 0, 1000000000), g2);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(3, 0, 1000000000), g2);
    add_edge(0, 1, SPPRC_Example_Graph_Arc_Prop(0, 1, 1), g2);
    add_edge(0, 2, SPPRC_Example_Graph_Arc_Prop(1, 2, 1), g2);
    add_edge(1, 3, SPPRC_Example_Graph_Arc_Prop(2, 3, 1), g2);
    add_edge(2, 3, SPPRC_Example_Graph_Arc_Prop(3, 1, 1), g2);
    std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor >
        opt_solution;
    spp_spptw_res_cont pareto_opt_rc;
    r_c_shortest_paths(g2, get(&SPPRC_Example_Graph_Vert_Prop::num, g2),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g2), 0, 3, opt_solution,
        pareto_opt_rc, spp_spptw_res_cont(0, 0), ref_spptw(), dominance_spptw(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_spptw_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    BOOST_TEST(pareto_opt_rc.cost == 3);

    SPPRC_Example_Graph g3;
    add_vertex(SPPRC_Example_Graph_Vert_Prop(0, 0, 1000), g3);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(1, 0, 1000), g3);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(2, 0, 974), g3);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(3, 0, 972), g3);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(4, 0, 967), g3);
    add_vertex(SPPRC_Example_Graph_Vert_Prop(5, 678, 801), g3);
    add_edge(0, 2, SPPRC_Example_Graph_Arc_Prop(0, 0, 16), g3);
    add_edge(0, 3, SPPRC_Example_Graph_Arc_Prop(1, 0, 18), g3);
    add_edge(0, 4, SPPRC_Example_Graph_Arc_Prop(2, 0, 23), g3);
    add_edge(0, 5, SPPRC_Example_Graph_Arc_Prop(3, 0, 25), g3);
    add_edge(2, 3, SPPRC_Example_Graph_Arc_Prop(4, 0, 33), g3);
    add_edge(2, 4, SPPRC_Example_Graph_Arc_Prop(5, 0, 15), g3);
    add_edge(2, 5, SPPRC_Example_Graph_Arc_Prop(6, 0, 33), g3);
    add_edge(2, 1, SPPRC_Example_Graph_Arc_Prop(7, 0, 16), g3);
    add_edge(3, 2, SPPRC_Example_Graph_Arc_Prop(8, 0, 33), g3);
    add_edge(3, 4, SPPRC_Example_Graph_Arc_Prop(9, 0, 35), g3);
    add_edge(3, 5, SPPRC_Example_Graph_Arc_Prop(10, 0, 21), g3);
    add_edge(3, 1, SPPRC_Example_Graph_Arc_Prop(11, 0, 18), g3);
    add_edge(4, 2, SPPRC_Example_Graph_Arc_Prop(12, 0, 15), g3);
    add_edge(4, 3, SPPRC_Example_Graph_Arc_Prop(13, 0, 35), g3);
    add_edge(4, 5, SPPRC_Example_Graph_Arc_Prop(14, 0, 25), g3);
    add_edge(4, 1, SPPRC_Example_Graph_Arc_Prop(15, 0, 23), g3);
    add_edge(5, 2, SPPRC_Example_Graph_Arc_Prop(16, 0, 33), g3);
    add_edge(5, 3, SPPRC_Example_Graph_Arc_Prop(17, 0, 21), g3);
    add_edge(5, 4, SPPRC_Example_Graph_Arc_Prop(18, 0, 25), g3);
    add_edge(5, 1, SPPRC_Example_Graph_Arc_Prop(19, 0, 25), g3);

    std::vector<
        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor > >
        pareto_opt_marked_solutions;
    std::vector< spp_spptw_marked_res_cont >
        pareto_opt_marked_resource_containers;

    graph_traits< SPPRC_Example_Graph >::vertex_descriptor g3_source = 0,
                                                           g3_target = 1;
    r_c_shortest_paths(g3, get(&SPPRC_Example_Graph_Vert_Prop::num, g3),
        get(&SPPRC_Example_Graph_Arc_Prop::num, g3), g3_source, g3_target,
        pareto_opt_marked_solutions, pareto_opt_marked_resource_containers,
        spp_spptw_marked_res_cont(0, 0, 0), ref_spptw_marked(),
        dominance_spptw_marked(),
        std::allocator< r_c_shortest_paths_label< SPPRC_Example_Graph,
            spp_spptw_marked_res_cont > >(),
        default_r_c_shortest_paths_visitor());

    BOOST_TEST(!pareto_opt_marked_solutions.empty());
    std::vector< std::vector<
        graph_traits< SPPRC_Example_Graph >::edge_descriptor > >::const_iterator
        path_it,
        path_end_it;
    for (path_it = pareto_opt_marked_solutions.begin(),
        path_end_it = pareto_opt_marked_solutions.end();
         path_it != path_end_it; ++path_it)
    {
        const std::vector<
            graph_traits< SPPRC_Example_Graph >::edge_descriptor >& path
            = *path_it;
        BOOST_TEST(!path.empty());

        const graph_traits< SPPRC_Example_Graph >::edge_descriptor front
            = path.front();
        BOOST_TEST(boost::target(front, g3) == g3_target);

        std::vector< graph_traits< SPPRC_Example_Graph >::edge_descriptor >::
            const_iterator edge_it,
            edge_it_end;
        graph_traits< SPPRC_Example_Graph >::edge_descriptor prev_edge = front;

        for (edge_it = path.begin() + 1, edge_it_end = path.end();
             edge_it != edge_it_end; ++edge_it)
        {
            graph_traits< SPPRC_Example_Graph >::edge_descriptor edge
                = *edge_it;

            graph_traits< SPPRC_Example_Graph >::vertex_descriptor prev_end,
                current_end;
            prev_end = boost::source(prev_edge, g3);
            current_end = boost::target(edge, g3);
            BOOST_TEST(prev_end == current_end);

            prev_edge = edge;
        }

        const graph_traits< SPPRC_Example_Graph >::edge_descriptor back
            = path.back();
        BOOST_TEST(boost::source(back, g3) == g3_source);
    }

    return boost::report_errors();
}


#include "performance_test.hpp"

#include <imgproc/derivative_gradient.hpp>
#include <imgproc/threshold.hpp>
#include <edge/nms.hpp>
#include <utility/results.hpp>
#include <boost/filesystem.hpp>

using namespace lsfm;
namespace fs = boost::filesystem;

template<class FT>
struct Entry  : public PerformanceTaskDefault {
    Entry() {}

    Entry(const cv::Ptr<FilterI<uchar>>& a, const cv::Ptr<Threshold<FT>> &t, const std::string& n, int f = 0)
        : PerformanceTaskDefault(n,f), filter(a), threshold(t) {}

    cv::Ptr<FilterI<uchar>> filter;
    cv::Ptr<Threshold<FT>> threshold;

    FilterResults filterRes;
    cv::Mat thresholdRes;
    std::string resName;

    void run(const std::string& src_name, cv::Mat src, int loops, bool verbose) {
        this->measure.push_back(PerformanceMeasure(src_name, this->name, src.cols, src.rows));
        PerformanceMeasure& pm = this->measure.back();
        resName = "./results/visual/Threshold/" + src_name.substr(0,src_name.size()-4) + "_" + this->name;
        if (verbose)
            std::cout << "    Running " << this->name << " ... ";
        filter->process(src);
        filterRes = filter->results();
        cv::Mat mag = filterRes["mag"].data;
        uint64 start;
        for (int i = 0; i != loops; ++i) {
            start = cv::getTickCount();
            thresholdRes = threshold->process(mag);
            pm.measures.push_back(cv::getTickCount() - start);
        }
        if (verbose)
            std::cout << std::setprecision(3) << static_cast<double>((cv::getTickCount() - start) * 1000) / (loops * cv::getTickFrequency()) << "ms" << std::endl;
    }

    void saveResults(bool verbose) {

        NonMaximaSuppression<short, FT, FT> nms;
        cv::Mat high = thresholdRes.clone();
        cv::GaussianBlur(high, high, cv::Size(0, 0),15);
        cv::Mat low = high * 0.5;
        // set fixed lower threshold
        low.setTo(filterRes["mag"].range.upper * 0.004, low < filterRes["mag"].range.upper * 0.004);
        nms.process(filterRes["gx"].data, filterRes["gy"].data, filterRes["mag"].data, low, high);
        if (verbose)
            std::cout << "    Save visual results " << resName << std::endl;
        saveEdge(nms.hysteresis(), resName + "_mag");
        saveNormalized(high, resName + "_th");
    }
};

void createThresholdPerformanceTest(PerformanceTestPtr& test, const DataProviderMap& provider)
{
    test.reset(new PerformanceTest);
    test->name = "Threshold";
    try {
        addDefault(provider, test->data);

        //add other
    }
    catch (std::exception& e) {
        std::cout << test->name << " parse error: " << e.what() << std::endl;
        return;
    }

    fs::create_directory("./results/visual/Threshold");

    typedef float FT;

    test->tasks.push_back(PerformanceTaskPtr(new Entry<FT>(new DerivativeGradient<uchar, short,FT, FT, SobelDerivative>, new GlobalThreshold<FT,ThresholdOtsu<FT,256>>(1141), "Otsu_G")));
    test->tasks.push_back(PerformanceTaskPtr(new Entry<FT>(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThresholdTiles<FT, ThresholdOtsu<FT, 256>>(3, 3, 1141), "Otsu_LTiles4")));
    test->tasks.push_back(PerformanceTaskPtr(new Entry<FT>(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThresholdTiles<FT, ThresholdOtsu<FT, 256>>(10, 10, 1141), "Otsu_LTiles10")));
    test->tasks.push_back(PerformanceTaskPtr(new Entry<FT>(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new LocalThreshold<FT, ThresholdOtsu<FT, 256>>(30,30,true,1141), "Otsu_LWindow")));
    test->tasks.push_back(PerformanceTaskPtr(new Entry<FT>(new DerivativeGradient<uchar, short, FT, FT, SobelDerivative>, new DynamicThreshold<FT, ThresholdOtsu<FT, 256>>(5, 1141), "Otsu_D")));

}

bool addThreshold() {
    addPerformanceTestCreator(createThresholdPerformanceTest);
    std::cout << "Added threshold performance test" << std::endl;
    return true;
}

bool thresholdAdded = addThreshold();


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    int n; cin >> n;
    vector<int> a(n, 0);
    cpp_int sum = 0, ans = 0;
    for (int i = 0; i < n; i++) {
        cin >> a[i]; sum += a[i];
    }
    for (int i = 0; i < n - 1; i++) {
        ans += (sum -= a[i]) * a[i];
    }
    cout << ans % 1000000007 << endl;
}


#include <Eigen/Core>
#include <iostream>
#include "timer.h"
#include "ArpackFun.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::MatrixXcd;
using Eigen::VectorXcd;
using Eigen::Lower;
typedef Eigen::Map<VectorXd> MapVec;

void eigs_sym_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    prec_err = -1.0;
    start = get_wall_time();

    // Begin ARPACK
    //
    // Initial value of ido
    int ido = 0;
    // 'I' means standard eigen value problem, A * x = lambda * x
    char bmat = 'I';
    // dimension of A (n by n)
    int n = M.rows();
    // Specify selection criteria
    // "LM": largest magnitude
    char which[3] = {'L', 'M', '\0'};
    // Number of eigenvalues requested
    int nev = k;
    // Precision
    double tol = 1e-10;
    // Residual vector
    double *resid = new double[n]();
    std::copy(init_resid.data(), init_resid.data() + n, resid);
    // Number of Ritz values used
    int ncv = m;
    // Vector of eigenvalues
    VectorXd evals(nev);
    // Matrix of eigenvectors
    MatrixXd evecs(n, ncv);

    // Store final results of eigenvectors
    // double *V = new double[n * ncv]();
    double *V = evecs.data();
    // Leading dimension of V, required by FORTRAN
    int ldv = n;
    // Control parameters
    int *iparam = new int[11]();
    iparam[1 - 1] = 1;     // ishfts
    iparam[3 - 1] = 1000;  // maxitr
    iparam[7 - 1] = 1;     // mode
    // Some pointers
    int *ipntr = new int[11]();
    /* workd has 3 columns.
     * ipntr[2] - 1 ==> first column to store B * X,
     * ipntr[1] - 1 ==> second to store Y,
     * ipntr[0] - 1 ==> third to store X. */
    double *workd = new double[3 * n]();
    int lworkl = ncv * (ncv + 8);
    double *workl = new double[lworkl]();
    // Error flag. 0 means random initialization,
    // otherwise using resid as initial value
    int info = 1;

    saupd(ido, bmat, n, which,
          nev, tol, resid,
          ncv, V, ldv,
          iparam, ipntr, workd,
          workl, lworkl, info);
    // ido == -1 or ido == 1 means more iterations needed
    while (ido == -1 || ido == 1)
    {
        MapVec vec_in(&workd[ipntr[0] - 1], n);
        MapVec vec_out(&workd[ipntr[1] - 1], n);
        vec_out.noalias() = M.selfadjointView<Lower>() * vec_in;

        saupd(ido, bmat, n, which,
              nev, tol, resid,
              ncv, V, ldv,
              iparam, ipntr, workd,
              workl, lworkl, info);
    }

    // info > 0 means warning, < 0 means error
    if (info > 0)
        std::cout << "warnings occured" << std::endl;
    if (info < 0)
    {
        delete[] workl;
        delete[] workd;
        delete[] ipntr;
        delete[] iparam;
        delete[] resid;

        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    // Retrieve results
    //
    // Whether to calculate eigenvectors or not.
    bool rvec = true;
    // 'A' means to calculate Ritz vectors
    // 'P' to calculate Schur vectors
    char howmny = 'A';
    // Vector of eigenvalues
    double *d = evals.data();
    // Used to store results, will use V instead.
    double *Z = V;
    // Leading dimension of Z, required by FORTRAN
    int ldz = n;
    // Shift
    double sigma = 0;
    // Error information
    int ierr = 0;

    // Number of converged eigenvalues
    int nconv = 0;
    // Number of iterations
    int niter = 0;

    // Use seupd() to retrieve results
    seupd(rvec, howmny, d,
          Z, ldz, sigma, bmat,
          n, which, nev, tol,
          resid, ncv, V, ldv,
          iparam, ipntr, workd, workl,
          lworkl, ierr);

    // Obtain 'nconv' converged eigenvalues
    nconv = iparam[5 - 1];
    // 'niter' number of iterations
    niter = iparam[9 - 1];

    // Free memory of temp arrays
    delete[] workl;
    delete[] workd;
    delete[] ipntr;
    delete[] iparam;
    delete[] resid;

    // ierr < 0 means error
    if (ierr < 0)
    {
        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    /* std::cout << "computed eigenvalues D = \n" << evals.transpose() << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" <<
    evecs.topLeftCorner(5, nconv) << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "nops = " << niter << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;
    MatrixXd err = M * evecs.leftCols(nev) - evecs.leftCols(nev) * evals.asDiagonal();
    prec_err = err.cwiseAbs().maxCoeff();
    nops = niter;
}

void eigs_gen_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    prec_err = -1.0;
    start = get_wall_time();

    // Begin ARPACK
    //
    // Initial value of ido
    int ido = 0;
    // 'I' means standard eigen value problem, A * x = lambda * x
    char bmat = 'I';
    // dimension of A (n by n)
    int n = M.rows();
    // Specify selection criteria
    // "LM": largest magnitude
    char which[3] = {'L', 'M', '\0'};
    // Number of eigenvalues requested
    int nev = k;
    // Precision
    double tol = 1e-10;
    // Residual vector
    double *resid = new double[n]();
    std::copy(init_resid.data(), init_resid.data() + n, resid);
    // Number of Ritz values used
    int ncv = m;
    // Vector of eigenvalues
    VectorXd evals_re(nev + 1);
    VectorXd evals_im(nev + 1);
    // Matrix of eigenvectors
    MatrixXd evecs(n, ncv);

    // Store final results of eigenvectors
    // double *V = new double[n * ncv]();
    double *V = evecs.data();
    // Leading dimension of V, required by FORTRAN
    int ldv = n;
    // Control parameters
    int *iparam = new int[11]();
    iparam[1 - 1] = 1;     // ishfts
    iparam[3 - 1] = 1000;  // maxitr
    iparam[7 - 1] = 1;     // mode
    // Some pointers
    int *ipntr = new int[14]();
    /* workd has 3 columns.
     * ipntr[2] - 1 ==> first column to store B * X,
     * ipntr[1] - 1 ==> second to store Y,
     * ipntr[0] - 1 ==> third to store X. */
    double *workd = new double[3 * n]();
    int lworkl = 3 * ncv * ncv + 6 * ncv;
    double *workl = new double[lworkl]();
    // Error flag. 0 means random initialization,
    // otherwise using resid as initial value
    int info = 1;

    naupd(ido, bmat, n, which,
          nev, tol, resid,
          ncv, V, ldv,
          iparam, ipntr, workd,
          workl, lworkl, info);
    // ido == -1 or ido == 1 means more iterations needed
    while (ido == -1 || ido == 1)
    {
        MapVec vec_in(&workd[ipntr[0] - 1], n);
        MapVec vec_out(&workd[ipntr[1] - 1], n);
        vec_out.noalias() = M * vec_in;

        naupd(ido, bmat, n, which,
              nev, tol, resid,
              ncv, V, ldv,
              iparam, ipntr, workd,
              workl, lworkl, info);
    }

    // info > 0 means warning, < 0 means error
    if (info > 0)
        std::cout << "warnings occured" << std::endl;
    if (info < 0)
    {
        delete[] workl;
        delete[] workd;
        delete[] ipntr;
        delete[] iparam;
        delete[] resid;

        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    // Retrieve results
    //
    // Whether to calculate eigenvectors or not.
    bool rvec = true;
    // 'A' means to calculate Ritz vectors
    // 'P' to calculate Schur vectors
    char howmny = 'A';
    // Vector of eigenvalues
    double *dr = evals_re.data();
    double *di = evals_im.data();
    // Used to store results, will use V instead.
    double *Z = V;
    // Leading dimension of Z, required by FORTRAN
    int ldz = n;
    // Shift
    double sigmar = 0;
    double sigmai = 0;
    // Working space
    double *workv = new double[3 * ncv]();
    // Error information
    int ierr = 0;

    // Number of converged eigenvalues
    int nconv = 0;
    // Number of iterations
    int niter = 0;

    // Use seupd() to retrieve results
    neupd(rvec, howmny, dr, di,
          Z, ldz, sigmar, sigmai, workv,
          bmat, n, which, nev, tol,
          resid, ncv, V, ldv, iparam,
          ipntr, workd, workl, lworkl, ierr);

    // Obtain 'nconv' converged eigenvalues
    nconv = iparam[5 - 1];
    // 'niter' number of iterations
    niter = iparam[9 - 1];

    // Free memory of temp arrays
    delete[] workv;
    delete[] workl;
    delete[] workd;
    delete[] ipntr;
    delete[] iparam;
    delete[] resid;

    // ierr < 0 means error
    if (ierr < 0)
    {
        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    /* VectorXcd evals(evals_re.size());
    evals.real() = evals_re;
    evals.imag() = evals_im;
    std::cout << "computed eigenvalues D = \n" << evals << std::endl;
    std::cout << "first 5 rows of computed eigenvectors U = \n" <<
        evecs.topLeftCorner(5, nconv) << std::endl;
    std::cout << "nconv = " << nconv << std::endl;
    std::cout << "nops = " << niter << std::endl; */

    end = get_wall_time();
    time_used = (end - start) * 1000;
    nops = niter;
}


#include <igl/opengl/glfw/Viewer.h>
#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <igl/readOBJ.h>
#include <igl/marching_tets.h>
#include <Eigen/Core>

#include "tutorial_shared_path.h"

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

  // Load a surface mesh which is a cube
  Eigen::MatrixXd surfaceV;
  Eigen::MatrixXi surfaceF;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/cube.obj", surfaceV, surfaceF);

  // Find the centroid of the loaded mesh
  Eigen::RowVector3d surfaceCenter = surfaceV.colwise().sum() / surfaceV.rows();

  // Center the mesh about the origin
  surfaceV.rowwise() -= surfaceCenter;

  // Tetrahedralize the surface mesh
  Eigen::MatrixXd TV; // Tet mesh vertices
  Eigen::MatrixXi TF; // Tet mesh boundary face indices
  Eigen::MatrixXi TT; // Tet mesh tetrahedron indices
  igl::copyleft::tetgen::tetrahedralize(surfaceV, surfaceF, "pq1.414a0.0001", TV, TT, TF);

  // Compute a scalar at each tet vertex which is the distance from the vertex to the origin
  Eigen::VectorXd S = TV.rowwise().norm();

  // Compute a mesh (stored in SV, SF) representing the iso-level-set for the isovalue 0.5
  Eigen::MatrixXd SV;
  Eigen::MatrixXi SF;
  igl::marching_tets(TV, TT, S, 0.45, SV, SF);

  // Draw the mesh stored in (SV, SF)
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(SV, SF);
  viewer.callback_key_down =
    [&](igl::opengl::glfw::Viewer & viewer, unsigned char key, int mod)->bool
    {
      viewer.data().set_face_based(true);
      return true;
    };
  viewer.launch();
}


// Copyright (c) 2021 fortiss GmbH
//
// Authors: Klemens Esterle and Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include "parameter_preparer.hpp"
#include <math.h>
#include <algorithm>
#include <boost/math/constants/constants.hpp>
#include "common/math/math.hpp"

namespace miqp {
namespace common {
namespace parameter {
using bark::commons::ParamsPtr;
using miqp::common::parameter::RotateVector;

ParameterPreparer::ParameterPreparer(
    const ParamsPtr& params, const int nrRegions,
    const float maxVelocityFitting, const float minVelocityFitting,
    const float accLonMaxLimit, const float accLonMinLimit,
    const float jerkLonMaxLimit, const float accLatMinMaxLimit,
    const float jerkLatMinMaxLimit)
    : bark::commons::BaseType(params),
      vehicle_params_(accLonMaxLimit, accLonMinLimit, jerkLonMaxLimit,
                      accLatMinMaxLimit, jerkLatMinMaxLimit),
      fittingPolynomial_params_(FittingPolynomialParameters(
          nrRegions, maxVelocityFitting, minVelocityFitting)),
      nrRegions_(nrRegions),
      maxVelocityFitting_(maxVelocityFitting) {
  CalculateFractionParameters();
  CalculateMeanAngleVector();
}

void ParameterPreparer::CalculateFractionParameters() {
  const int nrRegions = nrRegions_;
  Eigen::VectorXd alphaVec(nrRegions_ + 1);
  alphaVec.setLinSpaced(0, 2 * M_PI);
  Eigen::MatrixXd fraction_parameters_col(nrRegions_, 2);
  fraction_parameters_col.col(0) =
      maxVelocityFitting_ * alphaVec.head(nrRegions_).array().cos();
  fraction_parameters_col.col(1) =
      maxVelocityFitting_ * alphaVec.head(nrRegions_).array().sin();
  fraction_params_.resize(nrRegions, 4);
  fraction_params_.block(0, 0, nrRegions, 2) = fraction_parameters_col;
  fraction_params_.block(0, 2, nrRegions - 1, 2) =
      fraction_parameters_col.block(1, 0, nrRegions - 1, 2);
  fraction_params_.block(nrRegions - 1, 2, 1, 2) =
      fraction_parameters_col.block(0, 0, 1, 2);
}

LimitPerRegionParameters ParameterPreparer::CalculateAccLimitsPerCar() const {
  int nr_regions = fraction_params_.rows();
  const int NumCars = 1;  // only per-car calculation, see addCar() for multiple

  LimitPerRegionParameters acc_limits_per_regions =
      LimitPerRegionParameters(NumCars, nr_regions);

  for (std::size_t i = 0; i < mean_angle_vector_.size(); ++i) {
    XYPair max_acc_pair, min_acc_pair;
    RotateLimitVectors(vehicle_params_.straight_acc_limits,
                       mean_angle_vector_[i], max_acc_pair, min_acc_pair);

    acc_limits_per_regions.min_x(0, i) = min_acc_pair.x;
    acc_limits_per_regions.min_y(0, i) = min_acc_pair.y;
    acc_limits_per_regions.max_x(0, i) = max_acc_pair.x;
    acc_limits_per_regions.max_y(0, i) = max_acc_pair.y;
  }
  return acc_limits_per_regions;
};

LimitPerRegionParameters ParameterPreparer::CalculateJerkLimitsPerCar() const {
  int nr_regions = fraction_params_.rows();
  int NumCars = 1;  // only per-car calculation, see addCar() for multiple

  LimitPerRegionParameters jerk_limits_per_regions =
      LimitPerRegionParameters(NumCars, nr_regions);

  for (std::size_t i = 0; i < mean_angle_vector_.size(); ++i) {
    XYPair max_jerk_pair, min_jerk_pair;
    RotateLimitVectors(vehicle_params_.straight_jerk_limits,
                       mean_angle_vector_[i], max_jerk_pair, min_jerk_pair);

    jerk_limits_per_regions.min_x(0, i) = min_jerk_pair.x;
    jerk_limits_per_regions.min_y(0, i) = min_jerk_pair.y;
    jerk_limits_per_regions.max_x(0, i) = max_jerk_pair.x;
    jerk_limits_per_regions.max_y(0, i) = max_jerk_pair.y;
  }
  return jerk_limits_per_regions;
};

void ParameterPreparer::CalculateMeanAngleVector() {
  int nr_regions = fraction_params_.rows();
  for (int idxreg = 0; idxreg < nr_regions; ++idxreg) {
    double angle_line1 =
        atan2(fraction_params_(idxreg, 1), fraction_params_(idxreg, 0));
    double angle_line2 =
        atan2(fraction_params_(idxreg, 3), fraction_params_(idxreg, 2));

    miqp::common::math::WrapRadiantTo2Pi(angle_line1);
    miqp::common::math::WrapRadiantTo2Pi(angle_line2);

    if (idxreg + 1 == nr_regions) {
      angle_line2 = angle_line2 + 2 * boost::math::constants::pi<double>();
    }
    mean_angle_vector_.push_back((angle_line1 + angle_line2) / 2);
  }
}

void ParameterPreparer::RotateLimitVectors(const StraightLimits& straight_lim,
                                           const float angle, XYPair& max_pair,
                                           XYPair& min_pair) const {
  XYPair max_max = RotateVector(
      straight_lim.straight_long_max + straight_lim.straight_lat_max, angle);
  XYPair max_min = RotateVector(
      straight_lim.straight_long_max + straight_lim.straight_lat_min, angle);
  XYPair min_max = RotateVector(
      straight_lim.straight_long_min + straight_lim.straight_lat_max, angle);
  XYPair min_min = RotateVector(
      straight_lim.straight_long_min + straight_lim.straight_lat_min, angle);

  float max_x =
      std::max(std::max(max_max.x, max_min.x), std::max(min_max.x, min_min.x));
  float min_x =
      std::min(std::min(max_max.x, max_min.x), std::min(min_max.x, min_min.x));
  float max_y =
      std::max(std::max(max_max.y, min_max.y), std::max(min_min.y, max_min.y));
  float min_y =
      std::min(std::min(max_max.y, min_max.y), std::min(min_min.y, max_min.y));

  miqp::common::math::SwapIfNeeded(max_x, min_x);
  miqp::common::math::SwapIfNeeded(max_y, min_y);
  assert(max_x >= min_x);
  assert(max_y >= min_y);

  max_pair.x = max_x;
  max_pair.y = max_y;

  min_pair.x = min_x;
  min_pair.y = min_y;
};

}  // namespace parameter
}  // namespace common
}  // namespace miqp


#ifndef NONRIGIDREGISTRATION_HPP
#define NONRIGIDREGISTRATION_HPP

#include <Eigen/Dense>
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include <time.h>
#include "../global.hpp"
#include "CorrespondenceFilter.hpp"
#include "SymmetricCorrespondenceFilter.hpp"
#include "InlierDetector.hpp"
#include "ViscoElasticTransformer.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat;

namespace registration{

class NonrigidRegistration
{
    /*
    # GOAL
    This class performs icp-based nonrigid registration between two oriented pointclouds.

    # INPUTS
    -ioFloatingFeatures
    -inTargetFeatures
    -inFloatingFlags
    -inTargetFlags

    # PARAMETERS
    -numNeighbours(=3):
    number of nearest neighbours

    # OUTPUT
    -outCorrespondingFeatures
    -outCorrespondingFlags
    */

    public:

        void set_input(FeatureMat * const ioFloatingFeatures,
                       const FeatureMat * const inTargetFeatures,
                       const FacesMat * const inFloatingFaces,
                       const VecDynFloat * const inFloatingFlags,
                       const VecDynFloat * const inTargetFlags);
        void set_parameters(bool symmetric,
                            size_t numNeighbours,
                            float flagThreshold,
                            bool equalizePushPull,
                            float kappaa,
                            bool inlierUseOrientation,
                            size_t numIterations,
                            float sigmaSmoothing,
                            size_t numViscousIterationsStart,
                            size_t numViscousIterationsEnd,
                            size_t numElasticIterationsStart,
                            size_t numElasticIterationsEnd);

        void get_annealing_rates(float &viscousAnnealingRate,
                                 float &elasticAnnealingRate){
                                 viscousAnnealingRate = _viscousAnnealingRate;
                                 elasticAnnealingRate = _elasticAnnealingRate;
                                 }
        void set_annealing_rates(const float viscousAnnealingRate,
                                 const float elasticAnnealingRate){
                                 _viscousAnnealingRate = viscousAnnealingRate;
                                 _elasticAnnealingRate = elasticAnnealingRate;
                                 }
        void get_viscoelastic_iterations(float &numViscousIterations,
                                         float &numElasticIterations){
                                         numViscousIterations = _numViscousIterations;
                                         numElasticIterations = _numElasticIterations;}

        void update();

    protected:

    private:
        //# Inputs/Outputs
        FeatureMat * _ioFloatingFeatures = NULL;
        const FeatureMat * _inTargetFeatures = NULL;
        const FacesMat * _inFloatingFaces;
        const VecDynFloat * _inFloatingFlags = NULL;
        const VecDynFloat * _inTargetFlags = NULL;

        //# User Parameters
        //## Correspondences
        bool _symmetric = true;
        size_t _numNeighbours = 3;
        float _flagThreshold = 0.9f;
        bool _equalizePushPull = false;
        //## Inliers
        float _kappaa = 3.0;
        bool _inlierUseOrientation = true;
        //## Transformation
        size_t _numIterations = 10;
        float _sigmaSmoothing = 3.0;
        size_t _numViscousIterationsStart = 100;
        size_t _numViscousIterationsEnd = 1;
        size_t _numElasticIterationsStart = 100;
        size_t _numElasticIterationsEnd = 1;
        size_t _numViscousIterations = 100;
        size_t _numElasticIterations = 100;

        //# Internal Data structures

        //# Internal Parameters
        //## Transformation
        float _viscousAnnealingRate = exp(log(float(_numViscousIterationsEnd)/float(_numViscousIterationsStart))/_numIterations);
        float _elasticAnnealingRate = exp(log(float(_numElasticIterationsEnd)/float(_numElasticIterationsStart))/_numIterations);

        //# Internal functions
};

}//namespace registration

#endif // NONRIGIDREGISTRATION_HPP


#include "MaximumLikelihood.h"
#include "../Ensemble.h"
#include "../Parameters.h"
#include "../Probabilistic.h"
#include "../Obs.h"
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

EstimatorMaximumLikelihood::EstimatorMaximumLikelihood(const Options& iOptions, const Data& iData, const Probabilistic& iScheme) :
      EstimatorProbabilistic(iOptions, iData, iScheme),
      mForceIdentityMatrix(false) {
   //! Should the covariance matrix for finding the minimum be forced to be an  identity matrix?
   iOptions.getValue("forceIdentityMatrix", mForceIdentityMatrix);
   iOptions.check();
}
void EstimatorMaximumLikelihood::update(const std::vector<Ensemble>& iEnsemble,
      const std::vector<Obs>& iObs,
      Parameters& iParameters) const {

   // Set up coefficients
   // Set up D2S
   int N = getNumCoefficients(iParameters);
   boost::numeric::ublas::matrix<float> D2S(N,N);
   boost::numeric::ublas::matrix<float> accumD2S(N,N);
   int Istart = N;
   for(int k = 0; k < N*N; k++) {
      int i = k % N;
      int j = floor(k/N);
      D2S(i,j) = iParameters[Istart + k];
      accumD2S(i,j) = 0;
   }
   boost::numeric::ublas::matrix<float> Rinv(N,N);
   bool status = getInverse(D2S, Rinv);
   assert(status);
   //std::cout << "D2S[0] = " << D2S(0,0) << " Inverse = " << Rinv(0,0) << std::endl;

   // Initialize new parameters
   std::vector<float> accumCoeff;
   accumCoeff.resize(N);

   for(int i = 0; i < N; i++) {
      accumCoeff[i] = 0;
   }
   int numUpdates = 0;

   for(int t = 0 ; t < (int) iObs.size(); t++) {
      float obs = iObs[t].getValue();
      Ensemble ens = iEnsemble[t];
      if(Global::isValid(obs)) {
         Parameters coeffs;
         getCoefficients(iParameters, coeffs);

         std::vector<float> H;
         bool status = getH(obs, ens, coeffs, H);
         if(status) {
            //std::cout << "H = " << H[0] << std::endl;
            for(int i = 0; i < N; i++) {
               for(int j = 0; j < N; j++) {
                  accumD2S(i,j) += H[i]*H[j];
                  //if(H[j] > 200)
                  //std::cout << "H[j] = " << H[j] << " Rinv(i,j)= " << Rinv(i,j)  << std::endl;
                  accumCoeff[i] += H[j]*Rinv(i,j);
               }
            }
            numUpdates++;
         }
      }
   }

   if(numUpdates > 0) {
      // Update coefficients Pinson (eq 19)
      for(int i = 0; i < N; i++) {
         //std::cout << "Old parameter = " << iParameters[i];
         iParameters[i] = iParameters[i] + 1/mEfold * accumCoeff[i]/numUpdates;
         //std::cout << " new = " << iParameters[i] << std::endl;
      }

      // Update D2S Pinson (eq 20)
      for(int i = 0; i < N; i++) {
         for(int j = 0; j < N; j++) {
            int index = Istart + i + N*j;
            //std::cout << "Old est parameter = " << iParameters[index];
            if(mForceIdentityMatrix)
               iParameters[index] = (i==j) ? 1 : 0;
            else
               iParameters[index] = getLambda() * D2S(i,j) + 1/mEfold * accumD2S(i,j)/numUpdates;
            //std::cout << " new = " << iParameters[index] << std::endl;
         }
      }
   }
   else {
      //std::cout << "Can't update" << std::endl;
   }

}

void EstimatorMaximumLikelihood::getDefaultParameters(const Parameters& iSchemeParameters,
      Parameters& iParameters) const {
   int N = iSchemeParameters.size();
   std::vector<float> param;
   for(int i = 0; i < N; i++) {
      for(int j = 0; j < N; j++) {
         param.push_back(i == j);
      }
   }
   iParameters.setAllParameters(param);
}
int EstimatorMaximumLikelihood::getIndex(int i, int j, int iSize) {
   return i*iSize + j;
}
float EstimatorMaximumLikelihood::getLambda() const {
   return 1 - 1/mEfold;
}

// TODO: Pass only coefficients in here
bool EstimatorMaximumLikelihood::getH(float iObs, const Ensemble& iEnsemble, const Parameters& iCoeffs, std::vector<float>& iH) const {
   int N = iCoeffs.size();

   iH.clear();
   iH.resize(N);

   std::vector<float> gradL;
   bool status = getGradL(iObs, iEnsemble, iCoeffs, gradL);
   if(!status) {
      return false;
   }
   //std::cout << "Grad = " << gradL[0] << std::endl;
   assert((int) gradL.size() == N);

   float L = mScheme.getLikelihood(iObs, iEnsemble, iCoeffs);
   if(L == 0) {
      std::cout << "MaximumLikelihood: Obs = " << iObs << std::endl;
   }
   assert(L > 0);
   if(!Global::isValid(L)) {
      return false;
   }
   for(int i = 0; i < N; i++) {
      float value = gradL[i]/L;
      iH[i] = value;
   }
   return true;
}

bool EstimatorMaximumLikelihood::getGradL(float iObs, const Ensemble& iEnsemble, const Parameters& iCoeffs, std::vector<float>& iGradL) const {
   int N = iCoeffs.size();

   iGradL.clear();
   iGradL.resize(N);

   float dx = 0.001;
   float L0 = mScheme.getLikelihood(iObs, iEnsemble, iCoeffs);
   if(!Global::isValid(L0))
      return false;
   for(int i = 0; i < N; i++) {
      // Perturb current parameter
      Parameters par = iCoeffs;
      par[i] += dx;

      // Compute likelihood derivative
      float currL = mScheme.getLikelihood(iObs, iEnsemble, par);
      if(!Global::isValid(currL))
         return false;
      float dL = currL - L0;
      iGradL[i] = dL/dx;
   }
   return true;
}

bool EstimatorMaximumLikelihood::getInverse(boost::numeric::ublas::matrix<float> iMatrix, boost::numeric::ublas::matrix<float>& iInverse) {
   // Taken from https://gist.github.com/2464434
   using namespace boost::numeric::ublas;
   typedef permutation_matrix<std::size_t> pmatrix;
   // create a working copy of the input
   matrix<float> A(iMatrix);
   // create a permutation matrix for the LU-factorization
   pmatrix pm(A.size1());

   // perform LU-factorization
   int res = lu_factorize(A,pm);
   if( res != 0 ) return false;

   // create identity matrix of "inverse"
   iInverse.assign(identity_matrix<float>(A.size1()));

   // backsubstitute to get the inverse
   lu_substitute(A, pm, iInverse);

   return true;
}

int EstimatorMaximumLikelihood::getNumCoefficients(const Parameters& iParameters) const {
   int N = (-1 + pow(1 + 4*iParameters.size(),0.5))/2;
   assert(N + N*N == iParameters.size());
   return N;
}


#include <boost/random/beta_distribution.hpp>


#include <boost/random/gamma_distribution.hpp>


#include <boost/math/special_functions/polygamma.hpp>


#ifndef PLANE_H
#define PLANE_H

//PointCloud#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/concave_hull.h>

#include <Eigen/Core>
#include <boost/shared_ptr.hpp>
#include "sophus/sim3.hpp"
#include "typedef.h"

// ------------------------- Plane -------------------------
class Plane {

private:
    typedef Eigen::Hyperplane<float, 3> EigenPlane;

public:
    typedef boost::shared_ptr<Plane> Ptr;

    Plane(std::vector<float> coefficients)
    {
        if (coefficients.size() != 4) {
            ROS_ERROR("Plane with wrong number of coefficients created");
        }
        a_ = coefficients[0];
        b_ = coefficients[1];
        c_ = coefficients[2];
        d_ = coefficients[3];
    }

    Eigen::VectorXf getCoefficients()
    {
        Eigen::VectorXf coeff(4);
        coeff << a_ , b_ , c_ , d_;
        return coeff;
    }

    void setCoefficients(const Eigen::VectorXf coefficients)
    {
        if (coefficients.size() != 4) {
            return;
        }
        a_ = coefficients(0);
        b_ = coefficients(1);
        c_ = coefficients(2);
        d_ = coefficients(3);
    }

    Eigen::Quaternionf getRotation()
    {
        Eigen::Vector3f normal;
        Eigen::Vector3f point;
        calculateNormalForm(point, normal);
        return Eigen::Quaternionf::FromTwoVectors(Eigen::Vector3f(1,0,0), normal);
    }

    void transform(const Eigen::Matrix4f &transform)
    {
        Eigen::Vector3f normal;
        Eigen::Vector3f point;
        calculateNormalForm(point, normal);
        Eigen::Vector4f normal_hom(normal[0], normal[1], normal[2], 0);
        Eigen::Vector4f point_hom(point[0], point[1], point[2], 1);
        normal_hom = transform * normal_hom;
        point_hom = transform * point_hom;
        for (int i = 0; i < 3; i++) {
            normal[i] = normal_hom[i];
            point[i] = point_hom[i] / point_hom[3];
        }
        calculateParameterForm(point, normal);
    }

    static void transformPlane(const Eigen::Matrix4f &transform, Eigen::Vector3f &point_inout, Eigen::Vector3f &normal_inout)
    {
        Eigen::Vector4f normal_hom(normal_inout[0], normal_inout[1], normal_inout[2], 0);
        Eigen::Vector4f point_hom(point_inout[0], point_inout[1], point_inout[2], 1);
        normal_hom = transform * normal_hom;
        point_hom = transform * point_hom;
        for (int i = 0; i < 3; i++) {
            normal_inout[i] = normal_hom[i];
            point_inout[i] = point_hom[i] / point_hom[3];
        }
    }

    // returns closest point to point_in on the plane
    Eigen::Vector3f rayIntersection(Eigen::Vector3f point_in, Eigen::Vector3f direction_in)
    {
        direction_in.normalize();
        Eigen::Vector3f planeNormal;
        Eigen::Vector3f planePoint;
        calculateNormalForm(planePoint, planeNormal);
        EigenPlane plane = EigenPlane(planeNormal, planePoint);
        Eigen::ParametrizedLine<float,3> pline = Eigen::ParametrizedLine<float,3>(point_in, direction_in);
        Eigen::Vector3f intersection = pline.intersectionPoint(plane);
        return intersection;
    }

    void calculateNormalForm(Eigen::Vector3f &point_out, Eigen::Vector3f &normal_out)
    {
        normal_out = Eigen::Vector3f(a_, b_, c_);
        float length = normal_out.norm();
        normal_out = normal_out / length;
        point_out = -d_/length * normal_out;

        // Checking the direction of the normal
        int scalerProduct = point_out.dot(normal_out);
        if(scalerProduct < 0){
            normal_out = -normal_out;
        }
    }

private:

    void calculateParameterForm(const Eigen::Vector3f &point, const Eigen::Vector3f &normal) {
        a_ = normal[0];
        b_ = normal[1];
        c_ = normal[2];

        d_ = -normal.dot(point);
    }

// Member variables ----------
private:
    float a_, b_, c_, d_;

};

#endif //PLANE_H


///////////////////////////////////////////////////////////////////////////////
// statistics::model::functional::log_posterior_evaluator.hpp                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_MODEL_FUNCTIONAL_LOG_POSTERIOR_EVALUATOR_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_FUNCTIONAL_LOG_POSTERIOR_EVALUATOR_HPP_ER_2009
#include <boost/type_traits.hpp>
#include <boost/call_traits.hpp>
#include <boost/operators.hpp>
#include <boost/binary_op/algorithm/for_each.hpp>
#include <boost/joint_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/scalar_dist/unscope/log_unnormalized_pdf.hpp>
#include <boost/statistics/model/wrap/aggregate/prior_model_dataset.hpp>
#include <boost/statistics/model/functional/log_likelihood_evaluator.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Augments algorithm::log_likehood with a prior of type D
    //
    // Requirements:
    // Let d denote an instance of D and p a parameter, then
    // log_unnormalized_pdf(p_dist,p) must return an object of type T
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    class log_posterior_evaluator : log_likelihood_evaluator<T,M,Rx,Ry>{
        typedef log_likelihood_evaluator<T,M,Rx,Ry> super_;
        public:
        typedef prior_model_dataset_<D,M,Rx,Ry> prior_model_dataset_type;
        typedef typename prior_model_dataset_type::prior_type prior_type;
        typedef typename super_::result_type result_type;

        // Constructor
        log_posterior_evaluator();
        log_posterior_evaluator(const prior_model_dataset_type&);
        log_posterior_evaluator(const log_posterior_evaluator&);
        log_posterior_evaluator& operator=(const log_posterior_evaluator&);

        // Evaluate
        template<typename P>
        result_type operator()(const P& p)const;

        // Access
        const prior_model_dataset_type& prior_model_dataset()const;

        private:
        prior_model_dataset_type pmd_;
    };

    // Implementation //

    // Construction
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator():
    super_(){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator(
        const prior_model_dataset_type& pmd
    ):super_(pmd),pmd_(pmd){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>::log_posterior_evaluator(
        const log_posterior_evaluator& that
    ):super_(that),pmd_(that.pmd_){}

    template<typename T,typename D,typename M,typename Rx,typename Ry>
    log_posterior_evaluator<T,D,M,Rx,Ry>&
    log_posterior_evaluator<T,D,M,Rx,Ry>::operator=(
        const log_posterior_evaluator& that
    ){
        if(&that!=this){
            super_::operator=(that);
            pmd_ = (that.pmd_);
        }
        return (*this);
    }

    // Evaluate
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    template<typename P>
    typename log_posterior_evaluator<T,D,M,Rx,Ry>::result_type
    log_posterior_evaluator<T,D,M,Rx,Ry>::operator()(const P& p)const{
        const super_& super = static_cast<const super_&>(*this);
        result_type res =  super(p);
        res += log_unnormalized_pdf(
            prior_model_dataset().prior(),
            p
        );
        return res;
    }

    // Access
    template<typename T,typename D,typename M,typename Rx,typename Ry>
    const
        typename
            log_posterior_evaluator<T,D,M,Rx,Ry>::prior_model_dataset_type&
    log_posterior_evaluator<T,D,M,Rx,Ry>::prior_model_dataset()const{
        return (this->pmd_);
    }

}// model
}// statistics
}// boost

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Fact_6 Fact_6 (function template)

  Generates the @c 6! constant

  @headerref{<boost/simd/constant/fact_6.hpp>}

  @par Description

  1.  @code
      template<typename T> T Fact_6();
      @endcode

  2.  @code
      template<typename T> T Fact_6( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to 6!.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c T that evaluates to @c T(720).

  @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/fact_6.hpp>
#include <boost/simd/constant/simd/fact_6.hpp>

#endif


#ifndef HImprovePoisedness
#define HImprovePoisedness

#include "ImprovePoisednessBaseClass.hpp"
#include "BlackBoxData.hpp"
#include "CholeskyFactorization.hpp"
#include "VectorOperations.hpp"
#include "QuadraticMinimization.hpp"
#include <Eigen/Dense>
#include <vector>
#include <math.h>

//! Improve poisedness of interpolation nodes
class ImprovePoisedness : public ImprovePoisednessBaseClass,
                          protected QuadraticMinimization {
  private:
    int dim;
    int nb_nodes;
    size_t max_nb_nodes;
    double *delta;

    //declare auxiliary variables for replace_node
    double maxvalue, LK, norm_dbl;
    //declare auxiliary variables for compute_poisedness_constant
    double poisedness_constant_tmp1, poisedness_constant_tmp2;
    double node_norm_scaling;
    std::vector<double> q1, q2;
    std::vector<double> basis_values;

    std::vector<double> basis_gradient;
    std::vector< std::vector<double> > basis_hessian;
    //define auxiliary variables
    Eigen::VectorXd tmp_node;
    bool print_output;
    int change_index;
    bool model_has_been_improved;
    std::vector<double> lower_bound_constraints;
    std::vector<double> upper_bound_constraints;
    bool use_hard_box_constraints = false;
    void compute_poisedness_constant ( int, std::vector<double>&, BlackBoxData& );
  public:
    //! Constructor
    /*!
     Set parameters required for the improvement of the poisedness of interploation nodes
     \param B basis for surrogate model
     \param poisedness_threshold threshold for poisedness constant
     \param m maximal number of interpolation nodes
     \param rad radius arround current best point of ball that contains well poised points
     \param verbose switch output on (verbose = 3) or off (verbose = 0)
     \see BlackBoxData
    */
    ImprovePoisedness ( BasisForSurrogateModelBaseClass&, double, int, double&, int, std::vector<double>, std::vector<double>, bool);
    //! Destructor
    ~ImprovePoisedness () { }
    //! Find node to be replaced by better poised node
    /*!
     Finds a node to replace another interpolation node to improve poisedness
     \param reference_node index of node that is not replaced
     \param evaluations interpolation nodes, \see BlackBoxData
     \param new_node new node to replace an existing interpolation node
    */
    int replace_node ( int, BlackBoxData&, std::vector<double> const& );
    //! Improves poisedness of interpolation nodes
    /*!
     Improves poisedness of interpolation nodes by maximizing the absolute value of basis functions.\n
     Nodes to replace existing interpolation nodes are computed and appended to the list of nodes,
     \see BlackBoxData \n
     The index of nodes to reduce the poisedness value are indicated in evaluations \see BlackBoxData \n
     \param reference_node index of node that is not replaced
     \param evaluations structure containing interpolation nodes, \see BlackBoxData
    */
    void improve_poisedness ( int, BlackBoxData& );
};

#endif


/**
 * \file CachedSinusGeneratorFilter.cpp
 */

#include "CachedSinusGeneratorFilter.h"

#include <boost/math/constants/constants.hpp>

#include <cmath>
#include <cstdint>
#include <cstring>

namespace ATK
{
  template<typename DataType_>
  CachedSinusGeneratorFilter<DataType_>::CachedSinusGeneratorFilter(int periods, int seconds)
  :Parent(0, 1), periods(periods), seconds(seconds)
  {
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_frequency(int periods, int seconds)
  {
    if(periods <= 0)
    {
      throw std::out_of_range("Periods must be strictly positive");
    }
    this->periods = periods;
    this->seconds = seconds;
    setup();
  }

  template<typename DataType_>
  std::pair<int, int> CachedSinusGeneratorFilter<DataType_>::get_frequency() const
  {
    return std::make_pair(periods, seconds);
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_volume(DataType_ volume)
  {
    this->volume = volume;
  }

  template<typename DataType_>
  DataType_ CachedSinusGeneratorFilter<DataType_>::get_volume() const
  {
    return volume;
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::set_offset(DataType_ offset)
  {
    this->offset = offset;
  }

  template<typename DataType_>
  DataType_ CachedSinusGeneratorFilter<DataType_>::get_offset() const
  {
    return offset;
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::setup()
  {
    indice = 0;
    cache.resize(output_sampling_rate * seconds);
    for(gsl::index i = 0; i < cache.size(); ++i)
    {
      cache[i] = static_cast<DataType>(std::sin(2 * boost::math::constants::pi<double>() * (i+1) * periods / seconds / output_sampling_rate));
    }
  }

  template<typename DataType_>
  void CachedSinusGeneratorFilter<DataType_>::process_impl(gsl::index size) const
  {
    DataType* ATK_RESTRICT output = outputs[0];
    gsl::index processed = 0;
    while (processed < size)
    {
      auto to_copy = std::min(size - processed, static_cast<gsl::index>(cache.size()) - indice);
      memcpy(reinterpret_cast<void*>(output + processed), reinterpret_cast<const void*>(cache.data() + indice), to_copy * sizeof(DataType_));
      indice += to_copy;
      processed += to_copy;
      if (indice >= cache.size())
      {
        indice = 0;
      }
    }
    for (gsl::index i = 0; i < size; ++i)
    {
      output[i] = static_cast<DataType>(offset + volume * output[i]);
    }
  }

#if ATK_ENABLE_INSTANTIATION
  template class CachedSinusGeneratorFilter<std::int16_t>;
  template class CachedSinusGeneratorFilter<std::int32_t>;
  template class CachedSinusGeneratorFilter<int64_t>;
  template class CachedSinusGeneratorFilter<float>;
#endif
  template class CachedSinusGeneratorFilter<double>;
}