#include <iostream>
#include <fstream>
#include <queue>
#include <vector>
#include <utility>
#include <stdlib.h>
#include <unordered_set>
#include <boost/graph/adjacency_list.hpp>
// #include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/graph/graph_utility.hpp>
#include "metis.h"

template <class Edge>
struct myhash
{
    std::size_t operator()(Edge const& e) const
    {
        return _h(e.idx);
    }
};

using namespace std;
using namespace boost;

// global declarations
const int K = 2;                                            // TODO: Change this to main param
idx_t ncon = 1;                                             // default METIS tuning param
idx_t nparts = K;                                           // set num partition to K

int main(int argc, char* argv[])
{
    // error check
    if (argc != 3)
    {
        cout << "usage: main.exe inputFile outputFile" << endl;
        cout << "inputFile must be in graphviz dot file format." << endl;
        return 1;
    }

    //********************************** Declarations **********************************

    // typedef declarations for templates
    typedef subgraph< adjacency_list<vecS, vecS, undirectedS, uint32_t,
        property< edge_index_t, int > > > graph_type;
    typedef graph_traits<graph_type>::vertex_iterator vertex_iter;
    typedef graph_traits<graph_type>::edge_iterator edge_iter;
    typedef graph_traits<graph_type>::out_edge_iterator out_edge_iter;
    typedef std::pair<vertex_iter, vertex_iter> vrange_t;
    typedef graph_traits<graph_type>::adjacency_iterator adj_iter;
    typedef std::pair<adj_iter, adj_iter> adjrange_t;
    typedef property_map<graph_type, vertex_index_t>::type IndexMap;
    typedef graph_traits<graph_type>::vertex_descriptor v_descriptor;
    typedef graph_traits<graph_type>::edge_descriptor e_descriptor;
    typedef std::vector<v_descriptor> vert_vec;
    typedef std::pair< v_descriptor, int > hop_pair_t;
    typedef std::vector<hop_pair_t> avt_vector_t;
    typedef std::queue<hop_pair_t> vert_que;
    typedef std::vector<bool> colormap;
    typedef std::vector<int> score_vec;
    typedef std::pair<v_descriptor, bool> found_t;
    //typedef std::unordered_set<e_descriptor, typename myhash> u_set_t;
    typedef std::set<e_descriptor> u_set_t;

    //*** variable declarations ***

    // The root graph
    graph_type graph1(0);

    // subgraphs from the root
    graph_type *subgraph_vect = new graph_type[K];

    // METIS library args
    idx_t nvert;
    int nedge;
    idx_t *xadj;
    idx_t *adjncy;
    idx_t objval;
    idx_t *part;
    idx_t options[METIS_NOPTIONS];

    // The graphviz dot file
    ifstream inputFile;

    // Output graphviz dot file
    ofstream outputFile;
    // ostream& os = cout;

    // iterators
    vrange_t vpair;
    int i = 0;
    int j = 0;
    graph_type::children_iterator ci;
    graph_type::children_iterator ci_end;
    vertex_iter v;
    vertex_iter v_end;
    //    vertex_iter v_next_col;
    edge_iter e;
    edge_iter e_end;
    out_edge_iter child_e;
    out_edge_iter child_e_end;
    out_edge_iter parent_e;
    out_edge_iter parent_e_end;
    adj_iter vi;
    adj_iter vi_end;

    // index for random access
    IndexMap index;

    // Alignment Vertex table
    vert_vec *avt = new vert_vec[K];
    int *avt_lookup;
    avt_vector_t *avt_unmatched = new avt_vector_t[K];      // to store intermediate results
    v_descriptor *avtrow = new v_descriptor[K];             // the initial row
    int avtRows;                                            // number of rows in table
    int hopcount;
    int degrees;
    int score;
    int bestscore;
    int pos_right;
    int best_position;
    int maxlength;
    score_vec *score_table = new score_vec[K];
    v_descriptor bestV;
    hop_pair_t hopPair;

    // colormap & que used in BFS
    colormap *clr_arr = new colormap[K];
    vert_que *vque_arr = new vert_que[K];

    // edge copy
    u_set_t edge_set;
    u_set_t added_edge_set;
    found_t found;
    int from;
    int to;

    // descriptors
    v_descriptor vLocalID;
    v_descriptor vGlobalID;
    v_descriptor orig_source;
    v_descriptor orig_target;
    v_descriptor copy_source;
    v_descriptor copy_target;
    e_descriptor eLocalID;
    e_descriptor eGlobalID;

    //********************************** Reading and Building Graph **********************************
    // Read in the graph from disk
    inputFile.open(argv[1]);
    dynamic_properties dp(ignore_other_properties);
    read_graphviz(inputFile, graph1, dp);
    inputFile.close();

    //Initialize Graph-Dependent Variables
    nvert = num_vertices(graph1);
    nedge = num_edges(graph1);
    xadj = new idx_t[nvert + 1];
    adjncy = new idx_t[nedge * 2];
    part = new idx_t[nvert];
    index = get(vertex_index, graph1);

    //********************************** Partition the graph with METIS algorithm **********************************
    // initialise
    METIS_SetDefaultOptions(options);
    xadj[0] = 0;

    // Transform the adjacency list into the Compressed Storage Format
    // TODO: consider using tie function - syntactic sugar
    for(vpair = vertices(graph1); vpair.first != vpair.second; ++vpair.first)
    {
        xadj[i+1] = xadj[i] + out_degree(*vpair.first, graph1);
        adjrange_t adjpair = adjacent_vertices(*vpair.first, graph1);
        j = xadj[i];
        for(adjpair.first; adjpair.first != adjpair.second; ++adjpair.first)
        {
            adjncy[j] = *adjpair.first;
            ++j;
        }
        ++i;
    }

    // Print out to verify
    cout << "METIS partitioning: " << endl;
    for(i = 0; i < nvert + 1; ++i)
    {
        cout << xadj[i] << ' ';
    }
    cout << endl;
    for(i = 0; i < nedge * 2; ++i)
    {
        cout << adjncy[i] << ' ';
    }
    cout << endl;

    // Use k-way graph partition algorithm
    objval = 0;
    METIS_PartGraphKway(&nvert, &ncon, xadj, adjncy, NULL, NULL, NULL, &nparts, NULL, NULL, NULL, &objval, part);

    // Print out the partition vector
    for(i=0; i < nvert; ++i)
    {
        cout << part[i] << ' ';
    }
    cout << endl;

	//********************************** Create Subgraphs based on METIS result **********************************
    // Create induced subgraphs based on partition vector
    for (i=0; i<K; ++i)
    {
        subgraph_vect[i] = graph1.create_subgraph();
    }
    for (i=0; i < nvert; ++i)
    {
        cout << "Mapping " << index[i] << " to subgraph " << part[i] << endl;
        add_vertex(index[i], subgraph_vect[part[i]]);
    }

	// print for testing

    //This will print out the global vertex IDs from the root graph
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

    //********************************** Building the AVT **********************************
    // Start to build the AVT
    // Each vector will represent a column in the table
    // Each column in the table represents a subgraph

    // Build colormap
    // First step is to create the initial row
    for (i=0; i < K; ++i)
    {
        int maxdegree = 0;
        v_descriptor vertID = 0;
        // cout << "cp4" << endl;

        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            int temp = out_degree(*v, subgraph_vect[i]);
            if (temp > maxdegree)
            {
                maxdegree = temp;
                vertID = *v;
                avtrow[i] = vertID;
            }
            // cout << "maxdegree = " << maxdegree << endl;
            // while we are processing every vertex in each subgraph
            // initialise the colormap
            clr_arr[i].push_back(false) ;
        }
    }

    //Print out the color array
    cout << std::boolalpha;
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Print out the starting point of AVT
    cout << endl << "AVT:" <<endl;
    for (i=0; i<K; ++i)
    {
        cout << subgraph_vect[i].local_to_global(avtrow[i]) << ' ';
    }
    cout << endl;

    // Load in the first row
    for(i=0; i<K; ++i)
    {
        avt_unmatched[i].push_back(std::make_pair(avtrow[i], 0));
    }

    // build ques for BFS
    for(i=0; i<K; ++i)
    {
        vque_arr[i].push(std::make_pair(avtrow[i], 0));
    }

    // Process using BFS
    // Mark all starting nodes as visited
    for (i=0; i < K; ++i)
    {
        index = get(vertex_index, subgraph_vect[i]);

        clr_arr[i][index[avtrow[i]]] = true;
    }

    // Print  out the color array
    cout << std::boolalpha;
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Process the que
	//*** Breadth First Search ***
    for(i=0; i < K; ++i)
    {
        while (vque_arr[i].empty()==false)
        {
            //  v_descriptor vGlobalID = vque_arr[i].front();
            //  v_descriptor vLocalID = subgraph_vect[i].global_to_local(vGlobalID);
            hopPair = vque_arr[i].front();

            vLocalID = hopPair.first;
            hopcount = hopPair.second;
            //  cout << "Processing BFS node: " << vLocalID << endl;
            cout << "Processing BFS node: " << index[vLocalID] << endl;
            index = get(vertex_index, subgraph_vect[i]);
            vque_arr[i].pop();
            for(boost::tie(vi, vi_end) = adjacent_vertices(vLocalID, subgraph_vect[i]); vi != vi_end; ++vi)
            {
                //  cout << subgraph_vect[i][*vi] << endl;
                //  cout << graph1[*vi] << endl;
                if(clr_arr[i][index[*vi]] == false)
                {
                    cout << "Processing adjacent node: " << index[*vi] << endl;
                    vque_arr[i].push(std::make_pair(*vi, hopcount+1));
                    avt_unmatched[i].push_back(std::make_pair(*vi, hopcount+1));
                    clr_arr[i][index[*vi]] = true;
                    // Print out the color map as updated
                    for(int z=0; z<K; ++z)
                    {
                        for(j=0; j < clr_arr[z].size(); ++j)
                        {
                            cout << clr_arr[z][j] << ' ';
                        }
                        cout << endl;
                    }
                }
            }
        }
    }

    // Print out the AVT
    cout << endl << "AVT (unmatched - UNbalanced) <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt_unmatched[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << '(' << avt_unmatched[i][j].second << ") ";
        }
        cout << endl;
    }
    cout << endl;

    // balance out the table if odd number of vertices
    // find the max length column
    maxlength = 0;
    for (i=0; i<K; ++i)
    {
        if (avt_unmatched[i].size() > maxlength)
        {
            maxlength = avt_unmatched[i].size();
        }
    }

    //Add vertices until all columns are maxlength
    for (i=0; i<K; ++i)
    {
		while (avt_unmatched[i].size() < maxlength)
    	{
	    	vLocalID = add_vertex(subgraph_vect[i]);
			avt_unmatched[i].push_back(std::make_pair(vLocalID, 99));
			clr_arr[i].push_back(false);
    	}
    }

    // Print out the AVT
    cout << endl << "AVT (unmatched - balanced) <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt_unmatched[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << '(' << avt_unmatched[i][j].second << ") ";
        }
        cout << endl;
    }
    cout << endl;

    //********************************** using local scores **********************************

    // use colormap for tracking matches
    // initialise
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            if(j==0)
            {
                clr_arr[i][j] = true;
            }
            else
            {
                clr_arr[i][j] = false;
            }
        }
    }

    // Print  out the color array
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // create the avt_lookup table
    // We used a 2d array flattened to 1d
    // The values in this table are the index positions in the avt
    // Every time we push into avt we update avt_lookup
    avt_lookup = new int[K * avt_unmatched[0].size()];

    // copy the first column as is

    //use index to index into avt_lookup
    index = get(vertex_index, subgraph_vect[0]);
    for (i=0; i < avt_unmatched[0].size(); ++i)
    {
        avt[0].push_back(avt_unmatched[0][i].first);
        avt_lookup[index[avt_unmatched[0][i].first]] = i;
    }
    // match from left to right by score
    // score of 0 is considered a perfect match
    // as in same degree and same distance from hub

    // for each column, fill in next column row by row
    for (i=0; i < K-1; ++i)
    {
        // copy the first row as is
        avt[i+1].push_back(avt_unmatched[i+1][0].first);
        // get new index for each subgraph
        index = get(vertex_index, subgraph_vect[i]);
        avt_lookup[(i+1) * avt_unmatched[0].size() + (index[avt_unmatched[i+1][0].first])] = i;

        // for each item in left column, match against items in right column
        // skip over first iteration so j=1
        for(j=1; j < avt[0].size(); ++j)
        {
            cout << "avt size=" << avt[i].size() << endl;
            bestscore = 10000;  // initialise to silly high number; track best score so far
            pos_right = 0;      // current position of item on right
            best_position = 0;  // save position of best match
            // for each item in right column see if that item is a good match
            for(auto v_next_col = avt_unmatched[i+1].begin(); v_next_col != avt_unmatched[i+1].end(); ++v_next_col)
            {
                if(clr_arr[i+1][pos_right] == false)
                {
                    int dleft = out_degree(avt_unmatched[i][j].first, subgraph_vect[i]);
                    int dright = out_degree(v_next_col->first, subgraph_vect[i+1]);
                    cout << "comparing " << subgraph_vect[i].local_to_global(avt_unmatched[i][j].first) << " to " << subgraph_vect[i+1].local_to_global(v_next_col->first) << endl;
                    cout << "degree left: " << dleft
                        << " degree right: " << dright
                        << endl;
                    degrees = std::abs(dleft - dright);
                    hopcount = std::abs(avt_unmatched[i][j].second - v_next_col->second);
                    score = degrees + hopcount;  // can change to weighted score
                    cout << "degree diff: " << degrees << " hopcount diff: " << hopcount << " score: " << score << endl;
                    if(score < bestscore)
                    {
                        bestscore = score;
                        vLocalID = v_next_col->first;
                        best_position = pos_right;
                    }
                }
                ++pos_right;
            }

            // update the colormap, push in to avt, update lookup table
            clr_arr[i+1][best_position] = true;
            avt[i+1].push_back(vLocalID);
            avt_lookup[(i + 1) * avt_unmatched[i+1].size() + (index[avt_unmatched[i+1][best_position].first])] = j;
        }

    }

    // Print out the lookup table
    for(i = 0; i < avt[0].size() * K; ++i)
    {
       if(i % avt[0].size() == 0)
       {
           cout << endl;
       }
       cout << avt_lookup[i] << ' ';
    }
    cout << endl;

    // Print  out the color array
    for(i=0; i<K; ++i)
    {
        for(j=0; j < clr_arr[i].size(); ++j)
        {
            cout << clr_arr[i][j] << ' ';
        }
        cout << endl;
    }

    // Print out the AVT
    cout << endl << "AVT <read this sideways>:" << endl;
    for(i=0; i < K; ++i)
    {
        avtRows = avt[i].size();
        for(j=0; j < avtRows; ++j)
        {
            cout << subgraph_vect[i].local_to_global(avt[i][j]) << ' ';
        }
        cout << endl;
    }
    cout << endl;

	//********************************** Perform Block Alignment **********************************

	//*** First add all edges to the first column ***
	//*** first column will become the rolemodel ***
	adj_iter one;
	adj_iter one_end;
	adj_iter two;
	adj_iter two_end;
	bool matchFound;
	//*** Make the first column the rolemodel with all edges copied ***
	for(i = 0; i < avt[0].size(); ++i)
	{
		for(j = 1; j < K; ++j)
		{
			index = get(vertex_index ,subgraph_vect[j]);
			for(boost::tie(two, two_end) = adjacent_vertices(avt[j][i], subgraph_vect[j]); two != two_end; ++two)
			{
				//find pair to vertex in column one
				v_descriptor pair_vertex = index[avt[0][avt_lookup[j * avt[0].size() + (*two)]]];
				matchFound = false;
				for(boost::tie(one, one_end) = adjacent_vertices(avt[0][i], subgraph_vect[0]); one != one_end; ++one)
				{
					if(pair_vertex == *one)
					{
						matchFound = true;
					}
				}
				if(matchFound == false)
					add_edge(avt[0][i], pair_vertex, subgraph_vect[0]);
			}
		}
	}
	//*** compare all other columns to the rolemodel column and add edges ***
	for(i = 0; i < avt[0].size(); ++i)
	{
		for(j = 1; j < K; ++j)
		{
			index = get(vertex_index ,subgraph_vect[0]);
			for(boost::tie(one, one_end) = adjacent_vertices(avt[0][i], subgraph_vect[0]); one != one_end; ++one)
			{
				v_descriptor pair_vertex = index[avt[j][avt_lookup[*one]]];
				matchFound = false;
				for(boost::tie(two, two_end) = adjacent_vertices(avt[j][i], subgraph_vect[j]); two != two_end; ++two)
				{
					if(pair_vertex == *two)
					{
						matchFound = true;
					}
				}
				if(matchFound == false)
					add_edge(avt[j][i], pair_vertex, subgraph_vect[j]);
			}
		}
	}

	// print results of block alignment
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

	//********************************** Perform Edge Copy **********************************
    // for each child subgraph
    for (i=0; i < K; ++i)
    {
        cout << "cp1" << endl;
        // for each vertex in subgraph, compare degree to parent vertex
        for(boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            // if degrees don't match then there is at least one crossing edge
            cout << "comparing subgraph " << i << ' ' << *v << " to " << subgraph_vect[i].local_to_global(*v) << endl;
            if (out_degree(*v, subgraph_vect[i]) != out_degree(subgraph_vect[i].local_to_global(*v), graph1))
            {
                cout << "Detected a crossing edge from vertex: " << subgraph_vect[i].local_to_global(*v) << endl;
                // for each child edge, load into hash table
                edge_set.clear();
                for(boost::tie(child_e, child_e_end) = out_edges(*v, subgraph_vect[i]); child_e != child_e_end; ++child_e)
                {
                    eGlobalID = subgraph_vect[i].local_to_global(*child_e);
                    edge_set.insert(eGlobalID);
                }
                // for each edge of parent vertex, check if it's in hash table
                for(boost::tie(parent_e, parent_e_end) = out_edges(subgraph_vect[i].local_to_global(*v), graph1); parent_e != parent_e_end; ++ parent_e)
                {
                    if(edge_set.find(*parent_e) == edge_set.end()
                        && added_edge_set.find(*parent_e) == added_edge_set.end())
                    {
                        // do edge copy
                        // get the source & target of the edge
                        orig_source = source(*parent_e, graph1);
                        orig_target = target(*parent_e, graph1);
                        cout << "edge source " << orig_source << " target " << orig_target << endl;
                        // find them in the avt
                        // use avt_lookup to find the index of the source
                        index = get(vertex_index, subgraph_vect[i]);
                        vLocalID = subgraph_vect[i].global_to_local(orig_source);
                        from = avt_lookup[i * avt[0].size() + index[vLocalID]];
                        // find the subgraph of the target & get it's local ID
                        for (j=0; j < K; ++j)
                        {
                            found = subgraph_vect[j].find_vertex(orig_target);
                            if(found.second)
                            {
                                // do lookup of the target
                                index = get(vertex_index, subgraph_vect[j]);
                                vLocalID = found.first;
                                to = avt_lookup[j * avt[0].size() + index[vLocalID]];
                                copy_source = subgraph_vect[j].local_to_global(avt[j][from]);
                                copy_target = subgraph_vect[i].local_to_global(avt[i][to]);
                                cout << "i=" << i << " j=" << j << " from=" << from << " to=" << to << endl;
                                if(edge(copy_source, copy_target, graph1).second == false)
                                {
                                    add_edge(copy_source, copy_target, graph1);
                                    added_edge_set.insert(eGlobalID);
                                }
                                break;
                            }
                        }

                    }
                }
            }
        }
    }

    // *******End***********

    cout << endl;
    // Dump the output to file
    outputFile.open(argv[2]);
    write_graphviz_dp(outputFile, graph1, dp.property("node_id", get(boost::vertex_index, graph1)));
    // uncomment this to print dot file to std out
    //write_graphviz_dp(cout, graph1, dp.property("node_id", get(boost::vertex_index, graph1)));
    outputFile.close();
    // also print out to std out
    cout << "root:" << endl;
    print_graph(graph1, get(vertex_index, graph1));
    cout << endl;
    for (i=0; i < K; ++i)
    {
        cout << "subgraph " << i << ":" << endl;
        cout << "vertices = ";
        for (boost::tie(v, v_end) = vertices(subgraph_vect[i]); v != v_end; ++v)
        {
            cout << subgraph_vect[i].local_to_global(*v) << ", ";
        }
        cout << endl;
        cout << "edges = ";
        for (boost::tie(e, e_end) = edges(subgraph_vect[i]); e != e_end; ++e)
        {
            cout << subgraph_vect[i].local_to_global(*e) << ", ";
        }
        cout << endl;
    }

    //cleanup
    // TODO: move these higher
    delete[] xadj;
    delete[] adjncy;
    delete[] part;
    delete[] subgraph_vect;
    delete[] avt;
    delete[] avt_lookup;
    delete[] avtrow;
    delete[] clr_arr;
    delete[] vque_arr;
    delete[] avt_unmatched;
    delete[] score_table;
    xadj = NULL;
    adjncy = NULL;
    part = NULL;
    subgraph_vect = NULL;
    avt = NULL;
    avtrow = NULL;
    clr_arr = NULL;
    vque_arr = NULL;
    avt_unmatched = NULL;
    score_table = NULL;
    avt_lookup = NULL;

    return 0;
}


