/******************************************************************************
 * vcc.cpp
 * *
 * Source of ReduVCC
 * Darren Strash <dstrash@hamilton.edu>
 * Louise Thompson <lmthomps@hamilton.edu>
 *****************************************************************************/
#include <argtable3.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <regex.h>
#include <sstream>
#include <stdio.h>
#include <string.h>
#include <utility> // for std::pair
#include <unordered_map>
#include <unordered_set>
#include <algorithm>
#include <climits>

#ifdef BETA
#include <boost/functional/hash.hpp>
#endif // BETA

#include "balance_configuration.h"
#include "data_structure/graph_access.h"
#include "data_structure/matrix/normal_matrix.h"
#include "data_structure/matrix/online_distance_matrix.h"
#include "graph_io.h"
#include "macros_assertions.h"
#include "mapping/mapping_algorithms.h"
#include "parse_parameters.h"
#include "partition/graph_partitioner.h"
#include "partition/partition_config.h"
#include "partition/uncoarsening/refinement/cycle_improvements/cycle_refinement.h"
#include "quality_metrics.h"
#include "random_functions.h"
#include "timer.h"
#include "mis/initial_mis/greedy_mis.h"

#include "mis/kernel/branch_and_reduce_algorithm.h"

#include "ccp/Chalupa/cli.h"
#include <time.h>

#include "redu_vcc/redu_vcc.h"
#include "redu_vcc/reducer.h"
#include "branch_and_reduce/b_and_r.h"

#include "sigmod_mis/Graph.h"
#include "mis/mis_config.h"

#ifdef BETA
namespace std
{
    template<> struct hash<std::pair<NodeID,NodeID>>
    {
        std::size_t operator()(std::pair<NodeID,NodeID> const & node_pair) const noexcept
        {
            std::size_t seed = 0;
            boost::hash_combine(seed, std::get<0>(node_pair));
            boost::hash_combine(seed, std::get<1>(node_pair));
            return seed;
        }
    };
};

void transform_graph(graph_access const &G,
                     graph_access       &transformed_graph,
                     std::vector<std::pair<NodeID,NodeID>> &vertex_to_edge) {

        // Step 1: acyclic orientation
        std::vector<NodeID> order;
        order.reserve(G.number_of_nodes());
        for (NodeID node = 0; node < G.number_of_nodes(); node++) {
            order.push_back(node);
        }

        // Step 2: line graph
        std::unordered_set<std::pair<NodeID,NodeID>> neighbor_hash(G.number_of_edges());
        forall_nodes(G, v) {
            forall_out_edges(G, e, v) {
                NodeID u = G.getEdgeTarget(e);
                neighbor_hash.insert(std::pair<NodeID,NodeID>(u,v));
                neighbor_hash.insert(std::pair<NodeID,NodeID>(v,u));
            } endfor
        } endfor

        vertex_to_edge.clear();
        vertex_to_edge.reserve(G.number_of_edges() / 2);
        std::unordered_map<std::pair<NodeID, NodeID>, NodeID> edge_to_vertex(G.number_of_edges() / 2);
        std::vector<std::vector<NodeID>> edge_to_edges(G.number_of_edges() / 2);

        std::vector<NodeID> const empty;

        std::cout << "Making transformed graph..." << std::endl;
        std::size_t full_num_edges = 0;
        forall_nodes(G, v) {
            forall_out_edges(G, e, v) {
                NodeID u = G.getEdgeTarget(e);
                std::pair<NodeID,NodeID> node_pair(u, v);
                if (order[v] < order[u]) {
                    node_pair = std::pair<NodeID,NodeID>(v, u);
                }
                if (edge_to_vertex.find(node_pair) == edge_to_vertex.end()) {
                    vertex_to_edge.push_back(node_pair);
                    edge_to_vertex[node_pair] = vertex_to_edge.size() - 1;
                    //edge_to_edges.push_back(empty);
                }
            } endfor

            forall_out_edges(G, e1, v) {
                NodeID u = G.getEdgeTarget(e1);
                std::pair<NodeID,NodeID> first_pair(v, u);
                if (order[u] < order[v]) {
                    first_pair = std::pair<NodeID,NodeID>(u, v);
                }

                forall_out_edges(G, e2, v) {
                    NodeID w = G.getEdgeTarget(e2);
                    if (w < u) continue;
                    if (w == u) continue;
                    std::pair<NodeID,NodeID> second_pair(v, w);
                    if (order[w] < order[v]) {
                        second_pair = std::pair<NodeID,NodeID>(w, v);
                    }

                    bool bad = std::get<0>(first_pair) == std::get<0>(second_pair);
                    std::pair<NodeID,NodeID> const bad_edge =
                        std::pair<NodeID,NodeID>(std::get<1>(first_pair), std::get<1>(second_pair));
                    bad = bad and (neighbor_hash.find(bad_edge) != neighbor_hash.end());
                    if (bad) continue;

                    // Step 3: filter triples, TODO/DS: appears to work
                    edge_to_edges[edge_to_vertex[first_pair]].push_back(edge_to_vertex[second_pair]);
                    edge_to_edges[edge_to_vertex[second_pair]].push_back(edge_to_vertex[first_pair]);
                    full_num_edges++;

                } endfor
            } endfor
        } endfor

        // Step 3: trim triples (integrated above)

        std::cout << "Sorting neighborhoods..." << std::endl;
        std::size_t num_edges = 0;
        for (std::vector<NodeID> &neighbors : edge_to_edges) {
            std::sort(neighbors.begin(), neighbors.end());
            num_edges += neighbors.size();
        }
        num_edges /= 2;

        std::cout << "Transformed Graph has " << edge_to_edges.size() << " vertices and " << num_edges << " edges" << std::endl;

        std::cout << "Constructing graph_access data structure..." << std::endl;

        transformed_graph.start_construction(edge_to_edges.size(), 2 * num_edges);
        for (NodeID v = 0; v < edge_to_edges.size(); v++) {
            NodeID shadow_node = transformed_graph.new_node();
            transformed_graph.setPartitionIndex(shadow_node, 0);
            transformed_graph.setNodeWeight(shadow_node, 1);
            for (NodeID const neighbor : edge_to_edges[v]) {
                EdgeID shadow_edge = transformed_graph.new_edge(shadow_node, neighbor);
                transformed_graph.setEdgeWeight(shadow_edge, 1);
            }
        }

        transformed_graph.finish_construction();

        //graph_io::writeGraph(transformed_graph, "transformed.graph");

        //graph_access new_transformed_graph;
        //graph_io::readGraphWeighted(new_transformed_graph, "transformed-sorted.graph");
}

void transform_is_to_clique_cover(
        graph_access &transformed_graph,
        std::vector<std::pair<NodeID, NodeID>> const &vertex_to_edge,
        std::size_t const num_vertices_original_graph,
        NodeID *solution_is,
        std::size_t const num_cliques,
        std::vector<std::vector<int>> &clique_cover) {

    std::cout << "Reconstructing clique cover..." << std::endl;
    clique_cover.clear();
    clique_cover.reserve(num_cliques);
    unordered_map<NodeID, NodeID> vertex_to_clique_id(num_cliques);
    std::vector<bool> covered(num_vertices_original_graph, false);

    forall_nodes(transformed_graph, v) {
        if (solution_is[v] == 1) {
            std::pair<NodeID,NodeID> const &edge = vertex_to_edge[v];
            //std::cout << "Edge " << std::get<0>(edge) << "," << std::get<1>(edge)
            //         << " is in the independent set" << std::endl;
            if (vertex_to_clique_id.find(std::get<0>(edge))
                    == vertex_to_clique_id.end()) {
                vertex_to_clique_id[std::get<0>(edge)] = clique_cover.size();
                clique_cover.push_back(std::vector<int>{std::get<0>(edge)});
                covered[std::get<0>(edge)] = true;
            }
            clique_cover[vertex_to_clique_id[std::get<0>(edge)]].push_back(std::get<1>(edge));
            covered[std::get<1>(edge)] = true;
        }
    } endfor

    for (NodeID v = 0; v < num_vertices_original_graph; v++) {
        if (!covered[v]) {
            //std::cout << "DS: Vertex " << v << " is uncovered...covering with singleton" << std::endl;
            clique_cover.emplace_back(std::vector<int>{v});
        }
    }
    std::cout << "clique_cover has size: " << clique_cover.size() << std::endl;
}

void run_ils(ils &ils_instance,
             PartitionConfig const &partition_config,
             graph_access &graph,
             timer &the_timer,
             std::size_t const is_offset) {
        std::cout << "Performing ILS..." << std::endl;
        MISConfig ils_config;
        ils_config.seed = partition_config.seed;
        ils_config.time_limit = partition_config.solver_time_limit;
        ils_config.force_cand = 4;
        ils_config.ils_iterations = UINT_MAX;
        ils_instance.perform_ils(ils_config, graph, ils_config.ils_iterations,
                                 the_timer.elapsed(), is_offset, partition_config.mis);
}

void run_peeling(graph_access &graph,
                 graph_access &peeled_graph,
                 std::vector<NodeID> &new_to_old_id,
                 std::size_t &cover_offset) {
    Graph mis_G;
    mis_G.read_graph(graph);
    std::vector<std::vector<NodeID>> kernel;
    cover_offset = 0;
    std::cout << "Running peeling..." << endl;
    std::vector<bool> in_initial_is;
    unsigned int res_mis = mis_G.near_linear_kernel_and_offset(kernel, new_to_old_id, in_initial_is, cover_offset);
    std::cout << "    Reduced " << graph.number_of_nodes() << " -> " << kernel.size() << " nodes" << std::endl;
    std::cout << "    Offset =  " << cover_offset << std::endl;

    std::size_t num_edges = 0;
    for (std::vector<NodeID> & neighbors : kernel) {
        std::sort(neighbors.begin(), neighbors.end());
        num_edges += neighbors.size();
    }
    num_edges /= 2;

    peeled_graph.set_partition_count(2);
    peeled_graph.start_construction(kernel.size(), 2 * num_edges);
    for (NodeID v = 0; v < kernel.size(); v++) {
        NodeID shadow_node = peeled_graph.new_node();
        peeled_graph.setPartitionIndex(shadow_node, in_initial_is[v] ? 1 : 0);
        peeled_graph.setNodeWeight(shadow_node, 1);
        for (NodeID const neighbor : kernel[v]) {
            EdgeID shadow_edge = peeled_graph.new_edge(shadow_node, neighbor);
            peeled_graph.setEdgeWeight(shadow_edge, 1);
        }
    }

    peeled_graph.finish_construction();
}
#endif // BETA

