/*
 * unit_tests.cpp
 * Author: Aven Bross
 *
 * Unit testing suite for path coloring.
 */

// STL headers
#include <algorithm>
#include <iostream>
#include <stdexcept>
#include <vector>
#include <utility>
#include <string>
#include <cmath>
#include <set>
#include <random>

// Basic graph headers
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// Random graph headers
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>

// Planar graph headers
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>

// Local project headers
#include "../include/path_coloring/poh_color.hpp"
#include "../include/path_coloring/poh_color_bfs.hpp"
#include "../include/path_coloring/augmented_embedding.hpp"
#include "../include/path_coloring/hartman_skrekovski_color.hpp"
#include "../include/visualization/draw_tikz_graph.hpp"

using namespace boost;

/*
 * -----------------------------------------------------------------------------
 *                             Test option macros
 * -----------------------------------------------------------------------------
 */

// Comment line below to hide passing tests.
//#define SHOW_PASSES

// Comment line below to hide tikz drawing printouts
//#define SHOW_VISUALIZATION

// Comment line below to hide color list assignment printouts
//#define SHOW_COLOR_LISTS

bool failed=false;

/*
 * -----------------------------------------------------------------------------
 *                         Template type definitions
 * -----------------------------------------------------------------------------
 */

// Define the graph type for all test graphs
typedef adjacency_list<
        setS,
        vecS,
        undirectedS,
        property<vertex_index_t, std::size_t>,
        property<edge_index_t, std::size_t>
    > graph_t;

// Vertex and edge types
typedef typename graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename graph_traits<graph_t>::edge_descriptor edge_t;

// Vertex iterator type
typedef typename graph_traits<graph_t>::vertex_iterator vertex_iterator_t;
typedef typename graph_traits<graph_t>::edge_iterator edge_iterator_t;

// Vertex and edge index map types
typedef typename property_map<graph_t, vertex_index_t>::const_type
    vertex_index_map_t;
typedef typename property_map<graph_t, edge_index_t>::const_type
    edge_index_map_t;

// Vertex property map type for an integer property
typedef std::vector<int> integer_property_storage_t;
typedef iterator_property_map<
        typename integer_property_storage_t::iterator,
        vertex_index_map_t
    > integer_property_map_t;

// Vertex property map type for a planar embedding
typedef std::vector<std::vector<edge_t>> planar_embedding_storage_t;
typedef iterator_property_map<
        typename planar_embedding_storage_t::iterator,
        vertex_index_map_t
    > planar_embedding_t;

// Vertex property map type for the neighbor ranges of planar_embedding_t
typedef typename property_traits<planar_embedding_t>::value_type
        ::const_iterator embedding_iterator_t;
typedef typename std::vector<
        std::pair<embedding_iterator_t, embedding_iterator_t>
    > neighbor_range_storage_t;
typedef iterator_property_map<
        typename neighbor_range_storage_t::iterator,
        vertex_index_map_t
    > neighbor_range_map_t;

// Vertex property map to store color lists
typedef std::vector<std::list<int>> color_list_storage_t;
typedef iterator_property_map<
        typename color_list_storage_t::iterator,
        vertex_index_map_t
    > color_list_map_t;

// Vertex property for an augmented embedding
struct adjacency_node_t {
        vertex_t vertex;
        typename std::vector<adjacency_node_t>::iterator iterator;
    };
typedef typename std::vector<std::vector<adjacency_node_t>>
    augmented_embedding_storage_t;
typedef iterator_property_map<
        typename augmented_embedding_storage_t::iterator,
        vertex_index_map_t
    > augmented_embedding_t;

// Vertex property map type for the neighbor ranges of augmented_embedding_t
typedef typename property_traits<augmented_embedding_t>::value_type
        ::const_iterator augmented_embedding_iterator_t;
typedef typename std::vector<
        std::pair<
                augmented_embedding_iterator_t, augmented_embedding_iterator_t
            >
    > augmented_neighbor_range_storage_t;
typedef iterator_property_map<
            typename augmented_neighbor_range_storage_t::iterator,
            vertex_index_map_t
        > augmented_neighbor_range_map_t;

// Vertex property for a graph drawing; gives each vertex cartesians coordinates
struct coord_t {
        std::size_t x;
        std::size_t y;
    };
typedef std::vector<coord_t> straight_line_drawing_storage_t;
typedef iterator_property_map<
        straight_line_drawing_storage_t::iterator,
        vertex_index_map_t
    > straight_line_drawing_t;

// Random graph generator
typedef erdos_renyi_iterator<minstd_rand, graph_t> ERGen;

/*
 * -----------------------------------------------------------------------------
 *                         Planar graph triangulation
 * -----------------------------------------------------------------------------
 */

void make_triangulated(graph_t & graph) {
    make_connected(graph);

    // Initialize the interior edge index
    auto edge_index_map = get(edge_index, graph);
    typename graph_traits<graph_t>::edges_size_type edge_count = 0;
    edge_iterator_t edge_iter, edge_end;
    for(tie(edge_iter, edge_end) = edges(graph); edge_iter != edge_end;
        ++edge_iter)
    {
        put(edge_index_map, *edge_iter, edge_count++);
    }

    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    if(!boyer_myrvold_planarity_test(boyer_myrvold_params::graph = graph,
        boyer_myrvold_params::embedding = planar_embedding))
    {
        std::string error = "Non-planar graph.";
        throw std::logic_error(error);
    }

    make_biconnected_planar(graph, planar_embedding);

    // Re-initialize the edge index, since we just added a few edges
    edge_count = 0;
    for(tie(edge_iter, edge_end) = edges(graph); edge_iter != edge_end;
        ++edge_iter)
    {
        put(edge_index_map, *edge_iter, edge_count++);
    }

    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    make_maximal_planar(graph, planar_embedding);

    // Re-initialize the edge index, since we just added a few edges
    edge_count = 0;
    for(tie(edge_iter, edge_end) = edges(graph); edge_iter != edge_end;
        ++edge_iter)
    {
        put(edge_index_map, *edge_iter, edge_count++);
    }
}

/*
 * -----------------------------------------------------------------------------
 *                            Planar graph drawing
 * -----------------------------------------------------------------------------
 */

void draw_graph_no_color(graph_t & graph) {
    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Create a vertex property map for the coloring
    integer_property_storage_t color_map_storage(num_vertices(graph));
      integer_property_map_t color_map(
              color_map_storage.begin(), get(vertex_index, graph)
        );

    // Find a canonical ordering
    std::vector<vertex_t> ordering;
    planar_canonical_ordering(
            graph, planar_embedding, std::back_inserter(ordering)
        );

    // Compute the straight line drawing
    straight_line_drawing_storage_t straight_line_drawing_storage(
            num_vertices(graph)
        );
    straight_line_drawing_t straight_line_drawing(
            straight_line_drawing_storage.begin(),
            get(vertex_index, graph)
        );
    chrobak_payne_straight_line_drawing(graph,
            planar_embedding,
            ordering.begin(),
            ordering.end(),
            straight_line_drawing
        );

    std::cout << draw_tikz_graph(
            graph, color_map, straight_line_drawing
        ) << "\n";
}

void draw_graph_color(
        const graph_t & graph, const integer_property_map_t & coloring
    )
{
    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );

    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Find a canonical ordering
    std::vector<vertex_t> ordering;
    planar_canonical_ordering(
            graph, planar_embedding, std::back_inserter(ordering)
        );

    // Compute the straight line drawing
    straight_line_drawing_storage_t straight_line_drawing_storage(
            num_vertices(graph)
        );
    straight_line_drawing_t straight_line_drawing(
            straight_line_drawing_storage.begin(),
            get(vertex_index, graph)
        );
    chrobak_payne_straight_line_drawing(graph,
            planar_embedding,
            ordering.begin(),
            ordering.end(),
            straight_line_drawing
        );

    std::cout << draw_tikz_graph(graph, coloring, straight_line_drawing)
        << "\n";
}

/*
 * -----------------------------------------------------------------------------
 *                        Augmented embedding tests
 * -----------------------------------------------------------------------------
 */

void test_augmented_embedding(
        const graph_t & graph, const planar_embedding_t & planar_embedding,
        const augmented_embedding_t & augmented_embedding
    )
{
    // Iterate over each vertex
    vertex_iterator_t v_iter, v_end;
    for (tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;

        auto edge_iter = planar_embedding[v].begin();
        auto adjacency_node_iter = augmented_embedding[v].begin();

        while(edge_iter != planar_embedding[v].end() &&
            adjacency_node_iter != augmented_embedding[v].end())
        {
            vertex_t u_0 = get_incident_vertex(v, *edge_iter, graph);
            vertex_t u_1 = adjacency_node_iter -> vertex;

            if(u_0 != u_1) {
                std::string error = "vertex " + std::to_string(v)
                    + " has neighbor " + std::to_string(u_0)
                    + " in embedding, but neighbor " + std::to_string(u_1)
                    + " in augmented_embedding.";
                throw std::runtime_error(error);
            }

            vertex_t v_0 = adjacency_node_iter -> iterator -> vertex;

            if(v_0 != v) {
                std::string error = "edge " + std::to_string(v) + " to "
                    + std::to_string(u_0) + " back iterator points to "
                    + std::to_string(v_0) + " in augmented_embedding.";
                throw std::runtime_error(error);
            }

            ++edge_iter;
            ++adjacency_node_iter;
        }

        if(edge_iter != planar_embedding[v].end()) {
            std::string error = "vertex " + std::to_string(v)
                + " augmented adjacency list too long.";
            throw std::runtime_error(error);
        }
        else if(adjacency_node_iter != augmented_embedding[v].end()) {
            std::string error = "vertex " + std::to_string(v)
                + " augmented adjacency list too short.";
            throw std::runtime_error(error);
        }
    }
}

void test_augmented_embedding_construction(
        const graph_t & graph, const planar_embedding_t & planar_embedding
    )
{
    // Compute an augmented embedding
    std::vector<std::vector<adjacency_node_t>>
        adjacency_node_storage(num_vertices(graph));
    augmented_embedding_t augmented_embedding(
            adjacency_node_storage.begin(),
            get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        augmented_embedding[v].reserve(out_degree(v, graph));
    }
    augment_embedding(graph, planar_embedding, augmented_embedding);

    // Run the test
    test_augmented_embedding(graph, planar_embedding, augmented_embedding);
}

/*
 * -----------------------------------------------------------------------------
 *                           Path coloring test
 * -----------------------------------------------------------------------------
 */

void test_path_coloring(
        const graph_t & graph, const integer_property_map_t & coloring
    )
{
    // Type definitions
    typedef typename property_traits<integer_property_map_t>::value_type
        color_t;
    typedef typename graph_traits<graph_t>::adjacency_iterator
        adjacency_iterator_t;

    std::set<vertex_t> visited;

    // Iterate over each vertex
    vertex_iterator_t v_iter, v_end;
    for (tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t curr_vertex = *v_iter;
        color_t curr_color = coloring[curr_vertex];

        // If we have not yet visited this vertex, start bfs on same color vertices
        if(visited.count(curr_vertex) == 0) {
            std::queue<vertex_t> bfs_queue;
            bfs_queue.push(curr_vertex);

            std::size_t extra = 1;

            while(!bfs_queue.empty()) {
                vertex_t v = bfs_queue.front();
                bfs_queue.pop();

                if(visited.count(v) == 0) {
                    std::size_t new_neighbor_count = 0, old_neighbor_count = 0;

                    adjacency_iterator_t n_iter, n_end;
                    for(tie(n_iter, n_end) = adjacent_vertices(v, graph); n_iter != n_end; n_iter++) {
                        vertex_t n = *n_iter;

                        // If this vertex is unvisited
                        if(visited.count(n) == 0 && coloring[n] == curr_color) {
                            new_neighbor_count++;
                            bfs_queue.push(n);
                        }
                        else if(coloring[n] == curr_color) {
                            old_neighbor_count++;
                        }
                    }

                    // Find more than one unvisited neighbor, not a path
                    // First vertex may have two neighbors, so we use extra
                    if(new_neighbor_count > 1 + extra) {
                        std::string error = "vertex " + std::to_string(v) + " forms a color "
                            + std::to_string(curr_color) + " branch.";
                        throw std::runtime_error(error);
                    }
                    if(old_neighbor_count > 1) {
                        std::string error = "vertex " + std::to_string(v) + " forms a color "
                            + std::to_string(curr_color) + " cycle.";
                        throw std::runtime_error(error);
                    }
                    extra = 0;

                    visited.insert(v);
                }
            }
        }
    }
}