#ifndef Interpolators__2D_InterpolatorBase_hpp
#define Interpolators__2D_InterpolatorBase_hpp

/** @file InterpolatorBase.hpp
  * @author C.D. Clark III
  * @date 12/24/16
  */

#include <memory>
#include <vector>
#include <Eigen/Dense>
#include<boost/range/algorithm.hpp>
#include <boost/range/adaptor/strided.hpp>

namespace _2D {

template<typename T>
struct RealTypeOf { using type = double; };
template<template<typename> class T,typename R>
struct RealTypeOf<T<R>> { using type = R; };

/** @class
  * @brief A base class for 2D interpolators that provides default implementations of the interface.
  * @author C.D. Clark III
  *
  * This class provides an implementation for the setData method as well as adding a few additional
  * useful methods, including derivative and integral methods.
  */
template<class Derived, typename Real = typename RealTypeOf<Derived>::type>
class InterpolatorBase
{
  public:
    using VectorType = Eigen::Matrix<Real,Eigen::Dynamic,1>;
    using MapType = Eigen::Map<const VectorType>;
    // types used to view data as 2D coordinates
    using MatrixType = typename Eigen::Matrix<Real,Eigen::Dynamic,Eigen::Dynamic>;
    using _2DVectorView = Eigen::Map<const VectorType,Eigen::Unaligned,Eigen::InnerStride<Eigen::Dynamic>>;
    using _2DMatrixView = Eigen::Map<const MatrixType,Eigen::Unaligned,Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic>>;