int main(int argn, char **argv) {

    PartitionConfig partition_config;
    std::string graph_filename;

    bool is_graph_weighted = false;
    bool suppress_output   = false;
    bool recursive         = false;

    int ret_code = parse_parameters(argn, argv,
                                    partition_config,
                                    graph_filename,
                                    is_graph_weighted,
                                    suppress_output, recursive);

    if(ret_code) {
        return 0;
    }

    std::streambuf* backup = std::cout.rdbuf();
    std::ofstream ofs;
    ofs.open("/dev/null");
    if(suppress_output) {
        std::cout.rdbuf(ofs.rdbuf());
    }

    partition_config.LogDump(stdout);
    graph_access G;

    timer t;
    graph_io::readGraphWeighted(G, graph_filename);

    timer s;

    if (partition_config.run_type == "Chalupa") {
        timer total_timer;
        redu_vcc reduVCC(G);
        reduVCC.build_cover();
        double time_to_solution = 0.0;
        reduVCC.solveKernel(partition_config, total_timer, time_to_solution, 0 /* clique cover offset */);
        reduVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);

        std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
        std::cout << "run_type=" << partition_config.run_type << std::endl;
        std::cout << "input_graph_vertices=" << G.number_of_nodes() << std::endl;
        std::cout << "input_graph_edges=" << G.number_of_edges() / 2 << std::endl;
        std::cout << "total_time_to_best=" << time_to_solution << std::endl;
        std::cout << "clique_cover_size=" << reduVCC.clique_cover.size() << std::endl;
        std::cout << "verified_cover=" << (reduVCC.validateCover(G) ? "passed" : "failed") << std::endl;
        std::cout << "optimal=" << (reduVCC.clique_cover.size() == partition_config.mis ? "yes" : "unknown") << std::endl;
        return 0;
    }
    else if (partition_config.run_type == "Redu") {
        redu_vcc reduVCC(G);
        std::vector<unsigned int> iso_degree;
        iso_degree.assign(G.number_of_nodes(), 0);
        std::vector<unsigned int> dom_degree;
        dom_degree.assign(G.number_of_nodes(), 0);
        reducer R;
        R.exhaustive_reductions(reduVCC, iso_degree, dom_degree);
        reduVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);

        return 0;
    } else if (partition_config.run_type == "ReduVCC") {
        redu_vcc reduVCC(G);
        std::vector<unsigned int> iso_degree;
        iso_degree.assign(G.number_of_nodes(), 0);
        std::vector<unsigned int> dom_degree;
        dom_degree.assign(G.number_of_nodes(), 0);
        reducer R;
        R.exhaustive_reductions(reduVCC, iso_degree, dom_degree);

        reduVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);
        reduVCC.build_cover();
        std::cout << "cover build" << std::endl;
        double time_to_solution = 0.0;
        reduVCC.solveKernel(partition_config, s, time_to_solution, R.get_cover_size_offset());
        std::cout << "kernel solve" << std::endl;
        timer unwind_timer;
        R.unwindReductions(reduVCC, time_to_solution);
        std::cout << "unwind redutions" << std::endl;
        double time_to_unwind = unwind_timer.elapsed();
        reduVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);

        std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
        std::cout << "run_type=" << partition_config.run_type << std::endl;
        std::cout << "input_graph_vertices=" << G.number_of_nodes() << std::endl;
        std::cout << "input_graph_edges=" << G.number_of_edges() / 2 << std::endl;
        std::cout << "reduced_graph_vertices=" << reduVCC.kernel_adj_list.size() << std::endl;
        std::cout << "reduced_graph_edges=" << reduVCC.kernel_edges << std::endl;
        std::cout << "total_time_to_best=" << time_to_solution << std::endl;
        std::cout << "time_to_best_without_unwind=" << time_to_solution - time_to_unwind << std::endl;
        std::cout << "clique_cover_size=" << reduVCC.clique_cover.size() << std::endl;
        std::cout << "verified_cover=" << (reduVCC.validateCover(G) ? "passed" : "failed") << std::endl;
        std::cout << "optimal=" << (reduVCC.clique_cover.size() == partition_config.mis ? "yes" : "unknown") << std::endl;

        return 0;
    }