void test_list_coloring(
        const graph_t & graph, const color_list_map_t & original_color_list_map,
        const color_list_map_t & color_list_map
    )
{
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        if(color_list_map[v].size() < 1) {
            std::string error = "vertex "
                + std::to_string(v) + " has an empty color list.";
            throw std::runtime_error(error);
        }

        if(color_list_map[v].size() > 1) {
            std::string error = "vertex "
                + std::to_string(v) + " has more than one color in its list.";
            throw std::runtime_error(error);
        }

        auto v_color = color_list_map[v].front();
        auto color_iter = std::find(
                original_color_list_map[v].begin(),
                original_color_list_map[v].end(), v_color
            );

        if(color_iter == original_color_list_map[v].end()) {
            std::string error = "vertex "
                + std::to_string(v) + " has the color "
                + std::to_string(v_color) + " not in the original list.";
            throw std::runtime_error(error);
        }
    }
}

/*
 * -----------------------------------------------------------------------------
 *                     Algorithm tests for single graph
 * -----------------------------------------------------------------------------
 */

void poh_color_bfs_test(const graph_t & graph) {
    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Create a vertex property map for the coloring
    integer_property_storage_t color_map_storage(num_vertices(graph));
      integer_property_map_t color_map(
              color_map_storage.begin(), get(vertex_index, graph)
        );

    // Construct a vertex property for marking vertices
    integer_property_storage_t mark_storage(num_vertices(graph));
    integer_property_map_t mark_map(
                mark_storage.begin(), get(vertex_index, graph)
            );

    // Construct a vertex property for storing parents to track the BFS tree
    std::vector<vertex_t> parent_storage(num_vertices(graph));
    iterator_property_map<
            typename std::vector<vertex_t>::iterator,
            vertex_index_map_t
        > parent_map(
                parent_storage.begin(), get(vertex_index, graph)
            );

    // Find a canonical ordering
    std::vector<vertex_t> ordering;
    planar_canonical_ordering(
            graph, planar_embedding, std::back_inserter(ordering)
        );

    // Create vectors to hold chordless paths
    std::vector<vertex_t> p;
    std::vector<vertex_t> q;

    // Initialize path p with top vertex of outer face
    p.push_back(ordering[0]);

    // Initialize path q with bottom vertices of outer face
    q.push_back(ordering[1]);
    q.push_back(ordering.back());

    // Call Poh algorithm
    poh_color_bfs(
            graph,
            planar_embedding,
            p.begin(), p.end(),
            q.begin(), q.end(),
            1, 2, 3,
            mark_map,
            parent_map,
            color_map
        );

    #ifdef SHOW_VISUALIZATION
        draw_graph_color(graph, color_map);
    #endif

    // Test correctness of path coloring
    test_path_coloring(graph, color_map);
}

void poh_color_test(const graph_t & graph) {
    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Create a vertex property map for the coloring
    integer_property_storage_t color_map_storage(num_vertices(graph));
    integer_property_map_t color_map(
            color_map_storage.begin(), get(vertex_index, graph)
        );

    // Construct a vertex property for marking vertices
    integer_property_storage_t mark_storage(num_vertices(graph));
    integer_property_map_t mark_map(
            mark_storage.begin(), get(vertex_index, graph)
        );

    // Construct a vertex property for neighbor ranges
    neighbor_range_storage_t neighbor_range_storage(num_vertices(graph));
    neighbor_range_map_t neighbor_range_map(
            neighbor_range_storage.begin(), get(vertex_index, graph)
        );

    // Find a canonical ordering
    std::vector<vertex_t> ordering;
    planar_canonical_ordering(
            graph, planar_embedding, std::back_inserter(ordering)
        );

    // Create vectors to hold chordless paths
    std::vector<vertex_t> p;
    std::vector<vertex_t> q;

    // Initialize path p with top vertex of outer face
    p.push_back(ordering[0]);

    // Initialize path q with bottom vertices of outer face
    q.push_back(ordering[1]);
    q.push_back(ordering.back());

    // Call Poh algorithm
    poh_color(
            graph,
            planar_embedding,
            p.begin(), p.end(),
            q.begin(), q.end(),
            1, 2, 3,
            neighbor_range_map,
            mark_map,
            color_map
        );

    #ifdef SHOW_VISUALIZATION
        draw_graph_color(graph, color_map);
    #endif

    // Test correctness of path coloring
    test_path_coloring(graph, color_map);
}

void hartman_skrekovski_test(const graph_t & graph, std::size_t num_colors) {
    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    vertex_iterator_t v_iter, v_end;
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Create the augmented embedding
    augmented_embedding_storage_t augmented_embedding_storage(
            num_vertices(graph)
        );
    augmented_embedding_t augmented_embedding(
            augmented_embedding_storage.begin(), get(vertex_index, graph)
        );
    for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        augmented_embedding[v].reserve(out_degree(v, graph));
    }
    augment_embedding(graph, planar_embedding, augmented_embedding);

    // Construct a vertex property for marking vertices
    integer_property_storage_t face_location_storage(num_vertices(graph));
    integer_property_map_t face_location_map(
            face_location_storage.begin(), get(vertex_index, graph)
        );

    // Construct a vertex property for neighbor ranges
    augmented_neighbor_range_storage_t neighbor_range_storage(
            num_vertices(graph)
        );
    augmented_neighbor_range_map_t neighbor_range_map(
            neighbor_range_storage.begin(), get(vertex_index, graph)
        );

    // Find a canonical ordering
    std::vector<vertex_t> ordering;
    planar_canonical_ordering(
            graph, planar_embedding, std::back_inserter(ordering)
        );

    // Grab a triangle to use as the outer face from the ordering
    std::vector<vertex_t> outer_face = {
            ordering[1], ordering[0], ordering.back()
        };

    // Create property map to hold the list assignment
    color_list_storage_t color_list_storage(num_vertices(graph));
    color_list_map_t color_list_map(
            color_list_storage.begin(), get(vertex_index, graph)
        );
    color_list_storage_t original_color_list_storage(num_vertices(graph));
    color_list_map_t original_color_list_map(
            original_color_list_storage.begin(), get(vertex_index, graph)
        );

    // Uniform distribution to generate random colors
    std::mt19937 generator;
    std::uniform_int_distribution<int> distribution(1, num_colors);

    // Iterate over each vertex
    for (tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        std::vector<int> random_colors(3);
        while(
                random_colors[0] == random_colors[1] ||
                random_colors[0] == random_colors[2] ||
                random_colors[1] == random_colors[2]
            )
        {
            for(std::size_t i = 0; i < 3; ++i) {
                random_colors[i] = distribution(generator);
            }
        }

        #ifdef SHOW_COLOR_LISTS
            std::cout << "color_list_map[" << *v_iter << "] = { ";
            for(std::size_t i = 0; i < 3; ++i) {
                std::cout << random_colors[i] << ((i != 2) ? ", " : "");
            }
            std::cout << "}\n";
        #endif

        std::copy(
                random_colors.begin(), random_colors.end(),
                std::back_inserter(color_list_map[*v_iter])
            );
        std::copy(
                random_colors.begin(), random_colors.end(),
                std::back_inserter(original_color_list_map[*v_iter])
            );
    }

    // Call path list-coloring algorithm
    hartman_skrekovski_color(
            graph,
            augmented_embedding,
            outer_face.begin(), outer_face.end(),
            neighbor_range_map,
            face_location_map,
            color_list_map
        );

    // Test correctness of list coloring
    test_list_coloring(graph, original_color_list_map, color_list_map);

    // Create a vertex property map for the coloring
    integer_property_storage_t color_map_storage(num_vertices(graph));
    integer_property_map_t color_map(
            color_map_storage.begin(), get(vertex_index, graph)
        );

    // Grab the color from each vertex's list
    for (tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
        vertex_t v = *v_iter;
        color_map[v] = color_list_map[v].front();
    }

    #ifdef SHOW_VISUALIZATION
        draw_graph_color(graph, color_map);
    #endif

    // Test correctness of path coloring
    test_path_coloring(graph, color_map);
}

/*
 * -----------------------------------------------------------------------------
 *             Run each algorithm test on a set of generated graphs
 * -----------------------------------------------------------------------------
 */

void test_augmenting_embeddings() {
    std::cout << "Augmenting Planar Embeddings" << std::endl;

    minstd_rand gen;
    gen.seed(8573);

    for(std::size_t order = 4; order <= 200; order+=10) {
        bool found_planar = false;
        std::size_t count = 4;

        while(!found_planar) {
            try {
                // Construct a random trriangulated graph
                graph_t graph(ERGen(gen, order, 2 * order - count), ERGen(), order);

                ++count;

                make_triangulated(graph);

                found_planar = true;

                // Create the planar embedding
                planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
                planar_embedding_t planar_embedding(
                        planar_embedding_storage.begin(), get(vertex_index, graph)
                    );
                vertex_iterator_t v_iter, v_end;
                for(tie(v_iter, v_end) = vertices(graph); v_iter != v_end; v_iter++) {
                    vertex_t v = *v_iter;
                    planar_embedding[v].reserve(out_degree(v, graph));
                }
                boyer_myrvold_planarity_test(
                        boyer_myrvold_params::graph = graph,
                        boyer_myrvold_params::embedding = planar_embedding
                    );

                test_augmented_embedding_construction(graph, planar_embedding);

                #ifdef SHOW_PASSES
                    std::cout<<"    PASS " << order << " vertex augmented embedding construction."<<std::endl;
                #endif
            }
            catch(std::logic_error error) {
                // Generated a non-planar graph, ignore this case
            }
            catch(std::exception& error) {
                std::cout<<"    FAIL " << order << "vertex augmented embedding construction ("<<error.what()<<")."<<std::endl;
                failed=true;
            }
            catch(...) {
                std::cout<<"    FAIL " << order << " vertex augmented embedding construction (unknown error)."<<std::endl;
                failed=true;
            }
        }
    }
}

void test_poh_color_bfs() {
    std::cout<<"Path 3-coloring (Poh w/BFS)"<<std::endl;

    minstd_rand gen;
    gen.seed(8573);

    for(std::size_t order = 4; order <= 200; order+=10) {
        bool found_planar = false;
        std::size_t count = 4;

        while(!found_planar) {
            try {
                // Construct a random trriangulated graph
                graph_t graph(ERGen(gen, order, 2 * order - count), ERGen(), order);

                ++count;

                make_triangulated(graph);

                found_planar = true;

                poh_color_bfs_test(graph);

                #ifdef SHOW_PASSES
                    std::cout<<"    PASS " << order << " vertex path 3-color."<<std::endl;
                #endif
            }
            catch(std::logic_error error) {
                // Generated a non-planar graph, ignore this case
            }
            catch(std::exception& error) {
                std::cout<<"    FAIL " << order << " vertex path 3-color ("<<error.what()<<")."<<std::endl;
                failed=true;
            }
            catch(...) {
                std::cout<<"    FAIL " << order << " vertex path 3-color (unknown error)."<<std::endl;
                failed=true;
            }
        }
    }
}