  private:
    std::vector<Real> xData, yData, zData;        // data storage

  protected:
    std::unique_ptr<MapType> xView, yView, zView; // map view of the data

    // these maps are used to view the x,y,z data as two coordinate vectors and a function matrix, instead of three vectors.
    std::unique_ptr<_2DVectorView> X,Y;
    std::unique_ptr<_2DMatrixView> Z;

  private:
    // making constructors private and the derived class a friend
    // helps to make sure that the derived class actually
    // passes itself as the template argument.
    friend Derived;

    InterpolatorBase(){ }

    InterpolatorBase( const InterpolatorBase& rhs )
    :xData(rhs.xData)
    ,yData(rhs.yData)
    ,zData(rhs.zData)
    ,xView( rhs.xView ? new MapType( xData.data(), xData.size() ) : nullptr )
    ,yView( rhs.xView ? new MapType( yData.data(), yData.size() ) : nullptr )
    ,zView( rhs.zView ? new MapType( zData.data(), zData.size() ) : nullptr )
    {
      this->setup2DDataViews();
    }

    // we use the copy-swap idiom for copy assignment
    friend void swap( InterpolatorBase& lhs, InterpolatorBase& rhs)
    {
      std::swap( lhs.xData, rhs.xData );
      std::swap( lhs.yData, rhs.yData );
      std::swap( lhs.zData, rhs.zData );
      std::swap( lhs.xView, rhs.xView );
      std::swap( lhs.yView, rhs.yView );
      std::swap( lhs.zView, rhs.zView );
      std::swap( lhs.X, rhs.X);
      std::swap( lhs.Y, rhs.Y);
      std::swap( lhs.Z, rhs.Z);
    }

    InterpolatorBase& operator=(InterpolatorBase rhs)
    {
      swap(*this,rhs);
      return *this;
    }

  public:

    const std::vector<Real>& getXData() const { return xData; }
    const std::vector<Real>& getYData() const { return yData; }
    const std::vector<Real>& getZData() const { return zData; }
    std::vector<Real> getXData() { return xData; }
    std::vector<Real> getYData() { return yData; }
    std::vector<Real> getZData() { return zData; }

    /**
     * Set references to the interpolated data to memory outside the interpolator.
     * This is potentially *unsafe*. The interpolator will not take ownership of the memory,
     * the caller will still be required to free it when it is no longer needed. Freeing memory
     * before calling the interpolator may cause uninitialized memory access and is undefined
     * behavior.
     *
     * Modifying the data after setting the interpolator references is undefined behavior as some
     * interpolators do some setup work that will be negated when the data has changed.
     */
    template<typename I>
    void
    setUnsafeDataReference( I n, const Real *x, const Real *y, const Real *z)
    {
      this->xView.reset( new MapType( x, n ) );
      this->yView.reset( new MapType( y, n ) );
      this->zView.reset( new MapType( z, n ) );

      this->setup2DDataViews();
      this->callSetupInterpolator<Derived>();
    }

    /**
     * Set the data taht will be interpolated from a set of iterators.
     */
    template<typename XIter, typename YIter, typename ZIter>
    auto
    setData( const XIter &x_begin, const XIter &x_end, const YIter &y_begin, const YIter &y_end, const ZIter &z_begin, const ZIter &z_end)
    -> decltype(std::copy(x_begin,x_end,xData.begin()),std::copy(y_begin,y_end,yData.begin()),std::copy(z_begin,z_end,zData.begin()),void())
    {
      xData.clear();
      yData.clear();
      zData.clear();
      xData.reserve(x_end-x_begin);
      yData.reserve(y_end-y_begin);
      zData.reserve(z_end-z_begin);
      std::copy( x_begin, x_end, std::back_inserter(xData) );
      std::copy( y_begin, y_end, std::back_inserter(yData) );
      std::copy( z_begin, z_end, std::back_inserter(zData) );

      this->setUnsafeDataReference( xData.size(), xData.data(), yData.data(), zData.data() );
    }

    /**
     * Reads data from x, y, and z arrays. Arrays should all be the same length with each element corresponding to a data point.
     * Basically, x[i], y[i], and z[i] should correspond to the first, second, and third columns of a gnuplot file.
     */
    template<typename I, typename X, typename Y, typename Z>
    void
    setData( I n, const X *x, const Y *y, const Z *z )
    {
      this->setData(x,x+n,y,y+n,z,z+n);
    }

    /**
     * Set the data to be interpolated from vector-like containers that provide a .data() and .size()
     * methods.
     */
    template<typename XT, typename YT, typename ZT>
    auto
    setData( const XT &x, const YT &y, const ZT &z )
    -> decltype( x.size(), x.data(), y.data(), z.data(), void())
    {
      return this->setData( x.size(), x.data(), y.data(), z.data() );
    }

    /**
     * Given an x value, returns the index i of the stored x data X
     * that is just to the "left" of x such that X[i] < x < X[i+1]
     */
    int
    get_x_index_to_left_of(Real x) const
    {
      // NOTE: X data is strided.
      auto xrng = std::make_pair( X->data(), X->data()+X->size()*X->innerStride() ) | boost::adaptors::strided(X->innerStride());
      return boost::lower_bound( xrng, x) - boost::begin(xrng) - 1;
    }
    /**
     * Given an x value, returns the index i of the stored x data X
     * that is just to the "right" of x such that X[i-1] < x < X[i]
     */
    int
    get_x_index_to_right_of(Real x) const
    {
      return this->get_x_index_to_right_of(x)+1;
    }

    /**
     * Given an y value, returns the index i of the stored y data Y
     * that is just "below" y such that Y[i] < y < Y[i+1]
     */
    int
    get_y_index_below(Real y) const
    {
    // NOTE: Y data is NOT strided
      auto yrng = std::make_pair( Y->data(), Y->data()+Y->size() );
      return boost::lower_bound( yrng, y) - boost::begin(yrng) - 1;
    }
    /**
     * Given an y value, returns the index i of the stored y data y
     * that is just "above" y such that Y[i-1] < y < Y[i]
     */
    int
    get_y_index_above(Real y) const
    {
      return this->get_y_index_below(y)+1;
    }

  protected:
    void checkData() const; ///< Check that data has been initialized and throw exception if not.
    void setup2DDataViews(); ///< Setups up 2D views of 1D data arrays

  private:
    // callSetupInterpolator will call a function named setupInterpolator in the derived class, if
    // it exists. this is just some template magic to detect if the derived class has implemented a
    // setupInterpolator function, and to call it if it does.

    template<typename T>
    struct has_setupInterpolator
    {
      private:
        typedef std::true_type yes;
        typedef std::false_type no;

        template<typename U> static auto test(int) -> decltype(std::declval<U>().setupInterpolator(),yes());
        template<typename  > static no   test(...);

      public:
        static constexpr bool value = std::is_same<decltype(test<T>(0)),yes>::value;
    };

    template<typename T>
    typename std::enable_if<has_setupInterpolator<T>::value>::type
    callSetupInterpolator(){ static_cast<Derived*>(this)->setupInterpolator(); }

    template<typename T>
    typename std::enable_if<!has_setupInterpolator<T>::value>::type
    callSetupInterpolator(){ }

};

template<class Derived, typename Real>
void
InterpolatorBase<Derived,Real>::checkData() const
{
  if(!this->xView || !this->yView || !this->zView)
    throw std::logic_error("Interpolator data is not initialized. Did you call setData()?");
  if(this->xView->size() == 0 || this->yView->size() == 0 || this->zView->size() == 0)
    throw std::logic_error("Interpolator data is zero size. Did you call setData() with non-zero sized vectors?");
}

template<class Derived, typename Real>
void
InterpolatorBase<Derived,Real>::setup2DDataViews()
{
  // make sure data has actually been initialized
  if( !xView || !yView || !zView )
    return;

  // setup 2D view of the data
  // We need to figure out what the x and y dimensions are.
  int N = zView->size();
  int Nx = 0, Ny = 0;
  // Ny will be the number of elements that have the same x coordinate
  Real xlast = (*xView)(0);
  while( Ny < N-1 && fabs((*xView)(Ny)-xlast) < 1e-40 )
    Ny++;
  Nx = N/Ny;

  // consecutive values in the x data are separated by Ny, so this is the inner stride for X
  X.reset( new _2DVectorView( xView->data(), Nx, Eigen::InnerStride<Eigen::Dynamic>(Ny) ) );

  // consecutive values in the y data are next to each other, so the stride is just 1
  Y.reset( new _2DVectorView( yView->data(), Ny, Eigen::InnerStride<Eigen::Dynamic>(1) ) );

  // Eigen defaults to COLUMN MAJOR
  // consecutive elements in a column are separated by Ny (this is the inner stride)
  // consecutive elements in a row are located next to each other (this is the outer stride)
  // Stride object takes outer,inner as arguments.
  Z.reset( new _2DMatrixView( zView->data(), Nx, Ny, Eigen::Stride<Eigen::Dynamic,Eigen::Dynamic>(1,Ny) ) );
}

}

#endif // include protector


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file ObjectSegmentation.cpp
 * @author Johann Prankl (prankl@acin.tuwien.ac.at), Aitor Aldoma (aldoma@acin.tuwien.ac.at)
 * @date 2015
 * @brief
 *
 */

#ifndef Q_MOC_RUN
#include "ObjectSegmentation.h"

#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <boost/filesystem.hpp>
#include <cmath>

#include <pcl/segmentation/extract_clusters.h>
#include <v4r/common/convertCloud.h>
#include <v4r/common/convertImage.h>
#include <v4r/common/convertNormals.h>
#include <v4r/common/noise_models.h>
#include <v4r/registration/MvLMIcp.h>
#include <v4r/registration/noise_model_based_cloud_integration.h>
#include <opencv2/highgui/highgui.hpp>
#include <v4r/keypoints/impl/PoseIO.hpp>
#include <v4r/keypoints/impl/invPose.hpp>
#include <v4r/reconstruction/impl/projectPointToImage.hpp>

//#define ER_TEST_MLS

#ifdef ER_TEST_MLS
#include <pcl/surface/mls.h>
#endif
#endif

using namespace std;

/**
 * @brief ObjectSegmentation::ObjectSegmentation
 */
