//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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)
//=======================================================================
//

/*
  This file implements the function

  template <class EdgeListGraph, class Size, class P, class T, class R>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N,
     const bgl_named_params<P, T, R>& params)

 */

#ifndef BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{

template < class Visitor, class Graph > struct BellmanFordVisitorConcept
{
    void constraints()
    {
        BOOST_CONCEPT_ASSERT((CopyConstructibleConcept< Visitor >));
        vis.examine_edge(e, g);
        vis.edge_relaxed(e, g);
        vis.edge_not_relaxed(e, g);
        vis.edge_minimized(e, g);
        vis.edge_not_minimized(e, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits< Graph >::edge_descriptor e;
};

template < class Visitors = null_visitor > class bellman_visitor
{
public:
    bellman_visitor() {}
    bellman_visitor(Visitors vis) : m_vis(vis) {}

    template < class Edge, class Graph > void examine_edge(Edge u, Graph& g)
    {
        invoke_visitors(m_vis, u, g, on_examine_edge());
    }
    template < class Edge, class Graph > void edge_relaxed(Edge u, Graph& g)
    {
        invoke_visitors(m_vis, u, g, on_edge_relaxed());
    }
    template < class Edge, class Graph > void edge_not_relaxed(Edge u, Graph& g)
    {
        invoke_visitors(m_vis, u, g, on_edge_not_relaxed());
    }
    template < class Edge, class Graph > void edge_minimized(Edge u, Graph& g)
    {
        invoke_visitors(m_vis, u, g, on_edge_minimized());
    }
    template < class Edge, class Graph >
    void edge_not_minimized(Edge u, Graph& g)
    {
        invoke_visitors(m_vis, u, g, on_edge_not_minimized());
    }

protected:
    Visitors m_vis;
};
template < class Visitors >
bellman_visitor< Visitors > make_bellman_visitor(Visitors vis)
{
    return bellman_visitor< Visitors >(vis);
}
typedef bellman_visitor<> default_bellman_visitor;

template < class EdgeListGraph, class Size, class WeightMap,
    class PredecessorMap, class DistanceMap, class BinaryFunction,
    class BinaryPredicate, class BellmanFordVisitor >
bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N, WeightMap weight,
    PredecessorMap pred, DistanceMap distance, BinaryFunction combine,
    BinaryPredicate compare, BellmanFordVisitor v)
{
    BOOST_CONCEPT_ASSERT((EdgeListGraphConcept< EdgeListGraph >));
    typedef graph_traits< EdgeListGraph > GTraits;
    typedef typename GTraits::edge_descriptor Edge;
    typedef typename GTraits::vertex_descriptor Vertex;
    BOOST_CONCEPT_ASSERT((ReadWritePropertyMapConcept< DistanceMap, Vertex >));
    BOOST_CONCEPT_ASSERT((ReadablePropertyMapConcept< WeightMap, Edge >));

    typename GTraits::edge_iterator i, end;

    for (Size k = 0; k < N; ++k)
    {
        bool at_least_one_edge_relaxed = false;
        for (boost::tie(i, end) = edges(g); i != end; ++i)
        {
            v.examine_edge(*i, g);
            if (relax(*i, g, weight, pred, distance, combine, compare))
            {
                at_least_one_edge_relaxed = true;
                v.edge_relaxed(*i, g);
            }
            else
                v.edge_not_relaxed(*i, g);
        }
        if (!at_least_one_edge_relaxed)
            break;
    }

    for (boost::tie(i, end) = edges(g); i != end; ++i)
        if (compare(combine(get(distance, source(*i, g)), get(weight, *i)),
                get(distance, target(*i, g))))
        {
            v.edge_not_minimized(*i, g);
            return false;
        }
        else
            v.edge_minimized(*i, g);

    return true;
}

namespace detail
{

    template < typename VertexAndEdgeListGraph, typename Size,
        typename WeightMap, typename PredecessorMap, typename DistanceMap,
        typename P, typename T, typename R >
    bool bellman_dispatch2(VertexAndEdgeListGraph& g,
        typename graph_traits< VertexAndEdgeListGraph >::vertex_descriptor s,
        Size N, WeightMap weight, PredecessorMap pred, DistanceMap distance,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< DistanceMap >::value_type D;
        bellman_visitor<> null_vis;
        typedef typename property_traits< WeightMap >::value_type weight_type;
        typename graph_traits< VertexAndEdgeListGraph >::vertex_iterator v,
            v_end;
        for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v)
        {
            put(distance, *v, (std::numeric_limits< weight_type >::max)());
            put(pred, *v, *v);
        }
        put(distance, s, weight_type(0));
        return bellman_ford_shortest_paths(g, N, weight, pred, distance,
            choose_param(
                get_param(params, distance_combine_t()), closed_plus< D >()),
            choose_param(
                get_param(params, distance_compare_t()), std::less< D >()),
            choose_param(get_param(params, graph_visitor), null_vis));
    }

    template < typename VertexAndEdgeListGraph, typename Size,
        typename WeightMap, typename PredecessorMap, typename DistanceMap,
        typename P, typename T, typename R >
    bool bellman_dispatch2(VertexAndEdgeListGraph& g, param_not_found, Size N,
        WeightMap weight, PredecessorMap pred, DistanceMap distance,
        const bgl_named_params< P, T, R >& params)
    {
        typedef typename property_traits< DistanceMap >::value_type D;
        bellman_visitor<> null_vis;
        return bellman_ford_shortest_paths(g, N, weight, pred, distance,
            choose_param(
                get_param(params, distance_combine_t()), closed_plus< D >()),
            choose_param(
                get_param(params, distance_compare_t()), std::less< D >()),
            choose_param(get_param(params, graph_visitor), null_vis));
    }

    template < class EdgeListGraph, class Size, class WeightMap,
        class DistanceMap, class P, class T, class R >
    bool bellman_dispatch(EdgeListGraph& g, Size N, WeightMap weight,
        DistanceMap distance, const bgl_named_params< P, T, R >& params)
    {
        dummy_property_map dummy_pred;
        return detail::bellman_dispatch2(g, get_param(params, root_vertex_t()),
            N, weight,
            choose_param(get_param(params, vertex_predecessor), dummy_pred),
            distance, params);
    }
} // namespace detail

template < class EdgeListGraph, class Size, class P, class T, class R >
bool bellman_ford_shortest_paths(
    EdgeListGraph& g, Size N, const bgl_named_params< P, T, R >& params)
{
    return detail::bellman_dispatch(g, N,
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
        params);
}

template < class EdgeListGraph, class Size >
bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N)
{
    bgl_named_params< int, int > params(0);
    return bellman_ford_shortest_paths(g, N, params);
}

template < class VertexAndEdgeListGraph, class P, class T, class R >
bool bellman_ford_shortest_paths(
    VertexAndEdgeListGraph& g, const bgl_named_params< P, T, R >& params)
{
    BOOST_CONCEPT_ASSERT((VertexListGraphConcept< VertexAndEdgeListGraph >));
    return detail::bellman_dispatch(g, num_vertices(g),
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
        params);
}
} // namespace boost

#endif // BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2009 Trustees of Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek, Michael Hansen
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP
#define BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP

#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost
{

// No init version
template < typename Graph, typename DijkstraVisitor, typename PredecessorMap,
    typename DistanceMap, typename WeightMap, typename VertexIndexMap,
    typename DistanceCompare, typename DistanceWeightCombine,
    typename DistanceInfinity, typename DistanceZero >
void dijkstra_shortest_paths_no_color_map_no_init(const Graph& graph,
    typename graph_traits< Graph >::vertex_descriptor start_vertex,
    PredecessorMap predecessor_map, DistanceMap distance_map,
    WeightMap weight_map, VertexIndexMap index_map,
    DistanceCompare distance_compare,
    DistanceWeightCombine distance_weight_combine,
    DistanceInfinity distance_infinity, DistanceZero distance_zero,
    DijkstraVisitor visitor)
{
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename property_traits< DistanceMap >::value_type Distance;

    typedef indirect_cmp< DistanceMap, DistanceCompare >
        DistanceIndirectCompare;
    DistanceIndirectCompare distance_indirect_compare(
        distance_map, distance_compare);

    // Default - use d-ary heap (d = 4)
    typedef detail::vertex_property_map_generator< Graph, VertexIndexMap,
        std::size_t >
        IndexInHeapMapHelper;
    typedef typename IndexInHeapMapHelper::type IndexInHeapMap;
    typedef d_ary_heap_indirect< Vertex, 4, IndexInHeapMap, DistanceMap,
        DistanceCompare >
        VertexQueue;

    boost::scoped_array< std::size_t > index_in_heap_map_holder;
    IndexInHeapMap index_in_heap = IndexInHeapMapHelper::build(
        graph, index_map, index_in_heap_map_holder);
    VertexQueue vertex_queue(distance_map, index_in_heap, distance_compare);

    // Add vertex to the queue
    vertex_queue.push(start_vertex);

    // Starting vertex will always be the first discovered vertex
    visitor.discover_vertex(start_vertex, graph);

    while (!vertex_queue.empty())
    {
        Vertex min_vertex = vertex_queue.top();
        vertex_queue.pop();

        visitor.examine_vertex(min_vertex, graph);

        // Check if any other vertices can be reached
        Distance min_vertex_distance = get(distance_map, min_vertex);

        if (!distance_compare(min_vertex_distance, distance_infinity))
        {
            // This is the minimum vertex, so all other vertices are unreachable
            return;
        }

        // Examine neighbors of min_vertex
        BGL_FORALL_OUTEDGES_T(min_vertex, current_edge, graph, Graph)
        {
            visitor.examine_edge(current_edge, graph);

            // Check if the edge has a negative weight
            if (distance_compare(get(weight_map, current_edge), distance_zero))
            {
                boost::throw_exception(negative_edge());
            }

            // Extract the neighboring vertex and get its distance
            Vertex neighbor_vertex = target(current_edge, graph);
            Distance neighbor_vertex_distance
                = get(distance_map, neighbor_vertex);
            bool is_neighbor_undiscovered = !distance_compare(
                neighbor_vertex_distance, distance_infinity);

            // Attempt to relax the edge
            bool was_edge_relaxed
                = relax_target(current_edge, graph, weight_map, predecessor_map,
                    distance_map, distance_weight_combine, distance_compare);

            if (was_edge_relaxed)
            {
                visitor.edge_relaxed(current_edge, graph);
                if (is_neighbor_undiscovered)
                {
                    visitor.discover_vertex(neighbor_vertex, graph);
                    vertex_queue.push(neighbor_vertex);
                }
                else
                {
                    vertex_queue.update(neighbor_vertex);
                }
            }
            else
            {
                visitor.edge_not_relaxed(current_edge, graph);
            }

        } // end out edge iteration

        visitor.finish_vertex(min_vertex, graph);
    } // end while queue not empty
}

// Full init version
template < typename Graph, typename DijkstraVisitor, typename PredecessorMap,
    typename DistanceMap, typename WeightMap, typename VertexIndexMap,
    typename DistanceCompare, typename DistanceWeightCombine,
    typename DistanceInfinity, typename DistanceZero >
void dijkstra_shortest_paths_no_color_map(const Graph& graph,
    typename graph_traits< Graph >::vertex_descriptor start_vertex,
    PredecessorMap predecessor_map, DistanceMap distance_map,
    WeightMap weight_map, VertexIndexMap index_map,
    DistanceCompare distance_compare,
    DistanceWeightCombine distance_weight_combine,
    DistanceInfinity distance_infinity, DistanceZero distance_zero,
    DijkstraVisitor visitor)
{
    // Initialize vertices
    BGL_FORALL_VERTICES_T(current_vertex, graph, Graph)
    {
        visitor.initialize_vertex(current_vertex, graph);

        // Default all distances to infinity
        put(distance_map, current_vertex, distance_infinity);

        // Default all vertex predecessors to the vertex itself
        put(predecessor_map, current_vertex, current_vertex);
    }

    // Set distance for start_vertex to zero
    put(distance_map, start_vertex, distance_zero);

    // Pass everything on to the no_init version
    dijkstra_shortest_paths_no_color_map_no_init(graph, start_vertex,
        predecessor_map, distance_map, weight_map, index_map, distance_compare,
        distance_weight_combine, distance_infinity, distance_zero, visitor);
}

namespace detail
{

    // Handle defaults for PredecessorMap, DistanceCompare,
    // DistanceWeightCombine, DistanceInfinity and DistanceZero
    template < typename Graph, typename DistanceMap, typename WeightMap,
        typename VertexIndexMap, typename Params >
    inline void dijkstra_no_color_map_dispatch2(const Graph& graph,
        typename graph_traits< Graph >::vertex_descriptor start_vertex,
        DistanceMap distance_map, WeightMap weight_map,
        VertexIndexMap index_map, const Params& params)
    {
        // Default for predecessor map
        dummy_property_map predecessor_map;

        typedef
            typename property_traits< DistanceMap >::value_type DistanceType;
        DistanceType inf = choose_param(get_param(params, distance_inf_t()),
            (std::numeric_limits< DistanceType >::max)());
        dijkstra_shortest_paths_no_color_map(graph, start_vertex,
            choose_param(
                get_param(params, vertex_predecessor), predecessor_map),
            distance_map, weight_map, index_map,
            choose_param(get_param(params, distance_compare_t()),
                std::less< DistanceType >()),
            choose_param(get_param(params, distance_combine_t()),
                std::plus< DistanceType >()),
            inf,
            choose_param(get_param(params, distance_zero_t()), DistanceType()),
            choose_param(get_param(params, graph_visitor),
                make_dijkstra_visitor(null_visitor())));
    }

    template < typename Graph, typename DistanceMap, typename WeightMap,
        typename IndexMap, typename Params >
    inline void dijkstra_no_color_map_dispatch1(const Graph& graph,
        typename graph_traits< Graph >::vertex_descriptor start_vertex,
        DistanceMap distance_map, WeightMap weight_map, IndexMap index_map,
        const Params& params)
    {
        // Default for distance map
        typedef typename property_traits< WeightMap >::value_type DistanceType;
        typename std::vector< DistanceType >::size_type vertex_count
            = is_default_param(distance_map) ? num_vertices(graph) : 1;

        std::vector< DistanceType > default_distance_map(vertex_count);

        detail::dijkstra_no_color_map_dispatch2(graph, start_vertex,
            choose_param(distance_map,
                make_iterator_property_map(default_distance_map.begin(),
                    index_map, default_distance_map[0])),
            weight_map, index_map, params);
    }
} // namespace detail

// Named parameter version
template < typename Graph, typename Param, typename Tag, typename Rest >
inline void dijkstra_shortest_paths_no_color_map(const Graph& graph,
    typename graph_traits< Graph >::vertex_descriptor start_vertex,
    const bgl_named_params< Param, Tag, Rest >& params)
{
    // Default for edge weight and vertex index map is to ask for them
    // from the graph. Default for the visitor is null_visitor.
    detail::dijkstra_no_color_map_dispatch1(graph, start_vertex,
        get_param(params, vertex_distance),
        choose_const_pmap(get_param(params, edge_weight), graph, edge_weight),
        choose_const_pmap(get_param(params, vertex_index), graph, vertex_index),
        params);
}

} // namespace boost