void test_poh_color() {
    std::cout<<"Path 3-coloring (Poh w/outer face tracing)"<<std::endl;

    minstd_rand gen;
    gen.seed(8573);

    for(std::size_t order = 4; order <= 200; order+=10) {
        bool found_planar = false;
        std::size_t count = 4;

        while(!found_planar) {
            try {
                // Construct a random trriangulated graph
                graph_t graph(ERGen(gen, order, 2 * order - count), ERGen(), order);

                ++count;

                make_triangulated(graph);

                found_planar = true;

                poh_color_test(graph);

                #ifdef SHOW_PASSES
                    std::cout<<"    PASS " << order << " vertex path 3-color."<<std::endl;
                #endif
            }
            catch(std::logic_error error) {
                // Generated a non-planar graph, ignore this case
            }
            catch(std::exception& error) {
                std::cout<<"    FAIL " << order << " vertex path 3-color ("<<error.what()<<")."<<std::endl;
                failed=true;
            }
            catch(...) {
                std::cout<<"    FAIL " << order << " vertex path 3-color (unknown error)."<<std::endl;
                failed=true;
            }
        }
    }
}

void test_hartman_skrekovski_color()
{
    minstd_rand gen;
    gen.seed(8573);

    for(std::size_t colors = 3; colors < 14; colors += 2) {
        std::cout << "Path 3-choosing (Hartman-Skrekovski) with " << colors << " colors" << std::endl;

        for(std::size_t order = 4; order <= 200; order+=10) {
            bool found_planar = false;
            std::size_t count = 4;

            while(!found_planar) {
                try {
                    // Construct a random trriangulated graph
                    graph_t graph(ERGen(gen, order, 2 * order - count), ERGen(), order);

                    ++count;

                    make_triangulated(graph);

                    found_planar = true;

                    hartman_skrekovski_test(graph, colors);

                    #ifdef SHOW_PASSES
                        std::cout<<"    PASS " << order << " vertex, " << colors
                            << " colors path 3-list-color."<<std::endl;
                    #endif
                }
                catch(std::logic_error error) {
                    // Generated a non-planar graph, ignore this case
                }
                catch(std::exception& error) {
                    std::cout<<"    FAIL " << order << " vertex, " << colors
                            << " colors path 3-list-color ("<<error.what()<<")."<<std::endl;
                    failed=true;
                }
                catch(...) {
                    std::cout<<"    FAIL " << order << " vertex, " << colors
                            << " colors path 3-list-color (unknown error)."<<std::endl;
                    failed=true;
                }
            }
        }
    }
}

/*
 * -----------------------------------------------------------------------------
 *                             Main: run tests
 * -----------------------------------------------------------------------------
 */

int main() {
    test_augmenting_embeddings();
    test_poh_color_bfs();
    test_poh_color();
    test_hartman_skrekovski_color();

    if(failed)
        std::cout<<"THERE ARE FAILING TESTS"<<std::endl;
    else
        std::cout<<"ALL TESTS PASSED"<<std::endl;

    return 0;
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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 OWNER 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.

#include <cmath>
#include <boost/array.hpp>
#include "calculator.h"
#include "packet.h"

namespace snark { namespace velodyne { namespace puck {

static boost::array< double, 16 > elevation_ = {{ -15.0 * M_PI / 180
                                                , 1 * M_PI / 180
                                                , -13 * M_PI / 180
                                                , 3 * M_PI / 180
                                                , -11 * M_PI / 180
                                                , 5 * M_PI / 180
                                                , -9 * M_PI / 180
                                                , 7 * M_PI / 180
                                                , -7 * M_PI / 180
                                                , 9 * M_PI / 180
                                                , -5 * M_PI / 180
                                                , 11 * M_PI / 180
                                                , -3 * M_PI / 180
                                                , 13 * M_PI / 180
                                                , -1 * M_PI / 180
                                                , 15 * M_PI / 180 }};

struct laser
{
    double sin;
    double cos;

    laser() {}
    laser( unsigned int index ) : sin( std::sin( elevation_[ index ] ) ), cos( std::cos( elevation_[ index ] ) ) {}
};

typedef boost::array< laser, puck::packet::number_of_lasers > lasers_t;

static lasers_t init_lasers()
{
    lasers_t lasers;
    for( unsigned int j = 0; j < puck::packet::number_of_lasers; ++j ) { lasers[j] = laser( j ); }
    return lasers;
}

static lasers_t lasers = init_lasers();

std::pair< ::Eigen::Vector3d, ::Eigen::Vector3d > calculator::ray( unsigned int laser, double range, double angle ) const { return std::make_pair( ::Eigen::Vector3d::Zero(), point( laser, range, angle ) ); }

::Eigen::Vector3d calculator::point( unsigned int laser, double range, double angle ) const
{
    // todo: once puck/packet.cpp is fixed, use the commented line below
    // return ::Eigen::Vector3d( range * lasers[laser].cos * std::cos( angle ), range * lasers[laser].cos * std::sin( angle ), range * lasers[laser].sin );
    return ::Eigen::Vector3d( range * lasers[laser].cos * std::sin( angle ), range * lasers[laser].cos * std::cos( angle ), range * lasers[laser].sin );
}

double calculator::range( unsigned int, double range ) const { return range; }

// todo! super quick and dirty; by right the places to fix:
//       - puck/packet.cpp: fiddly part: fix azimuth and make sure step works correctly
//       - puck/calculator.cpp: easy part: simply swap sin and cos in calculator::point()
//       - scan_tick.cpp: packet_angle_( puck::packet ): hardcoded offset
//       - if time permits, see why there is such a 90-degree discrepancy in puck spec
// double calculator::azimuth( unsigned int, double azimuth ) const { return azimuth; }
double calculator::azimuth( unsigned int, double azimuth ) const { return M_PI / 2 - azimuth; }

double calculator::intensity( unsigned int, unsigned char intensity, double ) const { return intensity; }

} } } // namespace snark { namespace velodyne { namespace puck {


//***************************************************************************
//* Copyright (c) 2015 Saint Petersburg State University
//* Copyright (c) 2011-2014 Saint Petersburg Academic University
//* All Rights Reserved
//* See file LICENSE for details.
//***************************************************************************

#ifndef __HAMMER_IT_PENALTY_ESTIMATOR_HPP__
#define __HAMMER_IT_PENALTY_ESTIMATOR_HPP__

#include "HSeq.hpp"
#include "config_struct.hpp"
#include "consensus.hpp"
#include "flow_space_read.hpp"
#include "hkmer_distance.hpp"
#include "valid_hkmer_generator.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/optional.hpp>

#include <bamtools/api/BamAlignment.h>
#include <bamtools/api/SamHeader.h>
#include "seqeval/BaseHypothesisEvaluator.h"

#include <algorithm>
#include <cassert>
#include <deque>
#include <fstream>
#include <iterator>
#include <limits>
#include <list>
#include <string>
#include <vector>

#if 1
#include <iomanip>
#include <iostream>
#endif

#include <atomic>
#include <fstream>
#include "gamma_poisson_model.hpp"
#include "read_corrector_structs_new.h"

namespace hammer {
namespace correction {

struct Interval {
  size_t left_;
  size_t right_;
};

class GammaPoissonLikelihoodCalcer {
 private:
  const n_gamma_poisson_model::GammaDistribution prior_;
  n_gamma_poisson_model::PoissonGammaDistribution count_distribution_;
  double upper_quantile_;
  double lower_quantile_;
  size_t noise_quantiles_lower_;
  size_t noise_quantile_upper_;
  double correction_penalty_;
  double bad_kmer_penalty_;
  const KMerData& data_;

 public:
  class PenaltyState {
    friend class GammaPoissonLikelihoodCalcer;

   private:
    double likelihood_ = 0;
    size_t last_correction_distances_ = 0;
    size_t hkmer_distance_to_read_ = 0;
    HKMer read_kmer_;

   public:
    double Penalty() const { return likelihood_; }
  };

 public:
  class Factory {
   private:
    KMerData& data_;

   public:
    Factory(KMerData& data) : data_(data) {}

    GammaPoissonLikelihoodCalcer operator()(const std::string& read) const {
      ValidHKMerGenerator<hammer::K> generator(read.data(), nullptr,
                                               read.length());

      std::vector<size_t> counts;
      double sum_count = 0;
      double sum_weight = 0;

      while (generator.HasMore()) {
        size_t idx = data_.checking_seq_idx(generator.kmer());

        if (idx != -1ULL) {
          const auto& kmer_stat = data_[idx];
          if (kmer_stat.skip()) {
            counts.push_back(data_[idx].count);
          }
          const double p = exp(kmer_stat.posterior_genomic_ll);
          sum_count += p * data_[idx].count;
          sum_weight += p;
        }
        generator.Next();
      }

      n_gamma_poisson_model::GammaDistribution read_prior =
          [&]() -> n_gamma_poisson_model::GammaDistribution {
        if (counts.size() < 10) {
          return n_gamma_poisson_model::GammaDistribution(sum_count + 0.1,
                                                        sum_weight + 0.1);
        } else {
          return n_gamma_poisson_model::TClusterModelEstimator::EstimatePrior(
              counts);
        }
      }();

      return GammaPoissonLikelihoodCalcer(read_prior, data_);
    }
  };

  using PenaltyCalcerFactory = Factory;

  GammaPoissonLikelihoodCalcer(
      const n_gamma_poisson_model::GammaDistribution& prior, const KMerData& data)
      : prior_(prior), count_distribution_(prior_), data_(data) {
    upper_quantile_ = count_distribution_.Quantile(1.0 - cfg::get().count_dist_skip_quantile);
    lower_quantile_ = count_distribution_.Quantile(cfg::get().count_dist_skip_quantile);

    const double eps = cfg::get().count_dist_eps;
    noise_quantiles_lower_ = (size_t)max(count_distribution_.Quantile(eps), 1.0);
    noise_quantile_upper_ = (size_t)count_distribution_.Quantile(1.0 - eps);

    correction_penalty_ = cfg::get().correction_penalty;
    bad_kmer_penalty_ = cfg::get().bad_kmer_penalty;
    assert(lower_quantile_ < upper_quantile_);
  }

  inline void UpdateInitial(PenaltyState& state, const IonEvent& event,
                            const hammer::KMerStat* const) const {
    state.read_kmer_ <<= event.FixedHRun();
  }

  inline void Update(PenaltyState& state, const IonEvent& event,
                     const hammer::KMerStat* const last_kmer_stats) const {
    assert(event.fixed_size_ >= 0);

    if (std::isinf(state.likelihood_)) {
      return;
    }

    const size_t last_kmer_count = last_kmer_stats ? last_kmer_stats->count : 0;

    const int bits = 4;
    const unsigned dist =
            std::min((unsigned)std::abs(event.fixed_size_ - event.overserved_size_),
                     (unsigned)(1 << bits) - 1);

    {
      state.hkmer_distance_to_read_ += dist;
      state.hkmer_distance_to_read_ -=
          (state.last_correction_distances_ >> (bits * (hammer::K - 1))) &
          ((1 << bits) - 1);
      state.last_correction_distances_ =
          ((state.last_correction_distances_ << bits) | (dist));
      state.read_kmer_ <<= event.ObservedHRun();
    }

    if (state.hkmer_distance_to_read_ > hammer::K / 2) {
      state.likelihood_ = -std::numeric_limits<double>::infinity();
      return;
    }

    const bool is_good = last_kmer_stats ? last_kmer_stats->good() : false;

    if (!is_good || (dist)) {
      const size_t cnt = min(max(noise_quantiles_lower_, last_kmer_count), noise_quantile_upper_);

      // state.Likelihood += dist * log(Model.ErrorRate(event.FixedSize));
      state.likelihood_ += (double)state.hkmer_distance_to_read_ * correction_penalty_;
      state.likelihood_ += count_distribution_.LogLikelihood(cnt);
    }

    if (!is_good) {
      state.likelihood_ += bad_kmer_penalty_;
    } else {
      state.likelihood_ += std::max((double)(last_kmer_stats->posterior_genomic_ll), bad_kmer_penalty_);
    }
  }

  inline bool Skip(const HKMer& kmer) const {
    size_t idx = data_.checking_seq_idx(kmer);
    if (idx == -1ULL) {
      return false;
    }
    const auto& stat = data_[idx];

    return stat.good() && (stat.count <= upper_quantile_) && (stat.count >= lower_quantile_) && !stat.dist_one_subcluster;
  }

  inline bool IsGood(const HKMer& kmer) const {
    size_t idx = data_.checking_seq_idx(kmer);
    if (idx == -1ULL) {
      return false;
    }

    return data_[idx].good();
  }

  inline std::function<bool(const hammer::HKMer&)> Good() const {
    return [this](const HKMer& hkMer) { return this->IsGood(hkMer); };
  }

  static PenaltyState CreateState(const bool, const unsigned) {
    return PenaltyState();
  }

  std::string TrimLeft(const std::string& read) const {

    ValidHKMerGenerator<K> generator(read.data(), nullptr, read.size());
    size_t offset = 0;
    while (generator.HasMore()) {
      const auto& hkmer = generator.kmer();
      if (IsGood(hkmer)) {
        break;
      }
      offset += hkmer[0].len;
      generator.Next();
    }
    const auto from = offset;//generator.pos() - generator.kmer().size();
    if (from > 0) {
      if (read[from - 1] == read[from])
      {
        assert(read[from - 1] != read[from]);
      }
    }
    return read.substr(from);
  }

  std::string TrimBadQuality(const std::string& read) const {
    return TrimLeft(ReverseComplement(TrimLeft(ReverseComplement(read))));
  }

  inline Interval SolidIsland(
      ValidHKMerGenerator<K>& generator,
      std::function<bool(const HKMer& kmer)> is_good_predicate) const {
    size_t bestLeft = (size_t)-1ULL;
    size_t bestRight = (size_t)-1ULL;
    size_t solidLength = 0;

    size_t leftPos = 0;
    size_t rightPos = 0;

    while (generator.HasMore()) {
      const auto& hkmer = generator.kmer();
      bool isGood = is_good_predicate(hkmer);

      if (isGood) {
        const auto lastHRunSize = hkmer[K - 1].len;
        const auto hkmerSize = hkmer.size();
        const auto hkmerStartPosition = generator.pos() - hkmerSize;
        const auto prevEndPosition = generator.pos() - lastHRunSize;

        if (prevEndPosition != rightPos) {
          leftPos = hkmerStartPosition;
        }
        rightPos = generator.pos();

        if (rightPos - leftPos > solidLength) {
          bestLeft = leftPos;
          bestRight = rightPos;
          solidLength = rightPos - leftPos;
        }
      }
      generator.Next();
    }
    return {bestLeft, bestRight};
  }

  inline Interval SolidIslandGood(ValidHKMerGenerator<K>& generator) const {
    return SolidIsland(generator,
                       [&](const HKMer& kmer) -> bool { return IsGood(kmer); });
  }

  inline Interval SolidIslandConservative(
      ValidHKMerGenerator<K>& generator) const {
    return SolidIsland(generator,
                       [&](const HKMer& kmer) -> bool { return Skip(kmer); });
  }

  inline Interval SolidIsland(const std::string& read) const {
    {
      ValidHKMerGenerator<K> generator(&read[0], nullptr, read.size());
      auto conservative = SolidIslandConservative(generator);
      if (conservative.left_ != conservative.right_) {
        return conservative;
      }
    }
    {
      ValidHKMerGenerator<K> generator(&read[0], nullptr, read.size());
      return SolidIslandGood(generator);
    }
  }

  inline Interval SolidIsland(const io::SingleRead& read) const {
    {
      ValidHKMerGenerator<K> generator(read);
      auto conservative = SolidIslandConservative(generator);
      if (conservative.left_ != conservative.right_) {
        return conservative;
      }
    }
    {
      ValidHKMerGenerator<K> generator(read);
      return SolidIslandGood(generator);
    }
  }
};

};  // namespace correction
};  // namespace hammer
#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_FUNCTION_INTERLEAVE_FIRST_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INTERLEAVE_FIRST_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-swar
    Function object implementing interleave_first capabilities

    Computes a vector from a combination of the two inputs.

    @par Semantic:

    For every parameters of types respectively T, the following code:

    @code
    T r = interleave_first(a,b);
    @endcode

    is equivalent to :

    @code
    r = [ a[0] b[0] a[1] b[1] ... a[n/2-1] b[n/2-1] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  const boost::dispatch::functor<tag::interleave_first_> interleave_first = {};
} }
#endif