ObjectSegmentation::ObjectSegmentation()
: cmd(UNDEF), m_run(false), image_idx(-1), first_click(true), have_roi(false), roi_pose(Eigen::Matrix4f::Identity()),
  bb_min(Eigen::Vector3f(0., 0., 0.)), bb_max(Eigen::Vector3f(0., 0., 0.)),
  object_base_transform(Eigen::Matrix4f::Identity()), use_roi_segmentation(false), roi_offs(0.01), use_dense_mv(false) {
  v4r::ZAdaptiveNormals::Parameter n_param;
  n_param.adaptive = true;
  nest.reset(new v4r::ZAdaptiveNormals(n_param));

  max_point_dist = 0.03;  // ec clustering threshold

  v4r::ClusterNormalsToPlanes::Parameter p_param;
  p_param.thrAngle = 45;
  p_param.inlDist = 0.01;
  p_param.minPoints = 5000;
  p_param.least_squares_refinement = false;
  p_param.smooth_clustering = true;
  p_param.thrAngleSmooth = 30;
  p_param.inlDistSmooth = 0.01;
  p_param.minPointsSmooth = 3;
  pest.reset(new v4r::ClusterNormalsToPlanes(p_param));

  vx_size = 0.001;
  max_dist = 0.01f;
  max_iterations = 50;
  diff_type = 1;

  tmp_cloud1.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
  tmp_cloud2.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
  oc_cloud.reset(new Sensor::AlignedPointXYZRGBVector());
  ncloud_filt.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
}

/**
 * @brief ObjectSegmentation::~ObjectSegmentation
 */
ObjectSegmentation::~ObjectSegmentation() {}

/******************************** public *******************************/

/**
 * @brief ObjectSegmentation::start
 * @param cam_id
 */
void ObjectSegmentation::start() {
  QThread::start();
}

/**
 * @brief ObjectSegmentation::stop
 */
void ObjectSegmentation::stop() {
  if (m_run) {
    m_run = false;
    this->wait();
  }
}

/**
 * @brief ObjectSegmentation::isRunning
 * @return
 */
bool ObjectSegmentation::isRunning() {
  return m_run;
}

/**
 * @brief ObjectSegmentation::finishedSegmentation
 */
void ObjectSegmentation::finishSegmentation() {
  cmd = FINISH_OBJECT_MODELLING;
  start();
}

/**
 * @brief ObjectSegmentation::optimizeMultiview
 */
bool ObjectSegmentation::optimizeMultiview() {
  if (clouds.get() != 0 && clouds->size() > 0) {
    cmd = OPTIMIZE_MULTIVIEW;
    start();
    return true;
  }
  return false;
}

/**
 * @brief ObjectSegmentation::activateROI
 * @param enable
 */
void ObjectSegmentation::activateROI(bool enable) {
  if (fabs(bb_min[0] - bb_max[0]) < 0.00001)
    have_roi = false;
  else
    have_roi = enable;
}

/**
 * @brief ObjectSegmentation::set_segmentation_params
 * @param use_roi_segm
 * @param offs
 * @param _use_dense_mv
 */
void ObjectSegmentation::set_segmentation_params(bool use_roi_segm, const double &offs, bool _use_dense_mv,
                                                 const double &_edge_radius_px) {
  use_roi_segmentation = use_roi_segm;
  roi_offs = offs;
  use_dense_mv = _use_dense_mv;
  om_params.edge_radius_px = _edge_radius_px;
}

/**
 * @brief ObjectSegmentation::set_roi
 * @param _bb_min
 * @param _bb_max
 * @param _roi_pose
 */
void ObjectSegmentation::set_roi(const Eigen::Vector3f &_bb_min, const Eigen::Vector3f &_bb_max,
                                 const Eigen::Matrix4f &_roi_pose) {
  bb_min = _bb_min;
  bb_max = _bb_max;
  object_base_transform = roi_pose = _roi_pose;
  have_roi = true;
}

/**
 * @brief ObjectSegmentation::cam_params_changed
 * @param _cam_params
 */
void ObjectSegmentation::cam_params_changed(const RGBDCameraParameter &_cam_params) {
  intrinsic = cv::Mat_<double>::eye(3, 3);

  intrinsic(0, 0) = _cam_params.f[0];
  intrinsic(1, 1) = _cam_params.f[1];
  intrinsic(0, 2) = _cam_params.c[0];
  intrinsic(1, 2) = _cam_params.c[1];
}

/**
 * @brief ObjectSegmentation::segmentation_parameter_changed
 * @param param
 */
void ObjectSegmentation::segmentation_parameter_changed(const SegmentationParameter &param) {
  seg_params = param;

  v4r::ClusterNormalsToPlanes::Parameter p_param;

  p_param.thrAngle = 45;
  p_param.inlDist = seg_params.inl_dist_plane;
  p_param.minPoints = seg_params.min_points_plane;
  p_param.least_squares_refinement = false;
  p_param.smooth_clustering = true;
  p_param.thrAngleSmooth = seg_params.thr_angle;
  p_param.inlDistSmooth = 0.01;
  p_param.minPointsSmooth = 3;

  pest.reset(new v4r::ClusterNormalsToPlanes(p_param));
}

/**
 * @brief ObjectSegmentation::object_modelling_parameter_changed
 * @param param
 */
void ObjectSegmentation::object_modelling_parameter_changed(const ObjectModelling &param) {
  om_params = param;
}

/**
 * @brief ObjectSegmentation::storeMasks
 * @param _folder
 */
void ObjectSegmentation::storeMasks(const std::string &_folder) {
  char filename[PATH_MAX];

  std::string mask_names = _folder + "/mask_%04d.png";

  for (unsigned i = 0; i < masks.size(); i++) {
    snprintf(filename, PATH_MAX, mask_names.c_str(), i);
    cv::imwrite(filename, masks[i]);
  }
}

/**
 * @brief ObjectSegmentation::storePointCloudModel
 * @param _folder
 */
bool ObjectSegmentation::storePointCloudModel(const std::string &_folder) {
  pcl::PointCloud<pcl::PointXYZRGB> &cloud = *tmp_cloud1;
  const Sensor::AlignedPointXYZRGBVector &oc = *oc_cloud;

  cloud.resize(oc.size());
  cloud.width = oc.size();
  cloud.height = 1;
  cloud.is_dense = true;

  for (unsigned i = 0; i < oc.size(); i++)
    cloud.points[i] = oc[i];

  if (cloud.points.size() > 0) {
    pcl::io::savePCDFileBinaryCompressed(_folder + "/model.pcd", cloud);
    return true;
  }

  return false;
}

/**
 * @brief ObjectSegmentation::savePointClouds
 * @param _folder
 * @param _modelname
 * @return
 */
bool ObjectSegmentation::savePointClouds(const std::string &_folder, const std::string &_modelname) {
  if (octree_cloud.get() == 0 || big_normals.get() == 0 || clouds.get() == 0 || clouds->empty() ||
      octree_cloud->empty() || octree_cloud->points.size() != big_normals->points.size() ||
      clouds->size() != indices.size())
    return false;

  char filename[PATH_MAX];
  boost::filesystem::create_directories(_folder + "/models/" + _modelname + "/views");

  // store global model
  pcl::io::savePCDFileBinaryCompressed(_folder + "/models/" + _modelname + "/3D_model.pcd", *ncloud_filt);

  std::string cloud_names = _folder + "/models/" + _modelname + "/views/cloud_%08d.pcd";
  std::string image_names = _folder + "/models/" + _modelname + "/views/image_%08d.jpg";
  std::string pose_names = _folder + "/models/" + _modelname + "/views/pose_%08d.txt";
  std::string mask_names = _folder + "/models/" + _modelname + "/views/mask_%08d.png";
  std::string idx_names = _folder + "/models/" + _modelname + "/views/object_indices_%08d.txt";

  for (unsigned i = 0; i < clouds->size(); i++) {
    if (indices[i].empty())
      continue;

    // store indices
    snprintf(filename, PATH_MAX, idx_names.c_str(), i);
    std::ofstream mask_f(filename);
    for (unsigned j = 0; j < indices[i].size(); j++)
      mask_f << indices[i][j] << std::endl;
    mask_f.close();

    // store cloud
    snprintf(filename, PATH_MAX, cloud_names.c_str(), i);
    pcl::io::savePCDFileBinaryCompressed(filename, *clouds->at(i).second);

    // store image
    v4r::convertImage(*clouds->at(i).second, image);
    snprintf(filename, PATH_MAX, image_names.c_str(), i);
    cv::imwrite(filename, image);

    // store poses
    snprintf(filename, PATH_MAX, pose_names.c_str(), i);
    v4r::writePose(filename, std::string(), inv_poses[i]);

    // store masks
    snprintf(filename, PATH_MAX, mask_names.c_str(), i);
    cv::imwrite(filename, masks[i]);
  }

  return true;
}

/**
 * @brief ObjectSegmentation::set_image
 * @param idx
 */
void ObjectSegmentation::set_image(int idx) {
  if (clouds->size() == 0 || idx < 0 || idx >= int(clouds->size()))
    return;

  image_idx = idx;

  if (masks.size() == clouds->size())
    getMaskedImage(*(*clouds)[image_idx].second, masks[image_idx], image, 0.5);
  else
    convertImage(*clouds->at(idx).second, image);

  emit new_image(clouds->at(idx).second, image);
  emit update_visualization();
}

/**
 * @brief ObjectSegmentation::segment_image
 * @param x
 * @param y
 */
void ObjectSegmentation::segment_image(int x, int y) {
  if (image_idx < 0 || image_idx >= int(clouds->size()) || labels.size() != clouds->size())
    return;

  const pcl::PointCloud<pcl::PointXYZRGB> &c_cloud = *(*clouds)[image_idx].second;
  const Eigen::Matrix4f &glob_pose = cameras[(*clouds)[image_idx].first];

  // init masks
  if (first_click) {
    for (unsigned i = 0; i < masks.size(); i++)
      masks[i] = cv::Mat_<unsigned char>::zeros(c_cloud.height, c_cloud.width);
  }

  cv::Mat_<int> &ls = labels[image_idx];
  cv::Mat_<unsigned char> &mask = masks[image_idx];

  if (mask.cols != int(c_cloud.width) || mask.rows != int(c_cloud.height) || ls.cols != int(c_cloud.width) ||
      ls.rows != int(c_cloud.height))
    return;

  std::vector<v4r::ClusterNormalsToPlanes::Plane::Ptr> &cls = planes[image_idx];
  int segm_idx = ls(y, x);

  if (segm_idx >= int(cls.size()))
    return;

  unsigned char mark_obj = (first_click || mask(y, x) < 128 ? 255 : 0);

  v4r::ClusterNormalsToPlanes::Plane &plane = *cls[segm_idx];

  // mark object
  for (unsigned i = 0; i < plane.size(); i++)
    mask(plane.indices[i]) = mark_obj;

  // propagate to all other images
  if (first_click) {
    first_click = false;
    Eigen::Matrix4f inv_pose;
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(c_cloud, plane.indices, centroid);
    v4r::invPose(glob_pose, inv_pose);
    Eigen::Vector3f glob_centroid = (inv_pose * centroid).segment<3>(0);

    for (unsigned i = 0; i < masks.size(); i++) {
      if (int(i) == image_idx)
        continue;

      Eigen::Matrix4f &pose = cameras[(*clouds)[i].first];
      Eigen::Vector3f pt = pose.topLeftCorner<3, 3>() * glob_centroid + pose.block<3, 1>(0, 3);

      cv::Point2f im_pt, tmp_pt;

      if (dist_coeffs.empty())
        v4r::projectPointToImage(&pt[0], &intrinsic(0, 0), &im_pt.x);
      else
        v4r::projectPointToImage(&pt[0], &intrinsic(0, 0), &dist_coeffs(0, 0), &im_pt.x);

      int _segm_idx = INT_MAX;

      if (im_pt.x >= 0 && im_pt.x < mask.cols && im_pt.y >= 0 && im_pt.y < mask.rows)
        _segm_idx = labels[i]((int)(im_pt.y + .5), (int)(im_pt.x + .5));

      if (_segm_idx > (int)planes[i].size()) {
        for (int v = -1; v <= 1; v++) {
          for (int u = -1; u <= 1; u++) {
            tmp_pt = im_pt + cv::Point2f(u, v);

            if (tmp_pt.x >= 0 && tmp_pt.x < mask.cols && tmp_pt.y >= 0 && tmp_pt.y < mask.rows) {
              _segm_idx = labels[i]((int)(tmp_pt.y + .5), (int)(tmp_pt.x + .5));
            }
          }
        }
      }

      if (_segm_idx >= (int)planes[i].size())
        continue;

      v4r::ClusterNormalsToPlanes::Plane &_plane = *planes[i][_segm_idx];
      cv::Mat_<unsigned char> &_mask = masks[i];

      // mark object
      for (unsigned j = 0; j < _plane.size(); j++)
        _mask(_plane.indices[j]) = mark_obj;
    }
  }

  // get masked image
  getMaskedImage(*(*clouds)[image_idx].second, mask, image, 0.5);

  emit new_image((*clouds)[image_idx].second, image);
  emit update_visualization();
}