#endif // BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP


#include "plugins/featureeval/featureeval.h"
#include <iostream>
#include <QDebug>
#include <QAction>
#include <QToolBar>
#include <QtWidgets>
#include <QScrollArea>
#include <QDoubleSpinBox>

#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/octree/octree.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_container.h>
#include <boost/serialization/shared_ptr.hpp>

#include "model/layerlist.h"
#include "model/cloudlist.h"
#include "gui/glwidget.h"
#include "gui/flatview.h"
#include "gui/mainwindow.h"
#include "commands/select.h"
#include "pluginsystem/core.h"
#include "utilities/cv.h"
#include "utilities/utils.h"
#include "plugins/normalestimation/normalestimation.h"
#include "plugins/featureeval/utils.h"

QString FeatureEval::getName(){
    return "featureeval";
}

void FeatureEval::initialize(Core *core){
    settings_ = nullptr;
    tab_idx_ = -1;

    image_container_ = nullptr;
    image_ = nullptr;

    core_= core;
    cl_ = core_->cl_;
    ll_ = core_->ll_;
    glwidget_ = core_->mw_->glwidget_;
    flatview_ = core_->mw_->flatview_;
    mw_ = core_->mw_;
    report_ = nullptr;
}

void FeatureEval::initialize2(PluginManager * pm) {
    ne_ = pm->findPlugin<NormalEstimator>();
    if (ne_ == nullptr) {
        qDebug() << "Normal estimator plugin needed for normal viz";
        return;
    }

    // GUI STUFF
    visualise_on_ = true;
    function_idx_ = 0;
    layer_idx_ = -1;

    // PARAMS
    subsample_res_ = 0.1;
    subsample_res_2_ = 0.2;
    search_radius_ = 0.2;
    max_nn_ = 20;
    bins_ = 20;

    // Set up viz tab
    depth_widget_ = new QWidget(0);
    tab_idx_ = core_->mw_->addTab(depth_widget_, "Feature visualisation");

    QVBoxLayout * tablayout = new QVBoxLayout(depth_widget_);
    QScrollArea * scrollarea = new QScrollArea();
    scrollarea->setBackgroundRole(QPalette::Dark);
    tablayout->addWidget(scrollarea);
    image_container_ = new QLabel();
    scrollarea->setWidget(image_container_);

    // set up settings
    is_enabled_ = false;
    enable_ = new QAction(QIcon(":/images/featureeval.png"), "Correlate and visualise", 0);
    enable_->setCheckable(true);

    connect(enable_, SIGNAL(triggered()), this, SLOT(enable()));
    mw_->toolbar_->addAction(enable_);
    time = 0;
    settings_ = new QWidget();
    QVBoxLayout * layout = new QVBoxLayout(settings_);
    settings_->setLayout(layout);
    mw_->tooloptions_->addWidget(settings_);

    // settings widgets
    layout->addWidget(new QLabel("Feature"));
    feature_cb_ = new QComboBox(settings_);
    layout->addWidget(feature_cb_);

    // round up parameters
    param_map_["subsample_res"].f = &subsample_res_;
    param_map_["subsample_res.small"].f = &subsample_res_;
    param_map_["subsample_res.big"].f = &subsample_res_2_;
    param_map_["bins"].i =  &bins_;
    param_map_["search_radius"].f = &search_radius_;
    param_map_["max_nn"].i = &max_nn_;

    // round up functions
    feature_cb_->addItem("Difference of normals", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::difference_of_normals, this));
    name_to_function_["difference_of_normals"] = functions_[functions_.size()-1];

    feature_cb_->addItem("intensity_histogram", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::intensity_histogram, this));
    name_to_function_["intensity_histogram"] = functions_[functions_.size()-1];

    feature_cb_->addItem("Fast point feature histograms", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::fast_point_feature_histogram, this));
    name_to_function_["fast_point_feature_histogram"] = functions_[functions_.size()-1];

    feature_cb_->addItem("Curvature", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::curvature, this));
    name_to_function_["curvature"] = functions_[functions_.size()-1];

    feature_cb_->addItem("Distance standard deviation", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::distance_standard_deviation, this));
    name_to_function_["distance_standard_deviation"] = functions_[functions_.size()-1];

    feature_cb_->addItem("Normal standard deviation", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::normal_standard_deviation, this));
    name_to_function_["normal_standard_deviation"] = functions_[functions_.size()-1];

    feature_cb_->addItem("Eigen ratio", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::pca_eigen_value_ratio, this));
    name_to_function_["pca_eigen_value_ratio"] = functions_[functions_.size()-1];

    feature_cb_->addItem("PCA", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::pca, this));
    name_to_function_["pca"] = functions_[functions_.size()-1];

    feature_cb_->addItem("eigen_plane_consine_similarity", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::eigen_plane_consine_similarity, this));
    name_to_function_["eigen_plane_consine_similarity"] = functions_[functions_.size()-1];

    feature_cb_->addItem("intensity", (int)functions_.size());
    functions_.push_back(std::bind(&FeatureEval::intensity, this));
    name_to_function_["intensity"] = functions_[functions_.size()-1];

//    layout->addWidget(new QLabel("Correlate with layer:"));
//    layer_cb_ = new QComboBox(settings_);
//    layout->addWidget(layer_cb_);

    // Downsample settings
    QDoubleSpinBox * downsample_sb = new QDoubleSpinBox();
    downsample_sb->setAccelerated(true);
    downsample_sb->setMinimum(0.001);
    downsample_sb->setMaximum(1);
    downsample_sb->setValue(subsample_res_);
    connect(downsample_sb, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        subsample_res_ = value;
    });

    layout->addWidget(new QLabel("Downsample resolution (meters)"));
    layout->addWidget(downsample_sb);

    QDoubleSpinBox * downsample_sb2 = new QDoubleSpinBox();
    downsample_sb2->setAccelerated(true);
    downsample_sb2->setMinimum(0.001);
    downsample_sb2->setMaximum(1);
    downsample_sb2->setValue(subsample_res_2_);
    connect(downsample_sb2, static_cast<void(QDoubleSpinBox::*)(double)>(&QDoubleSpinBox::valueChanged), [=] (double value){
        subsample_res_2_ = value;
    });

    QLabel * sb2_label = new QLabel("Downsample resolution 2 (meters)");
    layout->addWidget(sb2_label);
    layout->addWidget(downsample_sb2);

    // feature change
    connect(feature_cb_, static_cast<void(QComboBox::*)(int)>(&QComboBox::currentIndexChanged), [=] (int idx){
        function_idx_ = feature_cb_->itemData(idx).toInt();
        sb2_label->setVisible(function_idx_ == 0);
        downsample_sb2->setVisible(function_idx_ == 0);
        qDebug() << "changed";
    });

    // Run button
    QPushButton * run = new QPushButton("Correlate and visualise");
    connect(run, &QPushButton::clicked, [=] (){
        if(function_idx_ != -1){
            qDebug() << "call!";
            functions_[function_idx_]();
        }

    });
    layout->addWidget(run);

    layout->addStretch();

    connect(ll_, SIGNAL(dataChanged(QModelIndex,QModelIndex)), this, SLOT(layersModified()));
}

void FeatureEval::layersModified(){
    if(layer_cb_ == nullptr)
        return;

    layer_cb_->clear();
    layer_cb_->addItem("Select", -1);

    layer_idx_ = -1;

    for(uint idx = 0; idx < ll_->getLayers().size(); idx++) {
        boost::shared_ptr<Layer> layer = ll_->getLayers()[idx];
        layer_cb_->addItem(layer->getName(), layer->getId());
    }
}

void FeatureEval::cleanup(){
    mw_->tooloptions_->removeWidget(settings_);
    mw_->toolbar_->removeAction(enable_);
    mw_->removeTab(tab_idx_);
    delete enable_;
    delete depth_widget_;
}

FeatureEval::~FeatureEval(){
    if(image_ != nullptr)
        delete image_;
}

std::function<void()> FeatureEval::getFunction(QString name){
    return name_to_function_[name];
}

void FeatureEval::setReportFuction(QDebug *dbg){
    report_ = dbg;
}

void FeatureEval::reportResult(float r2, float * correl, int correl_size){
    if(report_ == nullptr)
        return;

    *report_
             << scan_ << ", "
             << layer_ << ", "
             << fname_ << ", "
             << time_ << ", "
             << subsample_res_ << ", "
             << subsample_res_2_ << ", "
             << search_radius_ << ", "
             << max_nn_ << ", "
             << bins_ << ", "
             << r2;

    for(int idx = 0; idx < correl_size; idx++){
        *report_ << ", " << correl[idx];
    }

    *report_ << "\n";
}

void FeatureEval::resetParams(){
    subsample_res_ = -1;
    subsample_res_2_ = -1;
    search_radius_ = -1;
    max_nn_ = -1;
    bins_ = -1;
}

void FeatureEval::reportHeader(){
    *report_ << "\"scan\", \"layer\", \"feature\", \"time\", \"subsample_res_\", \"subsample_res_2_\", \"search_radius_ \", \"max_nn_\", \"bins_\", \"R^2\", \"component correlations\"\n";
}

void FeatureEval::enable() {
    if(is_enabled_){
        disable();
        return;
    }

    mw_->options_dock_->show();
    mw_->tooloptions_->setCurrentWidget(settings_);
    emit enabling();
    connect(core_, SIGNAL(endEdit()), this, SLOT(disable()));
    is_enabled_ = true;
}

void FeatureEval::disable(){
    enable_->setChecked(false);
    disconnect(core_, SIGNAL(endEdit()), this, SLOT(disable()));
    is_enabled_ = false;
}