#include <boost/simd/function/scalar/interleave_first.hpp>
#include <boost/simd/function/simd/interleave_first.hpp>

#endif


#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <string>
#include <utility>
#include <vector>

std::string spin(std::string&& data, const std::string& cmd)
{
    std::string result;
    int count = atoi(&cmd[1]);
    auto middle = std::next(data.begin(), data.size()-count);
    std::copy(middle, data.end(), std::back_insert_iterator<std::string>(result));
    std::copy(data.begin(), middle, std::back_insert_iterator<std::string>(result));
    return result;
}

std::string swap_prog(std::string&& data, const std::string& cmd)
{
    auto _cmd = cmd;
    _cmd.erase(0, 1);
    std::vector<std::string> words;
    boost::algorithm::split(words, _cmd, boost::is_any_of("/"), boost::algorithm::token_compress_on);
    std::swap(data[atoi(words[0].c_str())], data[atoi(words[1].c_str())]);
    return data;
}

std::string swap_pos(std::string&& data, const std::string& cmd)
{
    std::swap(data[data.find(cmd[1])], data[data.find(cmd[3])]);
    return data;
}

int main()
{
    std::string data{"abcdefghijklmnop"};

    std::string line;
    std::getline(std::cin, line);
    std::vector<std::string> words;
    boost::algorithm::split(
        words, line, boost::is_any_of(","), boost::algorithm::token_compress_on);
    for (const auto& cmd : words) {
        if (cmd[0] == 's') {
            data = spin(std::move(data), cmd);
        } else if (cmd[0] == 'x') {
            data = swap_prog(std::move(data), cmd);
        } else if (cmd[0] == 'p') {
            data = swap_pos(std::move(data), cmd);
        } else {
            std::cerr << "ERROR: " << cmd << "\n";
            return 1;
        }
    }
    std::cout << "data: " << data << "\n";
    return 0;
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *  Authors: Andrea Mola, Luca Heltai, 2014
 */

// @sect3{Include files}

// 我们首先包括一堆我们将在程序的各个部分使用的文件。它们中的大多数已经在以前的教程中讨论过了。

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_in.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>

// 这些是opencascade支持类和函数的头文件。注意，只有当你在编译deal.II库时支持OpenCASCADE，即在deal.II配置过程中调用 -DDEAL_II_WITH_OPENCASCADE=ON 和 -DOPENCASCADE_DIR=/path/to/your/opencascade/installation 时，这些将包含合理的数据。

#include <deal.II/opencascade/manifold_lib.h>
#include <deal.II/opencascade/utilities.h>

// 最后，几个C++标准头文件

#include <cmath>
#include <iostream>
#include <fstream>
#include <string>

// 我们将程序的其他部分隔离在自己的命名空间中

namespace Step54
{
  using namespace dealii;

//  @sect3{The TriangulationOnCAD class}

// 这是主类。它真正做的是存储输入和输出文件的名称，以及一个三角图。然后，它提供了一个函数，可以从一个粗略的网格中生成这样一个三角形，使用介绍中所讨论的策略之一，并在类的顶部的枚举类型中列出。

// 这个类的成员函数与你在其他大多数教程程序中可以找到的类似，都是在模拟的网格设置阶段。

  class TriangulationOnCAD
  {
  public:
    enum ProjectionType
    {
      NormalProjection       = 0,
      DirectionalProjection  = 1,
      NormalToMeshProjection = 2
    };

    TriangulationOnCAD(
      const std::string &  initial_mesh_filename,
      const std::string &  cad_file_name,
      const std::string &  output_filename,
      const ProjectionType surface_projection_kind = NormalProjection);

    void run();

  private:
    void read_domain();

    void refine_mesh();

    void output_results(const unsigned int cycle);

    Triangulation<2, 3> tria;

    const std::string initial_mesh_filename;
    const std::string cad_file_name;
    const std::string output_filename;

    const ProjectionType surface_projection_kind;
  };
// @sect4{TriangulationOnCAD::TriangulationOnCAD}

// TriangulationOnCAD类的构造函数非常简单。输入参数是输入和输出文件名的字符串，以及决定在网格细化循环中使用哪种曲面投影仪的枚举类型（详见下文）。

  TriangulationOnCAD::TriangulationOnCAD(
    const std::string &  initial_mesh_filename,
    const std::string &  cad_file_name,
    const std::string &  output_filename,
    const ProjectionType surface_projection_kind)
    : initial_mesh_filename(initial_mesh_filename)
    , cad_file_name(cad_file_name)
    , output_filename(output_filename)
    , surface_projection_kind(surface_projection_kind)
  {}
// @sect4{TriangulationOnCAD::read_domain}

// 下面的函数代表了这个程序的核心。 在这个函数中，我们导入CAD形状，在此基础上生成并完善我们的三角测量。我们假设CAD曲面包含在 @p cad_file_name 文件中（我们在输入目录中提供了一个名为 "input/DTMB-5415_bulbous_bow.iges "的IGES文件例子，它代表了一艘船的球形船头）。几个凸和凹的高曲率区域的存在使得我们提供的几何体成为一个特别有意义的例子。

// 在导入船首表面后，我们提取了一些构成它的曲线和曲面，并使用它们来生成一组投影仪。这些投影仪定义了三角法在细化单元时必须遵循的规则，以定位每个新节点。

// 为了初始化Triangulation，就像在以前的教程中一样，我们导入一个以VTK格式保存的现有网格。在这里我们假设用户已经在外部生成了一个粗略的网格，与IGES的几何图形相匹配。在编写本教程的时候，deal.II库并不自动支持生成这样的网格，但是有一些工具可以从CAD文件开始为你提供合理的初始网格。在我们的例子中，导入的网格是由一个四边形单元组成的，其顶点被放置在CAD形状上。

// 在导入IGES几何体和初始网格后，我们将之前讨论过的投影仪分配给每个需要在CAD表面上进行细化的边和单元。

// 在本教程中，我们将测试介绍中所描述的三种不同的CAD表面投影器，并将分析每一种投影器所得到的结果。 如前所述，这些投影策略中的每一种都是在不同的类中实现的，这些类型的对象可以用 Triangulation::set_manifold 的方法分配给一个三角形。
//然后，
//下面的函数首先导入给定的CAD文件。函数的参数是一个包含所需文件名的字符串，以及一个比例因子。在这个例子中，比例因子被设置为1e-3，因为原始几何体是以毫米为单位的（这是大多数IGES文件的典型计量单位），而我们更喜欢以米为单位工作。 该函数的输出是一个OpenCASCADE通用拓扑形状类的对象，即一个 @p TopoDS_Shape. 。
  void TriangulationOnCAD::read_domain()
  {
    TopoDS_Shape bow_surface = OpenCASCADE::read_IGES(cad_file_name, 1e-3);

// 每个CAD几何对象都被定义了一个公差，它表示其位置可能的不精确性。例如，顶点的公差 @p tol 表示它可以位于以标称位置为中心，半径为 @p tol. 的球体中的任何一点。

// 下面的方法是提取给定形状的公差，并使其变大一些以避免麻烦。

    const double tolerance = OpenCASCADE::get_shape_tolerance(bow_surface) * 5;

// 我们现在要从通用形状中提取一组复合子形状。特别是，CAD文件的每个面都是由类型为 @p TopoDS_Wire, 的修剪曲线组成的，它是构成曲面边界的 @p TopoDS_Edges 的集合，以及曲面本身的NURBS描述。我们将使用线型投影仪将我们的三角形的边界与划定曲面的线联系起来。 为了提取所有的复合子形状，如线、壳或实体，我们求助于OpenCASCADE命名空间的一种方法。  OpenCASCADE::extract_compound_shapes 的输入是一个形状和一组空的 std::vectors 子形状，它将被填入在给定拓扑形状中发现的所有复合形状。

    std::vector<TopoDS_Compound>  compounds;
    std::vector<TopoDS_CompSolid> compsolids;
    std::vector<TopoDS_Solid>     solids;
    std::vector<TopoDS_Shell>     shells;
    std::vector<TopoDS_Wire>      wires;

    OpenCASCADE::extract_compound_shapes(
      bow_surface, compounds, compsolids, solids, shells, wires);

// 接下来的几个步骤比较熟悉，允许我们从外部VTK文件中导入一个现有的网格，并将其转换为一个交易三角图。

    std::ifstream in;

    in.open(initial_mesh_filename);

    GridIn<2, 3> gi;
    gi.attach_triangulation(tria);
    gi.read_vtk(in);

// 我们输出这个初始网格，将其保存为细化步骤0。

    output_results(0);

// 导入的网格有一个位于三维空间中的单一的二维单元。我们现在要确保它是根据上面导入的CAD几何图形进行细化的。为此，我们得到一个单元的迭代器，并给它分配manifold_id 1（见  @ref GlossManifoldIndicator  "这个词汇表条目"）。我们还得到了一个指向其四个面的迭代器，并为每个面分配了manifold_id 2。

    Triangulation<2, 3>::active_cell_iterator cell = tria.begin_active();
    cell->set_manifold_id(1);

    for (const auto &face : cell->face_iterators())
      face->set_manifold_id(2);

// 一旦CAD几何体和初始网格都被导入和消化，我们就用CAD的曲面和曲线来定义投影仪，并将它们分配给刚才指定的流形ID。

// 使用我们的CAD文件中的单线来定义第一个投影仪。 ArclengthProjectionLineManifold将确保位于导线上的每条网格边缘都被细化为一个位于导线上的点，并将其分割为两个位于边缘顶点之间的相等弧线。我们首先检查线的向量是否至少包含一个元素，然后为它创建一个Manifold对象。

// 一旦投影仪被创建，我们就把它分配给三角形的所有部分，manifold_id = 2。

    Assert(
      wires.size() > 0,
      ExcMessage(
        "I could not find any wire in the CAD file you gave me. Bailing out."));

    OpenCASCADE::ArclengthProjectionLineManifold<2, 3> line_projector(
      wires[0], tolerance);

    tria.set_manifold(2, line_projector);

// 根据构造函数的 @p surface_projection_kind 选项所指定的内容来创建表面投影仪。特别是，如果surface_projection_kind的值等于 @p NormalProjection, ，我们选择 OpenCASCADE::NormalProjectionManifold. ，那么新的网格点最初将在所考虑的单元格/边的arycenter处生成，然后沿其法线方向投影到CAD表面。 NormalProjectionManifold构造函数只需要一个形状和一个公差，然后我们把它分配给三角结构，用于所有具有id 1的流形的部分。

    switch (surface_projection_kind)
      {
        case NormalProjection:
          {
            OpenCASCADE::NormalProjectionManifold<2, 3> normal_projector(
              bow_surface, tolerance);
            tria.set_manifold(1, normal_projector);

            break;
          }
// @p If  surface_projection_kind值为 @p DirectionalProjection,  我们选择 OpenCASCADE::DirectionalProjectionManifold 类。新的网格点将在所考虑的单元格/边的arycenter处初始生成，然后沿着 OpenCASCADE::DirectionalProjectionManifold 构造函数指定的方向投影到CAD表面上。在这个例子中，投影是沿着Y轴进行的。

        case DirectionalProjection:
          {
            OpenCASCADE::DirectionalProjectionManifold<2, 3>
              directional_projector(bow_surface,
                                    Point<3>(0.0, 1.0, 0.0),
                                    tolerance);
            tria.set_manifold(1, directional_projector);

            break;
          }

// 作为第三个选项，如果 @p surface_projection_kind 的值是 @p NormalToMeshProjection, ，我们选择 OpenCASCADE::NormalToMeshProjectionManifold.  新的网格点将再次在所考虑的单元/边的arycenter处初始生成，然后沿着一个估计为网格法线方向的方向投影到CAD表面。 OpenCASCADE::NormalToMeshProjectionManifold  构造函数只需要一个形状（至少包含一个面）和一个公差。

        case NormalToMeshProjection:
          {
            OpenCASCADE::NormalToMeshProjectionManifold<2, 3>
              normal_to_mesh_projector(bow_surface, tolerance);
            tria.set_manifold(1, normal_to_mesh_projector);

            break;
          }

// 最后，我们使用良好的软件清洁性，确保这真的涵盖了 @p case 语句的所有可能选项。如果我们得到任何其他的值，我们就直接中止程序。

        default:
          AssertThrow(false, ExcInternalError());
      }
  }
// @sect4{TriangulationOnCAD::refine_mesh}

// 这个函数是全局细化网格的。在其他教程中，它通常也会分配自由度，并调整矩阵和向量的大小。这里没有进行这些工作，因为我们没有在生成的三角形上运行任何模拟。

// 虽然这个函数看起来很简单，但这是我们对这个教程程序感兴趣的大部分工作的实际发生地点。特别是，在完善定义船体表面的四边形和直线时，Triangulation类将询问我们分配给处理单个流形ID的各种对象，以确定新顶点的位置。

  void TriangulationOnCAD::refine_mesh()
  {
    tria.refine_global(1);
  }

//  @sect4{TriangulationOnCAD::output_results}

// 输出我们的计算结果是一个相当机械的任务。这个函数的所有组成部分之前已经讨论过了。

  void TriangulationOnCAD::output_results(const unsigned int cycle)
  {
    const std::string filename =
      (output_filename + "_" + Utilities::int_to_string(cycle) + ".vtk");
    std::ofstream logfile(filename);
    GridOut       grid_out;
    grid_out.write_vtk(tria, logfile);
  }
// @sect4{TriangulationOnCAD::run}

// 这是主函数。它应该是不言自明的。

  void TriangulationOnCAD::run()
  {
    read_domain();

    const unsigned int n_cycles = 5;
    for (unsigned int cycle = 0; cycle < n_cycles; ++cycle)
      {
        refine_mesh();
        output_results(cycle + 1);
      }
  }
} // namespace Step54
// @sect3{The main() function}

// 这是本程序的主要功能。它的基本结构与之前所有的教程程序一样，但通过新顶点放置的三种可能性来运行主类。

int main()
{
  try
    {
      using namespace Step54;

      const std::string in_mesh_filename = "input/initial_mesh_3d.vtk";
      const std::string cad_file_name    = "input/DTMB-5415_bulbous_bow.iges";

      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << "Testing projection in direction normal to CAD surface"
                << std::endl;
      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::string        out_mesh_filename = ("3d_mesh_normal_projection");
      TriangulationOnCAD tria_on_cad_norm(in_mesh_filename,
                                          cad_file_name,
                                          out_mesh_filename,
                                          TriangulationOnCAD::NormalProjection);
      tria_on_cad_norm.run();
      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << std::endl;
      std::cout << std::endl;

      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << "Testing projection in y-axis direction" << std::endl;
      std::cout << "----------------------------------------------------------"
                << std::endl;
      out_mesh_filename = ("3d_mesh_directional_projection");
      TriangulationOnCAD tria_on_cad_dir(
        in_mesh_filename,
        cad_file_name,
        out_mesh_filename,
        TriangulationOnCAD::DirectionalProjection);
      tria_on_cad_dir.run();
      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << std::endl;
      std::cout << std::endl;

      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << "Testing projection in direction normal to mesh elements"
                << std::endl;
      std::cout << "----------------------------------------------------------"
                << std::endl;
      out_mesh_filename = ("3d_mesh_normal_to_mesh_projection");
      TriangulationOnCAD tria_on_cad_norm_to_mesh(
        in_mesh_filename,
        cad_file_name,
        out_mesh_filename,
        TriangulationOnCAD::NormalToMeshProjection);
      tria_on_cad_norm_to_mesh.run();
      std::cout << "----------------------------------------------------------"
                << std::endl;
      std::cout << std::endl;
      std::cout << std::endl;
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


// Filename: static_assert_example.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

template <typename Matrix>
inline void f(const Matrix&)
{
    MTL_STATIC_ASSERT((mtl::traits::is_matrix<Matrix>::value && mtl::traits::is_dense<Matrix>::value),
		      "f works only on dense matrices. Dork!");
    // ... now we can be sure that we have a dense matrix
}

int main()
{
    mtl::dense2D<double>      A(3, 4);
    mtl::compressed2D<double> B(3, 4);

    f(A);
    // f(B); // yields user-defined error message (try it!)

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/is_prime.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/tests/exceptions.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/eight.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/seven.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/core/container/table/table.hpp>

NT2_TEST_CASE_TPL ( is_prime_real__1_0,  NT2_REAL_TYPES)
{
  using nt2::is_prime;
  using nt2::tag::is_prime_;

  // specific values tests
  NT2_TEST(!is_prime(nt2::Eight<T>()));
  NT2_TEST(is_prime(nt2::Seven<T>()));
  NT2_TEST(is_prime(nt2::Two<T>()));
  NT2_TEST(!is_prime(nt2::One<T>()));
  NT2_TEST_ASSERT(is_prime(nt2::Mone<T>()));
}

NT2_TEST_CASE_TPL ( is_prime_integer__1_0,  NT2_INTEGRAL_TYPES)
{
  using nt2::is_prime;
  using nt2::tag::is_prime_;

  // specific values tests
  NT2_TEST(!is_prime(nt2::Eight<T>()));
  NT2_TEST(is_prime(nt2::Seven<T>()));
  NT2_TEST(is_prime(nt2::Two<T>()));
  NT2_TEST(!is_prime(nt2::One<T>()));
}

NT2_TEST_CASE_TPL ( is_prime_integer__2_0,  NT2_INTEGRAL_TYPES)
{
  using nt2::is_prime;
  using nt2::tag::is_prime_;

  // specific values tests
  nt2::container::table<T> p =  nt2::primes(T(8));
  NT2_TEST(!is_prime(nt2::Eight<T>(), p));
  NT2_TEST(is_prime(nt2::Seven<T>(), p));
  NT2_TEST(is_prime(nt2::Two<T>(), p));
  NT2_TEST(!is_prime(nt2::One<T>(), p));
}

NT2_TEST_CASE_TPL ( is_prime_real__2_0,  NT2_REAL_TYPES)
{
  using nt2::is_prime;
  using nt2::tag::is_prime_;

  // specific values tests
  nt2::container::table<uint32_t> p =  nt2::primes(uint32_t(8));
  NT2_TEST(!is_prime(nt2::Eight<T>(), p));
  NT2_TEST(is_prime(nt2::Seven<T>(), p));
  NT2_TEST(is_prime(nt2::Two<T>(), p));
  NT2_TEST(!is_prime(nt2::One<T>(), p));
  NT2_TEST_ASSERT(is_prime(nt2::Mone<T>()));
}


#include "WSPRLog.hxx"
#include "SolarTime.hxx"
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <math.h>
#include <set>

// plot distance, "solar time" pairs as histogram
class myWSPRLog : public WSPRLog {
public:
  myWSPRLog(const std::string & out_base_name) : WSPRLog() {
    osrx.open(out_base_name + "_FD_T_rx.dat");
    ostx.open(out_base_name + "_FD_T_tx.dat");

    for(int i = 0; i < 24; i++) {
      for(int j = 0; j < 200; j++) {
	rxhisto[i][j] = 0;
	txhisto[i][j] = 0;
      }
    }
  }

  ~myWSPRLog() {
  }

  enum SEL { RX, TX };

  bool processEntry(WSPRLogEntry * ent) {
    std::string from, to;
    double ft, tt, ftt;
    unsigned long et;
    double freq_diff;

    if(ent == NULL) return false;

    to = ent->rxgrid;
    from = ent->txgrid;
    et = ent->dtime;
    freq_diff = ent->freq_diff;

    // calculate the "local time" for to, from, and midpath
    SolarTime tx_time(et, from);
    SolarTime rx_time(et, to);

    int ifreq_diff = (int) freq_diff;

    makeEntry(RX, rx_time.getFHour(), ifreq_diff);
    makeEntry(TX, tx_time.getFHour(), ifreq_diff);

    return false;
  }

  void makeEntry(SEL sel, double hr, int offset) {
    int off_idx = offset + 100;
    if((off_idx < 0) || (off_idx >= 200)) return;

    int ihr = (int) hr;
    if(ihr > 23) ihr -= 24;

    if((ihr < 0) || (ihr > 23)) {
      std::cerr << boost::format("makeEntry got hr = %f ihr = %d offset = %d off_idx = %d\n") % hr % ihr % offset % off_idx;
      return;
    }

    switch (sel) {
    case RX:
      rxhisto[ihr][off_idx]++;
      break;
    case TX:
      txhisto[ihr][off_idx]++;
      break;
    }
  }

  void dumpTables() {
    dumpTable(RX, osrx);
    dumpTable(TX, ostx);

    osrx.close();
    ostx.close();
  }

  void dumpTable(SEL sel, std::ostream & os) {
    // first find total
    long total = 0;
    long smax = 1;
    for(int i = 0; i < 24; i++) {
      for(int j = 0; j < 200; j++) {
	int val;
	switch (sel) {
	case RX:
	  val = rxhisto[i][j];
	  break;
	case TX:
	  val = txhisto[i][j];
	  break;
	}
	total += val;
	smax = (smax > val) ? smax : val;
      }
    }

    double ftotal = ((double) total) + 1.0e-6;
    double fsmax = ((double) smax);
    for(int i = 0; i < 24; i++) {
      for(int j = 0; j < 200; j++) {
	int val;
	switch (sel) {
	case RX:
	  val = rxhisto[i][j];
	  break;
	case TX:
	  val = txhisto[i][j];
	  break;
	}

	double fval = ((double) val) / ftotal;
	double rmag = ((double) val) / fsmax;
	os << boost::format("%d %d %f %f\n") % i % (j - 100) % fval % rmag;
      }
      os << std::endl;
    }
  }

private:
  std::ofstream osrx, ostx;
  unsigned rxhisto[24][200];
  unsigned txhisto[24][200];
};

int main(int argc, char * argv[])
{
  bool input_gzipped;
  std::string in_name, out_name;
  namespace po = boost::program_options;

  po::options_description desc("Options:");

  desc.add_options()
    ("help", "help message")
    ("log", po::value<std::string>(&in_name)->required(), "Input log file (csv) in WSPR log format")
    ("out_base", po::value<std::string>(&out_name)->required(), "Output data file basename")
    ("igz", po::value<bool>(&input_gzipped)->default_value(false), "if true, input file is gzip compressed");

  po::positional_options_description pos_opts ;
  pos_opts.add("log", 1);
  pos_opts.add("out_base", 1);

  po::variables_map vm;

  std::string what_am_i("Make [hr][delta f] histogram from  WSPR log\n\t");
  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  myWSPRLog wlog(out_name);

  wlog.readLog(in_name, input_gzipped);

  wlog.dumpTables();
}


/**
 * Fast map matching.
 *
 * Network class
 *
 * @author: Can Yang
 * @version: 2017.11.11
 */

#ifndef FMM_NETWORK_HPP
#define FMM_NETWORK_HPP

#include "network/type.hpp"
#include "config/network_config.hpp"
#include "core/gps.hpp"
#include "mm/mm_type.hpp"
#include <ogrsf_frmts.h> // C++ API for GDAL
#include <iostream>
#include <math.h> // Calulating probability
#include <iomanip>
#include <algorithm> // Partial sort copy
#include <unordered_set> // Partial sort copy

// Data structures for Rtree
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/function_output_iterator.hpp>

namespace FMM {
/**
 * Classes related with network and graph
 */
namespace NETWORK {
/**
 * Road network class
 */
class Network {
public:
  /**
   * Box of a edge
   */
  typedef boost::geometry::model::box<FMM::CORE::Point> boost_box;
  /**
   * Item stored in a node of Rtree
   */
  typedef std::pair<boost_box, Edge *> Item;
  /**
   * Rtree of road edges
   */
  typedef boost::geometry::index::rtree<
      Item, boost::geometry::index::quadratic<16> > Rtree;
  /**
   *  Constructor of Network
   *
   *  @param filename: the path to a network file in ESRI shapefile format
   *  @param id_name: the name of the id field
   *  @param source_name: the name of the source field
   *  @param target_name: the name of the target field
   *  @param mode: mode name, only applies to OSM network
   *
   */
  Network(const std::string &filename,
          const std::string &id_name = "id",
          const std::string &source_name = "source",
          const std::string &target_name = "target",
          const std::string &mode = "drive"
        );
  Network(const CONFIG::NetworkConfig &config):Network(
    config.file,config.id,config.source,config.target,config.mode){};
  /**
   * Get number of nodes in the network
   * @return number of nodes
   */
  int get_node_count() const;
  /**
   * Get number of edges in the network
   * @return number of edges
   */
  int get_edge_count() const;
  const Edge& get_edge(EdgeID id) const;
  const Edge& get_edge(EdgeIndex index) const;
  /**
   * Get edges in the network
   * @return a constant reference to the edges
   */
  const std::vector<Edge> &get_edges() const;
  /**
   * Get edge ID from index
   * @param index index of edge
   * @return edge ID
   */
  EdgeID get_edge_id(EdgeIndex index) const;
  /**
   * Get edge index from ID
   * @param id edge id
   * @return edge index
   */
  EdgeIndex get_edge_index(EdgeID id) const;
  /**
   * Get node ID from index
   * @param index index of node
   * @return node ID
   */
  NodeID get_node_id(NodeIndex index) const;
  /**
   * Get node index from id
   * @param id node id
   * @return node index
   */
  NodeIndex get_node_index(NodeID id) const;
  /**
   * Get node geometry from index
   * @param index node index
   * @return point of a node
   */
  FMM::CORE::Point get_node_geom_from_idx(NodeIndex index) const;

  /**
   *  Search for k nearest neighboring (KNN) candidates of a
   *  trajectory within a search radius
   *
   *  @param trajectory: input trajectory
   *  @param k: the number of candidates
   *  @param radius: the search radius
   *  @return a 2D vector of Candidates containing
   *  the candidates selected for each point in a trajectory
   *
   */
  FMM::MM::Traj_Candidates search_tr_cs_knn(
    FMM::CORE::Trajectory &trajectory, std::size_t k, double radius) const;

  /**
   * Search for k nearest neighboring (KNN) candidates of a
   * linestring within a search radius
   *
   * @param geom
   * @param k number of candidates
   * @param radius search radius
   * @return a 2D vector of Candidates containing
   * the candidates selected for each point in a linestring
   */
  FMM::MM::Traj_Candidates search_tr_cs_knn(const FMM::CORE::LineString &geom,
                                            std::size_t k,
                                            double radius) const;
  /**
   * Get edge geometry
   * @param edge_id edge id
   * @return Geometry of edge
   */
  const FMM::CORE::LineString &get_edge_geom(EdgeID edge_id) const;
  /**
   * Extract the geometry of a complete path, whose two end segment will be
   * clipped according to the input trajectory
   * @param traj input trajectory
   * @param complete_path complete path
   */
  FMM::CORE::LineString complete_path_to_geometry(
    const FMM::CORE::LineString &traj,
    const MM::C_Path &complete_path) const;
  /**
   * Get all node geometry
   * @return a vector of points
   */
  const std::vector<FMM::CORE::Point> &get_vertex_points() const;
  /**
   * Get node geometry
   * @param u node index
   * @return geometry of node
   */
  const FMM::CORE::Point &get_vertex_point(NodeIndex u) const;
  /**
   * Extract the geometry of a route in the network
   * @param path a route stored with edge ID
   * @return the geometry of the route
   */
  FMM::CORE::LineString route2geometry(const std::vector<EdgeID> &path) const;
  /**
   * Extract the geometry of a route in the network
   * @param path a route stored with edge Index
   * @return the geometry of the route
   */
  FMM::CORE::LineString route2geometry(
    const std::vector<EdgeIndex> &path) const;
  /**
   * Compare two candidate according to their GPS error
   * @param a candidate a
   * @param b candidate b
   * @return true if a.dist<b.dist
   */
  static bool candidate_compare(const MM::Candidate &a, const MM::Candidate &b);
  void add_edge(EdgeID edge_id, NodeID source, NodeID target,
    const FMM::CORE::LineString &geom);
private:
  void read_ogr_file(const std::string &filename,
                     const std::string &id_name,
                     const std::string &source_name,
                     const std::string &target_name);
  void read_osm_file(const std::string &filename,const std::string &mode);
  /**
   * Concatenate a linestring segs to a linestring line, used in the
   * function complete_path_to_geometry
   *
   * @param line: linestring which will be updated
   * @param segs: segs that will be appended to line
   * @param offset: the number of points skipped in segs.
   */
  static void append_segs_to_line(FMM::CORE::LineString *line,
                                  const FMM::CORE::LineString &segs,
                                  int offset = 0);
  /**
   * Build rtree for the network
   */
  void build_rtree_index();
  int srid;   // Spatial reference id
  Rtree rtree;   // Network rtree structure
  std::vector<Edge> edges;   // all edges in the network
  NodeIDVec node_id_vec;
  unsigned int num_vertices;
  NodeIndexMap node_map;
  EdgeIndexMap edge_map;
  std::vector<FMM::CORE::Point> vertex_points;
}; // Network
} // NETWORK
} // FMM
#endif /* FMM_NETWORK_HPP */


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file bes_test.cpp
/// \brief Some tests for BES.

#include <boost/test/minimal.hpp>
#include "mcrl2/bes/bdd_expression.h"
#include "mcrl2/bes/print.h"

using namespace mcrl2;
using namespace mcrl2::bdd;

void test_bdd()
{
  if_ b1("b1", true_(), false_());
  if_ b2("b2", b1, b1);
  if_ b3("b3", b2, b1);
  std::cout << "b1 = " << b1 << std::endl;
  std::cout << "b2 = " << b2 << std::endl;
  std::cout << "b3 = " << b3 << std::endl;
  BOOST_CHECK(bdd::pp(true_()) == "true");
  BOOST_CHECK(bdd::pp(b1) == "if(true, false)");
  BOOST_CHECK(bdd::pp(b2) == "if(if(true, false), if(true, false))");
  BOOST_CHECK(bdd::pp(b3) == "if(if(if(true, false), if(true, false)), if(true, false))");
  BOOST_CHECK(is_if(b1));
  BOOST_CHECK(b2.left() == b1);
  BOOST_CHECK(b2.right() == b1);
  BOOST_CHECK(b3.left() == b2);
  BOOST_CHECK(b3.right() == b1);
  BOOST_CHECK(b1.name() == core::identifier_string("b1"));
  BOOST_CHECK(b2.name() == core::identifier_string("b2"));
  BOOST_CHECK(b3.name() == core::identifier_string("b3"));
}

int test_main(int argc, char* argv[])
{
  test_bdd();

  return 0;
}


/**
 * Non-metric Space Library
 *
 * Authors: Bilegsaikhan Naidan (https://github.com/bileg), Leonid Boytsov (http://boytsov.info).
 * With contributions from Lawrence Cayton (http://lcayton.com/) and others.
 *
 * For the complete list of contributors and further details see:
 * https://github.com/searchivarius/NonMetricSpaceLib
 *
 * This is a wrapper class for the Wei Dong implementation of https://code.google.com/p/nndes/,
 * which also contains some of the original code from Wei Dong's repository.
 * Wei Dong implemented only the graph construction algorithm [1].
 * We implemented two search algorithms
 * i)  A greedy walk that starts from a random point and always proceeds to the closest neighbor [2].
 * ii) A priority queue based procedure where the queue may contain more distance not necessarily
 *     adjacent nodes [3]
 *
 * 1. Wei Dong, Charikar Moses, and Kai Li. 2011.
 *    Efficient k-nearest neighbor graph construction for generic similarity measures.
 *    In Proceedings of the 20th international conference on World wide web (WWW '11).
 *    ACM, New York, NY, USA, 577-586.
 *
 * 2. K. Hajebi, Y. Abbasi-Yadkori, H. Shahbazi, and H. Zhang.
 *    Fast approximate nearest-neighbor search with k-nearest neighbor graph.
 *    In IJCAI Proceedings-International Joint Conference on Artificial Intelligence,
 *    volume 22, page 1312, 2011
 *
 * 3. The main publication is as follows, but the basic algorithm was also presented as SISAP'12:
 *    Malkov, Y., Ponomarenko, A., Logvinov, A., & Krylov, V., 2014.
 *    Approximate nearest neighbor algorithm based on navigable small world graphs. Information Systems, 45, 61-68.
 *
 * The Wei Dong's code (to construct the knn-graph) can be redistributed given
 * that the license (see below) is retained in the source code.
 */
/*
Copyright (C) 2010,2011 Wei Dong <wdong@wdong.org>
All rights reserved.

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

#include <iomanip>
#include <map>
#include <unordered_set>
#include <queue>

#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include "space.h"
#include "utils.h"
#include "rangequery.h"
#include "knnquery.h"
#include "method/nndes.h"

#define USE_BITSET_FOR_SEARCHING 1

namespace similarity {

template <typename dist_t>
NNDescentMethod<dist_t>::NNDescentMethod(
    bool  PrintProgress,
    const Space<dist_t>& space,
    const ObjectVector& data) :
      space_(space), data_(data), PrintProgress_(PrintProgress),
      controlQty_(0), // default value from Wei Dong's code
      nndesOracle_(space, data)
{}

template <typename dist_t>
void NNDescentMethod<dist_t>::CreateIndex(const AnyParams& IndexParams) {
  AnyParamManager pmgr(IndexParams);

  pmgr.GetParamOptional("NN",           NN_,           20);// default value from Wei Dong's code
  pmgr.GetParamOptional("iterationQty", iterationQty_, 100); // default value from Wei Dong's code

  pmgr.GetParamOptional("rho",          rho_,          1.0); // Fast rho is 0.5, 1.0 is from Wei Dong's code
  pmgr.GetParamOptional("delta",        delta_,        0.001), // default value from Wei Dong's code

  LOG(LIB_INFO) <<  "NN           = " << NN_;
  LOG(LIB_INFO) <<  "iterationQty = " << iterationQty_;
  LOG(LIB_INFO) <<  "rho          = " << rho_;
  LOG(LIB_INFO) <<  "delta        = " << delta_;

  SetQueryTimeParams(getEmptyParams()); // reset query-time parameter

  LOG(LIB_INFO) << "Starting NN-Descent...";

  nndesObj_.reset(new NNDescent<SpaceOracle>(data_.size(), // N
                                             NN_, //K
                                             rho_, //S,
                                             nndesOracle_, GRAPH_BOTH));

    float total = float(data_.size()) * (data_.size() - 1) / 2;
    cout.precision(5);
    cout.setf(ios::fixed);
    for (int it = 0; it < iterationQty_; ++it) {
        int t = nndesObj_->iterate(PrintProgress_);
        float rate = float(t) / (NN_ * data_.size());

// TODO @leo computation of recall needs to be re-written, can't use original Wei Dong's code
/*
        float recall = 0;
        if (controlQty_) {  // report accuracy
            const vector<KNN> &nn = nndes.getNN();
#pragma omp parallel for default(shared) reduction(+:recall)
            for (int i = 0; i < control_knn.size(); ++i) {
                recall += nndes::recall(control_knn[i], nn[control_index[i]], K);
            }
            recall /= control;
        }
        cout << setw(2) << it << " update:" << rate << " recall:" << recall << " cost:" << float(nndes.getCost())/total  << endl;
*/
        LOG(LIB_INFO) << setw(2) << it << " update:" << rate << " cost:" << float(nndesObj_->getCost())/total;
        if (rate < delta_) break;
    }

  LOG(LIB_INFO) << "NN-Descent finished!";
}

template <typename dist_t>
void NNDescentMethod<dist_t>::Search(RangeQuery<dist_t>* query, IdType) const {
  throw runtime_error("Range search is not supported!");
}

template <typename dist_t>
void NNDescentMethod<dist_t>::Search(KNNQuery<dist_t>* query, IdType) const {
  greedy_ ? SearchGreedy(query) : SearchSmallWorld(query);
}

template <typename dist_t>
void NNDescentMethod<dist_t>::SearchSmallWorld(KNNQuery<dist_t>* query) const {
  const vector<KNN> &nn = nndesObj_->getNN();

#if USE_BITSET_FOR_SEARCHING
  vector<bool>                      visitedBitset(data_.size());
#else
  unordered_set <IdType>            visitedNodes;
#endif

  for (size_t i=0; i < initSearchAttempts_; i++) {
  /**
   * Search of most k-closest elements to the query.
   */
    IdType randPoint = RandomInt() % data_.size();

    priority_queue <dist_t>             closestDistQueue; //The set of all elements which distance was calculated
    priority_queue <EvaluatedNode>      candidateSet; //the set of elements which we can use to evaluate

    const Object* currObj = data_[randPoint];
    dist_t         d = query->DistanceObjLeft(currObj);
    query->CheckAndAddToResult(d, currObj);

    EvaluatedNode  ev(-d, randPoint);

    candidateSet.push(ev);
    closestDistQueue.push(d);
#if USE_BITSET_FOR_SEARCHING
    visitedBitset[randPoint] = true;
#else
    visitedNodes.insert(randPoint);
#endif

    while(!candidateSet.empty()){
      const EvaluatedNode& currEv = candidateSet.top();
      dist_t lowerBound = closestDistQueue.top();

      // Did we reach a local minimum?
      if ((-currEv.first) > lowerBound) {
        break;
      }

      IdType currEvId = currEv.second;

      // Can't access curEv anymore! The reference would become invalid
      candidateSet.pop();

      //calculate distance to each neighbor
      for (const KNNEntry& e: nn[currEvId]) {
        IdType currNew = e.key;
        if (currNew == KNNEntry::BAD) continue;

#if USE_BITSET_FOR_SEARCHING
        if (!visitedBitset[currNew]) {
          visitedBitset[currNew] = true;
#else
        if (visitedNodes.find(currNew) == visitedNodes.end()){
            visitedNodes.insert(currNew);
#endif

            currObj = data_[currNew];
            d = query->DistanceObjLeft(currObj);
            query->CheckAndAddToResult(d, currObj);
            EvaluatedNode  evE1(-d, currNew);

            closestDistQueue.push(d);
            if (closestDistQueue.size() > efSearch_) {
              closestDistQueue.pop();
            }
            candidateSet.push(evE1);
          }
      }
    }
  }
}

template <typename dist_t>
void NNDescentMethod<dist_t>::SearchGreedy(KNNQuery<dist_t>* query) const {
  const vector<KNN> &nn = nndesObj_->getNN();

  for (size_t i=0; i < initSearchAttempts_; i++) {
    IdType curr = RandomInt() % data_.size();

    dist_t currDist = query->DistanceObjLeft(data_[curr]);
    query->CheckAndAddToResult(currDist, data_[curr]);

    IdType currOld;

    do {
      currOld = curr;
      // Iterate over neighbors
      for (const KNNEntry&e: nn[currOld]) {
        IdType currNew = e.key;
        if (currNew != KNNEntry::BAD) {
          dist_t currDistNew = query->DistanceObjLeft(data_[currNew]);
          query->CheckAndAddToResult(currDistNew, data_[currNew]);
          if (currDistNew < currDist) {
            curr = currNew;
            currDist = currDistNew;
          }
        }
      }
    } while (currOld != curr);
  }
}

template <typename dist_t>
void
NNDescentMethod<dist_t>::SetQueryTimeParams(const AnyParams& QueryTimeParams) {
  AnyParamManager pmgr(QueryTimeParams);
  pmgr.GetParamOptional("initSearchAttempts", initSearchAttempts_,  3);
  pmgr.GetParamOptional("efSearch",           efSearch_,            NN_);
  pmgr.GetParamOptional("greedy",             greedy_,              false);
  pmgr.CheckUnused();
  LOG(LIB_INFO) << "Set NNDescentMethod query-time parameters:";
  LOG(LIB_INFO) << "initSearchAttempts=" << initSearchAttempts_;
  LOG(LIB_INFO) << "efSearch="           << efSearch_;
  LOG(LIB_INFO) << "greedy="             << greedy_;
}

template class NNDescentMethod<float>;
template class NNDescentMethod<double>;
template class NNDescentMethod<int>;

}


#include "ActiveSetQPMomentumOptimizer.h"

#define USE_ACTIVE_SET
//#define DUMP_TO_FILE

#ifdef USE_ACTIVE_SET
#include "ActiveSetQP/fastQP.h"
#else
#include "ActiveSetQP/eiquadprog.hpp"
#endif

#include <Eigen/Dense>
#include <Eigen/Core>
#include <vector>
#include <set>
#include <iostream>
#include <fstream>
#define nRho us_ihmc_commonWalkingControlModules_controlModules_nativeOptimization_ActiveSetQPMomentumOptimizer_nRho
#define nWrench us_ihmc_commonWalkingControlModules_controlModules_nativeOptimization_ActiveSetQPMomentumOptimizer_nWrench

typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> MatrixXdR;

#define J2E(X, R, C)  double * c ## X = (double*)env->GetPrimitiveArrayCritical(X, &isCopy);   \
				Eigen::Map<MatrixXdR > (e ## X)(c ## X, R, C);

#define J2E_FREE(X) env->ReleasePrimitiveArrayCritical(X, c ## X, 0);

#define JNA2E(X, R, C)  Eigen::Map<MatrixXdR> (e ## X)(X, R, C);

int nDoF=-1;
jboolean isCopy=JNI_FALSE;
using namespace Eigen;

extern "C"
{
	extern int MAX_ITER; //instance in QP.cpp
	JNIEXPORT void initializeNative (int _nDoF, int _max_iter)
	{
		nDoF = _nDoF;
		MAX_ITER = _max_iter;
		std::cerr << "ActiveSetQPMomentumOptimizer Library initialized nDoF = " << nDoF << " MAX_ITER="<< MAX_ITER <<std::endl;
		std::cerr << "Compiled at " __TIME__ << std::endl;
	}
}

JNIEXPORT void JNICALL Java_us_ihmc_commonWalkingControlModules_controlModules_nativeOptimization_ActiveSetQPMomentumOptimizer_initializeNative (JNIEnv *env, jobject obj, jint _nDoF, jint _max_iter)
{
//	std::cout << "use JNI" << std::endl;
	initializeNative(_nDoF, _max_iter);
}

bool myIsNAN(double x)
{
	return !(x==x);
}

int solveEigen(
		MatrixXd A, MatrixXd  b, MatrixXd  C,
		MatrixXd Jp, MatrixXd  pp,
		MatrixXd Js, MatrixXd  ps, MatrixXd  Ws,
		MatrixXd WRho, MatrixXd  Lambda,
		MatrixXd WRhoSmoother,
		MatrixXd rhoPrevMean, MatrixXd  WRhoCoPPenalty,
		MatrixXd QRho, MatrixXd  c, MatrixXd  rhoMin,
		Eigen::Map<MatrixXdR>&  vd, Eigen::Map<MatrixXdR>& rho,
		int* activeSet)
{

	/*
	 *       [vd  ]'[ A' C A + Js' Ws Js + Lambda     0               ] [vd  ]  + 2 [vd  ]' [ -A' C b - Js' Ws ps          ]    +  b' C b + ps' Ws ps + Pprev' Wpsm Pprev + Ppavg' Wpcop Ppavg
	 *       [rho ] [              0                Wp + Wpsm + Wpcop ] [rho ]      [rho ]  [-Wpsm Pprev - Wpcop Ppavg     ]
	 *
	 *       min_x 0.5 x'Q x  + f'x  + g
	 *       st [-A QRho ] [vd  ] = [c ]
	 *          [Jp      ] [rho ] = [pp]
	 *        [0 -I] rho <= -rhoMin
	 */

	//Objective
	//Q
	std::vector<MatrixXd *> QblkDiag;
	MatrixXd Qblk1 = A.transpose()*C.diagonal().asDiagonal()*A + Js.transpose()*Ws.diagonal().asDiagonal()*Js + Lambda;
	MatrixXd Qblk2 = WRho.diagonal() + WRhoSmoother.diagonal() + WRhoCoPPenalty.diagonal();
	QblkDiag.push_back(&Qblk1);
	QblkDiag.push_back(&Qblk2);

#ifdef DUMP_TO_FILE
	MatrixXd Q(nDoF+nRho,nDoF+nRho);
	Q << Qblk1, MatrixXd::Zero(nDoF,nRho), MatrixXd::Zero(nRho,nDoF), MatrixXd(Qblk2.asDiagonal()); //Qblk2;
#endif
	//f
	VectorXd f(nRho+nDoF);
	f << (- A.transpose() * C.diagonal().asDiagonal() * b - Js.transpose()*Ws.diagonal().asDiagonal()*ps),
	  	 (- MatrixXd(WRhoSmoother.diagonal().asDiagonal())*rho - WRhoCoPPenalty.diagonal().asDiagonal()*rhoPrevMean);

	MatrixXd g = b.transpose()*C.diagonal().asDiagonal()*b + ps.transpose()*Ws.diagonal().asDiagonal()*ps
		+ rho.transpose()*WRhoSmoother.diagonal().asDiagonal()*rho + rhoPrevMean.transpose()*WRhoCoPPenalty*rhoPrevMean;

	//Equality
	MatrixXd Aeq,beq;
	if(myIsNAN(Jp(0,0)))
	{
		Aeq.resize(nWrench, nDoF+nRho);
		Aeq << -A, QRho;
		beq.resize(nWrench,nDoF+nRho);
		beq = c;
	}
	else
	{
		/*  Aeq -
		 *    nDoF  nRho
		 *  ----------------
		 *  | -A  |  QRho |  nWrench
		 *  ----------------
		 *  | Jp     Zero |  nDoF
		 *  ----------------
		 */
		Aeq.resize(nWrench+nDoF, nDoF+nRho);
		Aeq << -A, QRho, Jp, MatrixXd::Zero(nDoF, nRho) ;
		beq.resize(nWrench+nDoF,1);
		beq << c, pp;
	}

	//Inequality
	MatrixXd Ain(nRho, nDoF+nRho);
	Ain << MatrixXd::Zero(nRho,nDoF), -MatrixXd::Identity(nRho,nRho);
	VectorXd bin = -rhoMin;

	//final Call
	VectorXd x(nRho+nDoF);
	x.head(nDoF) =  vd;
	x.tail(nRho) = rho;

#define REG  1e-9
#ifdef  USE_ACTIVE_SET
	for(int i=0;i<QblkDiag.size();i++)
	{
		if(QblkDiag[i]->cols()==1)
			QblkDiag[i]->operator+=(VectorXd::Constant(QblkDiag[i]->rows(),REG));
		else
			QblkDiag[i]->diagonal() += VectorXd::Ones(QblkDiag[i]->rows())*REG;
	}
#ifdef DUMP_TO_FILE
	//use blkDiagQ in ActiveSet
	Q.diagonal() += VectorXd::Ones(nRho+nDoF)*REG;
#endif //DUMP_TO_FILE

#else
	Aeq = MatrixXd(Aeq.transpose());
	beq = -beq;
	Ain = -MatrixXd(Ain.transpose());
	Q.diagonal() += VectorXd::Ones(nRho+nDoF)*REG;
#endif //USE_ACTIVE_SET

#ifdef DUMP_TO_FILE
	std::ofstream ofP("/tmp/P.txt");
	ofP << "Q" << std::endl   << Q << std::endl;
	ofP << "f" << std::endl << f << std::endl;
	ofP << "Aeq" << std::endl << Aeq << std::endl;
	ofP << "beq" << std::endl << beq << std::endl;
	ofP << "Ain" << std::endl << Ain << std::endl;
	ofP << "bin" << std::endl << bin << std::endl;
	ofP << "x0" << std::endl << x << std::endl;
	ofP.close();
#endif

#ifdef USE_ACTIVE_SET
	std::set<int> stlActiveSet;
	for(int i=0;i<Aeq.rows();i++)
	{
		if(activeSet[i])
			stlActiveSet.insert(i);
	}

	int ret=fastQP(QblkDiag, f,  Aeq, beq, Ain, bin, stlActiveSet, x);

	std::fill(activeSet, activeSet+Aeq.rows(),0);
	for(std::set<int>::iterator p=stlActiveSet.begin();p!=stlActiveSet.end();p++)
	{
		activeSet[*p]=1;
	}
#else
	int ret=solve_quadprog(Q, f,  Aeq, beq, Ain, bin, x);
#endif

	vd = x.head(nDoF);
	rho = x.tail(nRho);

#ifdef DUMP_TO_FILE
	std::ofstream of("/tmp/Q.txt");
	of << "A" << std::endl    << A << std::endl;
	of << "b" << std::endl    << b << std::endl;
	of << "C" << std::endl    << C << std::endl;

	of << "Jp" << std::endl   << Jp << std::endl;
	of << "pp" << std::endl   << pp << std::endl;

	of << "Js" << std::endl   << Js << std::endl;
	of << "ps" << std::endl   << ps << std::endl;
	of << "Ws" << std::endl   << Ws << std::endl;

	of << "WRho" << std::endl << WRho.diagonal()<< std::endl;
	of << "Lambda" << std::endl   << Lambda.diagonal() << std::endl;

	of << "Wpsm" << std::endl << WRhoSmoother.diagonal() << std::endl;

	of << "rhoPrevMean" << std::endl << rhoPrevMean << std::endl;
	of << "Wpcop" << std::endl << WRhoCoPPenalty.diagonal()<< std::endl;

	of << "Qrho" << std::endl << QRho << std::endl;
	of << "c" << std::endl << c << std::endl;
	of << "rhoMin" << std::endl << rhoMin << std::endl;

	of << "vd" << std::endl << vd << std::endl;
	of << "rho" << std::endl << rho << std::endl;

	of << "optVal" << std::endl << g(0,0)+2*ret << std::endl;
	of.close();
#endif
	return ret;
}

JNIEXPORT jint JNICALL Java_us_ihmc_commonWalkingControlModules_controlModules_nativeOptimization_ActiveSetQPMomentumOptimizer_solveNative (JNIEnv *env, jobject obj,
		jdoubleArray A, jdoubleArray b, jdoubleArray C,
		jdoubleArray Jp, jdoubleArray pp,
		jdoubleArray Js, jdoubleArray ps, jdoubleArray Ws,
		jdoubleArray WRho, jdoubleArray Lambda,
		jdoubleArray WRhoSmoother,
		jdoubleArray rhoPrevMean, jdoubleArray WRhoCoPPenalty,
		jdoubleArray QRho, jdoubleArray c, jdoubleArray rhoMin,
		jdoubleArray vd, jdoubleArray rho, jintArray activeSet)
{
	int ret;
	J2E(A, nWrench, nDoF);  J2E(b,nWrench,1);  J2E(C,nWrench,nWrench);
	J2E(Jp,nDoF,nDoF);  J2E(pp,nDoF,1);
	J2E(Js,nDoF,nDoF);  J2E(ps,nDoF,1);  J2E(Ws,nDoF,nDoF);
	J2E(WRho,nRho,nRho);  J2E(Lambda,nDoF,nDoF);
	J2E(WRhoSmoother,nRho,nRho);
	J2E(rhoPrevMean,nRho,1);  J2E(WRhoCoPPenalty,nRho,nRho);
	J2E(QRho,nWrench,nRho);  J2E(c,nWrench,1);  J2E(rhoMin,nRho,1);
	J2E(vd,nDoF,1); J2E(rho,nRho,1);
	int* cActiveSet = (int*)env->GetPrimitiveArrayCritical(activeSet, &isCopy);

	ret = solveEigen(eA,  eb,  eC, eJp, epp, eJs,  eps,  eWs, eWRho,  eLambda, eWRhoSmoother, erhoPrevMean,  eWRhoCoPPenalty, eQRho,  ec,  erhoMin, evd, erho, cActiveSet);

	J2E_FREE(A);  J2E_FREE(b);  J2E_FREE(C);
	J2E_FREE(Jp);  J2E_FREE(pp);
	J2E_FREE(Js);  J2E_FREE(ps);  J2E_FREE(Ws);
	J2E_FREE(WRho);  J2E_FREE(Lambda);
	J2E_FREE(WRhoSmoother);
	J2E_FREE(rhoPrevMean);  J2E_FREE(WRhoCoPPenalty);
	J2E_FREE(QRho);  J2E_FREE(c);  J2E_FREE(rhoMin);
	J2E_FREE(vd); J2E_FREE(rho);
	env->ReleasePrimitiveArrayCritical(activeSet, cActiveSet, 0);

	return ret;
}

extern "C" {
	JNIEXPORT int solveNative(
			double* A, double* b, double* C,
			double* Jp, double* pp,
			double* Js, double* ps, double* Ws,
			double* WRho, double* Lambda,
			double* WRhoSmoother,
			double* rhoPrevMean, double* WRhoCoPPenalty,
			double* QRho, double* c, double* rhoMin,
			double* vd, double* rho,
			int* activeSet)
	{
		int ret;
		JNA2E(A, nWrench, nDoF);  JNA2E(b,nWrench,1);  JNA2E(C,nWrench,nWrench);
		JNA2E(Jp,nDoF,nDoF);  JNA2E(pp,nDoF,1);
		JNA2E(Js,nDoF,nDoF);  JNA2E(ps,nDoF,1);  JNA2E(Ws,nDoF,nDoF);
		JNA2E(WRho,nRho,nRho);  JNA2E(Lambda,nDoF,nDoF);
		JNA2E(WRhoSmoother,nRho,nRho);
		JNA2E(rhoPrevMean,nRho,1);  JNA2E(WRhoCoPPenalty,nRho,nRho);
		JNA2E(QRho,nWrench,nRho);  JNA2E(c,nWrench,1);  JNA2E(rhoMin,nRho,1);
		JNA2E(vd,nDoF,1); JNA2E(rho,nRho,1);

		ret = solveEigen(eA,  eb,  eC, eJp, epp, eJs,  eps,  eWs, eWRho,  eLambda, eWRhoSmoother, erhoPrevMean,  eWRhoCoPPenalty, eQRho,  ec,  erhoMin, evd, erho, activeSet);
		return ret;
	}
}