/**
 * @brief ObjectSegmentation::setData
 * @param _cameras
 * @param _clouds
 */
void ObjectSegmentation::setData(
    const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> &_cameras,
    const std::shared_ptr<std::vector<std::pair<int, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr>>> &_clouds) {
  cameras = _cameras;
  clouds = _clouds;

  if (clouds->size() <= 0)
    return;

  // segment planes and smooth clusters
  labels.resize(clouds->size());
  masks.resize(clouds->size());
  planes.resize(clouds->size());
  normals.resize(clouds->size());

  v4r::DataMatrix2D<Eigen::Vector3f>::Ptr kp_cloud(new v4r::DataMatrix2D<Eigen::Vector3f>());
  v4r::DataMatrix2D<Eigen::Vector3f>::Ptr kp_normals(new v4r::DataMatrix2D<Eigen::Vector3f>());

  for (unsigned i = 0; i < clouds->size(); i++) {
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &c = (*clouds)[i].second;
    cv::Mat_<int> &ls = labels[i];
    ls = cv::Mat_<int>::ones(c->height, c->width) * INT_MAX - 1;

    v4r::convertCloud(*c, *kp_cloud);

    if (have_roi && use_roi_segmentation)
      createMaskFromROI(*kp_cloud, masks[i], object_base_transform, bb_min, bb_max, roi_offs);
    else
      masks[i] = cv::Mat_<unsigned char>::ones(c->height, c->width) * 255;

    nest->compute(*kp_cloud, *kp_normals);
    pest->compute(*kp_cloud, *kp_normals, planes[i]);

    pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
    v4r::convertNormals(*kp_normals, *normal);
    normals[i] = normal;

    // cv::Mat_<cv::Vec3b> tmp_labels(cv::Mat_<cv::Vec3b>::zeros(c->height, c->width));

    for (unsigned j = 0; j < planes[i].size(); j++) {
      v4r::ClusterNormalsToPlanes::Plane &plane = *planes[i][j];
      // cv::Vec3b col(rand()%255,rand()%255,rand()%255);

      for (unsigned k = 0; k < plane.size(); k++) {
        ls(plane.indices[k]) = j;
        // tmp_labels(plane.indices[k]) = col;
      }
    }

    // cv::imwrite("log/labels.png", tmp_labels);
  }

  // vis. first image
  if (have_roi && use_roi_segmentation)
    first_click = false;
  else
    first_click = true;

  image_idx = 0;

  if (have_roi && use_roi_segmentation)
    getMaskedImage(*clouds->at(image_idx).second, masks[image_idx], image, 0.5);
  else
    convertImage(*clouds->at(image_idx).second, image);

  emit new_image(clouds->at(0).second, image);
  emit update_visualization();
}

/**
 * @brief ObjectSegmentation::drawObjectCloud
 */
void ObjectSegmentation::drawObjectCloud() {
  // createObjectCloud();
  // createObjectCloudFiltered();

  emit update_model_cloud(oc_cloud);
  emit update_visualization();
}

/*********************************** private *******************************************/
/**
 * @brief ObjectSegmentation::run
 * main loop
 */
void ObjectSegmentation::run() {
  m_run = true;

  switch (cmd) {
    case FINISH_OBJECT_MODELLING: {
      if (clouds.get() != 0 && clouds->size() > 0)
        postProcessingSegmentation(false);

      emit set_object_base_transform(object_base_transform);
      emit finishedObjectSegmentation();
      break;
    }
    case OPTIMIZE_MULTIVIEW: {
      if (clouds.get() != 0 && clouds->size() > 0)
        postProcessingSegmentation(true);

      emit set_object_base_transform(object_base_transform);
      emit finishedObjectSegmentation();
      break;
    }
    default:
      break;
  }

  cmd = UNDEF;
  m_run = false;
}

/**
 * @brief ObjectSegmentation::postProcessingSegmentation
 */
void ObjectSegmentation::postProcessingSegmentation(bool do_mv) {
  if (!have_roi) {
    detectCoordinateSystem(roi_pose);
  }

  unsigned cnt = 0;
  Eigen::Vector3d centroid(Eigen::Vector3d::Zero());
  Eigen::Matrix4f inv_pose, transform_centroid(Eigen::Matrix4f::Identity());

  for (unsigned i = 0; i < clouds->size() && i < masks.size(); i++) {
    const pcl::PointCloud<pcl::PointXYZRGB> &cloud = *clouds->at(i).second;
    const cv::Mat_<unsigned char> &mask = masks[i];
    v4r::invPose(cameras[clouds->at(i).first] * roi_pose, inv_pose);
    Eigen::Matrix3f R = inv_pose.topLeftCorner<3, 3>();
    Eigen::Vector3f t = inv_pose.block<3, 1>(0, 3);

    for (unsigned j = 0; j < cloud.points.size(); j++) {
      const Eigen::Vector3f &pt = cloud.points[j].getVector3fMap();
      if (mask(j) > 128 && !std::isnan(pt[0]) && !std::isnan(pt[1]) && !std::isnan(pt[2])) {
        cnt++;
        centroid += (R * pt + t).cast<double>();
      }
    }
  }

  if (cnt > 0) {
    transform_centroid(0, 3) = centroid[0] / (double)cnt;
    transform_centroid(1, 3) = centroid[1] / (double)cnt;
  }

  object_base_transform = roi_pose * transform_centroid;

  // use Aitor's mv optimization
  if (use_dense_mv || do_mv) {
    emit printStatus("Status: Dense multiview optimization ... Please be patient...");

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>);
    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds_filtered(clouds->size());
    std::vector<pcl::PointCloud<pcl::Normal>::Ptr> normals_segmented;

    inv_poses.resize(clouds->size());
    indices.resize(clouds->size());
    normals_segmented.resize(clouds->size());

    for (unsigned i = 0; i < clouds->size() && i < masks.size(); i++) {
      indices[i].clear();
      clouds_filtered[i].reset(new pcl::PointCloud<pcl::PointXYZRGB>());
      normals_segmented[i].reset(new pcl::PointCloud<pcl::Normal>());

      const pcl::PointCloud<pcl::PointXYZRGB> &cloud = *clouds->at(i).second;
      const cv::Mat_<unsigned char> &mask = masks[i];

      for (int j = 0; j < mask.rows * mask.cols; j++)
        if (mask(j) > 128 && !std::isnan(cloud.points[j].x))
          indices[i].push_back(j);

      pcl::copyPointCloud(cloud, indices[i], *cloud_segmented);

      pcl::VoxelGrid<pcl::PointXYZRGB> filter;
      filter.setInputCloud(cloud_segmented);
      filter.setDownsampleAllData(true);
      filter.setLeafSize(vx_size, vx_size, vx_size);
      filter.filter(*clouds_filtered[i]);

      pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> normal_est;
      normal_est.setInputCloud(clouds_filtered[i]);
      normal_est.setRadiusSearch(0.015f);
      normal_est.compute(*normals_segmented[i]);

      v4r::invPose(cameras[clouds->at(i).first] * object_base_transform, inv_poses[i]);
    }

    v4r::Registration::MvLMIcp<pcl::PointXYZRGB> nl_icp;
    nl_icp.setInputClouds(clouds_filtered);
    nl_icp.setPoses(inv_poses);
    nl_icp.setMaxCorrespondenceDistance(max_dist);
    nl_icp.setMaxIterations(max_iterations);
    nl_icp.setDiffType(diff_type);
    // nl_icp.setNormals(normals_segmented);
    // nl_icp.setNormalDot(0.5f);
    nl_icp.compute();

    inv_poses = nl_icp.getFinalPoses();

    Eigen::Matrix4f inv_base, tmp;
    v4r::invPose(object_base_transform, inv_base);

    for (unsigned i = 0; i < clouds->size() && i < masks.size(); i++) {
      v4r::invPose(inv_poses[i], tmp);
      cameras[clouds->at(i).first] = tmp * inv_base;
    }
  }

  // createObjectCloud();
  createObjectCloudFiltered();
}

/**
 * @brief ObjectSegmentation::createMaskFromROI
 * @param cloud
 * @param mask
 * @param object_base_transform
 * @param bb_min
 * @param bb_max
 * @param roi_offs
 */
void ObjectSegmentation::createMaskFromROI(const v4r::DataMatrix2D<Eigen::Vector3f> &cloud,
                                           cv::Mat_<unsigned char> &mask, const Eigen::Matrix4f &_object_base_transform,
                                           const Eigen::Vector3f &_bb_min, const Eigen::Vector3f &_bb_max,
                                           const double &_roi_offs) {
  Eigen::Vector3f pt;
  Eigen::Matrix4f inv_pose;

  v4r::invPose(_object_base_transform, inv_pose);

  Eigen::Matrix3f R = inv_pose.topLeftCorner<3, 3>();
  Eigen::Vector3f t = inv_pose.block<3, 1>(0, 3);

  mask = cv::Mat_<unsigned char>::zeros(cloud.rows, cloud.cols);

  for (unsigned i = 0; i < cloud.data.size(); i++) {
    const Eigen::Vector3f &pt0 = cloud.data[i];

    if (!std::isnan(pt0[0]) && !std::isnan(pt0[1]) && !std::isnan(pt0[2])) {
      pt = R * pt0 + t;

      if (pt[0] > _bb_min[0] && pt[0] < _bb_max[0] && pt[1] > _bb_min[1] && pt[1] < _bb_max[1] && pt[2] > _roi_offs &&
          pt[2] < _bb_max[2])
        mask(i) = 255;
    }
  }
}

/**
 * @brief ObjectSegmentation::createObjectCloud
 */