int sum_calc(float * data, int vector_size, int size, float * sum, float * sum_of_squares, int stride = 0, int offset = 0){
    stride = stride ? stride : vector_size;
    int skipped = 0;
    for(int j = 0; j < vector_size; j++){
        sum[j] = 0.0f;
        sum_of_squares[j] = 0.0f;
    }

    float val = 0;
    for(int i = 0; i < size; i++){
        for(int j = 0; j < vector_size; j++){
            val = data[i*stride + j + offset];
            if(val != val){
                skipped++;
                continue;
            }
            sum[j] += val;
            sum_of_squares[j] += pow(val, 2);
        }
    }
    return skipped;
}

// uses y value for the last row of mat
void sum_productmat_calc(float * y_data, float * x_data, int x_vector_size, int size, Eigen::MatrixXf & sum_product, int stride = 0, int offset = 0){
    stride = stride ? stride : x_vector_size;
    sum_product.setIdentity();

    float val1 = 0, val2 = 0;
    for(int i = 0; i < size; i++){
        for(int r = 0; r < x_vector_size; r++){
            for(int c = r+1; c < x_vector_size; c++){
                val1 = (x_data[i*stride + r + offset]);
                val2 = (x_data[i*stride + c + offset]);

                if(val1 != val1 || val1 != val1){
                    continue;
                }

                sum_product(r, c) += val1*val2;
                sum_product(c, r) = sum_product(r, c);
            }

            sum_product(r, x_vector_size) += y_data[i] * x_data[i*stride + r + offset];
            sum_product(x_vector_size, r) = sum_product(r, x_vector_size);
        }
    }
}

inline float correlate(float sx, float sxx, float sy, float syy, float sxy, float n){
    //qDebug() << "sx: " << sx << "sxx: " << sxx << "sy: " << sy << "syy: " << syy << "sxy: " << sxy << "n: " << n;
    return (n*sxy-sx*sy)/sqrt((n*sxx-sx*sx) * (n*syy-sy*sy));
}

// blend the large segmentation to small!!!

// correlate binaryish with vector
Eigen::MatrixXf  multi_correlate(std::vector<float> & y_data, float * x_data, int x_vector_size, int size, int stride, int offset){
    std::vector<float> sum(x_vector_size + 1);
    std::vector<float> sum_of_squares(x_vector_size + 1);

    int skipped = sum_calc(x_data, x_vector_size, size, sum.data(), sum_of_squares.data(), stride, offset);
    skipped += sum_calc(y_data.data(), 1, size, &sum[x_vector_size], &sum_of_squares[x_vector_size]);

    //qDebug() << "skipped: " << skipped << "size: " << size;

    Eigen::MatrixXf sum_productmat(x_vector_size + 1, x_vector_size + 1);
    sum_productmat_calc(y_data.data(), x_data, x_vector_size, size, sum_productmat, stride, offset);

    Eigen::MatrixXf correlation_mat(x_vector_size + 1, x_vector_size + 1);
    correlation_mat.setIdentity();

    for(int r = 0; r < x_vector_size + 1; r++){
        for(int c = r+1; c < x_vector_size + 1; c++){
            // qDebug() << r << c << ": " <<  sum[c] << sum_of_squares[c] << sum[r] << sum_of_squares[r] << sum_productmat(r, c);
            correlation_mat(r, c) = correlate(sum[c], sum_of_squares[c], sum[r], sum_of_squares[r], sum_productmat(r, c), size-skipped);
            correlation_mat(c, r) = correlation_mat(r, c);
        }
    }

    return correlation_mat;
}

std::vector<float> get_scaled_layer_mask(
        std::vector<uint16_t> labels_,
        std::set<uint16_t> segment_labels,
        std::vector<int> & big_to_small,
        int small_size){

    std::vector<float> mask(small_size, 0.0f);
    std::vector<int> mask_count(small_size, 0);

    for(uint big_idx = 0; big_idx < big_to_small.size(); big_idx++){
        int small_idx = big_to_small[big_idx];
        mask_count[small_idx]++;
        uint16_t label = labels_[big_idx];
        if(segment_labels.find(label) != segment_labels.end())
            mask[small_idx]++;
    }

    for(int i = 0; i < small_size; i++){
        mask[i] = mask_count[i] == 0 ? 0.0f : mask[i]/mask_count[i];
    }

    return mask;
}

int gridToCloudIdx(int x, int y, boost::shared_ptr<PointCloud> pc, int * lookup){
    if(x < 0 || x > pc->scan_width())
        return -1;
    else if(y < 0 || y > pc->scan_height())
        return -1;
    return lookup[x + y*pc->scan_width()];
}

void FeatureEval::computeCorrelation(float * data, int vector_size, int size, std::vector<int> & big_to_small, int stride, int offset){

    stride = stride ? stride : vector_size;

    if(ll_->getSelection().size() == 0)
        return;

    std::set<uint16_t> labels;
    for(boost::weak_ptr<Layer> wlayer: ll_->getSelection()){
        for(uint16_t label : wlayer.lock()->getLabelSet())
            labels.insert(label);
    }

    std::vector<float> layer = get_scaled_layer_mask(cl_->active_->labels_,
                          labels,
                          big_to_small,
                          size);

    Eigen::MatrixXf correlation_mat = multi_correlate(layer, data, vector_size, size, stride, offset);
    Eigen::MatrixXf Rxx = correlation_mat.topLeftCorner(vector_size, vector_size);
    Eigen::VectorXf c = correlation_mat.block(0, vector_size, vector_size, 1);

    //std::cout << correlation_mat << std::endl;

    float R = c.transpose() * (Rxx.inverse() * c);

    qDebug() << "R^2: " << R;
    //qDebug() << "R: " << sqrt(R);

    reportResult(R, c.data(), vector_size);

    //Eigen::VectorXf tmp = (Rxx.inverse() * c);

    qDebug() << "Y -> X correlation <<<<<<<<<<<<<";
    std::cout << c << std::endl;
    //qDebug() << "Coefs <<<<<<<<<<<<<";
    //std::cout << tmp << std::endl;

}

void FeatureEval::drawFloats(boost::shared_ptr<const std::vector<float> > out_img, boost::shared_ptr<PointCloud> cloud){
    if(!visualise_on_)
        return;

    qDebug() << "DRAW!";
    // translates grid idx to cloud idx
    boost::shared_ptr<const std::vector<int>> lookup = cloud->gridToCloudMap();

    if(image_ != nullptr)
        delete image_;
    image_ = new QImage(cloud->scan_width(), cloud->scan_height(), QImage::Format_Indexed8);

    for(int i = 0; i < 256; i++) {
        image_->setColor(i, qRgb(i, i, i));
    }

    float min, max;
    min_max(*out_img, min, max);
    qDebug() << "Minmax" << min << max;

    // Draw image
    auto select = boost::make_shared<std::vector<int> >();
    for(int y = 0; y < cloud->scan_height(); y++){
        for(int x = 0; x < cloud->scan_width(); x++){
            int i = (cloud->scan_height() -1 - y) + x * cloud->scan_height();

            // Mask disabled
            if(lookup->at(i) == -2) {
                image_->setPixel(x, y, 0);
                continue;
            }

            //int intensity = 255 * (1 - distmap[i]/max_dist);
            float mag = (*out_img)[i];
            //int intensity = 255 * (1 - (mag - min)/(max - min));

            int intensity = 255 * (mag - min)/(max - min);

            if(intensity > 255 || intensity < 0) {
                qDebug() << "Nope, sorry > 255 || < 0: " << mag;
                qDebug() << "Mag: " << mag;
                qDebug() << "Intensity" << intensity;
                return;
            }

/*
            // Select
            if(lookup->at(i) != -1 && intensity > 100) {
                select->push_back(lookup->at(i));
            }

            if(intensity < 40) {
                intensity = 0;
            } else {
                intensity = 255;
            }
*/
            image_->setPixel(x, y, intensity);
        }
    }

    qDebug() << "Done";

    core_->us_->beginMacro("Experiment");
    core_->us_->push(new Select(cloud, select));
    core_->us_->endMacro();
    qDebug() << "Done1";
    image_container_->setPixmap(QPixmap::fromImage(*image_));
    qDebug() << "Done1.1";
    image_container_->resize(image_->size());
    qDebug() << "Done 2";

}

void FeatureEval::drawVector3f(boost::shared_ptr<const std::vector<Eigen::Vector3f> > out_img, boost::shared_ptr<PointCloud> cloud){
    if(!visualise_on_)
        return;

    // translates grid idx to cloud idx
    boost::shared_ptr<const std::vector<int>> lookup = cloud->gridToCloudMap();

    if(image_ != nullptr)
        delete image_;
    image_ = new QImage(cloud->scan_width(), cloud->scan_height(), QImage::Format_RGB32);

    // Draw image
    for(int y = 0; y < cloud->scan_height(); y++){
        for(int x = 0; x < cloud->scan_width(); x++){
            int i = (cloud->scan_height() -1 - y) + x * cloud->scan_height();

            // Mask disabled
            if(lookup->at(i) == -2) {
                image_->setPixel(x, y, 0);
                continue;
            }

            int r = (*out_img)[i][0] * 255;
            int g = (*out_img)[i][1] * 255;
            int b = (*out_img)[i][2] * 255;

            QRgb value = qRgb(r, g, b);

            image_->setPixel(x, y, value);
        }
    }

    image_container_->setPixmap(QPixmap::fromImage(*image_));
    image_container_->resize(image_->size());
}

// Kullback-Leibler distance

inline double KLDist(float * feature1, float * feature2, int size) {
    double kl = 0;

    for(int i = 0; i < size; i++){
        float p = feature1[i];
        float q = feature2[i];
        double tmp = p*log(p/q);

        if(tmp != tmp || tmp >= FLT_MAX || tmp <= FLT_MIN)
            continue;
        kl += tmp;
    }

    /*
    for(int i = 0; i < size; i++){
        std::cout << feature1[i] << "/" << feature2[i] << " ";
    }

    std::cout << std::endl << "feature:" << kl << std::endl;
    fflush(stdout);
    */
    return kl;
}

inline float EuclidianDist(float * feature1, float * feature2, int size) {
    float dist = 0;

    for(int i = 0; i < size; i++){
        dist += pow(feature1[i] - feature2[i], 2.0);
    }

    dist = sqrt(dist);

    if(dist != dist || dist >= FLT_MAX || dist <= FLT_MIN) {
        return 0;
    }

    return dist;
}

pcl::PointCloud<pcl::PointXYZINormal>::Ptr zipNormals(
        pcl::PointCloud<pcl::PointXYZI> & cloud,
        pcl::PointCloud<pcl::Normal> & normals){

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr zipped (new pcl::PointCloud<pcl::PointXYZINormal>());
    zipped->resize(cloud.size());

    for(uint i = 0; i < cloud.size(); i ++){
        pcl::PointXYZI & p = cloud[i];
        pcl::Normal & n = normals[i];

        pcl::PointXYZINormal & pn = (*zipped)[i];
        pn.getNormalVector4fMap() = n.getNormalVector4fMap();
        pn.getVector4fMap() = p.getVector4fMap();
        pn.intensity = p.intensity;
    }

    return zipped;
}

pcl::PointCloud<pcl::PointXYZINormal>::Ptr FeatureEval::gridDownsample(boost::shared_ptr<PointCloud> input, float resolution, std::vector<int>& sub_idxs) {
    pcl::PointCloud<pcl::Normal>::Ptr normals = ne_->getNormals(input);

    // HACK: Make sure normals are not NaN or inf
    for (pcl::Normal & n : *normals) {
      if (!pcl::isFinite<pcl::Normal>(n)){
          n.normal_x = 0;
          n.normal_y = 0;
          n.normal_z = 1;
      }
    }

    // Zipper up normals and xyzi
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = zipNormals(*input, *normals);

    t_.start(); //qDebug() << "Timer started (Subsample)";
    /////////////////////////

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>());
    sub_idxs.resize(input->size(), 0);

    pcl::VoxelGrid<pcl::PointXYZINormal> sor;
    sor.setInputCloud(cloud);
    sor.setLeafSize(resolution, resolution, resolution);
    sor.setDownsampleAllData (true);
    sor.setSaveLeafLayout(true);
    sor.filter(*output);

    for(uint i = 0; i < input->size(); i++) {
        pcl::PointXYZINormal &p = (*cloud)[i];
        int idx = sor.getCentroidIndex(p);
        sub_idxs[i] = idx;
    }

    /////////////////////////
    time = t_.elapsed();

    return output;

}