#ifdef BETA
    else if (partition_config.run_type == "transform") {

        timer transformation_timer;
        // Step 1: acyclic orientation
        std::vector<NodeID> order;
        order.reserve(G.number_of_nodes());
        for (NodeID node = 0; node < G.number_of_nodes(); node++) {
            order.push_back(node);
        }

        // Step 2: line graph
        std::unordered_set<std::pair<NodeID,NodeID>> neighbor_hash;
        forall_nodes(G, v) {
            forall_out_edges(G, e, v) {
                NodeID u = G.getEdgeTarget(e);
                neighbor_hash.insert(std::pair<NodeID,NodeID>(u,v));
                neighbor_hash.insert(std::pair<NodeID,NodeID>(v,u));
            } endfor
        } endfor

        std::unordered_map<std::pair<NodeID, NodeID>, NodeID> edge_to_vertex;
        std::vector<std::pair<NodeID, NodeID>> vertex_to_edge;
        std::vector<std::vector<NodeID>> edge_to_edges;

        std::vector<NodeID> const empty;

        std::cout << "Making filtered line graph..." << std::endl;
        std::size_t full_num_edges = 0;
        forall_nodes(G, v) {
            forall_out_edges(G, e, v) {
                NodeID u = G.getEdgeTarget(e);
                std::pair<NodeID,NodeID> node_pair(u, v);
                if (order[v] < order[u]) {
                    node_pair = std::pair<NodeID,NodeID>(v, u);
                }
                if (edge_to_vertex.find(node_pair) == edge_to_vertex.end()) {
                    vertex_to_edge.push_back(node_pair);
                    edge_to_vertex[node_pair] = vertex_to_edge.size() - 1;
                    edge_to_edges.push_back(empty);
                }
            } endfor

            forall_out_edges(G, e1, v) {
                NodeID u = G.getEdgeTarget(e1);
                std::pair<NodeID,NodeID> first_pair(v, u);
                if (order[u] < order[v])
                    first_pair = std::pair<NodeID,NodeID>(u, v);

                forall_out_edges(G, e2, v) {
                    NodeID w = G.getEdgeTarget(e2);
                    if (w <= u) continue;
                    std::pair<NodeID,NodeID> second_pair(v, w);
                    if (order[w] < order[v]) {
                        second_pair = std::pair<NodeID,NodeID>(w, v);
                    }

                    // Step 3: filter triples, TODO/DS: appears to work
                    if (std::get<0>(first_pair) == std::get<0>(second_pair) &&
                        (neighbor_hash.find(std::pair<NodeID,NodeID>(std::get<1>(first_pair),std::get<1>(second_pair))) != neighbor_hash.end()) &&
                        (neighbor_hash.find(std::pair<NodeID,NodeID>(std::get<1>(second_pair),std::get<1>(first_pair))) != neighbor_hash.end())) continue;

                    edge_to_edges[edge_to_vertex[first_pair]].push_back(edge_to_vertex[second_pair]);
                    edge_to_edges[edge_to_vertex[second_pair]].push_back(edge_to_vertex[first_pair]);
        full_num_edges++;

                } endfor
            } endfor
        } endfor

        // Step 3: trim triples (integrated above)

        double transformation_time = transformation_timer.elapsed();

        // Step 4: Compute MIS
        if (false) // just peeling
        {
        Graph mis_G;
        mis_G.read_graph(edge_to_edges);
        unsigned int res_mis = mis_G.degree_two_kernal_dominate_lp_and_remove_max_degree_without_contraction();

        std::size_t const cover_upper_bound = G.number_of_nodes() - res_mis;
        std::cout << "Original Graph = " << G.number_of_nodes() << ", MIS=" << res_mis << std::endl;
        std::cout << "Transform (Near Linear) Upper Bound = " << cover_upper_bound << std::endl;
        }

        if (false) // just ILS
        {
        std::size_t num_edges = 0;
        for (std::vector<NodeID> &neighbors : edge_to_edges) {
            std::sort(neighbors.begin(), neighbors.end());
            num_edges += neighbors.size();
        }
        num_edges /= 2;

        std::cout << "Graph has " << edge_to_edges.size() << " vertices and " << num_edges << " edges" << std::endl;

        std::cout << "Constructing graph_access data structure..." << std::endl;

        graph_access new_graph;
        new_graph.start_construction(edge_to_edges.size(), 2 * num_edges);
        for (NodeID v = 0; v < edge_to_edges.size(); v++) {
            NodeID shadow_node = new_graph.new_node();
            new_graph.setPartitionIndex(shadow_node, 0);
            new_graph.setNodeWeight(shadow_node, 1);
            for (NodeID const neighbor : edge_to_edges[v]) {
                EdgeID shadow_edge = new_graph.new_edge(shadow_node, neighbor);
                new_graph.setEdgeWeight(shadow_edge, 1);
            }
        }

        new_graph.finish_construction();

        //graph_io::writeGraph(new_graph, "transformed.graph");

        //graph_access new_new_graph;
        //graph_io::readGraphWeighted(new_new_graph, "transformed-sorted.graph");

        timer ils_timer;
        greedy_mis greedy;
        greedy.initial_partition(partition_config.seed, new_graph);

        std::cout << "Preparing ILS..." << std::endl;
        MISConfig ils_config;
        ils_config.seed = partition_config.seed;
        ils_config.time_limit = partition_config.solver_time_limit;
        ils_config.force_cand = 4;
        ils_config.ils_iterations = UINT_MAX;
        ils ils_instance;
        std::cout << "Performing ILS..." << std::endl;
        ils_instance.perform_ils(ils_config, new_graph, ils_config.ils_iterations);
        cout << "ILS Running time: " << ils_timer.elapsed() << std::endl;

        std::size_t const ils_cover = G.number_of_nodes() - ils_instance.get_best_solution_size();

        std::cout << "Transform (ILS) Upper Bound = " << ils_cover << std::endl;
        }

        if (false) // just branch-and-reduce
        {
        std::cout << "Building a sorted int version of adjacency list..." << std::endl;
        vector<vector<int>> adjlist(edge_to_edges.size());
        for (NodeID v = 0; v < edge_to_edges.size(); v++) {
            std::sort(edge_to_edges[v].begin(), edge_to_edges[v].end());
            adjlist[v].reserve(edge_to_edges[v].size());
            for (NodeID const neighbor : edge_to_edges[v])
                adjlist[v].push_back(neighbor);
        }
        branch_and_reduce_algorithm mis_bnr(adjlist, adjlist.size());

        timer bnr_timer;
        bool timeout = mis_bnr.solve(bnr_timer, partition_config.solver_time_limit) == -1;

        std::size_t const exact_cover = G.number_of_nodes() - mis_bnr.get_current_is_size();

        std::cout << "BNR Running time: " << bnr_timer.elapsed() << std::endl;
        std::cout << "Transform (BNR) Upper Bound = " << exact_cover << std::endl;
        std::cout << "    Upper bound is " << (timeout ? "inexact" : "exact") << endl;
        }

        if (true) // peeling + ILS
        {
        timer total_timer;
        Graph mis_G;
        mis_G.read_graph(edge_to_edges);
        std::vector<std::vector<NodeID>> kernel;
        std::size_t offset = 0;

        std::cout << "Running reducing-peeling..." << endl;
        std::vector<bool> in_initial_is;
        timer peeling_timer;
        std::vector<NodeID> new_to_old_id_unused;
        unsigned int res_mis = mis_G.near_linear_kernel_and_offset(kernel, new_to_old_id_unused, in_initial_is, offset);
        double peeling_time = peeling_timer.elapsed();
        std::cout << "Done with reducing-peeling..." << endl;
        std::cout << "    Reduced " << edge_to_edges.size() << " -> " << kernel.size() << " nodes" << std::endl;
        std::cout << "    Offset =  " << offset << std::endl;

        std::size_t num_edges = 0;
        for (std::vector<NodeID> & neighbors : kernel) {
            std::sort(neighbors.begin(), neighbors.end());
            num_edges += neighbors.size();
        }
        num_edges /= 2;

        graph_access new_graph;
        new_graph.set_partition_count(2);
        new_graph.start_construction(kernel.size(), 2 * num_edges);
        for (NodeID v = 0; v < kernel.size(); v++) {
            NodeID shadow_node = new_graph.new_node();
            new_graph.setPartitionIndex(shadow_node, in_initial_is[v] ? 1 : 0);
            new_graph.setNodeWeight(shadow_node, 1);
            for (NodeID const neighbor : kernel[v]) {
                EdgeID shadow_edge = new_graph.new_edge(shadow_node, neighbor);
                new_graph.setEdgeWeight(shadow_edge, 1);
            }
        }

        new_graph.finish_construction();

        timer ils_timer;
        MISConfig ils_config;
        ils_config.seed = partition_config.seed;
        ils_config.time_limit = partition_config.solver_time_limit;
        ils_config.force_cand = 4;
        ils_config.ils_iterations = UINT_MAX;
        ils ils_instance;
        std::size_t solution_offset = G.number_of_nodes() - offset;
        ils_instance.perform_ils(ils_config, new_graph, ils_config.ils_iterations, total_timer.elapsed(), solution_offset, partition_config.mis);
        cout << "ILS Running time: " << ils_timer.elapsed() << std::endl;

        std::size_t const near_linear_ils_cover = G.number_of_nodes() - (ils_instance.get_best_solution_size() + offset);

        std::cout << "Transform (Near Linear / ILS) Upper Bound = " << near_linear_ils_cover << std::endl;

        std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
        std::cout << "run_type=" << partition_config.run_type << std::endl;
        std::cout << "transformation_time=" << transformation_time << std::endl;
        std::cout << "transformed_graph_vertices=" << edge_to_edges.size() << std::endl;
        std::cout << "transformed_graph_edges=" << full_num_edges << std::endl;
        std::cout << "peeling_time=" << peeling_time << std::endl;
        std::cout << "reduced_transformed_graph_vertices=" << new_graph.number_of_nodes() << std::endl;
        std::cout << "reduced_transformed_graph_edges=" << new_graph.number_of_edges() / 2 << std::endl;

        std::cout << "ils_time_to_best=" << ils_instance.get_last_update_time() << std::endl;
        std::cout << "total_time_to_best=" << ils_instance.get_last_total_update_time() << std::endl;
        std::cout << "clique_cover_size=" << near_linear_ils_cover << std::endl;
        std::cout << "verified_cover=no" << std::endl;
        std::cout << "optimal=" << (near_linear_ils_cover == partition_config.mis ? "yes":"unknown") << std::endl;
        }

        return 0;
    } else if (partition_config.run_type == "Redutransform") {

        timer total_timer;

        timer vcc_reductions_timer;
        redu_vcc oldVCC(G);
        std::vector<unsigned int> iso_degree;
        iso_degree.assign(G.number_of_nodes(), 0);
        std::vector<unsigned int> dom_degree;
        dom_degree.assign(G.number_of_nodes(), 0);
        reducer R;
        R.exhaustive_reductions(oldVCC, iso_degree, dom_degree);
        oldVCC.build_cover();

        std::size_t const cover_size_offset = R.get_cover_size_offset();
        std::cout << "cover_size_offset=" << cover_size_offset << std::endl;

        oldVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);

        double vcc_reduction_time = vcc_reductions_timer.elapsed();

        oldVCC.buildKernel();

        graph_access kernel_graph;
        kernel_graph.start_construction(oldVCC.kernel_adj_list.size(), oldVCC.kernel_edges);
        for (NodeID v = 0; v < oldVCC.kernel_adj_list.size(); v++) {
            NodeID shadow_node = kernel_graph.new_node();
            kernel_graph.setNodeWeight(shadow_node, 1);
            for (NodeID const neighbor : oldVCC.kernel_adj_list[v]) {
                EdgeID shadow_edge = kernel_graph.new_edge(shadow_node, neighbor);
                kernel_graph.setEdgeWeight(shadow_edge, 1);
            }
        }

        kernel_graph.finish_construction();

        timer transformation_timer;
        graph_access transformed_graph;
        std::vector<std::pair<NodeID, NodeID>> vertex_to_edge;
        transform_graph(kernel_graph, transformed_graph, vertex_to_edge);

        double transformation_time = transformation_timer.elapsed();
        //graph_io::writeGraph(new_graph, "transformed.graph");

        // Step 4: Compute MIS
        if (true) // Exhaustive MIS Reductions + Sigmod + ILS
        {
        std::cout << "Making copy of transformed graph..." << std::endl;
        std::vector<std::vector<int>> line_copy;
        line_copy.reserve(transformed_graph.number_of_nodes());
        forall_nodes(transformed_graph, v) {
            std::vector<int> int_neighbors(transformed_graph.getNodeDegree(v));
            forall_out_edges(transformed_graph, e, v) {
                NodeID u = transformed_graph.getEdgeTarget(e);
                int_neighbors.push_back(u);
            } endfor
            line_copy.emplace_back(int_neighbors);
        } endfor

        timer mis_reductions;

        std::cout << "Initializing branch-and-reduce..." << std::endl;
        branch_and_reduce_algorithm mis_bnr(line_copy, line_copy.size());
        std::cout << "Applying exhaustive MIS reductions..." << std::endl;
        mis_bnr.reduce();
        std::cout << "After exhaustive MIS reductions: " << line_copy.size() << " -> " << mis_bnr.number_of_nodes_remaining() << std::endl;

        std::vector<NodeID> reverse_mapping;

        double mis_reduction_time = mis_reductions.elapsed();

        graph_access full_kernel;
        mis_bnr.convert_adj_lists(full_kernel, reverse_mapping);
        std::cout << "Preparing ILS on full kernel of filtered line graph..." << std::endl;

        Graph mis_G;
        mis_G.read_graph(full_kernel);
        std::vector<std::vector<NodeID>> kernel;
        std::size_t offset = 0;
        std::cout << "Running reducing-peeling..." << endl;
        std::vector<bool> in_initial_is;

        timer peeling_timer;
        std::vector<NodeID> new_to_old_id;
        unsigned int res_mis = mis_G.near_linear_kernel_and_offset(kernel, new_to_old_id, in_initial_is, offset);
        double peeling_time = peeling_timer.elapsed();
        std::cout << "Done with reducing-peeling..." << endl;
        assert(offset == 0);

        std::cout << "    Reduced " << full_kernel.number_of_nodes() << " -> " << kernel.size() << " nodes" << std::endl;
        std::cout << "    Offset =  " << offset << std::endl;

        std::size_t num_kernel_edges = 0;
        for (std::vector<NodeID> & neighbors : kernel) {
            std::sort(neighbors.begin(), neighbors.end());
            num_kernel_edges += neighbors.size();
        }
        num_kernel_edges /= 2;

        graph_access peeled_graph;
        peeled_graph.set_partition_count(2);
        peeled_graph.start_construction(kernel.size(), 2 * num_kernel_edges);
        for (NodeID v = 0; v < kernel.size(); v++) {
            NodeID shadow_node = peeled_graph.new_node();
            peeled_graph.setPartitionIndex(shadow_node, in_initial_is[v] ? 1 : 0);
            peeled_graph.setNodeWeight(shadow_node, 1);
            for (NodeID const neighbor : kernel[v]) {
                EdgeID shadow_edge = peeled_graph.new_edge(shadow_node, neighbor);
                peeled_graph.setEdgeWeight(shadow_edge, 1);
            }
        }
        peeled_graph.finish_construction();

        //std::cout << "Writing out post-peeling graph peeled.graph" << std::endl;
        //graph_io::writeGraph(peeled_graph, "peeled.graph");

////        cout << "cover_size_offset=" << cover_size_offset << std::endl;
////        cout << "kernel_graph_number_of_nodes=" << kernel_graph.number_of_nodes() << std::endl;
////        cout << "mis_bnr.get_is_offset=" << mis_bnr.get_is_offset() << std::endl;
////        cout << "offset=" << offset << std::endl;

        timer ils_timer;
        ils ils_instance;
        std::size_t const ils_print_offset = cover_size_offset + kernel_graph.number_of_nodes() - (mis_bnr.get_is_offset() + offset /* from peeling **/);
        run_ils(ils_instance, partition_config, peeled_graph, total_timer, ils_print_offset);
        double time_for_ils = ils_timer.elapsed();
        cout << "ILS Running time: " << time_for_ils << std::endl;

        std::size_t const ils_cover = kernel_graph.number_of_nodes() - (ils_instance.get_best_solution_size() + mis_bnr.get_is_offset() + offset);

        double time_to_unwind = 0.0;
        // reconstruct is / cover
        {
            timer unwind_timer;
            std::vector<bool> full_is(line_copy.size(), false);
            for (NodeID v = 0; v < peeled_graph.number_of_nodes(); v++) {
                if (ils_instance.get_best_solution()[v] == 1)
                    full_is[reverse_mapping[new_to_old_id[v]]] = true;
            }
            std::cout << "Unwinding MIS reductions..." << std::endl;
            mis_bnr.extend_finer_is(full_is);
            NodeID *solution_is = new NodeID[line_copy.size()];
            for (NodeID v = 0; v < line_copy.size(); v++) {
                solution_is[v] = full_is[v] ? 1 : 0;
            }

            std::vector<std::vector<int>> clique_cover;
            std::cout << "Transforming IS to VCC..." << std::endl;
            transform_is_to_clique_cover(transformed_graph, vertex_to_edge,
                kernel_graph.number_of_nodes(), solution_is, ils_cover, clique_cover);
////            std::cout << "Adding " << clique_cover.size() << " kernel cliques..." << std::endl;
            oldVCC.addKernelCliques(clique_cover);
            std::cout << "Unwinding VCC reductions..." << std::endl;
            R.unwindReductions(oldVCC);
            std::cout << "Done unwinding..." << std::endl;
            oldVCC.analyzeGraph(graph_filename, G, s, false /* don't check cover */);
            time_to_unwind = unwind_timer.elapsed();
        }

        double const time_to_best = ils_instance.get_last_total_update_time() + time_to_unwind;

        cout << "Total Running time (beginning to end): " << total_timer.elapsed() << std::endl;

        std::cout << "Transform (Exhaustive+Peeling+ILS) Upper Bound = " << ils_cover + cover_size_offset << std::endl;

        std::size_t const clique_cover_size = ils_cover + cover_size_offset;

        std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
        std::cout << "run_type=" << partition_config.run_type << std::endl;
        std::cout << "vcc_reduction_time=" << vcc_reduction_time << std::endl;
        std::cout << "transformation_time=" << transformation_time << std::endl;
        std::cout << "transformed_graph_vertices=" << transformed_graph.number_of_nodes() << std::endl;
        std::cout << "transformed_graph_edges=" << transformed_graph.number_of_edges() / 2 << std::endl;
        std::cout << "mis_reduction_time=" << mis_reduction_time << std::endl;
        std::cout << "reduced_transformed_graph_vertices=" << full_kernel.number_of_nodes() << std::endl;
        std::cout << "reduced_transformed_graph_edges=" << full_kernel.number_of_edges() / 2 << std::endl;
        std::cout << "peeling_time=" << peeling_time << std::endl;
        std::cout << "ils_time_to_best=" << ils_instance.get_last_update_time() << std::endl;
        std::cout << "total_time_to_best=" << time_to_best << std::endl;
        std::cout << "time_to_best_without_unwind=" << time_to_best - time_to_unwind << std::endl;
        std::cout << "clique_cover_size=" << clique_cover_size << std::endl;
        std::cout << "verified_cover=" << (oldVCC.validateCover(G) ? "passed" : "failed") << std::endl;
        std::cout << "optimal=" << (clique_cover_size == partition_config.mis ? "yes":"unknown") << std::endl;
        }

        return 0;
    }