void ObjectSegmentation::createObjectCloud() {
  oc_cloud->clear();

  if (clouds->size() == 0 || masks.size() != clouds->size())
    return;

  std::vector<std::pair<int, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr>> &ref_clouds = *clouds;

  if (ref_clouds.size() > 0) {
    Eigen::Matrix4f inv_pose;
    octree.reset(new pcl::octree::OctreePointCloudVoxelCentroid<
                 pcl::PointXYZRGB, pcl::octree::OctreeVoxelCentroidContainerXYZRGB<pcl::PointXYZRGB>>(
        om_params.vx_size_object));

    for (unsigned i = 0; i < ref_clouds.size(); i++) {
      v4r::invPose(cameras[ref_clouds[i].first], inv_pose);

      segmentObject(*ref_clouds[i].second, masks[i], *tmp_cloud1);

      pcl::transformPointCloud(*tmp_cloud1, *tmp_cloud2, inv_pose);
      octree->setInputCloud(tmp_cloud2);
      octree->addPointsFromInputCloud();
    }

    octree->getVoxelCentroids(*oc_cloud);
  }
}

/**
 * @brief ObjectSegmentation::createObjectCloudFiltered
 */
void ObjectSegmentation::createObjectCloudFiltered() {
  oc_cloud->clear();

  if (clouds->size() == 0 || masks.size() != clouds->size())
    return;

  v4r::NguyenNoiseModelParameter nmparam;
  v4r::NguyenNoiseModel<pcl::PointXYZRGB> nm(nmparam);
  std::vector<std::pair<int, pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr>> &ref_clouds = *clouds;
  std::vector<std::vector<std::vector<float>>> pt_properties(ref_clouds.size());
  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr> ptr_clouds(ref_clouds.size());
  inv_poses.clear();
  indices.clear();
  inv_poses.resize(ref_clouds.size());
  indices.resize(ref_clouds.size());

  if (!ref_clouds.empty()) {
    for (unsigned i = 0; i < ref_clouds.size(); i++) {
      v4r::invPose(cameras[ref_clouds[i].first] * object_base_transform, inv_poses[i]);
      ptr_clouds[i] = ref_clouds[i].second;

      nm.setInputCloud(ref_clouds[i].second);
      nm.setInputNormals(normals[i]);
      nm.compute();
      pt_properties[i] = nm.getPointProperties();

      cv::Mat_<unsigned char> &m = masks[i];

      for (int j = 0; j < m.rows * m.cols; j++)
        if (m(j) > 128)
          indices[i].push_back(j);
    }

    v4r::NMBasedCloudIntegrationParameter _nmparam;
    _nmparam.octree_resolution_ = om_params.vx_size_object;
    _nmparam.min_px_distance_to_depth_discontinuity_ = om_params.edge_radius_px;
    _nmparam.min_points_per_voxel_ = 1;
    octree_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    v4r::NMBasedCloudIntegration<pcl::PointXYZRGB> nmIntegration(_nmparam);
    nmIntegration.setInputClouds(ptr_clouds);
    nmIntegration.setTransformations(inv_poses);
    nmIntegration.setInputNormals(normals);
    nmIntegration.setIndices(indices);
    nmIntegration.setPointProperties(pt_properties);
    nmIntegration.compute(octree_cloud);
    big_normals.reset(new pcl::PointCloud<pcl::Normal>);
    nmIntegration.getOutputNormals(big_normals);

// test mls
#ifdef ER_TEST_MLS
    pcl::PointCloud<pcl::PointXYZRGB> mls_points;
    pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> mls;
    mls.setComputeNormals(true);
    mls.setInputCloud(octree_cloud);
    mls.setPolynomialFit(true);
    mls.setSearchRadius(0.01);
    mls.process(mls_points);
    *octree_cloud = mls_points;
#endif

    // create model cloud with normals
    ncloud_filt.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ncloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
    pcl::concatenateFields(*octree_cloud, *big_normals, *ncloud);

    // filter ec
    pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
    tree->setInputCloud(ncloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZRGBNormal> ec;
    ec.setClusterTolerance(max_point_dist);
    ec.setMinClusterSize(50);
    ec.setSearchMethod(tree);
    ec.setInputCloud(ncloud);
    ec.extract(cluster_indices);

    int cnt_max = 0;
    std::vector<pcl::PointIndices>::const_iterator it_max = cluster_indices.end();
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end();
         ++it) {
      if (it->indices.size() > (unsigned)cnt_max) {
        it_max = it;
        cnt_max = it->indices.size();
      }
    }
    cout << "found " << cluster_indices.size() << " clusters" << endl;
    if (it_max != cluster_indices.end()) {
      cout << "use " << it_max->indices.size() << " of ";
      for (unsigned i = 0; i < cluster_indices.size(); i++)
        cout << cluster_indices[i].indices.size() << " ";
      cout << endl;
      Sensor::AlignedPointXYZRGBVector &ref_oc = *oc_cloud;
      ref_oc.resize(it_max->indices.size());
      for (unsigned i = 0; i < it_max->indices.size(); i++) {
        const pcl::PointXYZRGBNormal &pt = ncloud->points[it_max->indices[i]];
        ncloud_filt->points.push_back(pt);
        ref_oc[i].getVector4fMap() = pt.getVector4fMap();
        ref_oc[i].rgb = pt.rgb;
      }
      ncloud_filt->width = ncloud_filt->points.size();
      ncloud_filt->height = 1;
      ncloud_filt->is_dense = true;
    }
  }
}

/**
 * @brief ObjectSegmentation::segmentObject
 * @param cloud
 * @param mask
 * @param seg_cloud
 */
void ObjectSegmentation::segmentObject(const pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                                       const cv::Mat_<unsigned char> &mask,
                                       pcl::PointCloud<pcl::PointXYZRGB> &seg_cloud) {
  seg_cloud.points.clear();

  for (unsigned i = 0; i < cloud.points.size(); i++) {
    if (mask(i) > 128 && !isnan(cloud.points[i].getVector3fMap())) {
      seg_cloud.points.push_back(cloud.points[i]);
    }
  }

  seg_cloud.width = seg_cloud.points.size();
  seg_cloud.height = 1;
  seg_cloud.is_dense = true;
}

/**
 * @brief ObjectSegmentation::getInplaneCoordinateSystem
 * @param coeffs
 * @param centroid
 * @param pose
 */
void ObjectSegmentation::getInplaneTransform(const Eigen::Vector3f &pt, const Eigen::Vector3f &normal,
                                             Eigen::Matrix4f &pose) {
  pose.setIdentity();

  Eigen::Vector3f px, py;
  Eigen::Vector3f pz = normal;

  if (pt.dot(pz) > 0)
    pz *= -1;
  px = (Eigen::Vector3f(1, 0, 0).cross(pz)).normalized();
  py = (pz.cross(px)).normalized();

  pose.block<3, 1>(0, 0) = px;
  pose.block<3, 1>(0, 1) = py;
  pose.block<3, 1>(0, 2) = pz;
  pose.block<3, 1>(0, 3) = pt;

  //  //transform to camera
  //  Eigen::Matrix4f inv;
  //  v4r::invPose(pose,inv);
  //  pose = inv;
}

/**
 * @brief ObjectSegmentation::convertImage
 * @param cloud
 * @param image
 */
void ObjectSegmentation::convertImage(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, cv::Mat_<cv::Vec3b> &_image) {
  _image = cv::Mat_<cv::Vec3b>(cloud.height, cloud.width);

  for (unsigned v = 0; v < cloud.height; v++) {
    for (unsigned u = 0; u < cloud.width; u++) {
      cv::Vec3b &cv_pt = _image.at<cv::Vec3b>(v, u);
      const pcl::PointXYZRGB &pt = cloud(u, v);

      cv_pt[2] = pt.r;
      cv_pt[1] = pt.g;
      cv_pt[0] = pt.b;
    }
  }
}

/**
 * @brief ObjectSegmentation::getMaskedImage
 * @param cloud
 * @param mask
 * @param image
 * @param alpha
 */
void ObjectSegmentation::getMaskedImage(const pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                                        const cv::Mat_<unsigned char> &mask, cv::Mat_<cv::Vec3b> &_image, float alpha) {
  (void)alpha;
  _image = cv::Mat_<cv::Vec3b>(cloud.height, cloud.width);

  for (unsigned v = 0; v < cloud.height; v++) {
    for (unsigned u = 0; u < cloud.width; u++) {
      cv::Vec3b &cv_pt = _image.at<cv::Vec3b>(v, u);
      const pcl::PointXYZRGB &pt = cloud(u, v);

      if (mask(v, u) > 128) {
        cv_pt[2] = pt.r;
        cv_pt[1] = pt.g;
        cv_pt[0] = pt.b;
      } else {
        cv_pt[2] = pt.r * 0.5;
        cv_pt[1] = pt.g * 0.5;
        cv_pt[0] = pt.b * 0.5;
      }
    }
  }
}

/**
 * @brief StoreTrackingModel::detectCoordinateSystem
 * @param pose
 */
void ObjectSegmentation::detectCoordinateSystem(Eigen::Matrix4f &pose) {
  pose.setIdentity();

  pcl::PointXYZRGB pt;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr gcloud(new pcl::PointCloud<pcl::PointXYZRGB>());
  pcl::PointCloud<pcl::PointXYZRGB> &gc = *gcloud;
  Eigen::Vector3d centroid(0., 0., 0.);
  int cnt = 0;

  for (unsigned i = 0; i < clouds->size(); i++) {
    const pcl::PointCloud<pcl::PointXYZRGB> &cloud = *clouds->at(i).second;
    cv::Mat_<unsigned char> &mask = masks[i];
    Eigen::Matrix4f &_pose = cameras[clouds->at(i).first];
    Eigen::Matrix4f inv_pose;

    v4r::invPose(_pose, inv_pose);

    Eigen::Matrix3f R = inv_pose.topLeftCorner<3, 3>();
    Eigen::Vector3f t = inv_pose.block<3, 1>(0, 3);

    int xmin = INT_MAX, ymin = INT_MAX;
    int xmax = 0, ymax = 0;

    for (int v = 0; v < mask.rows; v++) {
      for (int u = 0; u < mask.cols; u++) {
        if (mask(v, u) < 128)
          continue;
        if (u < xmin)
          xmin = u;
        if (u > xmax)
          xmax = u;
        if (v < ymin)
          ymin = v;
        if (v > ymax)
          ymax = v;
      }
    }

    int deltax_h = (xmax - xmin) / 2.;
    int deltay_h = (ymax - ymin) / 2.;

    if (deltax_h <= 0 || deltay_h <= 0)
      continue;

    xmin = (xmin - deltax_h >= 0 ? xmin - deltax_h : 0);
    ymin = (ymin - deltay_h >= 0 ? ymin - deltay_h : 0);
    xmax = (xmax + deltax_h < mask.cols ? xmax + deltax_h : mask.cols - 1);
    ymax = (ymax + deltay_h < mask.rows ? ymax + deltay_h : mask.rows - 1);

    for (int v = ymin; v <= ymax; v++) {
      for (int u = xmin; u <= xmax; u++) {
        const pcl::PointXYZRGB &pt1 = cloud(u, v);

        if (std::isnan(pt1.x) || std::isnan(pt1.y) || std::isnan(pt1.z))
          continue;

        if (mask(v, u) < 128) {
          pt.getVector3fMap() = R * pt1.getVector3fMap() + t;
          gc.points.push_back(pt);
          centroid += pt.getVector3fMap().cast<double>();
          cnt++;
        }
      }
    }
  }

  gc.width = gc.points.size();
  gc.height = 1;
  gc.is_dense = true;

  if (cnt < 10 || gc.points.size() < 10)
    return;

  centroid /= double(cnt);

  // detect dominat plane
  Eigen::VectorXf plane_coeffs = Eigen::VectorXf(4);
  std::vector<int> inliers;

  pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p(
      new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>(gcloud));

  pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac(model_p);

  ransac.setDistanceThreshold(.05);
  ransac.computeModel();
  ransac.getInliers(inliers);

  if (inliers.size() < 10)
    return;

  model_p->optimizeModelCoefficients(inliers, plane_coeffs, plane_coeffs);

  // get coordinate system
  Eigen::Vector3f px, py;
  Eigen::Vector3f pz = plane_coeffs.segment<3>(0);

  if (centroid.dot(pz.cast<double>()) > 0)
    pz *= -1;
  px = (Eigen::Vector3f(1, 0, 0).cross(pz)).normalized();
  py = pz.cross(px);

  pose.block<3, 1>(0, 0) = px;
  pose.block<3, 1>(0, 1) = py;
  pose.block<3, 1>(0, 2) = pz;
  pose.block<3, 1>(0, 3) = centroid.cast<float>();
  pose.block<3, 1>(0, 3)[2] =
      -(plane_coeffs[0] * centroid[0] + plane_coeffs[1] * centroid[1] + plane_coeffs[3]) / plane_coeffs[2];
}