pcl::PointCloud<pcl::PointXYZINormal>::Ptr FeatureEval::downsample(
        boost::shared_ptr<PointCloud> input,
        float resolution,
        std::vector<int>& sub_idxs){

    pcl::PointCloud<pcl::Normal>::Ptr normals = ne_->getNormals(input);

    // HACK: Make sure normals are not NaN or inf
    for (pcl::Normal & n : *normals) {
      if (!pcl::isFinite<pcl::Normal>(n)){
          n.getNormalVector3fMap() << 0, 0, 1;
      }
    }

    // Zipper up normals and xyzi
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = zipNormals(*input, *normals);

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr output = octreeDownsample(cloud.get(), subsample_res_, sub_idxs);

    return output;

}

template<typename PointT, typename NormalT>
pcl::PointCloud<pcl::Normal>::Ptr don(
        pcl::PointCloud<PointT> & cloud,
        pcl::PointCloud<NormalT> & normals, float radius1 = 0.2,
        float radius2 = 0.5){

    pcl::PointCloud<pcl::Normal>::Ptr donormals;
    donormals.reset(new pcl::PointCloud<pcl::Normal>());
    donormals->resize(cloud.size());

    typename pcl::search::Search<PointT>::Ptr tree;
    tree.reset (new pcl::search::KdTree<PointT> (false));

    typename pcl::PointCloud<PointT>::ConstPtr cptr(&cloud, boost::serialization::null_deleter());
    tree->setInputCloud(cptr);

    std::vector<int> idxs;
    std::vector<float> sq_dists;

    auto avg_normal = [] (std::vector<int> idxs, pcl::PointCloud<NormalT> & normals) {
        pcl::Normal sum;
        for(int idx : idxs) {
            sum.getNormalVector3fMap() += normals[idx].getNormalVector3fMap();
        }
        sum.getNormalVector3fMap() /= idxs.size();
        return sum;
    };

    for(uint idx = 0; idx < cloud.size(); idx++){
        std::vector<float> rads;
        rads.push_back(radius1);
        rads.push_back(radius2);
        std::vector<pcl::Normal> avgs;
        for(float rad : rads){
            idxs.clear();
            sq_dists.clear();
            tree->radiusSearch(idx, rad, idxs, sq_dists);
            idxs.push_back(idx);
            avgs.push_back(avg_normal(idxs, normals));
        }

        (*donormals)[idx].getNormalVector3fMap() = (avgs[0].getNormalVector3fMap() - avgs[1].getNormalVector3fMap())/2.0;
    }

    return donormals;
}

void FeatureEval::difference_of_normals(){
    boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
    if(_cloud == nullptr)
        return;

    int w = _cloud->scan_width();
    int h = _cloud->scan_height();

    pcl::PointCloud<pcl::Normal>::Ptr normals = ne_->getNormals(_cloud);

    float res1 = subsample_res_;
    float res2 = subsample_res_2_;

    // Downsample
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr smaller_cloud;
    std::vector<int> sub_idxs;
    smaller_cloud = downsample(_cloud, res1, sub_idxs);

    t_.start();
    pcl::PointCloud<pcl::Normal>::Ptr donormals = don(*smaller_cloud, *smaller_cloud, res1, res2);
    (time_ = t_.elapsed());

    // Correlate
    computeCorrelation(reinterpret_cast<float*>(donormals->points.data()), 3, donormals->points.size(), sub_idxs, 4);
    if(!visualise_on_) return;

    // Draw
    pcl::PointCloud<pcl::Normal> big_donormals;
    map_small_to_big((*donormals).points, big_donormals.points, sub_idxs);

    const std::vector<int> & cloudtogrid = _cloud->cloudToGridMap();

    boost::shared_ptr<std::vector<Eigen::Vector3f> > grid = boost::make_shared<std::vector<Eigen::Vector3f> >(w*h, Eigen::Vector3f(0.0f, 0.0f, 0.0f));
    for(uint i = 0; i < normals->size(); i++){
        int grid_idx = cloudtogrid[i];
        (*grid)[grid_idx] = big_donormals[i].getNormalVector3fMap();
    }

    // Draw
    drawVector3f(grid, _cloud);
}

void FeatureEval::intensity(){
    boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
    if(_cloud == nullptr)
        return;

    // Subsample
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr smaller_cloud;
    std::vector<int> sub_idxs;
    smaller_cloud = downsample(_cloud, subsample_res_, sub_idxs);

    time_ = 0;

    // Copy to contigious structure
    std::vector<float> tmp(smaller_cloud->size());
    for(uint i = 0; i < smaller_cloud->size(); i++){
        tmp[i] = smaller_cloud->points[i].intensity;
    }

    // Correlate
    computeCorrelation(reinterpret_cast<float*>(tmp.data()), 1, tmp.size(), sub_idxs);
}

void FeatureEval::intensity_histogram(){
    boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
    if(_cloud == nullptr)
        return;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr smaller_cloud;
    std::vector<int> sub_idxs;
    smaller_cloud = downsample(_cloud, subsample_res_, sub_idxs);

    // Compute
    t_.start(); qDebug() << "Timer started (Intensity histogram)";
    boost::shared_ptr<std::vector<std::vector<float> > > hists = calcIntensityHist(*smaller_cloud, bins_, search_radius_, max_nn_);
    qDebug() << "Intensity histogram" << (time_ = t_.elapsed()) << " ms";

    // Copy to contigious structure
    std::vector<float> tmp(bins_*smaller_cloud->size());
    for(uint i = 0; i < smaller_cloud->size(); i++){
        for(int j = 0; j < bins_; j++){
            tmp[i*bins_+j] = (*hists)[i][j];
        }
    }

    // Correlate
    computeCorrelation(tmp.data(), bins_, tmp.size(), sub_idxs);
    if(!visualise_on_) return;

    // Draw, TODO; Multidimentional draw
    boost::shared_ptr<std::vector<std::vector<float> > > hists2 = boost::make_shared<std::vector<std::vector<float> > >(_cloud->size());

    // map back to cloud
    for(uint i = 0; i < _cloud->size(); i++) {
        int idx = sub_idxs[i];
        (*hists2)[i] = (*hists)[idx];
    }

    hists.reset();

    boost::shared_ptr<const std::vector<int>> grid_to_cloud = _cloud->gridToCloudMap();

    int w = _cloud->scan_width();
    int h = _cloud->scan_height();

    // in the grid, subtract (x, y+1) from every (x, y)
    boost::shared_ptr<std::vector<float>> diffs = boost::make_shared<std::vector<float>>(w*h, 0.0f);

    //auto distfunc = EuclidianDist;
    auto distfunc = KLDist;

    /*
    // Lambda
    auto maxval = [] (float * array, int size) {
        float max = FLT_MIN;
        for(int i = 0; i < size; i++) {
            if(array[i] > max)
                max = array[i];
        }
        return max;
    };
    */

    const int feature_size = bins_;

    for(int x = 1; x < w-1; x ++) {
        //qDebug() << "yes";
        for(int y = 1; y < h-1; y ++) {
            int center = (*grid_to_cloud)[x*h + y];
            int up = (*grid_to_cloud)[x*h + y-1];
            int down = (*grid_to_cloud)[x*h + y+1];
            int left = (*grid_to_cloud)[(x-1)*h + y];
            int right = (*grid_to_cloud)[(x+1)*h + y];

            (*diffs)[center] = 0;

            if(center == -1){
                continue;
            }

            // NB! assumed histograms are normalised

            if(up != -1 && down != -1){
                float * feature1 = &((*hists2)[up][0]);
                float * feature2 = &((*hists2)[down][0]);
                //float max1 = maxval(feature1, feature_size);
                //float max2 = maxval(feature2, feature_size);
                //(*diffs)[center] += fabs(max1 - max2);
                (*diffs)[center] += distfunc(feature1, feature2, feature_size);
            }

            if(left != -1 && right != -1){
                float * feature1 = &((*hists2)[left][0]);
                float * feature2 = &((*hists2)[right][0]);
                //float max1 = maxval(feature1, feature_size);
                //float max2 = maxval(feature2, feature_size);
                //(*diffs)[center] += fabs(max1 - max2);
                (*diffs)[center] += distfunc(feature1, feature2, feature_size);
            }

        }
    }

    boost::shared_ptr<const std::vector<float>> img = cloudToGrid(_cloud->cloudToGridMap(), w*h, diffs);

    drawFloats(img, _cloud);
    hists2.reset();
}

void FeatureEval::fast_point_feature_histogram(){
    qDebug() << "params used: " << subsample_res_ << search_radius_;
    qDebug() << "-----------------------------------";
    boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
    if(_cloud == nullptr)
        return;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr filt_cloud;
    std::vector<int> sub_idxs;
    filt_cloud = downsample(_cloud, subsample_res_, sub_idxs);

    // Compute
    t_.start(); qDebug() << "Timer started (FPFH)";
    pcl::FPFHEstimation<pcl::PointXYZINormal, pcl::PointXYZINormal, pcl::FPFHSignature33> fpfh;
    fpfh.setInputCloud(filt_cloud);
    fpfh.setInputNormals(filt_cloud);
    pcl::search::KdTree<pcl::PointXYZINormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZINormal>);
    fpfh.setSearchMethod(tree);
    fpfh.setRadiusSearch(search_radius_);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
    fpfh.compute(*fpfhs);
    qDebug() << "FPFH in " << (time_ = t_.elapsed()) << " ms";

    // Correlate
    computeCorrelation(reinterpret_cast<float*>(fpfhs->points.data()), 33, fpfhs->points.size(), sub_idxs);
    if(!visualise_on_) return;
}

void FeatureEval::curvature(){
    boost::shared_ptr<PointCloud> _cloud = core_->cl_->active_;
    if(_cloud == nullptr)
        return;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr filt_cloud;
    std::vector<int> big_to_small;
    filt_cloud = downsample(_cloud, subsample_res_, big_to_small);

    // Compute
    t_.start(); qDebug() << "Timer started (Curvature)";

    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZINormal, pcl::PointXYZINormal, pcl::PrincipalCurvatures> principal_curvatures_estimation;
    principal_curvatures_estimation.setInputCloud (filt_cloud);
    principal_curvatures_estimation.setInputNormals (filt_cloud);
    pcl::search::KdTree<pcl::PointXYZINormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZINormal>);
    principal_curvatures_estimation.setSearchMethod (tree);
    principal_curvatures_estimation.setRadiusSearch (search_radius_);
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
    principal_curvatures_estimation.compute (*principal_curvatures);
    qDebug() << "Curvature in " << (time_ = t_.elapsed()) << " ms";

    // Correlate
    size_t csize = sizeof(pcl::PrincipalCurvatures)/sizeof(float);
    computeCorrelation(reinterpret_cast<float*>(principal_curvatures->points.data()), 2, principal_curvatures->points.size(), big_to_small, csize, 3);
    if(!visualise_on_) return;

    // Draw
    int w = _cloud->scan_width();
    int h = _cloud->scan_height();

    boost::shared_ptr<std::vector<float>> grid = boost::make_shared<std::vector<float>>(w*h, 0.0f);
    const std::vector<int> & cloudtogrid = _cloud->cloudToGridMap();

    for(uint big_idx = 0; big_idx < _cloud->size(); big_idx++) {
        int small_idx = big_to_small[big_idx];
        int grid_idx = cloudtogrid[big_idx];
        pcl::PrincipalCurvatures & pc = (*principal_curvatures)[small_idx];
        (*grid)[grid_idx] = pc.pc1 + pc.pc2;
    }

    drawFloats(grid, _cloud);

}

void FeatureEval::normal_standard_deviation(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

    pcl::PointCloud<pcl::Normal>::Ptr normals = ne_->getNormals(cloud);

    // Downsample and zipper up normals and xyzi
    std::vector<int> sub_idxs;
    pcl::PointCloud<pcl::PointXYZINormal>::Ptr smaller_cloud = zipNormals(cloud, normals);
    smaller_cloud = octreeDownsample(smaller_cloud.get(), subsample_res_, sub_idxs);

    // Compute
    t_.start(); qDebug() << "Timer started (Normal stdev)";
    boost::shared_ptr<std::vector<float>> stdev = normal_stdev<pcl::PointXYZINormal, pcl::PointXYZINormal>(smaller_cloud, smaller_cloud, search_radius_, max_nn_);
    qDebug() << "Normal stdev in " << (time_ = t_.elapsed()) << " ms";

    // Correlate
    computeCorrelation(stdev->data(), 1, stdev->size(), sub_idxs);
    if(!visualise_on_) return;

    // Draw
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    boost::shared_ptr<std::vector<float>> grid = boost::make_shared<std::vector<float>>(w*h, 0.0f);
    const std::vector<int> & cloudtogrid = cloud->cloudToGridMap();

    for(uint big_idx = 0; big_idx < cloud->size(); big_idx++) {
        int small_idx = sub_idxs[big_idx];
        int grid_idx = cloudtogrid[big_idx];
        (*grid)[grid_idx] = (*stdev)[small_idx];
    }
    drawFloats(grid, cloud);
}