#endif // BETA
    else if (partition_config.run_type == "bnr") {
      redu_vcc reduVCC;
      branch_and_reduce B(G, reduVCC, partition_config);

      bool finished = false;

      vertex_queue *queue = nullptr;
      if (partition_config.redu_type == "cascading") queue = new vertex_queue(reduVCC);
      finished = B.bandr(reduVCC, 0, queue, partition_config, s);
      double total_time = s.elapsed();
      B.analyzeGraph(graph_filename, G, reduVCC, s);

      std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
      std::cout << "run_type=" << partition_config.run_type << std::endl;
      std::cout << "input_graph_vertices=" << G.number_of_nodes() << std::endl;
      std::cout << "input_graph_edges=" << G.number_of_edges() / 2 << std::endl;
      std::cout << "total_time_to_best=" << total_time << std::endl;
      std::cout << "branch_count=" << B.branch_count << std::endl;
      std::cout << "prune_count=" << B.prune_count << endl;
      std::cout << "decompose_count=" << B.decompose_count << std::endl;
      std::cout << "clique_cover_size=" << reduVCC.clique_cover.size() << std::endl;
      std::cout << "verified_cover=" << (reduVCC.validateCover(G) ? "passed" : "failed") << std::endl;
      std::cout << "optimal=" << (finished ? "yes" : "unknown") << std::endl;

      return 0;
    }
    else if (partition_config.run_type == "edge_bnr") {
      redu_vcc reduVCC;
      branch_and_reduce B(G, reduVCC, partition_config);

      bool finished = false;

      vertex_queue *queue = nullptr;
      if (partition_config.redu_type == "cascading") queue = new vertex_queue(reduVCC);
      finished = B.edge_bandr(reduVCC, 0, queue, partition_config, s, 0);
      double total_time = s.elapsed();
      B.analyzeGraph(graph_filename, G, reduVCC, s);

      std::cout << "BEGIN_OUTPUT_FOR_TABLES" << std::endl;
      std::cout << "run_type=" << partition_config.run_type << std::endl;
      std::cout << "input_graph_vertices=" << G.number_of_nodes() << std::endl;
      std::cout << "input_graph_edges=" << G.number_of_edges() / 2 << std::endl;
      std::cout << "total_time_to_best=" << total_time << std::endl;
      std::cout << "branch_count=" << B.branch_count << std::endl;
      std::cout << "prune_count=" << B.prune_count << endl;
      std::cout << "decompose_count=" << B.decompose_count << std::endl;
      std::cout << "clique_cover_size=" << reduVCC.clique_cover.size() << std::endl;
      std::cout << "verified_cover=" << (reduVCC.validateCover(G) ? "passed" : "failed") << std::endl;
      std::cout << "optimal=" << (finished ? "yes" : "unknown") << std::endl;

      return 0;
    }

#ifdef BETA

    else if (partition_config.run_type == "test") {

        timer total_timer;
        timer transformation_timer;

        graph_access transformed_graph;
        std::vector<std::pair<NodeID, NodeID>> vertex_to_edge;
        transform_graph(G, transformed_graph, vertex_to_edge);

        double transformation_time = transformation_timer.elapsed();

        // Step 4: Compute MIS
        if (true) // just ILS
        {
        timer ils_timer;
        greedy_mis greedy;
        greedy.initial_partition(partition_config.seed, transformed_graph);

        ils ils_instance;
        run_ils(ils_instance, partition_config, transformed_graph, total_timer, G.number_of_nodes());

        cout << "ILS Running time: " << ils_timer.elapsed() << std::endl;

        std::size_t const ils_cover = G.number_of_nodes() - ils_instance.get_best_solution_size();

        std::cout << "Transform (ILS) Upper Bound = " << ils_cover << std::endl;

        std::vector<std::vector<int>> clique_cover;
        size_t const num_cliques = G.number_of_nodes() - ils_instance.get_best_solution_size();

        transform_is_to_clique_cover(transformed_graph,
                                     vertex_to_edge,
                                     G.number_of_nodes(),
                                     ils_instance.get_best_solution(),
                                     num_cliques,
                                     clique_cover);
        }
        return 0;
    }
#endif // BETA

}


#define BOOST_TEST_MODULE pcraster dal space step mapper
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "dal_SpaceStepMapper.h"

BOOST_AUTO_TEST_CASE(test)
{
  using namespace dal;

  {
    SpaceStepMapper mapper;
    BOOST_CHECK(!mapper.isValid());
  }

  {
    SpaceStepMapper mapper(0, 5.0, 0.5);

    BOOST_CHECK(comparable(mapper.destination(1.0), 5.5));
    BOOST_CHECK(comparable(mapper.destination(0.0), 5.0));
    BOOST_CHECK(comparable(mapper.destination(-1.0), 4.5));

    BOOST_CHECK(comparable(mapper.destination(0.5), 5.25));
    BOOST_CHECK(comparable(mapper.destination(-0.5), 4.75));

    BOOST_CHECK(comparable(mapper.source(5.5), 1.0));
    BOOST_CHECK(comparable(mapper.source(5.0), 0.0));
    BOOST_CHECK(comparable(mapper.source(4.5), -1.0));
    BOOST_CHECK(comparable(mapper.source(5.25), 0.5));
    BOOST_CHECK(comparable(mapper.source(4.75), -0.5));
  }

  {
    SpaceStepMapper mapper(0, 74.85, -0.30);

    BOOST_CHECK(comparable(mapper.destination(0), 74.85));
    BOOST_CHECK(comparable(mapper.destination(1), 74.55));
    BOOST_CHECK(comparable(mapper.destination(-0.5), 75.0));
    BOOST_CHECK(comparable(mapper.source(0.15), 249.0));

    BOOST_CHECK(comparable(mapper.source(74.85), 0.0));
    BOOST_CHECK(comparable(mapper.source(74.55), 1.0));
    BOOST_CHECK(comparable(mapper.source(75.0), -0.5));
  }

  {
    SpaceStepMapper mapper(1, 74.85, -0.30);

    BOOST_CHECK(comparable(mapper.destination(1), 74.85));
    BOOST_CHECK(comparable(mapper.destination(2), 74.55));
    BOOST_CHECK(comparable(mapper.destination(0.5), 75.0));

    BOOST_CHECK(comparable(mapper.source(74.85), 1.0));
    BOOST_CHECK(comparable(mapper.source(74.55), 2.0));
    BOOST_CHECK(comparable(mapper.source(75.0), 0.5));
  }
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!
  @ingroup group-arithmetic
  Function object implementing dec

  Decrements a value by 1.

  @par semantic:
  For any given value @c x of type @c T:

  @code
  T r = dec(x);
  @endcode

  is equivalent to:

  @code
  T r =  x-T(1);
  @endcode

  @par Decorators
    saturated_ ensures that saturated_(dec)(x) will never be strictly greater than x.

  @see minus

  **/
  Value dec(Value const & v0);
} }
#endif

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

#endif


#define BOOST_TEST_MODULE vverlet test
#include <boost/test/included/unit_test.hpp>

#include <iterator>
#include <utility>
#include <type_traits>

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

#include <pastel/integrate/vverlet/update.hpp>
#include <pastel/system/simple_system.hpp>
#include <pastel/system/particles.hpp>
#include <pastel/system/neighbor_list.hpp>
#include <pastel/system/force.hpp>
#include <pastel/system/initialize.hpp>
#include <pastel/system/update_forces.hpp>
#include <pastel/system/total_kinetic_energy.hpp>
#include <pastel/system/total_potential_energy.hpp>
#include <pastel/system/meta/point_of.hpp>
#include <pastel/system/meta/vector_of.hpp>
#include <pastel/system/meta/scalar_of.hpp>
//#include <pastel/container/aligned_particles.hpp>
#include <pastel/container/aligned_simple_particles.hpp>
#include <pastel/container/get.hpp>
#include <pastel/geometry/get.hpp>
#include <pastel/geometry/vector_io.hpp>
#include <pastel/geometry/point_io.hpp>
#include <pastel/particle/tags.hpp>
#include <pastel/particle/particle_io.hpp>
#include <pastel/neighbor/neighbor_io.hpp>
#include <pastel/neighbor/fixed_neighbor_list.hpp>
#include <pastel/force/linear_spring.hpp>
#include <pastel/force/update_forces.hpp>

BOOST_AUTO_TEST_CASE(vverlet_test, * boost::unit_test::tolerance(0.001))
{
  //using particles_type = pastel::container::aligned_particles<3u>;
  using particles_type = pastel::container::aligned_simple_particles<3u>;
  using force_type = pastel::force::linear_spring<false>;
  using neighbor_list_type = pastel::neighbor::fixed_neighbor_list<force_type>;
  using system_type = pastel::system::simple_system<particles_type, neighbor_list_type>;

  using point_type = typename pastel::system::meta::point_of<0u, system_type>::type;
  using vector_type = typename pastel::system::meta::vector_of<0u, system_type>::type;
  using scalar_type = typename pastel::system::meta::scalar_of<0u, system_type>::type;
  constexpr auto initial_velocity = vector_type{0.0, 0.0, 0.0};
  constexpr auto initial_force = vector_type{0.0, 0.0, 0.0};
  constexpr auto initial_mass = scalar_type{1.0};
  //constexpr auto initial_diameter = scalar_type{1.0};

  constexpr auto spring_coefficient = 1.0;
  using std::sqrt;
  auto const period = 2.0 * boost::math::constants::pi<double>() / sqrt(initial_mass / (2.0 * spring_coefficient));
  constexpr auto num_steps = 1000000;
  auto const time_step = period / num_steps;

  auto system = system_type{time_step};

  pastel::system::particles<0u>(system).reserve(2);
  pastel::system::particles<0u>(system).emplace_back(
    point_type{+0.5, 0.0, 0.0},
    initial_velocity, initial_force);
  //  initial_velocity, initial_force, initial_mass, initial_diameter);
  pastel::system::particles<0u>(system).emplace_back(
    point_type{-0.5, 0.0, 0.0},
    initial_velocity, initial_force);
  //  initial_velocity, initial_force, initial_mass, initial_diameter);

  pastel::system::neighbor_list<0u>(system).reserve(1u);
  pastel::system::neighbor_list<0u>(system).emplace(0u, 1u);
  pastel::system::force<0u>(system).spring_coefficient(spring_coefficient);

  auto const initial_total_energy
    = ::pastel::system::total_kinetic_energy<0u>(system)
      + ::pastel::system::total_potential_energy<0u>(system);

  pastel::system::initialize(system, time_step);
  pastel::system::update_forces(system);
  BOOST_TEST((pastel::container::get<pastel::particle::tags::force>(pastel::system::particles<0u>(system), 0u) == vector_type{-1.0, 0.0, 0.0}));
  BOOST_TEST((pastel::container::get<pastel::particle::tags::force>(pastel::system::particles<0u>(system), 1u) == vector_type{+1.0, 0.0, 0.0}));

  for (auto step = 0; step < num_steps; ++step)
    pastel::integrate::vverlet::update(system, time_step);
  auto const x0 = pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 0u));
  auto const x1 = pastel::geometry::get<0u>(pastel::container::get<pastel::particle::tags::position>(pastel::system::particles<0u>(system), 1u));
  BOOST_TEST(x0 == +0.5);
  BOOST_TEST(x1 == -0.5);

  auto const total_energy
    = ::pastel::system::total_kinetic_energy<0u>(system)
      + ::pastel::system::total_potential_energy<0u>(system);
  BOOST_TEST(total_energy == initial_total_energy);
}