/*
 // Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
 // component of the Texas A&M University System.

 // All rights reserved.

 // The information and source code contained herein is the exclusive
 // property of TEES and may not be disclosed, examined or reproduced
 // in whole or in part without explicit written authorization from TEES.
 */

#include <iostream>
#include <sstream>
#include <map>

#include <stapl/utility/do_once.hpp>
#include <stapl/containers/array/array.hpp>
#include <stapl/views/array_view.hpp>
#include <stapl/views/functor_view.hpp>
#include <stapl/views/repeated_view.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include "agglomerativeclustering.hpp"
#include "cluster_testing.hpp"

using namespace stapl;

//////////////////////////////////////////////////////////////////////
/// @brief Generate a random point with a position between 0 and 1
///        in each dimension.
//////////////////////////////////////////////////////////////////////
struct random_point
{
  typedef my_point result_type;
  typedef std::size_t index_type;

  std::size_t m_seed;

  random_point(std::size_t seed = get_location_id())
    : m_seed(seed)
  { }

  result_type operator()(index_type const& idx) const
  {
    typedef boost::mt19937 engine_type;
    typedef boost::random::uniform_real_distribution<double> dist_type;
    typedef boost::variate_generator<engine_type, dist_type> generator_type;

    engine_type eng(idx*m_seed + idx);

    generator_type pos_gen(eng, dist_type(0, 1));

    my_point point;
    point.m_x = pos_gen();
    point.m_y = pos_gen();
    point.m_z = pos_gen();
    return point;
  }

  void define_type(stapl::typer& t)
  {
    t.member(m_seed);
  }
};

//////////////////////////////////////////////////////////////////////
/// @brief Fills an array with randomly generated points
//////////////////////////////////////////////////////////////////////
template <typename ArrayView>
void fill_with_random_points(ArrayView& av, size_t seed = get_location_id())
{
  using random_view_type =
    typename functor_view_type<functor_container<random_point, 1>>::type;

  // create a view of randomly generated points
  random_view_type random_points = functor_view(av.size(), random_point(seed));

  // populate the point view with random points
  copy(random_points, av);
}