void FeatureEval::distance_standard_deviation(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

    // Downsample
    std::vector<int> sub_idxs;
    pcl::PointCloud<pcl::PointXYZI>::Ptr smaller_cloud = octreeDownsample(cloud.get(), subsample_res_, sub_idxs);

    t_.start(); qDebug() << "Timer started (Dist stdev)";
    boost::shared_ptr<std::vector<float> > stdev = stdev_dist(smaller_cloud, search_radius_, max_nn_, false);
    qDebug() << "Dist stdev in " << t_.elapsed() << " ms";

    // Correlate
    computeCorrelation(stdev->data(), 1, stdev->size(), sub_idxs);
    if(!visualise_on_) return;

    // Draw
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    boost::shared_ptr<std::vector<float>> grid = boost::make_shared<std::vector<float>>(w*h, 0.0f);
    const std::vector<int> & cloudtogrid = cloud->cloudToGridMap();

    for(uint big_idx = 0; big_idx < cloud->size(); big_idx++) {
        int small_idx = sub_idxs[big_idx];
        int grid_idx = cloudtogrid[big_idx];
        (*grid)[grid_idx] = (*stdev)[small_idx];
    }

    drawFloats(grid, cloud);
}

// TODO: THIS IS A 2D FEATURE CALCULATION! help!
// Compute the distance for each point
// Gaussian weighted average in neighbourhood
// Subtract
void FeatureEval::difference_of_gaussian_distances(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    // Create distance map
    boost::shared_ptr<std::vector<float>> distmap = makeDistmap(cloud);

    //distmap = interpolate(distmap, w, h, 21);

    boost::shared_ptr<std::vector<float> > smooth_grad_image = convolve(distmap, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);

    boost::shared_ptr<std::vector<float>> highfreq = distmap;

    for(uint i = 0; i < distmap->size(); i++){
        (*highfreq)[i] = (*distmap)[i] - (*smooth_grad_image)[i];
    }

    drawFloats(highfreq, cloud);
}

// PCA for each point, ratio of eigen values
void FeatureEval::pca_eigen_value_ratio(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

    // Downsample
    std::vector<int> sub_idxs;
    pcl::PointCloud<pcl::PointXYZI>::Ptr smaller_cloud = octreeDownsample(cloud.get(), subsample_res_, sub_idxs);

    // Compute PCA
    t_.start(); qDebug() << "Timer started (PCA and speculaion)";
    boost::shared_ptr<std::vector<Eigen::Vector3f> > pca = getPCA(smaller_cloud.get(), search_radius_, max_nn_);

    // Compute ratio
    boost::shared_ptr<std::vector<float>> likely_veg =
               boost::make_shared<std::vector<float>>(pca->size(), 0.0f);

    for(uint i = 0; i < pca->size(); i++) {
        Eigen::Vector3f eig = (*pca)[i];

        // Not enough neighbours
        if(eig[1] < eig[2]) {
            (*likely_veg)[i] = 0;
            continue;
        }

        /*
        float eig_sum = eig.sum();
        eig /= eig_sum;
        float fudge_factor = 5.0f;

        bool is_planar = eig[1] < 0.05 * fudge_factor || eig[2] < 0.01 * fudge_factor;

        if(!is_planar) {
            (*likely_veg)[i] = 0;
        } else {
            (*likely_veg)[i] = 1;
        }

        */

        float curvature;
        if(eig[0] > eig[2])
            curvature = eig[1] / (eig[0]);
        else
            curvature = 1;

        (*likely_veg)[i] = curvature;

    }

    qDebug() << "PCA in " << (time_ = t_.elapsed()) << " ms";

    computeCorrelation(likely_veg->data(), 1, likely_veg->size(), sub_idxs);
    if(!visualise_on_) return;

    // Draw
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    boost::shared_ptr<std::vector<float>> likely_veg2 =
               boost::make_shared<std::vector<float>>(cloud->size(), 0.0f);

    for(uint i = 0; i < cloud->size(); i++) {
        uint subidx = sub_idxs[i];
        (*likely_veg2)[i] = (*likely_veg)[subidx];
    }

    boost::shared_ptr<const std::vector<float>> img = cloudToGrid(cloud->cloudToGridMap(), w*h, likely_veg2);

    drawFloats(img, cloud);
}

void FeatureEval::pca(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

    std::vector<int> sub_idxs;
    pcl::PointCloud<pcl::PointXYZI>::Ptr smaller_cloud = octreeDownsample(cloud.get(), subsample_res_, sub_idxs);

    t_.start(); qDebug() << "Timer started (PCA)";
    boost::shared_ptr<std::vector<Eigen::Vector3f> > pca = getPCA(smaller_cloud.get(), search_radius_, max_nn_);
    qDebug() << "PCA in " << (time_ = t_.elapsed()) << " ms";

    computeCorrelation(reinterpret_cast<float *>(pca->data()), 3, pca->size(), sub_idxs, sizeof(Eigen::Vector3f)/sizeof(float));
    if(!visualise_on_) return;

//    // Draw
//    boost::shared_ptr<std::vector<Eigen::Vector3f> > grid = boost::make_shared<std::vector<Eigen::Vector3f> >(grid_to_cloud->size(), Eigen::Vector3f(0.0f, 0.0f, 0.0f));
//    for(int i = 0; i < grid_to_cloud->size(); i++) {
//        int idx = (*grid_to_cloud)[i];
//        if(idx != -1)
//            (*grid)[i] = (*pca)[idx];
//    }

//    drawVector3f(grid, cloud);

}

void FeatureEval::eigen_plane_consine_similarity(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;

    std::vector<int> sub_idxs;
    pcl::PointCloud<pcl::PointXYZI>::Ptr smaller_cloud = octreeDownsample(cloud.get(), subsample_res_, sub_idxs);

    t_.start(); qDebug() << "Timer started (eigen_plane_consine_similarity)";
    boost::shared_ptr<std::vector<Eigen::Vector3f> > pca = getPCA(smaller_cloud.get(), 0.05f, 20);

    boost::shared_ptr<std::vector<float>> plane_likelyhood =
               boost::make_shared<std::vector<float>>(pca->size(), 0.0f);

    Eigen::Vector3f ideal_plane(1.0f, 0.0f, 0.0f);
    ideal_plane.normalize();

    for(uint i = 0; i < pca->size(); i++) {
        Eigen::Vector3f & val = (*pca)[i];

        // Not enough neighbours
        if(val[1] < val[2]) {
            (*plane_likelyhood)[i] = 0;
            continue;
        }

        float similarity = cosine(val, ideal_plane);
        (*plane_likelyhood)[i] = similarity;
    }

    qDebug() << "eigen_plane_consine_similarity in " << (time_ = t_.elapsed()) << " ms";

    // Correlate
    computeCorrelation(plane_likelyhood->data(), 1, plane_likelyhood->size(), sub_idxs);
    if(!visualise_on_) return;

    // Draw
    int h = cloud->scan_width();
    int w = cloud->scan_height();
/*
    boost::shared_ptr<std::vector<Eigen::Vector3f> > grid = boost::make_shared<std::vector<Eigen::Vector3f> >(grid_to_cloud->size(), Eigen::Vector3f(0.0f, 0.0f, 0.0f));
    for(int i = 0; i < grid_to_cloud->size(); i++) {
        int idx = (*grid_to_cloud)[i];
        if(idx != -1)
            (*grid)[i] = (*pca)[idx];
    }

    drawVector3f(grid, cloud);
*/

    boost::shared_ptr<const std::vector<float>> img = cloudToGrid(cloud->cloudToGridMap(), w*h, plane_likelyhood);

    drawFloats(img, cloud);
}

void FeatureEval::sobel_erode(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    // Create distance map
    boost::shared_ptr<std::vector<float>> distmap = makeDistmap(cloud);

    boost::shared_ptr<std::vector<float> > grad_image = gradientImage(distmap, w, h);
    boost::shared_ptr<std::vector<float> > smooth_grad_image = convolve(grad_image, w, h, gaussian, 5);

    // Threshold && Erode

    const int strct[] = {
        0, 1, 0,
        1, 0, 1,
        0, 1, 0,
    };

    boost::shared_ptr<std::vector<float> > dilated_image =  morphology(
            smooth_grad_image,
            w, h, strct, 3, Morphology::ERODE,
            grad_image); // <-- reuse

    drawFloats(dilated_image, cloud);
}

void FeatureEval::sobel_blur(){
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    // Create distance map
    boost::shared_ptr<std::vector<float>> distmap = makeDistmap(cloud);

    boost::shared_ptr<std::vector<float> > smooth_grad_image = gradientImage(distmap, w, h);
    int blurs = 0;
    for(int i = 0; i < blurs; i++)
        smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);

    drawFloats(smooth_grad_image, cloud);
}

void FeatureEval::blurred_intensity() {
    boost::shared_ptr<PointCloud> cloud = core_->cl_->active_;
    if(cloud == nullptr)
        return;
    int h = cloud->scan_width();
    int w = cloud->scan_height();

    boost::shared_ptr<std::vector<float>> intensity = boost::make_shared<std::vector<float>>(cloud->size());

    // Create intensity cloud
    for(uint i = 0; i < intensity->size(); i++){
        (*intensity)[i] = (*cloud)[i].intensity;
    }

    boost::shared_ptr<std::vector<float>> img = cloudToGrid(cloud->cloudToGridMap(), w*h, intensity);

    boost::shared_ptr<std::vector<float> > smooth_grad_image = convolve(img, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);
    smooth_grad_image = convolve(smooth_grad_image, w, h, gaussian, 5);
/*
    boost::shared_ptr<std::vector<float>> highfreq = img;

    for(int i = 0; i < highfreq->size(); i++){
        (*highfreq)[i] = (*highfreq)[i] - (*smooth_grad_image)[i];
    }

    drawFloats(highfreq, cloud);
*/
    drawFloats(smooth_grad_image, cloud);

}

Q_PLUGIN_METADATA(IID "za.co.circlingthesun.cloudclean.featureeval")


// SPDX-License-Identifier: NASA-1.3
/*******************************************************************************
 *
 * Data structures for the symbolic manipulation of linear constraints.
 *
 * Author: Arnaud J. Venet (arnaud.j.venet@nasa.gov)
 * Contributor: Jorge A. Navas (jorge.navas@sri.com)
 *
 * Notices:
 *
 * Copyright (c) 2011 United States Government as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All Rights Reserved.
 *
 * Disclaimers:
 *
 * No Warranty: THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF
 * ANY KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT LIMITED
 * TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO SPECIFICATIONS,
 * ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE,
 * OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL BE
 * ERROR FREE, OR ANY WARRANTY THAT DOCUMENTATION, IF PROVIDED, WILL CONFORM TO
 * THE SUBJECT SOFTWARE. THIS AGREEMENT DOES NOT, IN ANY MANNER, CONSTITUTE AN
 * ENDORSEMENT BY GOVERNMENT AGENCY OR ANY PRIOR RECIPIENT OF ANY RESULTS,
 * RESULTING DESIGNS, HARDWARE, SOFTWARE PRODUCTS OR ANY OTHER APPLICATIONS
 * RESULTING FROM USE OF THE SUBJECT SOFTWARE.  FURTHER, GOVERNMENT AGENCY
 * DISCLAIMS ALL WARRANTIES AND LIABILITIES REGARDING THIRD-PARTY SOFTWARE,
 * IF PRESENT IN THE ORIGINAL SOFTWARE, AND DISTRIBUTES IT "AS IS."
 *
 * Waiver and Indemnity:  RECIPIENT AGREES TO WAIVE ANY AND ALL CLAIMS AGAINST
 * THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS, AS WELL
 * AS ANY PRIOR RECIPIENT.  IF RECIPIENT'S USE OF THE SUBJECT SOFTWARE RESULTS
 * IN ANY LIABILITIES, DEMANDS, DAMAGES, EXPENSES OR LOSSES ARISING FROM SUCH
 * USE, INCLUDING ANY DAMAGES FROM PRODUCTS BASED ON, OR RESULTING FROM,
 * RECIPIENT'S USE OF THE SUBJECT SOFTWARE, RECIPIENT SHALL INDEMNIFY AND HOLD
 * HARMLESS THE UNITED STATES GOVERNMENT, ITS CONTRACTORS AND SUBCONTRACTORS,
 * AS WELL AS ANY PRIOR RECIPIENT, TO THE EXTENT PERMITTED BY LAW.
 * RECIPIENT'S SOLE REMEDY FOR ANY SUCH MATTER SHALL BE THE IMMEDIATE,
 * UNILATERAL TERMINATION OF THIS AGREEMENT.
 *
 ******************************************************************************/