#include "dvs_tracking/lk_se3.hpp"

#include <glog/logging.h>

#include <boost/range/irange.hpp>
#include <random>

#include "dvs_tracking/weight_functions.hpp"
#include "evo_utils/interpolation.hpp"

#define DEBUG_PRINT_LIMITS

//#define USE_WEIGHTED_UPDATE
//#define BLUR_EVENT_IMAGE

void LKSE3::projectMap() {
    static std::vector<float> z_values;
    z_values.clear();

    map_local_->clear();

    cv::Size s = c_ref_.fullResolution();
    cv::Mat &depthmap = depth_ref_;
    depthmap = cv::Mat(s, CV_32F, cv::Scalar(0.));

    cv::Mat img(s, CV_32F, cv::Scalar(0.));

    n_visible_ = 0;
    size_t n_points = 0;

    Eigen::Affine3f T_ref_world = (T_world_kf_ * T_kf_ref_).inverse();

    for (const auto &P : map_->points) {
        Eigen::Vector3f p(P.x, P.y, P.z);
        p = T_ref_world * p;
        p[0] = p[0] / p[2] * c_ref_.fx() + c_ref_.cx();
        p[1] = p[1] / p[2] * c_ref_.fy() + c_ref_.cy();

        z_values.push_back(p[2]);
        ++n_points;

        if (p[0] < 0 || p[1] < 0) continue;

        int x = p[0] + .5f, y = p[1] + .5f;

        if (x >= s.width || y >= s.height) continue;

        float z = p[2];

        // Minimalistic occlusion detection
        float &depth = depthmap.at<float>(y, x);
        if (depth == 0.)
            depth = z;
        else if (depth > z)
            depth = z;

        img.at<float>(y, x) = 1.;

        map_local_->push_back(P);
        ++n_visible_;
    }

    const int k = map_blur_;
    cv::GaussianBlur(img, img, cv::Size(k, k), 0.);
    cv::GaussianBlur(depthmap, depthmap, cv::Size(k, k), 0.);
    depthmap /= img;
    ref_img_ = img;

    // Compute Median Depth
    std::nth_element(z_values.begin(), z_values.begin() + z_values.size() / 2,
                     z_values.end());
    depth_median_ = z_values[z_values.size() / 2];

    kf_visibility_ = static_cast<float>(n_visible_) / n_points;

    precomputeReferenceFrame();
}

void LKSE3::precomputeReferenceFrame() {
    cv::Mat grad_x_img, grad_y_img;
    cv::Sobel(ref_img_, grad_x_img, CV_32F, 1, 0);
    cv::Sobel(ref_img_, grad_y_img, CV_32F, 0, 1);

    // Image Pointers
    float *depth_ref = depth_ref_.ptr<float>(0),
          *grad_x = grad_x_img.ptr<float>(0),
          *grad_y = grad_y_img.ptr<float>(0);

    int w = ref_img_.cols, h = ref_img_.rows;

    keypoints_.clear();

    Vector8 vec = Vector8::Zero();
    Eigen::Map<const Vector6> vec6(&vec(0));

    for (size_t y = 0; y != h; ++y) {
        float v = ((float)y - c_ref_.cy()) / c_ref_.fy();

        for (size_t x = 0; x != w; ++x) {
            size_t offset = y * w + x;
            float z = depth_ref[offset];
            float pixel_value = ref_img_.at<float>(y, x);

            if (pixel_value < .01) continue;

            float u = ((float)x - c_ref_.cx()) / c_ref_.fx();

            float gx = grad_x[offset] * c_ref_.fx(),
                  gy = grad_y[offset] * c_ref_.fy();

            Vector8 v1, v2;
            v1 << -1. / z, 0., u / z, u * v, -(1. + u * u), v, 0., 0.;
            v2 << 0., -1. / z, v / z, 1 + v * v, -u * v, -u, 0., 0.;

            vec = gx * v1 + gy * v2;

            keypoints_.push_back(Keypoint(Eigen::Vector3f(u * z, v * z, z),
                                          pixel_value, vec,
                                          vec6 * vec6.transpose()));
        }
    }

    // Stochastic Gradient Descent
    static std::seed_seq seq{1, 2, 3, 4, 5};
    static std::mt19937 g(seq);
    std::shuffle(keypoints_.begin(), keypoints_.end(), g);

    batches_ = std::ceil(keypoints_.size() / batch_size_);
}

void LKSE3::updateTransformation(const int offset, const int N,
                                 size_t pyr_lvl) {
    static Eigen::MatrixXf H;
    static Eigen::VectorXf Jres, dx;

    const cv::Mat &img = pyr_new_[pyr_lvl];
    const float *new_img = img.ptr<float>(0);
    float scale = std::pow(2.f, (float)pyr_lvl);
    float fx = fx_ / scale, fy = fy_ / scale, cx = cx_ / scale,
          cy = cy_ / scale;
    size_t w = img.cols, h = img.rows;

    H = Matrix6::Zero();
    Jres = Vector8::Zero();

    for (auto i = offset; i != offset + N; ++i) {
        const Keypoint &k = keypoints_[i];

        Eigen::Vector3f p = T_cur_ref_ * k.P;
        float u = p[0] / p[2] * fx + cx, v = p[1] / p[2] * fy + cy;

        float I_new = evo_utils::interpolate::bilinear(new_img, w, h, u, v);

        if (I_new == -1.f) continue;

        float res = I_new - k.pixel_value;

        if (res >= .95f) continue;

        Jres.noalias() += k.J * res;
        H.noalias() += k.JJt;
    }

    dx = H.ldlt().solve(Jres.head<6>() * scale);

    if ((bool)std::isnan((float)dx[0])) {
        LOG(WARNING) << "Matrix close to singular!";
        return;
    }

#ifdef USE_WEIGHTED_UPDATE
    for (size_t i = 0; i != 3; ++i)
        dx[i] *= weight_functions::Tukey(dx[i] * weight_scale_trans_ /
                                         depth_median_);
    for (size_t i = 3; i != 6; ++i)
        dx[i] *= weight_functions::Tukey(dx[i] * weight_scale_rot_);
#endif

    T_cur_ref_ *= SE3::exp(dx).matrix();
    x_ += dx;
}

void LKSE3::trackFrame() {
    T_cur_ref_ = T_ref_cam_.inverse();
    x_.setZero();

    for (size_t lvl = pyramid_levels_; lvl != 0; --lvl) {
        for (size_t iter = 0; iter != max_iterations_; ++iter) {
            updateTransformation((iter % batches_) * batch_size_, batch_size_,
                                 lvl - 1);
        }
    }
}

void LKSE3::drawEvents(EventQueue::iterator ev_first,
                       EventQueue::iterator ev_last, cv::Mat &out) {
    // Precompute rectification table
    static std::vector<cv::Point> points;
    static std::vector<Eigen::Vector4f> weights;
    if (points.size() == 0) {
        cv::Rect rect(0, 0, width_ - 1, height_ - 1);

        for (size_t y = 0; y != height_; ++y)
            for (size_t x = 0; x != width_; ++x) {
                cv::Point2d p = c_.rectifyPoint(cv::Point(x, y));
                cv::Point tl(std::floor(p.x), std::floor(p.y));
                Eigen::Vector4f w(0, 0, 0, 0);

                if (rect.contains(tl)) {
                    const float fx = p.x - tl.x, fy = p.y - tl.y;

                    w[0] = (1.f - fx) * (1.f - fy);
                    w[1] = (fx) * (1.f - fy);
                    w[2] = (1.f - fx) * (fy);
                    w[3] = (fx) * (fy);
                } else {
                    tl.x = -1;
                }

                points.push_back(tl);
                weights.push_back(w);
            }
    }

    // Draw Events
    auto draw = [](float &p, const float val) { p = std::min(p + val, 1.f); };

    out = cv::Scalar(0);

    for (auto e = ev_first; e != ev_last; ++e) {
        const cv::Point &p = points[e->x + e->y * width_];
        if (p.x == -1) continue;

        const Eigen::Vector4f &w = weights[e->x + e->y * width_];
        draw(out.at<float>(p.y, p.x), w[0]);
        draw(out.at<float>(p.y, p.x + 1), w[1]);
        draw(out.at<float>(p.y + 1, p.x), w[2]);
        draw(out.at<float>(p.y + 1, p.x + 1), w[3]);
    }
}

void LKSE3::drawEventsNN(EventQueue::iterator ev_first,
                         EventQueue::iterator ev_last, cv::Mat &out) {
    // Precompute rectification table
    static std::vector<cv::Point> points;
    if (points.size() == 0) {
        for (size_t y = 0; y != height_; ++y)
            for (size_t x = 0; x != width_; ++x) {
                cv::Point p = c_.rectifyPoint(cv::Point(x, y));

                if (!rect_.contains(p)) p.x = -1;

                points.push_back(p);
            }
    }

    out = cv::Scalar(0);

    for (auto e = ev_first; e != ev_last; ++e) {
        const cv::Point &p = points[e->x + e->y * width_];
        if (p.x == -1) continue;

        out.at<float>(p) = 1.;
    }

#ifdef BLUR_EVENT_IMAGE
    const int k = map_blur_;
    cv::GaussianBlur(out, out, cv::Size(k, k), 0.);
#endif
}


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/xtp/numerical_integrations.h>
#include <votca/ctp/logger.h>
#include <votca/xtp/espfit.h>
#include <votca/xtp/aomatrix.h>

//#include <boost/progress.hpp>

#include <math.h>
#include <votca/tools/constants.h>