stapl::exit_code stapl_main(int argc, char* argv[])
{
  typedef array<my_point> p_array_points;
  typedef array_view<p_array_points> parrayClustersView;
  typedef agglomerative_clustering::return_type clusters_type;

  int num_elem = (argc > 1) ? atoi(argv[1]) : 100;
  bool testdata = (argc > 2) ? atoi(argv[2]) : false;

  do_once([num_elem](void) {
    std::cerr << "Number of elements: " << num_elem << std::endl;
  });

  p_array_points p_array_test(num_elem);
  parrayClustersView p_view_test(p_array_test);

  if (!testdata) {
    fill_with_random_points(p_view_test);
  } else {
    fill_with_test_points(p_view_test);
  }

  stapl::counter<stapl::default_timer> t;
  t.start();
  agglomerative_clustering clustering;
  clusters_type clusters = clustering.cluster(p_view_test);
  t.stop();

  do_once([&t](void) {
    std::cerr << "TOTAL TIME ALGO: " << double(t.value()) << std::endl;
  });

  if (testdata) {
    cluster_testing(clusters);
  }

  #ifdef SHOW_RESULTS
  clustering.printgraph2dotfile(clusters);
  #endif
  return EXIT_SUCCESS;
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/contains.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/permutations.hpp>

#include <laws/base.hpp>
#include <support/minimal_product.hpp>
#include <support/seq.hpp>
namespace hana = boost::hana;

template <int i>
auto key() { return hana::test::ct_eq<i>{}; }

template <int i>
auto val() { return hana::test::ct_eq<-i>{}; }

template <int i, int j>
auto p() { return ::minimal_product(key<i>(), val<j>()); }

int main() {
    constexpr auto list = ::seq;

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::values(hana::make_map()),
        list()
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::values(hana::make_map(p<1, 1>())),
        list(val<1>())
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::contains(
        hana::permutations(list(val<1>(), val<2>())),
        hana::values(hana::make_map(p<1, 1>(), p<2, 2>()))
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::contains(
        hana::permutations(list(val<1>(), val<2>(), val<3>())),
        hana::values(hana::make_map(p<1, 1>(), p<2, 2>(), p<3, 3>()))
    ));
}


#include <Engine/MeshEdit/MinSurf.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

MinSurf::MinSurf(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void MinSurf::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool MinSurf::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	size_t nV = triMesh->GetPositions().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(triMesh->GetTriangles().size());
	for (auto triangle : triMesh->GetTriangles())
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary()) {
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundaries\n");
		heMesh->Clear();
		return false;
	}

	// triangle mesh's positions ->  half-edge structure's positions
	for (int i = 0; i < nV; i++) {
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool MinSurf::Run() {
	if (heMesh->IsEmpty() || !triMesh) {
		printf("ERROR::MinSurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	Minimize();

	// half-edge structure -> triangle mesh
	size_t nV = heMesh->NumVertices();
	size_t nF = heMesh->NumPolygons();
	vector<pointf3> positions;
	vector<unsigned> indice;
	positions.reserve(nV);
	indice.reserve(3 * nF);
	for (auto v : heMesh->Vertices())
		positions.push_back(v->pos.cast_to<pointf3>());
	for (auto f : heMesh->Polygons()) { // f is triangle
		for (auto v : f->BoundaryVertice()) // vertices of the triangle
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
	}

	triMesh->Init(indice, positions);

	return true;
}

void MinSurf::Minimize() {
	// TODO
	cout << "WARNING::MinSurf::Minimize:" << endl
		<< "\t" << "not implemented" << endl;
}


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

// Standard includes
#include <fstream>
#include <iomanip>
#include <iostream>

// Third party includes
#include <boost/lexical_cast.hpp>

// VOTCA includes
#include <votca/tools/constants.h>
#include <votca/tools/rangeparser.h>

// Local VOTCA includes
#include <votca/csg/beadlist.h>
#include <votca/csg/imcio.h>
#include <votca/csg/nblistgrid.h>

// Local private includes
#include "rdf_calculator.h"

namespace votca {
namespace csg {

RDFCalculator::RDFCalculator()
    : write_every_(0),
      do_blocks_(false),
      nblock_(0),
      subvol_rad_(0),
      do_vol_corr_(false),
      processed_some_frames_(false) {}

RDFCalculator::~RDFCalculator() = default;

// begin the coarse graining process
// here the data structures are prepared to handle all the data
void RDFCalculator::Initialize() {
  // do some output
  std::cout << "begin to calculate distribution functions\n";
  std::cout << "# of bonded interactions: " << bonded_.size() << std::endl;
  std::cout << "# of non-bonded interactions: " << nonbonded_.size()
            << std::endl;

  if (bonded_.size() + nonbonded_.size() == 0) {
    throw std::runtime_error(
        "No interactions defined in options xml-file - nothing to be done");
  }

  // initialize non-bonded structures
  for (Property *prop : nonbonded_) {
    interaction_t *i = AddInteraction(prop);
    i->is_bonded_ = false;
  }
}

void RDFCalculator::BeginEvaluate(Topology *top, Topology *) {
  Eigen::Matrix3d box = top->getBox();
  boxc_ = box.rowwise().sum() / 2.0;

  std::cout << "Using center of box: " << boxc_ << std::endl;
  // we didn't process any frames so far
  nframes_ = 0;
  nblock_ = 0;
  processed_some_frames_ = false;

  // initialize non-bonded structures
  for (Property *prop : nonbonded_) {
    std::string name = prop->get("name").value();

    interaction_t &i = *interactions_[name];

    // count total species for ideal densities

    BeadList allbeads1, allbeads2;
    allbeads1.Generate(*top, prop->get("type1").value());
    allbeads2.Generate(*top, prop->get("type2").value());

    if (allbeads1.size() == 0) {
      throw std::runtime_error("Topology does not have beads of type \"" +
                               prop->get("type1").value() +
                               "\"\n"
                               "This was specified in type1 of interaction \"" +
                               name + "\"");
    }
    if (allbeads2.size() == 0) {
      throw std::runtime_error("Topology does not have beads of type \"" +
                               prop->get("type2").value() +
                               "\"\n"
                               "This was specified in type2 of interaction \"" +
                               name + "\"");
    }
    // calculate normalization factor for rdf

    if (do_vol_corr_) {
      std::cout << "Volume correction on" << std::endl;
      i.norm_ = 1. / (4.0 * votca::tools::conv::Pi * i.step_);
    } else {
      std::cout << "Volume correction off" << std::endl;
      i.norm_ = 1. / (4. * votca::tools::conv::Pi * i.step_);
    }
  }
}
// create an entry for interactions
RDFCalculator::interaction_t *RDFCalculator::AddInteraction(Property *p) {
  std::string name = p->get("name").value();
  std::string group;

  group = "none";

  auto inter = std::make_unique<interaction_t>();
  interaction_t *i = inter.get();
  inter->index_ = interactions_.size();
  interactions_[name] = std::move(inter);
  getGroup(group)->interactions_.push_back(i);

  i->step_ = p->get("step").as<double>();
  i->min_ = p->get("min").as<double>();
  i->max_ = p->get("max").as<double>();
  i->norm_ = 1.0;
  i->p_ = p;

  // initialize the current and average histogram
  Index n = (Index)((i->max_ - i->min_) / i->step_ + 1.000000001);

  i->average_.Initialize(i->min_, i->max_ + i->step_, n);

  return i;
}

// end of trajectory, post processing data
void RDFCalculator::EndEvaluate() {
  if (nframes_ > 0) {
    if (!do_blocks_) {
      WriteDist();
    }
  }
  // clear interactions and groups
  interactions_.clear();
  groups_.clear();
  if (!processed_some_frames_) {
    throw std::runtime_error(
        "no frames were processed. Please check your input");
  }
}

// load options from xml file
void RDFCalculator::LoadOptions(const std::string &file) {
  options_.LoadFromXML(file);
  bonded_ = options_.Select("cg.bonded");
  nonbonded_ = options_.Select("cg.non-bonded");
}

// evaluate current conformation
void RDFCalculator::Worker::EvalConfiguration(Topology *top, Topology *) {
  cur_vol_ = 4.0 / 3.0 * votca::tools::conv::Pi * rdfcalculator_->subvol_rad_ *
             rdfcalculator_->subvol_rad_ * rdfcalculator_->subvol_rad_;
  // process non-bonded interactions
  DoNonbonded(top);
  // process bonded interactions
  DoBonded(top);
}

void RDFCalculator::ClearAverages() {

  nframes_ = 0;
  for (auto &interaction_ : interactions_) {
    interaction_.second->average_.Clear();
  }

  for (auto &group_ : groups_) {
    group_.second->corr_.setZero();
  }
}

class IMCNBSearchHandler {
 public:
  IMCNBSearchHandler(HistogramNew *hist, double subvol_rad,
                     Eigen::Vector3d boxc, bool do_vol_corr)
      : hist_(hist),
        subvol_rad_(subvol_rad),
        boxc_(boxc),
        do_vol_corr_(do_vol_corr) {}

  HistogramNew *hist_;
  double subvol_rad_;
  Eigen::Vector3d boxc_;  // center of box
  bool do_vol_corr_;

  bool FoundPair(Bead *b1, Bead *, const Eigen::Vector3d &, const double dist) {

    if (do_vol_corr_) {
      double dr = (b1->Pos() - boxc_).norm();
      if (dist + dr > subvol_rad_) {
        // 2.0 is because everything is normalized to 4 PI
        hist_->Process(dist, 2.0 / SurfaceRatio(dist, dr));
      } else {
        hist_->Process(dist);
      }

    } else {
      hist_->Process(dist);
    }
    return false;
  }

  double SurfaceRatio(double dist, double r) {
    // r: distance of particle from ex center
    // dist: distance between particles
    return (1.0 + (subvol_rad_ * subvol_rad_ - r * r - dist * dist) /
                      (2.0 * dist * r));
  }
};

// process non-bonded interactions for current frame
void RDFCalculator::Worker::DoNonbonded(Topology *top) {
  for (Property *prop : rdfcalculator_->nonbonded_) {
    std::string name = prop->get("name").value();

    interaction_t &i = *rdfcalculator_->interactions_[name];

    // generate the bead lists
    BeadList beads1, beads2;

    beads1.GenerateInSphericalSubvolume(*top, prop->get("type1").value(),
                                        rdfcalculator_->boxc_,
                                        rdfcalculator_->subvol_rad_);
    beads2.GenerateInSphericalSubvolume(*top, prop->get("type2").value(),
                                        rdfcalculator_->boxc_,
                                        rdfcalculator_->subvol_rad_);

    cur_beadlist_1_count_ = (double)beads1.size();
    cur_beadlist_2_count_ = (double)beads2.size();

    // same types, so put factor 1/2 because of already counted interactions
    if (prop->get("type1").value() == prop->get("type2").value()) {
      cur_beadlist_2_count_ /= 2.0;
    }

    // generate the neighbour list
    std::unique_ptr<NBList> nb;

    bool gridsearch = true;

    if (rdfcalculator_->options_.exists("cg.nbsearch")) {
      if (rdfcalculator_->options_.get("cg.nbsearch").as<std::string>() ==
          "grid") {
        gridsearch = true;
      } else if (rdfcalculator_->options_.get("cg.nbsearch")
                     .as<std::string>() == "simple") {
        gridsearch = false;
      } else {
        throw std::runtime_error("cg.nbsearch invalid, can be grid or simple");
      }
    }
    if (gridsearch) {
      nb = std::make_unique<NBListGrid>();
    } else {
      nb = std::make_unique<NBList>();
    }

    nb->setCutoff(i.max_ + i.step_);

    // clear the current histogram
    current_hists_[i.index_].Clear();

    IMCNBSearchHandler h(&(current_hists_[i.index_]),
                         rdfcalculator_->subvol_rad_, rdfcalculator_->boxc_,
                         rdfcalculator_->do_vol_corr_);
    nb->SetMatchFunction(&h, &IMCNBSearchHandler::FoundPair);

    // is it same types or different types?
    if (prop->get("type1").value() == prop->get("type2").value()) {
      nb->Generate(beads1);
    } else {
      nb->Generate(beads1, beads2);
    }

    // store particle number in subvolume for each interaction
    i.avg_beadlist_1_count_.Process(cur_beadlist_1_count_);
    i.avg_beadlist_2_count_.Process(cur_beadlist_2_count_);
  }
}

// process non-bonded interactions for current frame
void RDFCalculator::Worker::DoBonded(Topology *top) {
  for (Property *prop : rdfcalculator_->bonded_) {
    std::string name = prop->get("name").value();

    interaction_t &i = *rdfcalculator_->interactions_[name];

    // clear the current histogram
    current_hists_[i.index_].Clear();

    // now fill with new data
    std::vector<Interaction *> vec = top->InteractionsInGroup(name);

    for (auto ic : vec) {
      double v = ic->EvaluateVar(*top);
      current_hists_[i.index_].Process(v);
    }
  }
}

// returns a group, creates it if doesn't exist
RDFCalculator::group_t *RDFCalculator::getGroup(const std::string &name) {
  std::map<std::string, std::unique_ptr<group_t>>::iterator iter;
  iter = groups_.find(name);
  if (iter == groups_.end()) {
    return (groups_[name] = std::make_unique<group_t>()).get();
  }
  return (*iter).second.get();
}

// write the distribution function
void RDFCalculator::WriteDist(const std::string &suffix) {

  // for all interactions
  for (auto &interaction_ : interactions_) {
    // calculate the rdf
    Table &t = interaction_.second->average_.data();
    Table dist(t);

    interaction_.second->norm_ /=
        (interaction_.second->avg_beadlist_1_count_.getAvg() *
         interaction_.second->avg_beadlist_2_count_.getAvg());
    dist.y() = avg_vol_.getAvg() * interaction_.second->norm_ *
               dist.y().cwiseQuotient(dist.x().cwiseAbs2());

    dist.Save((interaction_.first) + suffix + ".dist.new");
    std::cout << "written " << (interaction_.first) + suffix + ".dist.new\n";

    std::cout << "Avg. number of particles in subvol for "
              << (interaction_.first) << std::endl;
    std::cout << "beadlist 1: "
              << interaction_.second->avg_beadlist_1_count_.getAvg()
              << std::endl;
    std::cout << "beadlist 2: "
              << interaction_.second->avg_beadlist_2_count_.getAvg()
              << std::endl;
  }

  std::cout << "Volume used for normalization: " << avg_vol_.getAvg()
            << std::endl;
}

std::unique_ptr<CsgApplication::Worker> RDFCalculator::ForkWorker() {
  auto worker = std::make_unique<RDFCalculator::Worker>();

  worker->current_hists_.resize(interactions_.size());
  worker->rdfcalculator_ = this;

  for (auto &interaction_ : interactions_) {
    interaction_t *i = interaction_.second.get();
    worker->current_hists_[i->index_].Initialize(
        i->average_.getMin(), i->average_.getMax(), i->average_.getNBins());
  }
  return worker;
}

void RDFCalculator::MergeWorker(CsgApplication::Worker *worker_) {
  processed_some_frames_ = true;
  RDFCalculator::Worker *worker =
      dynamic_cast<RDFCalculator::Worker *>(worker_);
  // update the average

  ++nframes_;

  avg_vol_.Process(worker->cur_vol_);

  for (auto &interaction_ : interactions_) {
    interaction_t *i = interaction_.second.get();
    i->average_.data().y() =
        (((double)nframes_ - 1.0) * i->average_.data().y() +
         worker->current_hists_[i->index_].data().y()) /
        (double)nframes_;
  }

  if (write_every_ != 0) {
    if ((nframes_ % write_every_) == 0) {
      nblock_++;
      std::string suffix =
          std::string("_") + boost::lexical_cast<std::string>(nblock_);
      WriteDist(suffix);
      if (do_blocks_) {
        ClearAverages();
      }
    }
  }
}

}  // namespace csg
}  // namespace votca


#include "../../SDP_Solver.hxx"
#include "../../read_text_block.hxx"

#include <boost/filesystem/fstream.hpp>

bool load_text_checkpoint(const boost::filesystem::path &checkpoint_directory,
                          const std::vector<size_t> &block_indices,
                          const Verbosity &verbosity, SDP_Solver &solver)
{
  if(!exists(checkpoint_directory / "x_0.txt"))
    {
      return false;
    }

  if(verbosity >= Verbosity::regular && El::mpi::Rank() == 0)
    {
      std::cout << "Loading text checkpoint from : " << checkpoint_directory
                << '\n';
    }

  for(size_t block = 0; block != block_indices.size(); ++block)
    {
      size_t block_index(block_indices.at(block));
      read_text_block(solver.x.blocks.at(block), checkpoint_directory, "x_",
                      block_index);
      read_text_block(solver.y.blocks.at(block),
                      checkpoint_directory / "y.txt");

      for(size_t psd_block(0); psd_block < 2; ++psd_block)
        {
          // Constant constraints have empty odd parity blocks, so we do not
          // need to load them.
          if(solver.X.blocks.at(2 * block + psd_block).Height() != 0)
            {
              const size_t psd_index(2 * block_index + psd_block);
              read_text_block(solver.X.blocks.at(2 * block + psd_block),
                              checkpoint_directory, "X_matrix_", psd_index);
              read_text_block(solver.Y.blocks.at(2 * block + psd_block),
                              checkpoint_directory, "Y_matrix_", psd_index);
            }
        }
    }
  return true;
}


#ifndef BODY_ANALIZER_H
#define BODY_ANALIZER_H

#include <string>

#include <ros/ros.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>

class body_analizer
{
  std::string root_;
  std::string target_number_;
  std::string base_name_;
  std::string right_hand_name_;
  std::string left_hand_name_;
  std::string right_knee_name_;
  std::string left_knee_name_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_ {tf_buffer_};
  Eigen::Vector3d head_ {};
  Eigen::Vector3d right_hand_ {};
  Eigen::Vector3d left_hand_ {};
  Eigen::Vector3d right_knee_ {};
  Eigen::Vector3d left_knee_ {};

public:
  body_analizer(std::string root, std::size_t target_number);

  void update();

  Eigen::Vector3d get_head() const noexcept
  {
    return head_;
  }

  Eigen::Vector3d get_right_hand() const noexcept
  {
    return right_hand_;
  }

  Eigen::Vector3d get_left_hand() const noexcept
  {
    return left_hand_;
  }

  Eigen::Vector3d get_right_knee() const noexcept
  {
    return right_knee_;
  }

  Eigen::Vector3d get_left_knee() const noexcept
  {
    return left_knee_;
  }

private:
  Eigen::Affine3d get_position(std::string target, ros::Time time) const
  {
    return tf2::transformToEigen(tf_buffer_.lookupTransform(root_, target, time));
  }
};

#endif