#pragma once

#include <optional>
#include <utility>
#include <vector>

#include <boost/container/flat_map.hpp>
#include <boost/functional/hash.hpp>

#include "crab/variable.hpp"
#include "crab_utils/patricia_trees.hpp"

namespace crab {

class linear_expression_t final {
  private:
    using map_t = boost::container::flat_map<variable_t, number_t>;
    using pair_t = typename map_t::value_type;

    const map_t _map;
    const number_t _cst = 0;

    linear_expression_t(map_t map, number_t cst) : _map(std::move(map)), _cst(std::move(cst)) {}

    static void add(map_t& map, variable_t x, const number_t& n) {
        typename map_t::iterator it = map.find(x);
        if (it != map.end()) {
            number_t r = it->second + n;
            if (r == 0) {
                map.erase(it);
            } else {
                it->second = r;
            }
        } else {
            if (n != 0) {
                map.insert(pair_t(x, n));
            }
        }
    }

  public:
    using iterator = typename map_t::iterator;
    using const_iterator = typename map_t::const_iterator;

    linear_expression_t() = default;

    linear_expression_t(linear_expression_t&& other) = default;
    linear_expression_t(const linear_expression_t& other) = default;

    explicit linear_expression_t(number_t n) : _cst(std::move(n)) {}

    linear_expression_t(signed long long int n) : _cst(number_t(n)) {}

    linear_expression_t(variable_t x) : _map{ {x, number_t(1)} } {
    }

    linear_expression_t(const number_t& n, variable_t x) : _map{ {x, n} } {
    }

    [[nodiscard]] const_iterator begin() const { return this->_map.begin(); }

    [[nodiscard]] const_iterator end() const { return this->_map.end(); }

    [[nodiscard]] size_t hash() const {
        size_t res = 0;
        for (const auto& p : *this) {
            boost::hash_combine(res, p);
        }
        boost::hash_combine(res, _cst);
        return res;
    }

    // syntactic equality
    [[nodiscard]] bool equal(const linear_expression_t& o) const {
        if (is_constant()) {
            return o.is_constant() && constant() == o.constant();
        }
        if (constant() != o.constant() || size() != o.size()) {
            return false;
        }
        for (const_iterator it = begin(), jt = o.begin(), et = end(); it != et; ++it, ++jt) {
            if (it->first != jt->first || it->second != jt->second) {
                return false;
            }
        }
        return true;
    }

    [[nodiscard]] bool is_constant() const { return (this->_map.empty()); }

    [[nodiscard]] number_t constant() const { return this->_cst; }

    [[nodiscard]] std::size_t size() const { return this->_map.size(); }

    number_t operator[](variable_t x) const {
        typename map_t::const_iterator it = this->_map.find(x);
        if (it != this->_map.end()) {
            return it->second;
        } else {
            return 0;
        }
    }

    linear_expression_t operator+(const number_t& n) const {
        linear_expression_t r(this->_map, this->_cst + n);
        return r;
    }

    linear_expression_t operator+(int n) const { return this->operator+(number_t(n)); }

    linear_expression_t operator+(variable_t x) const {
        map_t map = this->_map;
        add(map, x, number_t(1));
        return linear_expression_t(map, this->_cst);
    }

    linear_expression_t operator+(const linear_expression_t& e) const {
        map_t map = this->_map;
        for (auto [k, v] : e._map) {
            add(map, k, v);
        }
        return linear_expression_t(map, this->_cst + e._cst);
    }

    linear_expression_t operator-(const number_t& n) const { return this->operator+(-n); }

    linear_expression_t operator-(int n) const { return this->operator+(-number_t(n)); }

    linear_expression_t operator-(variable_t x) const {
        map_t map = this->_map;
        add(map, x, number_t(-1));
        return linear_expression_t(map, this->_cst);
    }

    linear_expression_t operator-() const { return this->operator*(number_t(-1)); }

    linear_expression_t operator-(const linear_expression_t& e) const {
        map_t map = this->_map;
        for (auto [k ,v] : e._map) {
            add(map, k, -v);
        }
        return linear_expression_t(map, this->_cst - e._cst);
    }

    linear_expression_t operator*(const number_t& n) const {
        if (n == 0) {
            return linear_expression_t();
        } else {
            map_t map;
            for (auto [k, v] : _map) {
                number_t c = n * v;
                if (c != 0) {
                    map.insert(pair_t(k, c));
                }
            }
            return linear_expression_t(map, n * this->_cst);
        }
    }

    linear_expression_t operator*(int n) const { return operator*(number_t(n)); }

    friend std::ostream& operator<<(std::ostream& o, const linear_expression_t& e) {
        bool start = true;
        for (auto [v, n] : e) {
            if (n > 0 && !start) {
                o << "+";
            }
            if (n == -1) {
                o << "-";
            } else if (n != 1) {
                o << n << "*";
            }
            o << v;
            start = false;
        }
        if (e._cst > 0 && !e._map.empty()) {
            o << "+";
        }
        if (e._cst != 0 || e._map.empty()) {
            o << e._cst;
        }
        return o;
    }
}; // class linear_expression_t

inline std::size_t hash_value(const linear_expression_t& e) { return e.hash(); }

enum class cst_kind { EQUALITY, DISEQUATION, INEQUALITY, STRICT_INEQUALITY };

class linear_constraint_t final {

  public:
    using iterator = typename linear_expression_t::iterator;
    using const_iterator = typename linear_expression_t::const_iterator;

  private:
    const cst_kind _kind;
    const linear_expression_t _expr;
    // This flag has meaning only if _kind == INEQUALITY or STRICT_INEQUALITY.
    // If true the inequality is signed otherwise unsigned.
    // By default all constraints are signed.
    const bool _signedness;

  public:
    // linear_constraint_t() : _kind(EQUALITY), _signedness(true) {}
    linear_constraint_t(linear_constraint_t&& other) = default;
    linear_constraint_t(const linear_constraint_t& other) = default;

    linear_constraint_t(linear_expression_t expr, cst_kind kind)
        : _kind(kind), _expr(std::move(expr)), _signedness(true) {}

    linear_constraint_t(linear_expression_t expr, cst_kind kind, bool signedness)
        : _kind(kind), _expr(std::move(expr)), _signedness(signedness) {
        if (_kind != cst_kind::INEQUALITY && _kind != cst_kind::STRICT_INEQUALITY) {
            CRAB_ERROR("Only inequalities can have signedness information");
        }
    }

    static linear_constraint_t get_true() {
        linear_constraint_t res(linear_expression_t(number_t(0)), cst_kind::EQUALITY);
        return res;
    }

    static linear_constraint_t get_false() {
        linear_constraint_t res(linear_expression_t(number_t(0)), cst_kind::DISEQUATION);
        return res;
    }

    [[nodiscard]] bool is_tautology() const {
        if (!this->_expr.is_constant())
            return false;
        const number_t c = this->_expr.constant();
        switch (this->_kind) {
        case cst_kind::DISEQUATION: return c != 0;
        case cst_kind::EQUALITY: return c == 0;
        case cst_kind::INEQUALITY: return c <= 0;
        case cst_kind::STRICT_INEQUALITY: return c < 0;
        default: CRAB_ERROR("Unreachable");
        }
    }

    [[nodiscard]] bool is_contradiction() const {
        if (!this->_expr.is_constant())
            return false;
        const number_t c = this->_expr.constant();
        switch (this->_kind) {
        case cst_kind::DISEQUATION: return c == 0;
        case cst_kind::EQUALITY: return c != 0;
        case cst_kind::INEQUALITY: return c > 0;
        case cst_kind::STRICT_INEQUALITY: return c >= 0;
        default: CRAB_ERROR("Unreachable");
        }
    }

    [[nodiscard]] bool is_inequality() const { return (this->_kind == cst_kind::INEQUALITY); }

    [[nodiscard]] bool is_strict_inequality() const { return (this->_kind == cst_kind::STRICT_INEQUALITY); }

    [[nodiscard]] bool is_equality() const { return (this->_kind == cst_kind::EQUALITY); }

    [[nodiscard]] bool is_disequation() const { return (this->_kind == cst_kind::DISEQUATION); }

    [[nodiscard]] const linear_expression_t& expression() const { return this->_expr; }

    [[nodiscard]] cst_kind kind() const { return this->_kind; }

    [[nodiscard]] bool is_signed() const {
        if (_kind != cst_kind::INEQUALITY && _kind != cst_kind::STRICT_INEQUALITY) {
            CRAB_WARN("Only inequalities have signedness");
        }
        return _signedness;
    }

    [[nodiscard]] bool is_unsigned() const { return (!is_signed()); }

    [[nodiscard]] const_iterator begin() const { return this->_expr.begin(); }

    [[nodiscard]] const_iterator end() const { return this->_expr.end(); }

    [[nodiscard]] std::size_t size() const { return this->_expr.size(); }

    // syntactic equality
    [[nodiscard]] bool equal(const linear_constraint_t& o) const {
        return (_kind == o._kind && _signedness == o._signedness && _expr.equal(o._expr));
    }

    [[nodiscard]] size_t hash() const {
        size_t res = 0;
        boost::hash_combine(res, _expr);
        boost::hash_combine(res, _kind);
        if (_kind == cst_kind::INEQUALITY || _kind == cst_kind::STRICT_INEQUALITY) {
            boost::hash_combine(res, _signedness);
        }
        return res;
    }

    number_t operator[](variable_t x) const { return this->_expr.operator[](x); }

    [[nodiscard]] linear_constraint_t negate() const {
        if (is_tautology()) {
            return get_false();
        } else if (is_contradiction()) {
            return get_true();
        } else {
            switch (kind()) {
            case cst_kind::INEQUALITY: {
                // try to take advantage if we use z_number_t.
                // negate(e <= 0) = e >= 1
                return linear_constraint_t(-(expression() - 1), cst_kind::INEQUALITY, is_signed());
            }
            case cst_kind::STRICT_INEQUALITY: {
                // negate(x + y < 0)  <-->  x + y >= 0 <--> -x -y <= 0
                linear_expression_t e = -this->_expr;
                return linear_constraint_t(e, cst_kind::INEQUALITY, is_signed());
            }
            case cst_kind::EQUALITY: return linear_constraint_t(this->_expr, cst_kind::DISEQUATION);
            case cst_kind::DISEQUATION: return linear_constraint_t(this->_expr, cst_kind::EQUALITY);
            default: CRAB_ERROR("Cannot negate linear constraint");
            }
        }
    }

    friend std::ostream& operator<<(std::ostream& o, const linear_constraint_t& cst) {
        if (cst.is_contradiction()) {
            o << "false";
        } else if (cst.is_tautology()) {
            o << "true";
        } else {
            linear_expression_t e = cst._expr - cst._expr.constant();
            o << e;
            switch (cst._kind) {
            case cst_kind::INEQUALITY: {
                if (cst.is_signed()) {
                    o << " <= ";
                } else {
                    o << " <=_u ";
                }
                break;
            }
            case cst_kind::STRICT_INEQUALITY: {
                if (cst.is_signed()) {
                    o << " < ";
                } else {
                    o << " <_u ";
                }
                break;
            }
            case cst_kind::EQUALITY: {
                o << " = ";
                break;
            }
            case cst_kind::DISEQUATION: {
                o << " != ";
                break;
            }
            }
            number_t c = -cst._expr.constant();
            o << c;
        }
        return o;
    }
}; // class linear_constraint_t

inline std::size_t hash_value(const linear_constraint_t& e) { return e.hash(); }

inline linear_expression_t var_sub(variable_t x, const number_t& n) { return linear_expression_t(x).operator-(n); }
inline linear_expression_t var_sub(variable_t x, variable_t y) { return linear_expression_t(x).operator-(y); }
inline linear_expression_t var_add(variable_t x, const number_t& n) { return linear_expression_t(x).operator+(n); }
inline linear_expression_t var_add(variable_t x, variable_t y) { return linear_expression_t(x).operator+(y); }
inline linear_expression_t var_mul(const number_t& n, variable_t x) { return linear_expression_t(n, x); }
} // namespace crab