namespace votca { namespace xtp {
  using std::flush;

void Espfit::Fit2Density(std::vector< QMAtom* >& atomlist,const Eigen::MatrixXd &dmat,const AOBasis &basis,std::string gridsize) {

    // setting up grid
    Grid grid;
    grid.setupCHELPGGrid(atomlist);
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() <<  " Done setting up CHELPG grid with " << grid.getsize() << " points " << flush;

    // Calculating nuclear potential at gridpoints
    AOOverlap overlap;
    overlap.Fill(basis);
    double N_comp=dmat.cwiseProduct(overlap.Matrix()).sum();

    NumericalIntegration numway;

    numway.GridSetup(gridsize,atomlist,basis);
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Setup "<<gridsize<<" Numerical Grid with "<<numway.getGridSize()<<" gridpoints."<< flush;
    double N=numway.IntegrateDensity(dmat);
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculated Densities at Numerical Grid, Number of electrons is "<< N << flush;

    if(std::abs(N-N_comp)>0.001){
        CTP_LOG(ctp::logDEBUG, *_log) <<"=======================" << flush;
        CTP_LOG(ctp::logDEBUG, *_log) <<"WARNING: Calculated Densities at Numerical Grid, Number of electrons "<< N <<" is far away from the the real value "<< N_comp<<", you should increase the accuracy of the integration grid."<< flush;
        N=N_comp;
        CTP_LOG(ctp::logDEBUG, *_log) <<"WARNING: Electronnumber set to "<< N << flush;
        CTP_LOG(ctp::logDEBUG, *_log) <<"=======================" << flush;
    }

    double netcharge=getNetcharge( atomlist,N );

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP at CHELPG grid points"  << flush;
    //boost::progress_display show_progress( _grid.getsize() );
    #pragma omp parallel for
    for ( unsigned i = 0 ; i < grid.getsize(); i++){
        grid.getGridValues()(i)=numway.IntegratePotential(grid.getGridPositions()[i]);
    }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Electron contribution calculated"  << flush;
    if (!_do_Transition){
      EvalNuclearPotential(  atomlist,  grid );
    }

    FitPartialCharges(atomlist,grid, netcharge);
    return;
    }

void Espfit::EvalNuclearPotential(const std::vector< QMAtom* >& atoms, Grid& grid) {

    const std::vector< tools::vec >& _gridpoints = grid.getGridPositions();
    Eigen::VectorXd& _gridvalues=grid.getGridValues();
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP of nuclei at CHELPG grid points" << flush;

    for (unsigned i = 0; i < _gridpoints.size(); i++) {
        for (unsigned j = 0; j < atoms.size(); j++) {
            const tools::vec& posatom=atoms[j]->getPos();
            double Znuc=atoms[j]->getNuccharge();
            double dist_j = tools::abs(_gridpoints[i]-posatom);
            _gridvalues(i) += Znuc / dist_j;
        }
    }
    return;
}

double Espfit::getNetcharge(const std::vector< QMAtom* >& atoms, double N ){
    double netcharge = 0.0;
      if (std::abs(N) < 0.05) {
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp()<< " Number of Electrons is " << N << " transitiondensity is used for fit" << flush;
        _do_Transition = true;
      } else {
        double Znuc = 0.0;
        for (unsigned j = 0; j < atoms.size(); j++) {
          Znuc += atoms[j]->getNuccharge();
        }

        if (std::abs(Znuc - N) < 4) {
          CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp()<< " Number of Electrons minus Nucleus charge is " << Znuc - N << "." << flush;
        } else {
          CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp()<< " Warning: Your molecule is highly ionized." << flush;
        }
        netcharge = Znuc - N;
        _do_Transition = false;
      }

      netcharge = std::round(netcharge);
      CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp()<< " Netcharge constrained to " << netcharge << flush;

      return netcharge;
}

void Espfit::Fit2Density_analytic(std::vector< QMAtom* >& atomlist,const Eigen::MatrixXd &dmat,const AOBasis &basis) {
    // setting up grid
    Grid grid;
    grid.setupCHELPGGrid(atomlist);

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() <<  " Done setting up CHELPG grid with " << grid.getsize() << " points " << std::endl;
    // Calculating nuclear potential at gridpoints
    AOOverlap overlap;
    overlap.Fill(basis);
    double N_comp=dmat.cwiseProduct(overlap.Matrix()).sum();

    double netcharge=getNetcharge( atomlist,N_comp );
    if(!_do_Transition){
        EvalNuclearPotential(atomlist, grid);
    }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Calculating ESP at CHELPG grid points"  << flush;
    #pragma omp parallel for
    for ( unsigned i = 0 ; i < grid.getsize(); i++){
         AOESP aoesp;
         aoesp.setPosition(grid.getGridPositions()[i]);
         aoesp.Fill(basis);
         grid.getGridValues()(i) -=dmat.cwiseProduct(aoesp.Matrix()).sum();
          }

   FitPartialCharges(atomlist,grid,netcharge);

    return;
    }

void Espfit::FitPartialCharges( std::vector< QMAtom* >& atomlist,const Grid& grid,double netcharge ){

  const int NoOfConstraints=1+_regionconstraint.size()+_pairconstraint.size();
  const int matrixSize=atomlist.size()+NoOfConstraints;
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Setting up Matrices for fitting of size "<< matrixSize <<" x " << matrixSize<< flush;

    const std::vector< tools::vec >& gridpoints=grid.getGridPositions();
    const Eigen::VectorXd & potential=grid.getGridValues();
    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Using "<< atomlist.size() <<" Fittingcenters and " << gridpoints.size()<< " Gridpoints."<< flush;

    Eigen::MatrixXd Amat = Eigen::MatrixXd::Zero(matrixSize,matrixSize);
    Eigen::VectorXd Bvec = Eigen::VectorXd::Zero(matrixSize);
    // setting up _Amat
    #pragma omp parallel for
    for ( unsigned _i =0 ; _i < atomlist.size(); _i++){
        for ( unsigned _j=_i; _j<atomlist.size(); _j++){
            for ( unsigned _k=0; _k < gridpoints.size(); _k++){
                double dist_i = tools::abs(atomlist[_i]->getPos()-gridpoints[_k]);
                double dist_j = tools::abs(atomlist[_j]->getPos()-gridpoints[_k]);

                 Amat(_i,_j) += 1.0/dist_i/dist_j;
            }
            Amat(_j,_i) = Amat(_i,_j);
        }
    }

     // setting up Bvec
    #pragma omp parallel for
    for ( unsigned _i =0 ; _i < atomlist.size(); _i++){
        for ( unsigned _k=0; _k < gridpoints.size(); _k++){
                double dist_i = tools::abs(atomlist[_i]->getPos()-gridpoints[_k]);
                Bvec(_i) += potential(_k)/dist_i;
        }
       }
    //Total charge constraint
    for ( unsigned _i =0 ; _i < atomlist.size()+1; _i++){
      Amat(_i,atomlist.size()) = 1.0;
      Amat(atomlist.size(),_i) = 1.0;
    }
    Amat(atomlist.size(),atomlist.size()) = 0.0;
     Bvec(atomlist.size()) = netcharge; //netcharge!!!!

    // Pairconstraint
     for (unsigned i=0;i<_pairconstraint.size();i++){
         const std::pair<int,int>& pair=_pairconstraint[i];
         Amat(pair.first,atomlist.size()+1+i)=1.0;
         Amat(atomlist.size()+1+i,pair.first)=1.0;
         Amat(pair.second,atomlist.size()+1+i)=-1.0;
         Amat(atomlist.size()+1+i,pair.second)=-1.0;
     }

     //Regionconstraint
     for (unsigned i=0;i<_regionconstraint.size();i++){
         const region& reg=_regionconstraint[i];
         for (const int& index:reg.atomindices){
             Amat(index,atomlist.size()+i+1+_pairconstraint.size())=1.0;
             Amat(atomlist.size()+i+1+_pairconstraint.size(),index)=1.0;
         }
         Bvec(atomlist.size()+i+1+_pairconstraint.size())=reg.charge;
     }

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Solving linear Equation "<< flush;
    Eigen::VectorXd charges;
    if(_do_svd){
      Eigen::JacobiSVD<Eigen::MatrixXd> svd;
      svd.setThreshold(_conditionnumber);
      svd.compute(Amat,Eigen::ComputeThinU | Eigen::ComputeThinV);
      charges=svd.solve(Bvec);
      CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " SVD Done. "<<flush;
      if((Bvec.size()-svd.nonzeroSingularValues())!=0){
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() <<Bvec.size()-svd.nonzeroSingularValues()<<" Sites could not be fitted and are set to zero."<< flush;
      }
    }
    else{
      Eigen::ColPivHouseholderQR<Eigen::MatrixXd> QR(Amat);
      if(QR.info()!=Eigen::ComputationInfo::Success){
        throw std::runtime_error("Espfit: Solving the constrained equation failed. Maybe try SVD.");
      }
      charges=QR.solve(Bvec);
      CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Solved linear least square fit ."<< flush;
    }
    //remove constraint
    charges.conservativeResize(atomlist.size());

    CTP_LOG(ctp::logDEBUG, *_log) << " Sum of fitted charges: " << charges.sum() << flush;
    for (unsigned i=0;i<atomlist.size();i++){
      atomlist[i]->setPartialcharge(charges(i));
    }

    // get RMSE
    double rmse = 0.0;
    double totalPotSq = 0.0;
    for ( unsigned _k=0 ; _k < gridpoints.size(); _k++ ){
        double temp = 0.0;
        for ( const QMAtom* atom:atomlist){
            double dist =  tools::abs(gridpoints[_k]-atom->getPos());
            temp += atom->getPartialcharge()/dist;
        }
        rmse += (potential(_k) - temp)*(potential(_k) - temp);
        totalPotSq += potential(_k)*potential(_k);
    }
    CTP_LOG(ctp::logDEBUG, *_log) << " RMSE of fit:  " << sqrt(rmse/gridpoints.size()) << flush;
    CTP_LOG(ctp::logDEBUG, *_log) << " RRMSE of fit: " << sqrt(rmse/totalPotSq) << flush;

    return;
   }

}}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD_IF(div_
                            , (typename A0, typename X)
                            , (brigand::and_<bd::is_upgradable<A0>, detail::is_native<X>>)
                            , bd::cpu_
                            , bs::tag::ceil_
                            , bs::pack_<bd::int_<A0>, X>
                            , bs::pack_<bd::int_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( bd::functor<bs::tag::ceil_> const&
                                   , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      auto s0 = bs::split(a0);
      auto s1 = bs::split(a1);
      auto d0 = saturated_(toint)(div(ceil, tofloat(s0[0]), tofloat(s1[0])));
      auto d1 = saturated_(toint)(div(ceil, tofloat(s0[1]), tofloat(s1[1])));
      return saturated_(group)(d0, d1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(div_
                            , (typename A0, typename X)
                            , (brigand::and_<bd::is_upgradable<A0>, detail::is_native<X>>)
                            , bd::cpu_
                            , bs::tag::ceil_
                            , bs::pack_<bd::uint_<A0>, X>
                            , bs::pack_<bd::uint_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( bd::functor<bs::tag::ceil_> const&
                                   , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      auto s0 = bs::split(a0);
      auto s1 = bs::split(a1);
      auto d0 = saturated_(touint)(div(ceil, tofloat(s0[0]), tofloat(s1[0])));
      auto d1 = saturated_(touint)(div(ceil, tofloat(s0[1]), tofloat(s1[1])));
      return saturated_(group)(d0, d1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(div_
                         , (typename A0, typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::tag::ceil_
                         , bs::pack_<bd::ints8_<A0>, X>
                         , bs::pack_<bd::ints8_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( bd::functor<bs::tag::ceil_> const&
                                   , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      auto s0 = bs::split(a0);
      auto s1 = bs::split(a1);
      auto d0 = div(ceil, s0[0], s1[0]);
      auto d1 = div(ceil, s0[1], s1[1]);
      return saturated_(group)(d0, d1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF(div_
                         , (typename A0, typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::tag::ceil_
                         , bs::pack_<bd::floating_<A0>, X>
                         , bs::pack_<bd::floating_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( bd::functor<bs::tag::ceil_> const&
                                   , const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      return bs::ceil(a0/a1);
    }
  };
} } }

#endif


#include <boost/graph/distributed/betweenness_centrality.hpp>


/**********************************************************************************************************************
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
Licensed under Apache2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#include <memory>
#include <array>

#include <Eigen/Dense>

#include <gtest/gtest.h>

#include <ct/optcon/optcon.h>

#include <ct/rbd/robot/jacobian/OperationalJacobianBase.h>
#include <ct/rbd/operationalSpace/rigid_body/OperationalModel.h>
#include <ct/rbd/operationalSpace/rigid_body/OperationalModelRBD.h>
#include "../models/testhyq/RobCoGenTestHyQ.h"

using namespace ct::rbd;

class TestJacobian : public OperationalJacobianBase<3, 12>
{
public:
    typedef ct::rbd::OperationalJacobianBase<3, 12> Base;
    typedef typename Base::state_t state_t;
    typedef typename Base::jacobian_t origin_jacobian_t;

    TestJacobian() {}
    ~TestJacobian() {}
    void getJacobianOrigin(const state_t& state, origin_jacobian_t& J) override { J.setIdentity(); }
    void getJacobianOriginDerivative(const state_t& state, origin_jacobian_t& dJdt) override { dJdt.setZero(); }
};

TEST(OperationalSpaceTest, OperationalSpaceTest)
{
    size_t nTests = 100;

    for (size_t i = 0; i < nTests; i++)
    {
        typedef Eigen::Matrix<double, 18, 1> robot_coordinate_t;
        typedef Eigen::Matrix<double, 3, 1> operational_coordinate_t;
        typedef typename OperationalModel<3, 12, 0>::state_t state_t;

        // pointer to the test robot RBDContainer
        std::shared_ptr<TestHyQ::RobCoGenContainer> testRbdContainerPtr(new TestHyQ::RobCoGenContainer());
        // end-effector array for the possible contact points
        std::array<EndEffector<12>, 0> EndEffectorArray;

        // test robot model
        OperationalModelRBD<TestHyQ::RobCoGenContainer, 0>::ptr testRobotModel(
            new OperationalModelRBD<TestHyQ::RobCoGenContainer, 0>(testRbdContainerPtr, EndEffectorArray));
        // test robot Jacobian
        TestJacobian::ptr testJacobian(new TestJacobian());
        // operational space
        OperationalModel<3, 12, 0> testOperationalModel(testRobotModel, testJacobian);

        Eigen::Matrix<double, 12, 1> tau = Eigen::Matrix<double, 12, 1>::Random();
        state_t state;
        state.fromStateVectorEulerXyz(state_t::state_vector_euler_t::Random());
        //robot_coordinate_t q  = state.toCoordinatePosition();
        //robot_coordinate_t qd = state.toCoordinateVelocity();

        // update the robot model
        testRobotModel->update(state);
        robot_coordinate_t qdd;
        qdd = testRobotModel->MInverse() *
              (-testRobotModel->C() - testRobotModel->G() + testRobotModel->S().transpose() * tau);

        // update the operational space model
        testOperationalModel.update(state);
        operational_coordinate_t xdd = testOperationalModel.getAccelerations(qdd);

        operational_coordinate_t testModelError = testOperationalModel.M() * xdd + testOperationalModel.C() +
                                                  testOperationalModel.G() - testOperationalModel.S().transpose() * tau;

        double maxError = testModelError.cwiseAbs().maxCoeff();

        ASSERT_LT(maxError, 1e-4);
    }
}

int main(int argc, char** argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#ifndef nomad__src__matrix__functions__sum_hpp
#define nomad__src__matrix__functions__sum_hpp

#include <Eigen/Core>

#include <src/var/var.hpp>
#include <src/var/derived/multi_sum_var_node.hpp>

namespace nomad {

  template<typename Derived>
  inline typename std::enable_if<
    is_var<typename Eigen::MatrixBase<Derived>::Scalar>::value,
           typename Eigen::MatrixBase<Derived>::Scalar >::type
  sum(const Eigen::MatrixBase<Derived>& input) {

    const short autodiff_order = Eigen::MatrixBase<Derived>::Scalar::order();
    const bool strict_smoothness = Eigen::MatrixBase<Derived>::Scalar::strict();
    const bool validate_io = Eigen::MatrixBase<Derived>::Scalar::validate();

    const nomad_idx_t n_inputs = static_cast<nomad_idx_t>(input.size());

    create_node<multi_sum_var_node<autodiff_order>>(n_inputs);

    double sum = 0;

    for (eigen_idx_t n = 0; n < n_inputs; ++n)
      sum += input(n).first_val();
    push_dual_numbers<autodiff_order>(sum);

    for (eigen_idx_t n = 0; n < n_inputs; ++n)
      push_inputs(input(n).dual_numbers());

    return var<autodiff_order, strict_smoothness, validate_io>(next_node_idx_ - 1);

  }

}

#endif


#include <iostream>
#include "BenchTimer.h"
#include <Eigen/Dense>
#include <map>
#include <string>
using namespace Eigen;

std::map<std::string,Array<float,1,4> > results;

template<typename Scalar,int Size>
void bench(int id, int size = Size)
{
  typedef Matrix<Scalar,Size,Size> Mat;
  Mat A(size,size);
  A.setRandom();
  A = A*A.adjoint();
  BenchTimer t_llt, t_ldlt, t_lu, t_fplu, t_qr, t_cpqr, t_fpqr, t_jsvd;

  int tries = 3;
  int rep = 1000/size;
  if(rep==0) rep = 1;
  rep = rep*rep;

  LLT<Mat> llt(A);
  LDLT<Mat> ldlt(A);
  PartialPivLU<Mat> lu(A);
  FullPivLU<Mat> fplu(A);
  HouseholderQR<Mat> qr(A);
  ColPivHouseholderQR<Mat> cpqr(A);
  FullPivHouseholderQR<Mat> fpqr(A);
  JacobiSVD<Mat> jsvd(A.rows(),A.cols());

  BENCH(t_llt, tries, rep, llt.compute(A));
  BENCH(t_ldlt, tries, rep, ldlt.compute(A));
  BENCH(t_lu, tries, rep, lu.compute(A));
  BENCH(t_fplu, tries, rep, fplu.compute(A));
  BENCH(t_qr, tries, rep, qr.compute(A));
  BENCH(t_cpqr, tries, rep, cpqr.compute(A));
  BENCH(t_fpqr, tries, rep, fpqr.compute(A));
  if(size<500) // JacobiSVD is really too slow for too large matrices
    BENCH(t_jsvd, tries, rep, jsvd.compute(A,ComputeFullU|ComputeFullV));

  results["LLT"][id] = t_llt.best();
  results["LDLT"][id] = t_ldlt.best();
  results["PartialPivLU"][id] = t_lu.best();
  results["FullPivLU"][id] = t_fplu.best();
  results["HouseholderQR"][id] = t_qr.best();
  results["ColPivHouseholderQR"][id] = t_cpqr.best();
  results["FullPivHouseholderQR"][id] = t_fpqr.best();
  results["JacobiSVD"][id] = size<500 ? t_jsvd.best() : 0;
}

int main()
{
  const int small = 8;
  const int medium = 100;
  const int large = 1000;
  const int xl = 4000;

  bench<float,small>(0);
  bench<float,Dynamic>(1,medium);
  bench<float,Dynamic>(2,large);
  bench<float,Dynamic>(3,xl);

  IOFormat fmt(3, 0, " \t", "\n", "", "");

  std::cout << "solver/size               " << small << "\t" << medium << "\t" << large << "\t" << xl << "\n";
  std::cout << "LLT                 (ms)  " << (results["LLT"]/1000.).format(fmt) << "\n";
  std::cout << "LDLT                 (%)  " << (results["LDLT"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "PartialPivLU         (%)  " << (results["PartialPivLU"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "FullPivLU            (%)  " << (results["FullPivLU"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "HouseholderQR        (%)  " << (results["HouseholderQR"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "ColPivHouseholderQR  (%)  " << (results["ColPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "FullPivHouseholderQR (%)  " << (results["FullPivHouseholderQR"]/results["LLT"]).format(fmt) << "\n";
  std::cout << "JacobiSVD            (%)  " << (results["JacobiSVD"]/results["LLT"]).format(fmt) << "\n";
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2013 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/pricingengines/swaption/basketgeneratingengine.hpp>
#include <ql/rebatedexercise.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/math/optimization/simplex.hpp>
#include <ql/models/shortrate/calibrationhelpers/swaptionhelper.hpp>
#include <ql/termstructures/volatility/swaption/swaptionvolcube.hpp>
#include <ql/quotes/simplequote.hpp>
#include <boost/make_shared.hpp>

using std::exp;
using std::fabs;

namespace QuantLib {

    Disposable<std::vector<boost::shared_ptr<CalibrationHelper> > >
    BasketGeneratingEngine::calibrationBasket(
        const boost::shared_ptr<Exercise> &exercise,
        boost::shared_ptr<SwapIndex> standardSwapBase,
        boost::shared_ptr<SwaptionVolatilityStructure> swaptionVolatility,
        const CalibrationBasketType basketType) const {

        QL_REQUIRE(
            !standardSwapBase->forwardingTermStructure().empty(),
            "standard swap base forwarding term structure must not be empty.");
        QL_REQUIRE(
            !standardSwapBase->exogenousDiscount() ||
                !standardSwapBase->discountingTermStructure().empty(),
            "standard swap base discounting term structure must not be empty.");

        std::vector<boost::shared_ptr<CalibrationHelper> > result;

        Size minIdxAlive = static_cast<Size>(
            std::upper_bound(exercise->dates().begin(), exercise->dates().end(),
                             Settings::instance().evaluationDate()) -
            exercise->dates().begin());

        boost::shared_ptr<RebatedExercise> rebEx =
            boost::dynamic_pointer_cast<RebatedExercise>(exercise);

        for (Size i = minIdxAlive; i < exercise->dates().size(); i++) {

            Date expiry = exercise->date(i);
            Real rebate = 0.0;
            Date rebateDate = expiry;
            if (rebEx != NULL) {
                rebate = rebEx->rebate(i);
                rebateDate = rebEx->rebatePaymentDate(i);
            }

            boost::shared_ptr<SwaptionHelper> helper;

            switch (basketType) {

            case Naive: {
                Real swapLength = swaptionVolatility->dayCounter().yearFraction(
                    standardSwapBase->valueDate(expiry), underlyingLastDate());
                boost::shared_ptr<SmileSection> sec =
                    swaptionVolatility->smileSection(
                        expiry,
                        static_cast<Size>(swapLength * 12.0 + 0.5) * Months,
                        true);
                Real atmStrike = sec->atmLevel();
                Real atmVol;
                if (atmStrike == Null<Real>())
                    atmVol = sec->volatility(0.03);
                else
                    atmVol = sec->volatility(atmStrike);
                Real shift = sec->shift();

                helper = boost::shared_ptr<SwaptionHelper>(new SwaptionHelper(
                    expiry, underlyingLastDate(),
                    Handle<Quote>(boost::make_shared<SimpleQuote>(atmVol)),
                    standardSwapBase->iborIndex(),
                    standardSwapBase->fixedLegTenor(),
                    standardSwapBase->dayCounter(),
                    standardSwapBase->iborIndex()->dayCounter(),
                    standardSwapBase->exogenousDiscount()
                        ? standardSwapBase->discountingTermStructure()
                        : standardSwapBase->forwardingTermStructure(),
                    CalibrationHelper::RelativePriceError, Null<Real>(), 1.0, shift));

                break;
            }

            case MaturityStrikeByDeltaGamma: {

                // determine the npv, first and second order derivatives at
                // $y=0$ of the underlying swap

                const Real h = 0.0001; // finite difference step in $y$, make
                                       // this a parameter of the engine ?
                Real zSpreadDsc =
                    oas_.empty() ? 1.0
                                 : exp(-oas_->value() *
                                       onefactormodel_->termStructure()
                                           ->dayCounter()
                                           .yearFraction(expiry, rebateDate));

                Real npvm = underlyingNpv(expiry, -h) +
                            rebate *
                                onefactormodel_->zerobond(rebateDate, expiry,
                                                          -h, discountCurve_) *
                                zSpreadDsc;
                Real npv = underlyingNpv(expiry, 0.0) +
                           rebate * onefactormodel_->zerobond(
                                        rebateDate, expiry, 0, discountCurve_) *
                               zSpreadDsc;
                Real npvp = underlyingNpv(expiry, h) +
                            rebate *
                                onefactormodel_->zerobond(rebateDate, expiry, h,
                                                          discountCurve_) *
                                zSpreadDsc;

                Real delta = (npvp - npvm) / (2.0 * h);
                Real gamma = (npvp - 2.0 * npv + npvm) / (h * h);

                QL_REQUIRE(npv * npv + delta * delta + gamma * gamma > 0.0,
                           "(npv,delta,gamma) must have a positive norm");

                // debug output
                // std::cout << "EXOTIC npv " << npv << " delta " << delta
                //           << " gamma " << gamma << std::endl;
                // Real xtmp = -5.0;
                // std::cout
                //     << "********************************************EXERCISE "
                //     << expiry << " ******************" << std::endl;
                // std::cout << "globalExoticNpv;";
                // while (xtmp <= 5.0 + QL_EPSILON) {
                //     std::cout << underlyingNpv(expiry, xtmp) << ";";
                //     xtmp += 0.1;
                // }
                // std::cout << std::endl;
                // end debug output

                // play safe, we restrict the maximum maturity so to easily fit
                // in the date class restriction
                Real maxMaturity =
                    swaptionVolatility->dayCounter().yearFraction(
                        expiry, Date::maxDate() - 365);

                boost::shared_ptr<MatchHelper> matchHelper_;
                matchHelper_ = boost::shared_ptr<MatchHelper>(new MatchHelper(
                    underlyingType(), npv, delta, gamma, onefactormodel_,
                    standardSwapBase, expiry, maxMaturity, h));

                // Optimize
                Array initial = initialGuess(expiry);
                QL_REQUIRE(initial.size() == 3,
                           "initial guess must have size 3 (but is "
                               << initial.size() << ")");

                EndCriteria ec(1000, 200, 1E-8, 1E-8, 1E-8); // make these
                                                             // criteria and the
                                                             // optimizer itself
                                                             // parameters of
                                                             // the method ?
                Constraint constraint = NoConstraint();
                Problem p(*matchHelper_, constraint, initial);
                LevenbergMarquardt lm;

                EndCriteria::Type ret = lm.minimize(p, ec);
                QL_REQUIRE(ret != EndCriteria::None &&
                               ret != EndCriteria::Unknown &&
                               ret != EndCriteria::MaxIterations,
                           "optimizer returns error (" << ret << ")");
                Array solution = p.currentValue();

                Real maturity = fabs(solution[1]);

                Size years = (Size)std::floor(maturity);
                maturity -= (Real)years;
                maturity *= 12.0;
                Size months = (Size)std::floor(maturity + 0.5);
                if (years == 0 && months == 0)
                    months = 1; // ensure a maturity of at least one months
                // maturity -= (Real)months; maturity *= 365.25;
                // Size days = (Size)std::floor(maturity);

                Period matPeriod =
                    years * Years + months * Months; //+days*Days;

                boost::shared_ptr<SmileSection> sec =
                    swaptionVolatility->smileSection(expiry, matPeriod, true);
                Real shift = sec->shift();

                // we have to floor the strike of the calibration instrument,
                // see warning in the header
                solution[2] = std::max(
                    solution[2], 0.00001 - shift); // floor at 0.1bp - shift

                // also the calibrated nominal may be zero, so we floor it, too
                solution[0] =
                    std::max(solution[0], 0.000001); // float at 0.01bp

                Real vol = sec->volatility(solution[2]);

                helper = boost::shared_ptr<SwaptionHelper>(new SwaptionHelper(
                    expiry, matPeriod,
                    Handle<Quote>(boost::make_shared<SimpleQuote>(
                                      vol)),
                    standardSwapBase->iborIndex(),
                    standardSwapBase->fixedLegTenor(),
                    standardSwapBase->dayCounter(),
                    standardSwapBase->iborIndex()->dayCounter(),
                    standardSwapBase->exogenousDiscount()
                        ? standardSwapBase->discountingTermStructure()
                        : standardSwapBase->forwardingTermStructure(),
                    CalibrationHelper::RelativePriceError, solution[2],
                    fabs(solution[0]),shift));
                break;
            }

            default:
                QL_FAIL("Calibration basket type not known (" << basketType
                                                              << ")");
            }

            result.push_back(helper);
        }

        return result;
    }
}


#include <ceres/ceres.h>
#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/MatrixFunctions>

#include "wave/wave_test.hpp"
#include "wave/geometry_og/transformation.hpp"
#include "wave/kinematics/constant_velocity_gp_prior.hpp"
#include "wave/optimization/ceres/trajectory_prior.hpp"
#include "wave/optimization/ceres/odom_gp/constant_velocity.hpp"
#include "wave/optimization/ceres/local_params/null_SE3_parameterization.hpp"

namespace wave {

/**
 * Test that a trajectory prior works correctly during optimization
 * Due to local parameterization hack, gradient checker does not work
 */
TEST(trajectory_prior, unary_factor) {
    Vec6 twist, prior_twist;
    twist << 0.1, -0.4, 0.3, 1, 10, -5;
    prior_twist << -0.3, 0.2, 0.5, -3, 2, -8;
    Transformation<> prior, initial;
    prior.setFromExpMap(twist);

    auto inv_prior = prior.transformInverse();

    ceres::CostFunction *cost_function =
      new TrajectoryPrior<Transformation<>>(Eigen::Matrix<double, 12, 12>::Identity(), inv_prior, prior_twist);

    ceres::LocalParameterization *se3_param = new NullSE3Parameterization;

    ceres::Problem problem;
    problem.AddParameterBlock(initial.storage.data(), 12, se3_param);
    problem.AddParameterBlock(twist.data(), 6);

    problem.AddResidualBlock(cost_function, nullptr, initial.storage.data(), twist.data());

    ceres::Solver::Options options;
    options.parameter_tolerance = 1e-10;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport();

    auto diff = initial.manifoldMinus(prior);
    EXPECT_NEAR(diff.norm(), 0, 1e-8);
    EXPECT_NEAR((twist - prior_twist).norm(), 0, 1e-8);
}

/**
 * Test the constant velocity constraint
 * optimization problem will be initialized a little ways from the solution,
 * and it will converge back
 */
TEST(trajectory_prior, binary_factor) {
    const double delta_T = 0.1;
    Vec6 init_twist_prior, next_twist_prior, init_twist, next_twist;
    init_twist_prior << 0.01, -0.04, 0.03, 1, 10, -5;

    /// Need to constrain the initial state with a prior.
    /// In this case Identity to treat it as the origin.
    Vec6 prior_vel;
    prior_vel = init_twist_prior / delta_T;
    Transformation<> prior_initial, prior_next, initial, next;
    prior_initial.setIdentity();

    // Need to provide prior for binary factor
    prior_next.setFromExpMap(delta_T * prior_vel);

    auto prior_initial_inv = prior_initial.transformInverse();

    ceres::CostFunction *cost_function =
      new TrajectoryPrior<Transformation<>>(10000*Eigen::Matrix<double, 12, 12>::Identity(), prior_initial_inv, prior_vel);

    ceres::LocalParameterization *se3_param = new NullSE3Parameterization;

    ceres::Problem problem;
    problem.AddParameterBlock(initial.storage.data(), 12, se3_param);
    problem.AddParameterBlock(init_twist.data(), 6);
    problem.AddParameterBlock(next.storage.data(), 12, se3_param);
    problem.AddParameterBlock(next_twist.data(), 6);

    problem.AddResidualBlock(cost_function, nullptr, initial.storage.data(), init_twist.data());

    double tk = 0;
    double tkp1 = 0.1;
    double tau = 0;

    Mat6 Qc = Mat6::Identity();
    Mat6 inv_Qc = Qc.inverse();
    wave_kinematics::ConstantVelocityPrior::Mat12 transition, Q, weight;

    wave_kinematics::ConstantVelocityPrior cv_model(tk, tkp1, &tau, Qc, inv_Qc);
    cv_model.calculateTransitionMatrix(transition, tk, tkp1);
    cv_model.calculateLinCovariance(Q, tk, tkp1);

    weight = 100*Q.inverse().sqrt();

    ceres::CostFunction *bin_cost_function = new ConstantVelocityPrior(weight, tkp1);

    init_twist.setZero();
    initial.setIdentity();
    next_twist.setZero();
    next.setIdentity();

    problem.AddResidualBlock(bin_cost_function,
                             nullptr,
                             initial.storage.data(),
                             next.storage.data(),
                             init_twist.data(),
                             next_twist.data());

    ceres::Solver::Options options;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout << summary.FullReport();

    auto init_diff = initial.manifoldMinus(prior_initial);
    auto next_diff = next.manifoldMinus(prior_next);
    Vec6 init_vel_diff = init_twist - prior_vel;
    Vec6 next_vel_diff = next_twist - prior_vel;

    EXPECT_NEAR(init_diff.norm(), 0, 1e-6);
    EXPECT_NEAR(next_diff.norm(), 0, 1e-6);
    EXPECT_NEAR(init_vel_diff.norm(), 0, 1e-6);
    EXPECT_NEAR(next_vel_diff.norm(), 0, 1e-6);
}
}