/**
 * @file
 * This file is part of SeisSol.
 *
 * @author Carsten Uphoff (c.uphoff AT tum.de,
 *http://www5.in.tum.de/wiki/index.php/Carsten_Uphoff,_M.Sc.)
 * @author Sebastian Wolf (wolf.sebastian AT in.tum.de,
 *https://www5.in.tum.de/wiki/index.php/Sebastian_Wolf,_M.Sc.)
 *
 * @section LICENSE
 * Copyright (c) 2017 - 2020, SeisSol Group
 * 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 copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from this
 *    software without specific prior written permission.
 *
 * 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.
 *
 * @section DESCRIPTION
 *
 **/
#include <Eigen/Eigenvalues>
#include <Kernels/precision.hpp>
#include <Initializer/typedefs.hpp>

#include <PUML/PUML.h>
#include <PUML/Downward.h>
#include <PUML/Upward.h>
#include "LtsWeights.h"
#include "WeightsModels.h"

#include <Initializer/ParameterDB.h>
#include <Parallel/MPI.h>

#include <generated_code/init.h>

namespace seissol::initializers::time_stepping {

class FaceSorter {
  private:
  std::vector<PUML::TETPUML::face_t> const& m_faces;

  public:
  FaceSorter(std::vector<PUML::TETPUML::face_t> const& faces) : m_faces(faces) {}

  bool operator()(unsigned int a, unsigned int b) const {
    return m_faces[a].gid() < m_faces[b].gid();
  }
};

void LtsWeights::computeNodeWeights(PUML::TETPUML const& mesh, double maximumAllowedTimeStep) {
  logInfo(seissol::MPI::mpi.rank()) << "Computing LTS weights.";

  // Note: Return value optimization is guaranteed while returning temp. objects in C++17
  m_mesh = &mesh;
  m_details = collectGlobalTimeStepDetails(maximumAllowedTimeStep);
  m_clusterIds = computeClusterIds();
  m_ncon = evaluateNumberOfConstraints();
  auto totalNumberOfReductions = enforceMaximumDifference();
  m_cellCosts = computeCostsPerTimestep();

  if (!m_vertexWeights.empty()) {
    m_vertexWeights.clear();
  }
  m_vertexWeights.resize(m_clusterIds.size() * m_ncon);

  // calling virtual functions
  setVertexWeights();
  setAllowedImbalances();

  logInfo(seissol::MPI::mpi.rank()) << "Computing LTS Node Weights. Done. " << utils::nospace << '('
                                    << totalNumberOfReductions << " reductions.)";
}

void LtsWeights::computeEdgeWeights(std::tuple<const std::vector<idx_t>&, const std::vector<idx_t>&,
                                               const std::vector<idx_t>&>& graph) {
  if (!m_edgeWeights.empty()) {
    m_edgeWeights.clear();
  }

  size_t edge_count = std::get<2>(graph).size();

  m_edgeWeights.resize(edge_count);

  setEdgeWeights(graph);

  logInfo(seissol::MPI::mpi.rank()) << "Computing LTS Edge Weights. Done. " << utils::nospace;
}

const int* LtsWeights::vertexWeights() const {
  assert(!m_vertexWeights.empty() && "vertex weights are not initialized");
  return m_vertexWeights.data();
}

const int* LtsWeights::edgeWeights() const {
  assert(!m_edgeWeights.empty() && "edge weights are not initialized");
  return m_edgeWeights.data();
}

int LtsWeights::edgeCount() const {
  assert(!m_edgeWeights.empty() && "edge weights are not initialized");
  return m_edgeWeights.size();
}

const double* LtsWeights::imbalances() const {
  assert(!m_imbalances.empty() && "weight imbalances are not initialized");
  return m_imbalances.data();
}

int LtsWeights::nWeightsPerVertex() const {
  assert(m_ncon != std::numeric_limits<int>::infinity() &&
         "num. constrains has not been initialized yet");
  return m_ncon;
}

void LtsWeights::computeMaxTimesteps(std::vector<double> const& pWaveVel,
                                     std::vector<double>& timeSteps,
                                     double maximumAllowedTimeStep) {
  std::vector<PUML::TETPUML::cell_t> const& cells = m_mesh->cells();
  std::vector<PUML::TETPUML::vertex_t> const& vertices = m_mesh->vertices();

  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    // Compute insphere radius
    Eigen::Vector3d barycentre(0., 0., 0.);
    Eigen::Vector3d x[4];
    unsigned vertLids[4];
    PUML::Downward::vertices(*m_mesh, cells[cell], vertLids);
    for (unsigned vtx = 0; vtx < 4; ++vtx) {
      for (unsigned d = 0; d < 3; ++d) {
        x[vtx](d) = vertices[vertLids[vtx]].coordinate()[d];
      }
    }
    Eigen::Matrix4d A;
    A << x[0](0), x[0](1), x[0](2), 1.0, x[1](0), x[1](1), x[1](2), 1.0, x[2](0), x[2](1), x[2](2),
        1.0, x[3](0), x[3](1), x[3](2), 1.0;

    double alpha = A.determinant();
    double Nabc = ((x[1] - x[0]).cross(x[2] - x[0])).norm();
    double Nabd = ((x[1] - x[0]).cross(x[3] - x[0])).norm();
    double Nacd = ((x[2] - x[0]).cross(x[3] - x[0])).norm();
    double Nbcd = ((x[2] - x[1]).cross(x[3] - x[1])).norm();
    double insphere = std::fabs(alpha) / (Nabc + Nabd + Nacd + Nbcd);

    // Compute maximum timestep (CFL=1)
    timeSteps[cell] = std::fmin(maximumAllowedTimeStep,
                                2.0 * insphere / (pWaveVel[cell] * (2 * CONVERGENCE_ORDER - 1)));
  }
}

int LtsWeights::getCluster(double timestep, double globalMinTimestep, unsigned rate) {
  if (rate == 1) {
    return 0;
  }

  double upper = rate * globalMinTimestep;

  int cluster = 0;
  while (upper <= timestep) {
    upper *= rate;
    ++cluster;
  }
  return cluster;
}

int LtsWeights::getBoundaryCondition(int const* boundaryCond, unsigned cell, unsigned face) {
  int bcCurrentFace = ((boundaryCond[cell] >> (face * 8)) & 0xFF);
  if (bcCurrentFace > 64) {
    bcCurrentFace = 3;
  }
  return bcCurrentFace;
}

int LtsWeights::ipow(int x, int y) {
  assert(y >= 0);

  if (y == 0) {
    return 1;
  }
  int result = x;
  while (--y) {
    result *= x;
  }
  return result;
}

LtsWeights::GlobalTimeStepDetails
LtsWeights::collectGlobalTimeStepDetails(double maximumAllowedTimeStep) {

  const auto& cells = m_mesh->cells();
  std::vector<double> pWaveVel;
  pWaveVel.resize(cells.size());

  seissol::initializers::ElementBarycentreGeneratorPUML queryGen(*m_mesh);
  // up to now we only distinguish between anisotropic elastic any other isotropic material
#ifdef USE_ANISOTROPIC
  std::vector<seissol::model::AnisotropicMaterial> materials(cells.size());
  seissol::initializers::MaterialParameterDB<seissol::model::AnisotropicMaterial> parameterDB;
#elif defined(USE_POROELASTIC)
  std::vector<seissol::model::PoroElasticMaterial> materials(cells.size());
  seissol::initializers::MaterialParameterDB<seissol::model::PoroElasticMaterial> parameterDB;
#else
  std::vector<seissol::model::ElasticMaterial> materials(cells.size());
  seissol::initializers::MaterialParameterDB<seissol::model::ElasticMaterial> parameterDB;
#endif
  parameterDB.setMaterialVector(&materials);
  parameterDB.evaluateModel(m_velocityModel, queryGen);
  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    pWaveVel[cell] = materials[cell].getMaxWaveSpeed();
  }

  GlobalTimeStepDetails details{};
  details.timeSteps.resize(cells.size());
  computeMaxTimesteps(pWaveVel, details.timeSteps, maximumAllowedTimeStep);

  double localMinTimestep = *std::min_element(details.timeSteps.begin(), details.timeSteps.end());
  double localMaxTimestep = *std::max_element(details.timeSteps.begin(), details.timeSteps.end());

#ifdef USE_MPI
  MPI_Allreduce(&localMinTimestep, &details.globalMinTimeStep, 1, MPI_DOUBLE, MPI_MIN,
                seissol::MPI::mpi.comm());
  MPI_Allreduce(&localMaxTimestep, &details.globalMaxTimeStep, 1, MPI_DOUBLE, MPI_MAX,
                seissol::MPI::mpi.comm());
#else
  details.globalMinTimeStep = localMinTimestep;
  details.globalMaxTimeStep = localMaxTimestep;
#endif
  return details;
}

std::vector<int> LtsWeights::computeClusterIds() {
  const auto& cells = m_mesh->cells();
  std::vector<int> clusterIds(cells.size(), 0);
  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    clusterIds[cell] = getCluster(m_details.timeSteps[cell], m_details.globalMinTimeStep, m_rate);
  }
  return clusterIds;
}

std::vector<int> LtsWeights::computeCostsPerTimestep() {
  const auto& cells = m_mesh->cells();

  std::vector<int> cellCosts(cells.size());
  int const* boundaryCond = m_mesh->cellData(1);
  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    int dynamicRupture = 0;
    int freeSurfaceWithGravity = 0;

    unsigned int faceids[4];
    PUML::Downward::faces(*m_mesh, cells[cell], faceids);

    for (unsigned face = 0; face < 4; ++face) {
      const auto faceType = static_cast<FaceType>(getBoundaryCondition(boundaryCond, cell, face));
      dynamicRupture += (faceType == FaceType::dynamicRupture) ? 1 : 0;
      freeSurfaceWithGravity += (faceType == FaceType::freeSurfaceGravity) ? 1 : 0;
    }

    const int costDynamicRupture = m_vertexWeightDynamicRupture * dynamicRupture;
    const int costDisplacement = m_vertexWeightFreeSurfaceWithGravity * freeSurfaceWithGravity;
    cellCosts[cell] = m_vertexWeightElement + costDynamicRupture + costDisplacement;
  }
  return cellCosts;
}

int LtsWeights::enforceMaximumDifference() {
  int totalNumberOfReductions = 0;
  int globalNumberOfReductions;
  do {
    int localNumberOfReductions = enforceMaximumDifferenceLocal();

#ifdef USE_MPI
    MPI_Allreduce(&localNumberOfReductions, &globalNumberOfReductions, 1, MPI_INT, MPI_SUM,
                  seissol::MPI::mpi.comm());
#else
    globalNumberOfReductions = localNumberOfReductions;
#endif
    totalNumberOfReductions += globalNumberOfReductions;
  } while (globalNumberOfReductions > 0);
  return totalNumberOfReductions;
}

int LtsWeights::enforceMaximumDifferenceLocal(int maxDifference) {
  int numberOfReductions = 0;

  std::vector<PUML::TETPUML::cell_t> const& cells = m_mesh->cells();
  std::vector<PUML::TETPUML::face_t> const& faces = m_mesh->faces();
  int const* boundaryCond = m_mesh->cellData(1);

#ifdef USE_MPI
  std::unordered_map<int, std::vector<int>> rankToSharedFaces;
  std::unordered_map<int, int> localFaceIdToLocalCellId;
#endif

  for (unsigned cell = 0; cell < cells.size(); ++cell) {
    int timeCluster = m_clusterIds[cell];

    unsigned int faceids[4];
    PUML::Downward::faces(*m_mesh, cells[cell], faceids);
    for (unsigned f = 0; f < 4; ++f) {
      int difference = maxDifference;
      int boundary = getBoundaryCondition(boundaryCond, cell, f);
      // Continue for regular, dynamic rupture, and periodic boundary cells
      if (boundary == 0 || boundary == 3 || boundary == 6) {
        // We treat MPI neighbours later
        auto const& face = faces[faceids[f]];
        if (!face.isShared()) {
          int cellIds[2];
          PUML::Upward::cells(*m_mesh, face, cellIds);

          int neighbourCell = (cellIds[0] == static_cast<int>(cell)) ? cellIds[1] : cellIds[0];
          int otherTimeCluster = m_clusterIds[neighbourCell];

          if (boundary == 3) {
            difference = 0;
          }

          if (timeCluster > otherTimeCluster + difference) {
            timeCluster = otherTimeCluster + difference;
            ++numberOfReductions;
          }
        }
#ifdef USE_MPI
        else {
          rankToSharedFaces[face.shared()[0]].push_back(faceids[f]);
          localFaceIdToLocalCellId[faceids[f]] = cell;
        }
#endif
      }
    }
    m_clusterIds[cell] = timeCluster;
  }

#ifdef USE_MPI
  FaceSorter faceSorter(faces);
  for (auto& sharedFaces : rankToSharedFaces) {
    std::sort(sharedFaces.second.begin(), sharedFaces.second.end(), faceSorter);
  }

  auto numExchanges = rankToSharedFaces.size();
  std::vector<MPI_Request> requests(2 * numExchanges);
  std::vector<std::vector<int>> ghost(numExchanges);
  std::vector<std::vector<int>> copy(numExchanges);

  auto exchange = rankToSharedFaces.begin();
  for (unsigned ex = 0; ex < numExchanges; ++ex) {
    auto exchangeSize = exchange->second.size();
    ghost[ex].resize(exchangeSize);
    copy[ex].resize(exchangeSize);

    for (unsigned n = 0; n < exchangeSize; ++n) {
      copy[ex][n] = m_clusterIds[localFaceIdToLocalCellId[exchange->second[n]]];
    }
    MPI_Isend(copy[ex].data(), exchangeSize, MPI_INT, exchange->first, 0, seissol::MPI::mpi.comm(),
              &requests[ex]);
    MPI_Irecv(ghost[ex].data(), exchangeSize, MPI_INT, exchange->first, 0, seissol::MPI::mpi.comm(),
              &requests[numExchanges + ex]);
    ++exchange;
  }

  MPI_Waitall(2 * numExchanges, requests.data(), MPI_STATUSES_IGNORE);

  exchange = rankToSharedFaces.begin();
  for (unsigned ex = 0; ex < numExchanges; ++ex) {
    auto exchangeSize = exchange->second.size();
    for (unsigned n = 0; n < exchangeSize; ++n) {
      int difference = maxDifference;
      int otherTimeCluster = ghost[ex][n];

      int cellIds[2];
      PUML::Upward::cells(*m_mesh, faces[exchange->second[n]], cellIds);
      int cell = (cellIds[0] >= 0) ? cellIds[0] : cellIds[1];

      unsigned int faceids[4];
      PUML::Downward::faces(*m_mesh, cells[cell], faceids);
      unsigned f = 0;
      for (; f < 4 && static_cast<int>(faceids[f]) != exchange->second[n]; ++f)
        ;
      assert(f != 4);

      int boundary = getBoundaryCondition(boundaryCond, cell, f);
      if (boundary == 3) {
        difference = 0;
      }

      if (m_clusterIds[cell] > otherTimeCluster + difference) {
        m_clusterIds[cell] = otherTimeCluster + difference;
        ++numberOfReductions;
      }
    }
    ++exchange;
  }

#endif

  return numberOfReductions;
}

const LtsWeights::GlobalTimeStepDetails& LtsWeights::getDetails() const { return m_details; }

const std::string& LtsWeights::getVelocityModel() const { return m_velocityModel; }

unsigned LtsWeights::getRate() const { return m_rate; }

std::vector<int>& LtsWeights::getVertexWeights() { return m_vertexWeights; }

std::vector<double>& LtsWeights::getImbalances() { return m_imbalances; }

const std::vector<int>& LtsWeights::getCellCosts() const { return m_cellCosts; }

int LtsWeights::getVertexWeightElement() const { return m_vertexWeightElement; }

int LtsWeights::getVertexWeightDynamicRupture() const { return m_vertexWeightDynamicRupture; }

int LtsWeights::getVertexWeightFreeSurfaceWithGravity() const {
  return m_vertexWeightFreeSurfaceWithGravity;
}

int LtsWeights::getNcon() const { return m_ncon; }

const PUML::TETPUML* LtsWeights::getMesh() const { return m_mesh; }

const std::vector<int>& LtsWeights::getClusterIds() const { return m_clusterIds; }

LtsWeights::LtsWeights(const LtsWeightsConfig& config) noexcept
    : m_velocityModel(config.velocityModel), m_rate(config.rate),
      m_vertexWeightElement(config.vertexWeightElement),
      m_vertexWeightDynamicRupture(config.vertexWeightDynamicRupture),
      m_vertexWeightFreeSurfaceWithGravity(config.vertexWeightFreeSurfaceWithGravity),
      m_nodeWeightModel(nullptr), m_edgeWeightModel(nullptr) {}

void LtsWeights::setVertexWeights() { m_nodeWeightModel->setVertexWeights(); }

void LtsWeights::setAllowedImbalances() { m_nodeWeightModel->setAllowedImbalances(); }

int LtsWeights::evaluateNumberOfConstraints() const {
  return m_nodeWeightModel->evaluateNumberOfConstraints();
}

void LtsWeights::setEdgeWeights(std::tuple<const std::vector<idx_t>&, const std::vector<idx_t>&,
                                           const std::vector<idx_t>&>& graph) {
  m_edgeWeightModel->setEdgeWeights(graph);
}

LtsWeights::~LtsWeights() noexcept {
  delete m_nodeWeightModel;
  delete m_edgeWeightModel;
}

void LtsWeights::addWeightModels(NodeWeightModel* nwm, EdgeWeightModel* ewm) {
  m_nodeWeightModel = nwm;
  m_edgeWeightModel = ewm;
}

std::vector<int>& LtsWeights::getEdgeWeights() { return m_edgeWeights; }

int LtsWeights::find_rank(const std::vector<idx_t>& vrtxdist, idx_t elemId){
  assert(!vrtxdist.empty());
  assert(elemId < vrtxdist.back());
  assert(elemId >= 0);
  assert(vrtxdist[0] == 0);

  for (size_t i = 0; i < vrtxdist.size()-1; i++){
    if (elemId >= vrtxdist[i] && elemId < vrtxdist[i+1])
    {
      return i;
    }
  }

  throw std::runtime_error("element id is not in valid range!");
}

std::vector<std::unordered_map<idx_t, int>>
LtsWeights::exchangeGhostLayer(std::tuple<const std::vector<idx_t>&, const std::vector<idx_t>&,
                                          const std::vector<idx_t>&>& graph) {
  // fails with intel compiler, mpi standard says use mpi size of
  // static_assert(sizeof(idx_t) <= sizeof(MPI_LONG_LONG_INT));

  const std::vector<idx_t>& vrtxdist = std::get<0>(graph);
  const std::vector<idx_t>& xadj = std::get<1>(graph);
  const std::vector<idx_t>& adjncy = std::get<2>(graph);

  const int rank = seissol::MPI::mpi.rank();
  const int size = seissol::MPI::mpi.size();

  assert(vrtxdist.size() == static_cast<size_t>(size + 1) && !xadj.empty() && !adjncy.empty());
  assert(vrtxdist[0] == 0);

  const size_t vertex_id_begin = vrtxdist[rank];
  // const size_t vertex_id_end = vrtxdist[rank + 1];

  assert(!m_clusterIds.empty());

  // gather all the ranks that have neighbors to these ranks
  std::set<int> neighbor_ranks;

  // ghost layers of other ranks
  // ghost_layer_to_send[i] means the actors and their cluster to send to rank i!
  // using a vector because we need continous data and therefore we will have
  // a vector of idx_t the length of element count * 2
  std::vector<std::vector<idx_t>> ghost_layer_to_send;
  std::vector<std::vector<idx_t>> ghost_layer_received;
  std::vector<std::unordered_map<idx_t, int>> ghost_layer_mapped;
  std::vector<MPI_Request> send_requests;
  std::vector<MPI_Status> recv_stats;

  // the at(my_rank) will be empty but it makes coding much easier
  ghost_layer_to_send.resize(size);
  ghost_layer_received.resize(size);
  ghost_layer_mapped.resize(size);

  for (size_t i = 0; i < xadj.size() - 1; i++) {
    //[xadj[i] .. xadj[i+1]) describes to offsets neighbor ids of local vertex i
    const size_t neighbors_offset_begin = xadj[i];
    const size_t neighbors_offset_end = xadj[i + 1];

    const int cluster_id = m_clusterIds[i];

    for (size_t j = neighbors_offset_begin; j < neighbors_offset_end; j++) {
      const idx_t neighbor_id = adjncy[j];

      const int neighbor_rank = find_rank(vrtxdist, neighbor_id);

      // if neighbor's rank is different than mine
      if (neighbor_rank != rank) {
        neighbor_ranks.insert(neighbor_rank);
        ghost_layer_to_send[neighbor_rank].push_back(
            static_cast<idx_t>(i + vertex_id_begin)); // send global id
        ghost_layer_to_send[neighbor_rank].push_back(static_cast<idx_t>(cluster_id));
      }
    }
  }

  //std::cout << "Neighbors of " << rank << " are: ";
  //for (int nb : neighbor_ranks) {
  //  std::cout << nb << " ";
  //}
  //std::cout << std::endl;

  // if we add empty requests and stats waiting becomes problematic
  send_requests.resize(neighbor_ranks.size());
  recv_stats.resize(neighbor_ranks.size());

  // now send ghost_layer_to_send to neighbors
  // per default sets are stored in ascending order
  {
    int offset = 0;
    for (int neighbor_rank : neighbor_ranks) {
      const std::vector<idx_t>& layer_ref = ghost_layer_to_send[neighbor_rank];

      assert(!layer_ref.empty());

      if (sizeof(idx_t) != 8)
      {
        if (rank == 0)
        {
          std::cerr << "If the size of idx_t is not 64 bits the ghost layer exchange will crush with a segmentation fault!" << std::endl;
        }
      }

      MPI_Isend(layer_ref.data(), layer_ref.size(), MPI_LONG_LONG_INT, neighbor_rank, rank,
                MPI_COMM_WORLD, &send_requests[offset]);

      offset += 1;
    }
  }

  volatile unsigned int got = 0;
  std::vector<bool> skip;
  skip.resize(neighbor_ranks.size());
  std::fill(skip.begin(), skip.end(), false);

  // now lets probe and try to get the ghost layer
  while (got < neighbor_ranks.size()) {
    int offset = 0;
    for (int neighbor_rank : neighbor_ranks) {
      if (neighbor_rank == rank) {
        throw std::runtime_error("A neighbor's rank should not be the same as self rank!");
      }

      if (skip[offset]) {

        offset += 1;

      } else {

        int flag = 0;
        MPI_Iprobe(neighbor_rank, neighbor_rank, MPI_COMM_WORLD, &flag, &recv_stats[offset]);

        if (flag) {
          int count = 0;

          MPI_Get_count(&recv_stats[offset], MPI_LONG_LONG_INT, &count);

          assert(ghost_layer_received[neighbor_rank].empty());
          ghost_layer_received[neighbor_rank].resize(count);
          assert(ghost_layer_mapped[neighbor_rank].empty());
          MPI_Recv(ghost_layer_received[neighbor_rank].data(), count, MPI_LONG_LONG_INT, neighbor_rank,
                   neighbor_rank, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
          got += 1;

          skip[offset] = true;

          // lets move the data here
        }

        offset += 1;
      }
    }
  }

  // wait for the communication to be completed
  MPI_Waitall(send_requests.size(), send_requests.data(), MPI_STATUS_IGNORE);

  // not sure if necessary as waiting for all send implies the loop will
  // completed too and the rest of communication is totally local
  // MPI_Barrier(MPI_COMM_WORLD);

  for (int i : neighbor_ranks) {
    assert(i != rank);
    assert(!ghost_layer_received[i].empty());
    assert(ghost_layer_received[i].size() % 2 == 0);

    for (unsigned int j = 0; j < ghost_layer_received[i].size(); j += 2) {
      ghost_layer_mapped[i].emplace(ghost_layer_received[i][j], ghost_layer_received[i][j + 1]);
    }
    //std::cout << "Ghost received from rank: " << i << " to " << rank << " has "
    //          << ghost_layer_received[i].size() / 2 << " elements" << std::endl;
  }

  return ghost_layer_mapped;
}

} // namespace seissol::initializers::time_stepping