/*
 * FindCenter.cpp
 *
 *  Created on: 2017-05-26
 *      Author: ivan
 */

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <boost/static_assert.hpp>

#include "zip.h"
#include "GraphOperations.h"
#define BAD_OUTPUT 10101010

using namespace std;
using namespace graph_buzz;

/*******************************************************
 * Tree
 */

GraphOperations::TreeVertex::TreeVertex ( Vertex id, int depth )
{
    _m = id;
    _depth = depth;
    _children = new std::vector<TreeVertex>();
    _m_parent = NULL;
}

void GraphOperations::TreeVertex::SetParent ( TreeVertex* parent )
{
    _m_parent = parent;
}

void GraphOperations::TreeVertex::SetChild ( TreeVertex* child )
{
    _children->push_back ( *child );
}

std::vector<GraphOperations::TreeVertex>* GraphOperations::TreeVertex::GetChildren()
{
    return _children;
}

GraphOperations::TreeVertex* GraphOperations::TreeVertex::GetParent()
{
    return _m_parent;
}

Vertex GraphOperations::TreeVertex::GetId()
{
    return _m;
}

/*********************************************************************
***** File related operations
**********************************************************************/

void GraphOperations::WriteGraphToFile ( const Graph& g, const std::string& filename )
{
    std::ofstream graphStream;
    graphStream.open ( filename.c_str() );
    boost::write_graphviz ( graphStream, g );
    graphStream.close();
}
std::string GraphOperations::WriteGraphToDotString ( const Graph& g, dynamic_properties& dp )
{
    std::stringstream stream;
    boost::write_graphviz_dp ( stream, g, dp );
    return stream.str();
}

std::string GraphOperations::WriteGraphToString ( const Graph& g, dynamic_properties& dp )
{
    std::stringstream stream;
    boost::write_graphviz_dp ( stream, g, dp );

    std::string delimiter = "\n";
    std::string result = "";
    std::string s = stream.str();

    size_t pos = 0;
    std::string token;
    int i = 0;
    while ( ( pos = s.find ( delimiter ) ) != std::string::npos ) {
        token = s.substr ( 0, pos );
        if ( i > num_vertices ( g ) ) {
            result += token;
        }
        s.erase ( 0, pos + delimiter.length() );
        i++;
    }

    boost::replace_all ( result, ";}",";" );
    boost::replace_all ( result, "--","-" );
    boost::replace_all ( result, " ","" );
    return result;
}

void GraphOperations::OpenFromXML ( Graph& g, dynamic_properties& dp, const std::string& filename )
{
    std::ifstream inFile;

    inFile.open ( filename.c_str(), std::ifstream::in );
    read_graphml ( inFile, g, dp );
}

void GraphOperations::OpenFromString ( Graph& g, dynamic_properties& dp, char buffer [] )
{
    membuf sbuf ( buffer, buffer + strlen ( buffer ) );
    std::istream in ( &sbuf );
    read_graphml ( in, g, dp );
}

void GraphOperations::SetNames ( Graph& g, NameMap& nameMap )
{
    for ( int i = 0; i < num_vertices ( g ); i++ ) {
        std::stringstream ss;
        ss << ( i + 15 );
        nameMap[i] = i;
    }
}

/*********************************************************************
***** Graph properties
**********************************************************************/

void GraphOperations::SetWeights ( Graph& g, DistanceMap& distanceMap, float weight )
{
    // sets the weight
    graph_traits<Graph>::edge_iterator ei, ee;
    for ( tie ( ei, ee ) = edges ( g ); ei != ee; ++ei ) {
        put ( edge_weight, g, *ei, weight );
    }

    for ( int i = 0; i < num_edges ( g ); i++ ) {
        distanceMap[i] = weight;
    }
}

void GraphOperations::RemoveEdges ( Graph &g )
{
    // Removes all edges to and from vertex.
    graph_traits<Graph>::vertex_iterator i, end;
    for ( tie ( i, end ) = vertices ( g ); i != end; ++i ) {
        //cout << "Name: " << get(nameMap, *i) << endl;
        clear_vertex ( *i, g );
    }
}

void GraphOperations::PrintGraphProperties ( Graph& g, NameMap& nameMap, DistanceMap& distanceMap )
{
    for ( int i = 0; i < num_vertices ( g ); i++ ) {
        cout << "Vertex: " << nameMap[i] << endl;
    }

    for ( int i = 0; i < num_edges ( g ); i++ ) {
        cout << "Weight: " << distanceMap[i] << endl;
    }
}
// pair vertice and value
std::vector< std::pair<int, float> > GraphOperations::GetCentralities ( Graph& g, NameMap& nameMap, IndexMap& indexMap )
{
    DistanceMatrix dsts ( num_vertices ( g ) );
    DistanceMatrixMap dm ( dsts, g );
    WeightMap wm ( 1 );
    floyd_warshall_all_pairs_shortest_paths ( g, dm, weight_map ( wm ) );

    // centrality
    ClosenessContainer cents ( num_vertices ( g ) );
    ClosenessMap cm ( cents, g );
    all_closeness_centralities ( g, dm, cm );

    std::vector< std::pair<int, float> > centralities;
    graph_traits<Graph>::vertex_iterator i, end;
    for ( boost::tie ( i, end ) = vertices ( g ); i != end; ++i ) {
        centralities.push_back ( std::make_pair ( ( int ) get ( indexMap, *i ), ( float ) get ( cm, *i ) ) );
    }

    std::sort ( centralities.begin(), centralities.end(), boost::bind ( &std::pair<int, float>::second, _1 ) > boost::bind ( &std::pair<int, float>::second, _2 ) );
    return centralities;
}

/*********************************************************************
***** Graph related opreations
**********************************************************************/

Vertex GraphOperations::GetFreeNeighbor ( Graph& g, Vertex vertex, std::vector<Vertex> taken )
{
    // loop through neighbours and extract one of the neigbours not in the taken list
    Graph::adjacency_iterator neighbourIt, neighbourEnd;
    tie ( neighbourIt, neighbourEnd ) = adjacent_vertices ( vertex, g );

    for ( ; neighbourIt != neighbourEnd; ++neighbourIt ) {
        Vertex v_current_neighbor = *neighbourIt;
        if ( std::find ( taken.begin(), taken.end(), v_current_neighbor ) == taken.end() ) {
            return v_current_neighbor;
        }
    }
    return BAD_OUTPUT;
}

Vertex GraphOperations::GetTakenNeighbor ( Graph& g, Vertex vertex, std::vector<Vertex> taken )
{
    // loop through neighbours and extract one of the neigbours not in the taken list
    Graph::adjacency_iterator neighbourIt, neighbourEnd;
    tie ( neighbourIt, neighbourEnd ) = adjacent_vertices ( vertex, g );

    for ( ; neighbourIt != neighbourEnd; ++neighbourIt ) {
        Vertex v_current_neighbor = *neighbourIt;
        if ( std::find ( taken.begin(), taken.end(), v_current_neighbor ) != taken.end() ) {
            return v_current_neighbor;
        }
    }
    return BAD_OUTPUT;
}

Graph GraphOperations::ConstructSubgraph ( Graph g, std::vector<int> subtree_vertices )
{
    Graph subgraph;
    BOOST_FOREACH ( int member, subtree_vertices ) {
        //std::cout<<"Member: "<<member<<std::endl;
        NameMap nameMap = boost::get ( boost::vertex_name, subgraph );
        Vertex a = add_vertex ( subgraph );
        std::stringstream int_stream;
        int_stream << member;
        nameMap[a] = int_stream.str();
    }

    NameMap v_names = boost::get ( boost::vertex_name, subgraph );
    graph_traits<Graph>::vertex_iterator s_i, s_end;
    graph_traits<Graph>::vertex_iterator t_i, t_end;

    for ( tie ( s_i, s_end ) = vertices ( subgraph ); s_i != s_end; ++s_i ) {
        for ( tie ( t_i, t_end ) = vertices ( subgraph ); t_i != t_end; ++t_i ) {
            int v_source = *s_i;
            int v_target = *t_i;
            int v_source_name = lexical_cast<int> ( get ( v_names, v_source ) );
            int v_target_name = lexical_cast<int> ( get ( v_names, v_target ) );
            if ( EdgeExists ( g, v_source_name, v_target_name ) && !EdgeExists ( subgraph, v_target, v_source ) ) {
                add_edge ( v_source, v_target, subgraph );
            }
        }
    }
    return subgraph;
}

bool GraphOperations::EdgeExists ( Graph g, int member, int submember )
{
    graph_traits<Graph>::edge_iterator ei, ee;
    for ( tie ( ei, ee ) = edges ( g ); ei != ee; ++ei ) {
        int v_source = source ( *ei, g );
        int v_target = target ( *ei, g );
        if ( v_source == member && v_target == submember ) {
            return true;
        }
    }
    return false;
}

bool GraphOperations::IsIn ( std::vector<int> elements, int element )
{
    BOOST_FOREACH ( int current, elements ) {
        if ( current == element ) {
            return true;
        }
    }
    return false;
}

std::vector<GraphOperations::TreeVertex*> GraphOperations::zipit ( std::vector<TreeVertex*> next_level_nodes, std::vector<TreeVertex>* children, int depth )
{
    std::vector<TreeVertex*> children_list;
    std::vector<TreeVertex*> zipped_list;
    for ( std::vector<TreeVertex>::iterator it = children->begin(); it != children->end(); ++it ) {
        //children_list.push_back ( new TreeVertex ( ( *it ).GetId(), depth ) );
        children_list.push_back ( & ( *it ) );

    }
    int longer_list = ( ( next_level_nodes.size() < children_list.size() ) ? next_level_nodes.size() : children_list.size() );
    //std::cout<<"Main: " << next_level_nodes.size() << "Children" << children_list.size() <<std::endl;
    //std::cout<<"Longer: " << longer_list << std::endl;

    std::vector<TreeVertex*>::iterator nln_iterator = next_level_nodes.begin();
    std::vector<TreeVertex*>::iterator cld_iterator = children_list.begin();

    while ( nln_iterator != next_level_nodes.end() || cld_iterator != children_list.end() ) {
        if ( nln_iterator != next_level_nodes.end() ) {
            //std::cout << "Added " << (*nln_iterator)->GetId() << std::endl;
            //zipped_list.push_back ( new TreeVertex ( ( *nln_iterator )->GetId(), depth ) );
            zipped_list.push_back ( ( *nln_iterator ) );
            ++nln_iterator;
        }
        if ( cld_iterator != children_list.end() ) {
            //std::cout << "Added(2) " << (*cld_iterator)->GetId() << std::endl;
            //zipped_list.push_back ( new TreeVertex ( ( *cld_iterator )->GetId(), depth ) );
            zipped_list.push_back ( ( *cld_iterator ) );
            ++cld_iterator;
        }
    }
    /*
        std::cout << "Returning: " << zipped_list.size() << std::endl;
        BOOST_FOREACH ( TreeVertex* current_vertex, zipped_list ) {
            std::cout<<"Current: " << current_vertex->GetId() << " ";
        }
    */
    //delete children_list;
    return zipped_list;

}

std::vector<int> GraphOperations::NonNeigbourVertices ( Graph g, std::vector< std::pair<int, float> > centralities )
{
    std::vector<int> result;
    NameMap nameMap = boost::get ( boost::vertex_name, g );

    // reverse sort
    std::sort ( centralities.begin(), centralities.end(), boost::bind ( &std::pair<int, float>::second, _1 ) > boost::bind ( &std::pair<int, float>::second, _2 ) );
    int v_least_central = vertex ( centralities[0].first, g ); // < change
    result.push_back ( v_least_central );

    for ( int node = 0; node < centralities.size(); node++ ) {
        bool push_it = true;
        BOOST_FOREACH ( int root, result ) {
            if ( AreNeigbours ( g, root, centralities[node].first ) || centralities[node].first == root ) {
                push_it = false;
            }
        }
        if ( push_it ) {
            result.push_back ( centralities[node].first );
        }
    }

    return result;
}

std::vector<int> GraphOperations::GetBorderCycle(Graph g, std::vector<int> existing_candidates){
  std::vector<int> cycle;
  cycle.push_back(existing_candidates.at(0));
  int cycle_size = 0;
  int last_to_add = existing_candidates.at(0);
  bool chain_not_complete = true;
  int missed = 0;

  while(chain_not_complete) {
    BOOST_FOREACH ( int vertex, existing_candidates ) {
      if(!IsIn(cycle, vertex) && AreNeigbours(g, cycle.at(cycle_size), vertex)){
	//std::cout<<"Add? " << vertex <<", " << NumNeigborsWithDegree(g, vertex, 2) << std::endl;
	if(cycle_size >= 2 && NumNeigborsWithDegree(g, cycle.at(cycle_size), 2) > 2){
	  //std::cout<<"pred? " << cycle.at(cycle_size) <<", " << NumNeigborsWithDegree(g, cycle.at(cycle_size), 2) << std::endl;
	  if(!AreNeigbours(g, cycle.at(cycle_size - 1), vertex) || AreNeigbours(g, cycle.at(0), last_to_add)){
	    std::cout<<"Adding: " << vertex << std::endl;
	    cycle.push_back(vertex);
	    last_to_add = vertex;
	    cycle_size++;
	    missed = 0;
	    break;
	  }
	} else {
	  std::cout<<"Adding: " << vertex << std::endl;
	  cycle.push_back(vertex);
	  last_to_add = vertex;
	  cycle_size++;
	  missed = 0;
	  break;
	}
      }
    } // end foreach
    //std::cout<<"Stuck? "<< cycle.at(0) << last_to_add <<std::endl;
    missed++;
    if(cycle_size > 2 && AreNeigbours(g, cycle.at(0), last_to_add)){
      chain_not_complete = false;
    } else if(missed > existing_candidates.size()){
      chain_not_complete = false;
    }
  }
  //cycle.pop_back();
  //cycle.at(cycle_size) = cycle.at(0);
  return cycle;
}

std::vector<int> GraphOperations::SortedByDegree ( Graph g, std::vector<int> existing_candidates, bool include_ones )
{
    std::vector<int> result = existing_candidates;
    std::vector< std::pair<int, int> > degree_sorted;
    //std::cout<<"Degree:" << degree(0, g) <<std::endl;
    for ( int i = 0; i < num_vertices ( g ); i++ ) {
	if(include_ones){
	  degree_sorted.push_back ( std::make_pair ( i, degree ( i, g ) ) );
	} else {
	  if(degree( i, g ) >= 2){
	    degree_sorted.push_back ( std::make_pair ( i, degree ( i, g ) ) );
	  }
	}
    }

    std::sort ( degree_sorted.begin(), degree_sorted.end(), boost::bind ( &std::pair<int, int>::second, _1 ) < boost::bind ( &std::pair<int, int>::second, _2 ) );
    for ( int candidate = 0; candidate < degree_sorted.size(); candidate++ ) {
        if ( !IsIn ( existing_candidates, degree_sorted[candidate].first ) ) {
            result.push_back ( degree_sorted[candidate].first );
        }
    }

    return result;
}

int GraphOperations::NumNeigborsWithDegree ( Graph g, int starting_node, int desired_degree )
{
    int neighbour_count = 0;
    Graph::adjacency_iterator neighbourIt, neighbourEnd;
    tie ( neighbourIt, neighbourEnd ) = adjacent_vertices ( starting_node, g );

    for ( ; neighbourIt != neighbourEnd; ++neighbourIt ) {
      int v_current_neighbor = *neighbourIt;
      if(degree(v_current_neighbor, g) > desired_degree){
	  neighbour_count++;
      }
    }
    return neighbour_count;
}

bool GraphOperations::AreNeigbours ( Graph g, int starting_node, int ending_node )
{
    Graph::adjacency_iterator neighbourIt, neighbourEnd;
    tie ( neighbourIt, neighbourEnd ) = adjacent_vertices ( starting_node, g );
    for ( ; neighbourIt != neighbourEnd; ++neighbourIt ) {
        int v_current_neighbor = *neighbourIt;
        if ( v_current_neighbor == ending_node ) {
            return true;
        }
    }
    return false;
}

int GraphOperations::TreeSize(TreeVertex* vertex, int max_depth ){
  TreeVertex* root;
  root = vertex->GetParent();
  if(vertex->GetParent() == NULL){
    root = vertex;
  } else {
    while(root->GetParent() != NULL){
      root = root->GetParent();
    }
  }
  std::cout<<"Root is: " << root->GetId();

  std::vector<TreeVertex*> current_children_list;
    for ( std::vector<TreeVertex>::iterator it = root->GetChildren()->begin(); it != root->GetChildren()->end(); ++it ) {
      current_children_list.push_back ( & ( *it ) );
  }

    std::vector<int> subtree_vertices;
    subtree_vertices.push_back ( ( int ) root->GetId());

  int current_depth = 0;
  // find all chilldren and build a list
  while ( current_depth < max_depth ) {
      std::vector<TreeVertex*> next_children_list;
      for ( std::vector<TreeVertex*>::iterator cc_i = current_children_list.begin(); cc_i != current_children_list.end(); ++cc_i ) {
	  subtree_vertices.push_back ( ( int ) ( *cc_i )->GetId() );
	  std::vector<TreeVertex>* current_childrens_children = ( *cc_i )->GetChildren();
	  for ( std::vector<TreeVertex>::iterator c_i = current_childrens_children->begin(); c_i != current_childrens_children->end(); ++c_i ) {
	      next_children_list.push_back ( & ( *c_i ) );
	  }
      }
      current_depth++;
      current_children_list = next_children_list;
  }

  return subtree_vertices.size();

}

Graph GraphOperations::ExtractSubgraph ( Graph g, TreeVertex* branch, int max_depth )
{
    // write the children in the list
    std::vector<TreeVertex*> current_children_list;
    for ( std::vector<TreeVertex>::iterator it = branch->GetChildren()->begin(); it != branch->GetChildren()->end(); ++it ) {
        current_children_list.push_back ( & ( *it ) );
    }

    std::vector<int> subtree_vertices;
    subtree_vertices.push_back ( ( int ) branch->GetId() );
    int current_depth = 0;

    // find all chilldren and build a list
    while ( current_depth < max_depth ) {
        std::vector<TreeVertex*> next_children_list;
        for ( std::vector<TreeVertex*>::iterator cc_i = current_children_list.begin(); cc_i != current_children_list.end(); ++cc_i ) {
            subtree_vertices.push_back ( ( int ) ( *cc_i )->GetId() );
            std::vector<TreeVertex>* current_childrens_children = ( *cc_i )->GetChildren();
            for ( std::vector<TreeVertex>::iterator c_i = current_childrens_children->begin(); c_i != current_childrens_children->end(); ++c_i ) {
                next_children_list.push_back ( & ( *c_i ) );
            }
        }
        current_depth++;
        current_children_list = next_children_list;
    }

    Graph subgraph = ConstructSubgraph ( g, subtree_vertices );
    return subgraph;
}

void GraphOperations::SetupRootCandidates(Graph g, std::vector<TreeVertex*> &subtrees, std::vector<Vertex> &taken_vertices, int num_partitions, std::vector< std::pair<int, float> > centralities){

    std::vector<int> cycle_candidates;
    cycle_candidates = SortedByDegree ( g, cycle_candidates, false);
    cycle_candidates = GetBorderCycle ( g, cycle_candidates );

    std::cout<<"Creating tree using the border cycle " << std::endl;
    std::cout << "Chain size: " << cycle_candidates.size() << std::endl;
    BOOST_FOREACH ( int c, cycle_candidates ) {
      std::cout<< "Chain: " << c << std::endl;
    }

    if((cycle_candidates.size() / 3) < num_partitions){
      int nth = cycle_candidates.size() / (cycle_candidates.size() / 3);
      for(int i = 0; i < cycle_candidates.size(); i+=nth){
	subtrees.push_back ( new TreeVertex ( cycle_candidates[i], 0 ) );
	taken_vertices.push_back ( cycle_candidates[i] );
	//cout<<"Added(c): " << cycle_candidates[i] << std::endl;
	if(taken_vertices.size() >= num_partitions) break;
      }

      std::vector<int> neighbour_candidates = NonNeigbourVertices ( g, centralities );

      neighbour_candidates = SortedByDegree ( g, neighbour_candidates, true);
      while(subtrees.size() < num_partitions){
	for(int i = 0; i < neighbour_candidates.size(); i++){
	  if(!IsIn(cycle_candidates, neighbour_candidates[i])){
	    std::cout<<"Pushing: " << neighbour_candidates[i] << std::endl;
	    subtrees.push_back ( new TreeVertex ( neighbour_candidates[i], 0 ) );
	    taken_vertices.push_back ( neighbour_candidates[i] );
	    cycle_candidates.push_back ( neighbour_candidates[i] );
	    break;
	  }
	}

      }
    } else {
      int nth = cycle_candidates.size() / num_partitions;
      for(int i = 0; i < cycle_candidates.size(); i+=nth){
	subtrees.push_back ( new TreeVertex ( cycle_candidates[i], 0 ) );
	taken_vertices.push_back ( cycle_candidates[i] );
	cout<<"Added(c): " << cycle_candidates[i] << std::endl;
	if(taken_vertices.size() >= num_partitions) break;
      }
    }
}

void GraphOperations::ConstructForest(Graph g, std::vector<TreeVertex*> &subtrees, std::vector<Vertex> &taken_vertices, std::vector<TreeVertex*> &current_level_nodes, int num_partitions, int max_depth){
    bool increase_depth = false;
    int d = 0;
    int depth = max_depth;

    while ( d < depth ) {
        // check to move to next depth level
        if ( increase_depth ) {
            std::vector<TreeVertex*> next_level_nodes;
            BOOST_FOREACH ( TreeVertex* current_vertex, current_level_nodes ) {
                std::vector<TreeVertex>* children = current_vertex->GetChildren();
                next_level_nodes = zipit ( next_level_nodes, children, d );
            }
            current_level_nodes = next_level_nodes;
            increase_depth = false;
        }
        if ( current_level_nodes.empty() ) {
            break;
        }
        // add children non-greedy
        int empty_set_counter = current_level_nodes.size();
        BOOST_FOREACH ( TreeVertex* current_vertex, current_level_nodes ) {
            Vertex neigbour = GetFreeNeighbor ( g, vertex ( current_vertex->GetId(), g ), taken_vertices );
	    std::cout<<"TreeSize: " << TreeSize(current_vertex, depth);
            if ( neigbour != BAD_OUTPUT && !(TreeSize(current_vertex, depth) > 3)) {
                taken_vertices.push_back ( neigbour );
                TreeVertex* new_child = new TreeVertex ( neigbour, d + 1 );
                new_child->SetParent ( current_vertex );
                current_vertex->SetChild ( new_child );
                //cout<< current_vertex->GetId() << " takes " << new_child->GetId() << std::endl;
            } else {
                empty_set_counter -= 1;
                if ( empty_set_counter == 0 ) {
                    d++;
                    increase_depth = true;
                }
            }
        }
    }

}

GraphOperations::TreeVertex* GraphOperations::FindChild(std::vector<TreeVertex*> subtrees, int node_id){

  int max_depth = 5;
  TreeVertex* result = NULL;
  std::vector<TreeVertex*> current_children_list;
  BOOST_FOREACH ( TreeVertex* branch, subtrees ) current_children_list.push_back ( branch );

  int current_depth = 0;
  // find all chilldren and build a list
  while ( current_depth < max_depth ) {
      std::vector<TreeVertex*> next_children_list;
      for ( std::vector<TreeVertex*>::iterator cc_i = current_children_list.begin(); cc_i != current_children_list.end(); ++cc_i ) {
	  //subtree_vertices.push_back ( ( int ) ( *cc_i )->GetId() );
	  if(( int ) ( *cc_i )->GetId() == node_id) {
	    result = *cc_i;
	    return result;

	  }
	  std::vector<TreeVertex>* current_childrens_children = ( *cc_i )->GetChildren();
	  for ( std::vector<TreeVertex>::iterator c_i = current_childrens_children->begin(); c_i != current_childrens_children->end(); ++c_i ) {
	      next_children_list.push_back ( & ( *c_i ) );
	  }
      }
      current_depth++;
      current_children_list = next_children_list;
  }

  return result;

}

/*********************************************************************
***** Tree related opreations
**********************************************************************/

std::string GraphOperations::CreateBalancedForest ( std::string text, int num_partitions, int max_depth)
{
    Graph g;
    dynamic_properties dp;

    const std::string vn = "vertex_name";
    dp.property ( vn,get ( vertex_name,g ) );

    //num_partitions = 20;

    // convert string to char array
    char *graph_xml = new char[text.size() +1];
    graph_xml[text.size()]=0;
    memcpy ( graph_xml,text.c_str(),text.size() );

    OpenFromString ( g, dp, graph_xml );
    //OpenFromXML(g, dp, "../samples/graph-li.xml");
    //OpenFromXML(g, dp, "../samples/temp.xml");

    //---
    // Create data for Dijkstra
    std::vector<Vertex> predecessors ( boost::num_vertices ( g ) ); 	// To store parents
    std::vector<Weight> distances ( boost::num_edges ( g ) ); 		// To store distances

    IndexMap indexMap = boost::get ( boost::vertex_index, g );
    NameMap nameMap = boost::get ( boost::vertex_name, g );

    PredecessorMap predecessorMap ( &predecessors[0], indexMap );
    DistanceMap distanceMap ( &distances[0], indexMap );

    SetWeights ( g, distanceMap, 1.0f );
    //PrintGraphProperties(g, nameMap, distanceMap);

    std::vector< std::pair<int, float> > centralities = GetCentralities ( g, nameMap, indexMap );

    if ( centralities.size() < num_partitions ) {
        std::cout<<"You know... the number of partitions is bigger than you have vertices! (HELLOO??ERROR!!!)" <<std::endl;
        return NULL;
    }

    std::vector<TreeVertex*> subtrees;
    std::vector<TreeVertex*> current_level_nodes;
    std::vector<Vertex> taken_vertices;

    std::vector<int> vertex_range;
    for ( int i = 0; i < num_vertices (g); i++ ) {
        vertex_range.push_back ( i );
    }
    // removes extra edges? - definetly destroys the names (only for Kheperas)
    //g = ConstructSubgraph ( g, vertex_range ); //-- same?

    // Setup root candidates (pass by ref... blah)
    SetupRootCandidates(g, subtrees, taken_vertices, num_partitions, centralities);

    BOOST_FOREACH ( int c, taken_vertices ) {
      std::cout<< "Roots: " << c << std::endl;
    }

    BOOST_FOREACH ( TreeVertex* branch, subtrees ) current_level_nodes.push_back ( branch );

    // create forest
    ConstructForest(g, subtrees, taken_vertices, current_level_nodes, num_partitions, max_depth);

    std::cout<<std::endl<<"Taken vertices: " << taken_vertices.size() << std::endl;

    if(taken_vertices.size() < num_vertices(g)) {
       std::cout<< "Handling islands..." << std::endl;
      // handle islands
      std::vector<int> taken_vertices_list;
      std::vector<int> non_taken_vertices;

      BOOST_FOREACH(int v, taken_vertices)
	taken_vertices_list.push_back(v);

      BOOST_FOREACH(int v, vertex_range)
	if(!IsIn(taken_vertices_list, v)) non_taken_vertices.push_back(v);

      BOOST_FOREACH(int current_island, non_taken_vertices){
	Vertex neigbour = GetTakenNeighbor ( g, vertex ( current_island, g ), taken_vertices );
	TreeVertex* v = FindChild(subtrees, (int) neigbour);
	std::cout<<"Candidate for " << current_island << " is " << v->GetId() << std::endl;
	if(v != NULL){
	  taken_vertices.push_back ( current_island );
	  TreeVertex* new_child = new TreeVertex ( current_island, 0);
	  new_child->SetParent ( v );
	  v->SetChild ( new_child );
	  std::cout << "Added : " << current_island << std::endl;
	}

      }

      std::cout<<std::endl<<"Taken vertices: " << taken_vertices.size() << std::endl;
    }

    // For each tree, print it
    std::string final_output = "";
    std::vector<Graph> partitions;
    int i = 0;

    std::vector< std::pair<int, int> > original_names;
    //NameMap nameMap = boost::get ( boost::vertex_name, g );
    for ( int i = 0; i < num_vertices ( g ); i++ ) {
      original_names.push_back(std::make_pair(i, std::atoi(nameMap[i].c_str())));
      std::cout<<"Original: " << i << ", " << nameMap[i] << std::endl;
    }

    BOOST_FOREACH ( TreeVertex* current_vertex, subtrees ) {

        Graph partition = ExtractSubgraph ( g, current_vertex, max_depth );
        partitions.push_back ( partition );
        // print
        dynamic_properties dp;
        dp.property ( "node_id", get ( vertex_name, partition ) );
	Graph tree = ConstructTreeFromGraph(partition);

	final_output += WriteGraphToString ( tree, dp );
	//std::cout<< "Partition: " << final_output << std::endl;
        std::cout<<"Partition: " << WriteGraphToDotString ( tree, dp ) <<std::endl;
	std::cout<< "Tree: " << i << ", size: "<< num_vertices(partition) << std::endl;
	i++;

    }

    //dynamic_properties dp2;
    //dp2.property ( "node_id", get ( vertex_name, partitions[0] ) );
    //return WriteGraphToString ( partitions[0], dp2 );

    // khepera name hack
    for(int i = original_names.size() - 1; i >= 0 ; i--){
      stringstream first, second;
      first << original_names[i].first;
      second << original_names[i].second;
      std::cout<<"Replacing: " << first.str() << " with " << second.str() << ", " << final_output << std::endl;
      boost::replace_all(final_output, first.str(), second.str());
    }

    return final_output;
}

std::string GraphOperations::CreateTree ( std::string text )
{

    Graph g;
    dynamic_properties dp;

    const std::string vn = "vertex_name";
    dp.property ( vn,get ( vertex_name,g ) );

    // convert string to char array
    char *graphml = new char[text.size() +1];
    graphml[text.size()]=0;
    memcpy ( graphml,text.c_str(),text.size() );

    OpenFromString(g, dp, graphml);
    //OpenFromXML ( g, dp, "../samples/graph-li.xml" );

    Graph tree = ConstructTreeFromGraph(g);

    dynamic_properties dp2;
    dp2.property ( "node_id", get ( vertex_name, tree ) );
    // WriteGraphToFile ( tree, "tree.dot" );
    return WriteGraphToString ( tree, dp2 );
    //return text;
}

Graph GraphOperations::ConstructTreeFromGraph(Graph g)
{
    // Create data for Dijkstra
    std::vector<Vertex> predecessors ( boost::num_vertices ( g ) ); 	// To store parents
    std::vector<Weight> distances ( boost::num_edges ( g ) ); 		// To store distances

    IndexMap indexMap = boost::get ( boost::vertex_index, g );
    NameMap nameMap = boost::get ( boost::vertex_name, g );

    PredecessorMap predecessorMap ( &predecessors[0], indexMap );
    DistanceMap distanceMap ( &distances[0], indexMap );

    // set names and weights
    //SetNames(g, nameMap);
    SetWeights ( g, distanceMap, 1.0f );
    //PrintGraphProperties(g, nameMap, distanceMap);

      // floyd warshall
    std::vector< std::pair<int, float> > centralities = GetCentralities ( g, nameMap, indexMap );

    // dijkstra
    Vertex v0 = vertex ( centralities[0].first, g ); // < change
    boost::dijkstra_shortest_paths ( g, v0, boost::distance_map ( distanceMap ).predecessor_map ( predecessorMap ) );

    Graph tree;
    boost::copy_graph ( g, tree );
    RemoveEdges ( tree );

    typedef std::vector<Graph::edge_descriptor> PathType;

    for ( int i = 0; i < num_vertices ( g ); i++ ) {
        PathType path;
        Vertex v3= vertex ( i, g );
        Vertex v = v3; // We want to start at the destination and work our way back to the source
        for ( Vertex u = predecessorMap[v];  u != v; v = u, u = predecessorMap[v] ) {
            std::pair<Graph::edge_descriptor, bool> edgePair = boost::edge ( u, v, g );
            Graph::edge_descriptor edge = edgePair.first;
            path.push_back ( edge );
            if ( boost::edge ( u,v, tree ).second == false ) {
                add_edge ( u, v, 1, tree );    // source, destination, weight, tree
            }
        }
    }

    return tree;
}

std::string GraphOperations::GetShortestPath(std::string text, int source, int destination)
{
    Graph g;
    dynamic_properties dp;

    const std::string vn = "vertex_name";
    dp.property ( vn,get ( vertex_name,g ) );

    // convert string to char array
    char *graph_xml = new char[text.size() +1];
    graph_xml[text.size()]=0;
    memcpy ( graph_xml,text.c_str(),text.size() );

    OpenFromString ( g, dp, graph_xml );

    // Create data for Dijkstra
    std::vector<Vertex> predecessors ( boost::num_vertices ( g ) ); 	// To store parents
    std::vector<Weight> distances ( boost::num_edges ( g ) ); 		// To store distances

    IndexMap indexMap = boost::get ( boost::vertex_index, g );
    NameMap nameMap = boost::get ( boost::vertex_name, g );

    PredecessorMap predecessorMap ( &predecessors[0], indexMap );
    DistanceMap distanceMap ( &distances[0], indexMap );

    /** khepera hack */
    Vertex v0;
    Vertex v3;

    std::vector< std::pair<int, int> > original_names;
    for ( int i = 0; i < num_vertices ( g ); i++ ) {
      original_names.push_back(std::make_pair(i, std::atoi(nameMap[i].c_str())));
      if(std::atoi(nameMap[i].c_str()) == source){
	  v3 = vertex ( i, g );
	  //std::cout<<"Selected " << i << " as " << source << std::endl;
      }
      if(std::atoi(nameMap[i].c_str()) == destination) {
	v0 = vertex ( i, g );
	//std::cout<<"Selected " << i << " as " << destination << std::endl;
      }

      //std::cout<<"Original: " << i << ", " << nameMap[i] << std::endl;
    }

    // set names and weights
    //SetNames(g, nameMap);
    SetWeights ( g, distanceMap, 1.0f );
    //PrintGraphProperties(g, nameMap, distanceMap);

      // floyd warshall
    //std::vector< std::pair<int, float> > centralities = GetCentralities ( g, nameMap, indexMap );

    // dijkstra
    boost::dijkstra_shortest_paths ( g, v0, boost::distance_map ( distanceMap ).predecessor_map ( predecessorMap ) );
    //std::cout<<"S start..." << std::endl;

    std::string shortest_path = "";
    int path_steps = 0;
	Vertex v = v3; // We want to start at the destination and work our way back to the source
	for ( Vertex u = predecessorMap[v];  u != v; v = u, u = predecessorMap[v] ) {
		std::ostringstream ss;
		ss << v;
		shortest_path += ";" + ss.str();
		path_steps++;
	}

      // khepera name hack
      for(int i = original_names.size() - 1; i >= 0 ; i--){
	stringstream first, second;
	first << original_names[i].first;
	second << original_names[i].second;
	//std::cout<<"Replacing: " << first.str() << " with " << second.str() << ", " << shortest_path << std::endl;
	boost::replace_all(shortest_path, first.str(), second.str());
      }

      std::ostringstream ss;
      ss << destination;
      shortest_path += ";" + ss.str();
      ss.str("");
      ss.clear();
      ss << path_steps;
      shortest_path = ss.str() + shortest_path;

    return shortest_path;
}


#include <iostream>
#include <fstream>
#include <chrono>
#include <string>
#include <vector>
#include <Eigen/Geometry>
#include "mpi.h"

//for creating folders
#include <sys/types.h> // required for stat.h
#include <sys/stat.h>	//required to create a folder

#include "General_Utilities.hpp"
#include "Physical_Parameters.hpp"
#include "ER_General.hpp"
#include "ER_CRESSTII.hpp"
#include "ER_LUX.hpp"

using namespace std;
using namespace std::chrono;

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

//INITIALIZATION
////////////////////////////////////////////////////////////
	//Read config file created by the simulation code. argv is the SimID
		std::string id(argv[1]);
		std::string path = "../data/"+id+".cfg";
		Read_Config_File(path.c_str());
	//MPI Enviroment
		// Initialize the MPI environment
	    	MPI_Init(NULL, NULL);
	    // Get the number of processes
	    	int numprocs;
	    	MPI_Comm_size(MPI_COMM_WORLD, &numprocs);
	    // Get the ID number of the process
	    	int myRank;
	    	MPI_Comm_rank(MPI_COMM_WORLD, &myRank);

	//Starting time
	    high_resolution_clock::time_point tStart,tEnd;
	    if(myRank==0)
	    {
	    	tStart = high_resolution_clock::now();
	    }

	//Divide the isodetection rings between the processes.
    	std::vector<int> iList = WorkDivision(numprocs,Isodetection_Rings);
	 	MPI_Barrier(MPI_COMM_WORLD);
	 	if(myRank==0&&numprocs>Isodetection_Rings)
	 	{
	 		cout <<"Warning: More MPI processes than isodetection rings. " <<numprocs-Isodetection_Rings <<" have nothing to do." <<endl;
	 	}

	//Create folder for histograms DaMaSCUS/results/SimID_histograms
		if(myRank==0)
		{
			std::chrono::time_point<std::chrono::system_clock> start;
			start = std::chrono::system_clock::now();
			std::time_t start_time = std::chrono::system_clock::to_time_t(start);
			cout <<"\n##############################"<<endl
			<<"DaMaSCUS"<<version<<" - Data Analysis"<<endl<<endl
			<<"Starting Time:\t" <<std::ctime(&start_time)
			<<"Simulation ID:\t" <<SimID <<endl
			<<"Experiment:\t" <<experiment <<endl<<endl;
			cout <<"Creating folder for histograms."<<endl;
			std::string sPath = "../results/"+SimID+"_histograms";
			mode_t nMode = 0733; // UNIX style permissions
			int nError = 0;
			#if defined(_WIN32)
				nError = _mkdir(sPath.c_str()); // can be used on Windows
			#else
			  nError = mkdir(sPath.c_str(),nMode); // can be used on non-Windows
			#endif
			if (nError != 0)
			{
			  cout <<"Folder already exists, existing data will be overwritten." <<endl<<endl;
			}
			else
			{
				cout <<"Done." <<endl<<endl;
			}
		}
	//Temporary binary output files, which will be translated to ascii at the end.
		if(myRank==0)cout <<"Creating temporary files."<<endl;
		MPI_Offset offset;
 		MPI_File   file_speed;
 		MPI_File   file_rate;
 	  	MPI_Status status;
 	  	//Speed output
		 	string SpeedFilename="../results/"+SimID+".speedTemp";
		 	int test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(SpeedFilename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_speed);
		    //if it already exists, it will be overwritten!
			if(test != MPI_SUCCESS)
			{
	  			if (myRank == 0) MPI_File_delete(const_cast<char*>(SpeedFilename.c_str()),MPI_INFO_NULL);
	  			//MPI_Barrier(MPI_COMM_WORLD);
		  		test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(SpeedFilename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_speed);
		  	}
		  	//Offset for (ring,vMean,vError)
	   		offset = iList[myRank] * 3 * sizeof(double);
	  		MPI_File_seek(file_speed, offset,MPI_SEEK_SET);
	  	//Event rate output
	  		string RateFilename="../results/"+SimID+".rateTemp";
	 		test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(RateFilename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_rate);
		   	//if it already exists, it will be overwritten!
				if(test != MPI_SUCCESS)
				{
		 				if (myRank == 0) MPI_File_delete(const_cast<char*>(RateFilename.c_str()),MPI_INFO_NULL);
		  			test=MPI_File_open(MPI_COMM_WORLD,const_cast<char*>(RateFilename.c_str()),MPI_MODE_CREATE|MPI_MODE_EXCL|MPI_MODE_WRONLY,MPI_INFO_NULL, &file_rate);
		  		}
		 		//Offset for (ring,rate,error,analytic rate)
	   			offset = iList[myRank] * 4 * sizeof(double);
	  			MPI_File_seek(file_rate, offset,MPI_SEEK_SET);

	//Synchronize processes:
		MPI_Barrier(MPI_COMM_WORLD);

////////////////////////////////////////////////////////////
		//DM Density
 		//The master reads and copies the rho file from the /data directory. Afterwards he shares the vector with everyone.
			std::vector<std::vector<double>> rho(Isodetection_Rings, vector<double>(2));
			if(myRank==0)
			{
				cout <<"Reading in local DM densities."<<endl;
				ifstream f2;
				ofstream copy;
				f2.open("../data/"+SimID+".rho");
				copy.open("../results/"+SimID+".rho");
				if(f2.is_open())
				{
					for(int i =0;i<Isodetection_Rings;i++)
					{
						int ring;
						double b1,b2;
						f2 >> ring;
						f2 >> b1;
						f2 >> b2;
						copy <<ring <<"\t" <<b1 <<"\t" <<b2 <<endl;
						rho[i][0]=b1*GeV/cm/cm/cm;
						rho[i][1]=b2*GeV/cm/cm/cm;
					}
				}
				else
				{
					cout <<SimID+".rho does not exist."<<endl;
				}
				f2.close();
				copy.close();
			}
			//Broadcast the rho vector
			if(myRank==0) cout <<"Broadcast local DM densities to all MPI processes." <<endl;
				for(int k=0;k<Isodetection_Rings;k++)
				{
					MPI_Bcast(&rho[k].front(),2,MPI_DOUBLE,0,MPI_COMM_WORLD);
				}

	//Go through the files and analyze isodetection ring by ring.
		if(myRank==0)
		{
			cout <<"Start data analysis..." <<endl<<endl
			<<"MPI rank\tIsodetection ring\tLocal Progress\tComputing time[s]\tResidual time estimate[s]"<<endl;
		}

		double R_A; //the analytic result for the event rate must only be computed once.
		double durationMean=0.0; //for the estimation of the residual time
		for(int i=iList[myRank];i<iList[myRank+1];i++)
		{
			//Beginning time of this isodetection ring
				high_resolution_clock::time_point tIRstart;
				tIRstart = high_resolution_clock::now();

			//Open Files
				MPI_File VelocityFile,WeightFile;
				MPI_Status status;
				string VelocityFilename="../data/"+SimID+"_data/velocity."+std::to_string(i);
				string WeightFilename="../data/"+SimID+"_data/weights."+std::to_string(i);
				int rc = MPI_File_open(MPI_COMM_SELF, const_cast<char*>(VelocityFilename.c_str()), MPI_MODE_RDONLY, MPI_INFO_NULL, &VelocityFile );
				if (rc) cout <<"Unable to read file " <<VelocityFilename <<"."<<endl;
				rc = MPI_File_open(MPI_COMM_SELF, const_cast<char*>(WeightFilename.c_str()), MPI_MODE_RDONLY, MPI_INFO_NULL, &WeightFile );
				if (rc) cout <<"Unable to read file " <<WeightFilename <<"."<<endl;

			//First we calculate the histogram domain, the weighted speed average, variance and standard error.
				std::vector<double> AverageSpeed;
				//1. Compute the weighted average speed and find the maximum speed in the sample, which defines our histogram's domain.
					double vMax = 0.0;
					double vMin=0.0;
					double WeightSum=0.0;
					double SpeedSum=0.0;
					for(int j=0;j<SampleSize;j++)
					{
						Eigen::Vector3d v;
						MPI_File_read(VelocityFile,v.data(),v.size(),MPI_DOUBLE,&status);
						double speed=v.norm();
						if(speed>vMax)			vMax=speed;
						//Compute weighted mean speed;
							double weight;
							MPI_File_read(WeightFile,&weight,1,MPI_DOUBLE,&status);
							WeightSum+=weight;
							SpeedSum+=speed*weight;
					}
					double MeanSpeed=SpeedSum/WeightSum;

				//2. Now we can compute the variance for Scott's rule, as well as the standard error (not deviation!) of the Mean Velocity with Cochran formula
					double SpeedVariance=0.0;
					double MeanWeight=WeightSum/SampleSize;
					double sum1=0.0;
					double sum2=0.0;
					double sum3=0.0;
					//Jump back to the top of file
						MPI_File_seek(VelocityFile, 0,MPI_SEEK_SET);
						MPI_File_seek(WeightFile,0,MPI_SEEK_SET);
					for(int j=0;j<SampleSize;j++)
					{
						//Read in velocity and data weight
							Eigen::Vector3d v;
							double weight;
							MPI_File_read(VelocityFile,v.data(),v.size(),MPI_DOUBLE,&status);
							MPI_File_read(WeightFile,&weight,1,MPI_DOUBLE,&status);
							double speed=v.norm();
						//Compute variance of the sample;
							SpeedVariance+=weight*pow(speed-MeanSpeed,2.0)/WeightSum;
						//Standard Error
							sum1+=pow(weight*speed-MeanWeight*MeanSpeed,2.0);
							sum2+=(weight-MeanWeight)*(weight*speed-MeanWeight*MeanSpeed);
							sum3+=pow(weight-MeanWeight,2.0);
					}
					double StandardError = sqrt(SampleSize/(SampleSize-1)/pow(WeightSum,2.0)*(sum1-2.0*MeanSpeed*sum2+pow(MeanSpeed,2.0)*sum3));
					//Save the average speed and standard error into a file
						AverageSpeed.push_back(i);
						AverageSpeed.push_back(MeanSpeed);
						AverageSpeed.push_back(StandardError);
						MPI_File_write(file_speed,AverageSpeed.data(),AverageSpeed.size(),MPI_DOUBLE,&status);
				//3. Use Scott's rule for the bin width
					double h = 3.5*sqrt(SpeedVariance)/pow(SampleSize,1.0/3.0);
					double bins = ceil((vMax-vMin)/h);

			//Create Histogram including errors
				vector< vector<double> > VelocityHistogram (bins, vector<double>(4));
				for(int j =0;j<bins;j++)
				{
					VelocityHistogram[j][0]=vMin+j*h+h/2.0;	//x coordinate / bin position
					VelocityHistogram[j][1]=0.0;			//y coordinate / bin height
					VelocityHistogram[j][2]=h/2.0;			//error on x
					VelocityHistogram[j][3]=0.0;			//error on y
				}
				//Jump back to the top of file
					MPI_File_seek(VelocityFile, 0,MPI_SEEK_SET);
					MPI_File_seek(WeightFile,0,MPI_SEEK_SET);
				for(int j=0;j<SampleSize;j++)
				{
					Eigen::Vector3d v;
					double weight;
					int bin;
					MPI_File_read(VelocityFile,v.data(),v.size(),MPI_DOUBLE,&status);
					MPI_File_read(WeightFile,&weight,1,MPI_DOUBLE,&status);
					bin=(v.norm()-vMin)/h;
					VelocityHistogram[bin][1]+=weight;
					VelocityHistogram[bin][3]+=weight*weight;
					// WeightSum+=weight;
					// VelocitySum+=v.norm()*weight;
				}
				//Normalize the histogram and the average, and the errors
					for (int j=0;j<bins;j++)
					{
						VelocityHistogram[j][1]/=h*WeightSum;
						VelocityHistogram[j][3]/=h*h*WeightSum*WeightSum;
						VelocityHistogram[j][3]=sqrt(VelocityHistogram[j][3]);
					}

			//Close Files
				MPI_File_close( &VelocityFile );
				MPI_File_close( &WeightFile );
			//Save velocity histogram
				ofstream f;
				f.open("../results/"+SimID+"_histograms/speed."+std::to_string(i));
				for(int j=0;j<bins;j++) f<<VelocityHistogram[j][0] <<"\t" <<VelocityHistogram[j][1] <<"\t" <<VelocityHistogram[j][2]<<"\t" <<VelocityHistogram[j][3] <<endl;
				f.close();
			//Create and save eta-Histogram
				std::vector<std::vector<double>> etaH = EtaHistogram(VelocityHistogram);
				f.open("../results/"+SimID+"_histograms/eta."+std::to_string(i));
				for(unsigned int j=0;j<etaH.size();j++) f<<etaH[j][0] <<"\t"<<etaH[j][1] <<"\t"<<etaH[j][2] <<"\t"<<etaH[j][3]<<endl;
				f.close();
			//Calculate the event rate.
				std::vector<double> TotalRate;
				if(experiment=="CRESST-II")
				{
					//Create Recoil Spectrum Histograms
						std::vector<std::vector<double>> dRdEH_O = dRdEHistogram(0.22,16,rho[i],mChi,sigma0,etaH);
						std::vector<std::vector<double>> dRdEH_Ca = dRdEHistogram(0.14,40,rho[i],mChi,sigma0,etaH);
						std::vector<std::vector<double>> dRdEH_W = dRdEHistogram(0.64,184,rho[i],mChi,sigma0,etaH);
					 //Save dRdE histograms for CRESST-II
						f.open("../results/"+SimID+"_histograms/dRdE."+std::to_string(i));
						//Emin and Emax for the histogram
							double Emin=0.3*keV;
							std::vector<double> EmaxV;
							EmaxV.push_back(dRdEH_O.back()[0]+3.0*62*eV);
							EmaxV.push_back(dRdEH_Ca.back()[0]+3.0*62*eV);
							EmaxV.push_back(dRdEH_W.back()[0]+3.0*62*eV);
							double Emax = *std::max_element( EmaxV.begin(), EmaxV.end() );
							//For very low signal rates we only save the first low 0.1keV of the spectrum.
							if(Emax<0.4*keV) Emax=0.4*keV;
						double dE=(Emax-Emin)/100.;
						//Below 0.3keV the efficiency is assumed to zero. 0.3 keV is the threshold
						for(double E=0.3*keV;E<=Emax;E+=dE)
						{
							std::vector<double> dR = dRdE_CRESSTII_MC(E,dRdEH_O,dRdEH_Ca,dRdEH_W,vEarth.norm());
							f <<E <<"\t" <<dR[0] <<"\t" <<dE/2.0<<"\t" <<dR[1] <<"\t" <<dRdE_CRESSTII_A(E,rhoDM,mChi,sigma0,vEarth.norm()) <<endl;
						}
						f.close();
					//Calculate Total event rate
						//Compute the analytic result at the first iteration
							if(i==iList[myRank])
							{
								R_A = R_CRESSTII_A(rhoDM,mChi,sigma0,vEarth.norm());
							}
						//Compute result for MC
							std::vector<double> R_MC = R_CRESSTII_MC(dRdEH_O,dRdEH_Ca,dRdEH_W,mChi,vEarth.norm());;
							// cout <<"MPI Process:\t" <<myRank <<"\t IsoRing:\t" <<i<<"\tR = "<<R_MC[0] <<"+-"<<R_MC[1] <<"\t("<<R_A<<")"<<endl;
					//Save rate including the analytic result for the transparent earth
						TotalRate.push_back(i);
						TotalRate.push_back(R_MC[0]);
						TotalRate.push_back(R_MC[1]);
						TotalRate.push_back(R_A);
						MPI_File_write(file_rate,TotalRate.data(),TotalRate.size(),MPI_DOUBLE,&status);

				}
				else if(experiment=="LUX")
				{
					//Create and save dRdE-Histogram
						std::vector<std::vector<double>> dRdEH = dRdEHistogram(1,131,rho[i],mChi,sigma0,etaH);
						f.open("../results/"+SimID+"_histograms/dRdE."+std::to_string(i));
						for(unsigned int j=0;j<dRdEH.size();j++) f<<dRdEH[j][0] <<"\t"<<dRdEH[j][1] <<"\t"<<dRdEH[j][2] <<"\t"<<dRdEH[j][3]<<"\t"<<dRdErA(dRdEH[j][0],1,131,rhoDM,mChi,sigma0,vEarth.norm())<<endl;
						f.close();
					//Calculate Total event rate
						//Compute the analytic result at the first iteration
							if(i==iList[myRank]) R_A = R_LUX_A(rhoDM,mChi,sigma0,vEarth.norm());

						//Compute the MC result
							std::vector<double> R_MC = R_LUX_MC(dRdEH);

					//Save rate including the analytic result for the transparent earth
						TotalRate.push_back(i);
						TotalRate.push_back(R_MC[0]);
						TotalRate.push_back(R_MC[1]);
						TotalRate.push_back(R_A);
						MPI_File_write(file_rate,TotalRate.data(),TotalRate.size(),MPI_DOUBLE,&status);
				}
				else if(experiment!="None")
				{
					if(myRank==0) cout <<"Error: Experiment not recognized. No detection rate will be computed." <<endl;
					experiment="None";
				}
				//Console output to give an indication of the progress
					//computing time of this isodetection ring
						high_resolution_clock::time_point tIRend;
						tIRend = high_resolution_clock::now();
						double durationIR =1e-6*duration_cast<microseconds>( tIRend - tIRstart ).count();
					//Mean computing time of all finished isodetection rings so far. This will be used to make the prognosis.
						durationMean=1.0*(i-iList[myRank])/(i-iList[myRank]+1)*durationMean+1.0*durationIR/(i-iList[myRank]+1);

					cout <<myRank<<"\t\t" <<i<<"\t\t\t" <<floor(100.0*(i-iList[myRank]+1)/(iList[myRank+1]-iList[myRank]))<<"\%\t\t"<<ceil(durationIR)<<"\t\t\t"<<ceil((iList[myRank+1]-i-1)*durationMean)<<endl;

		}
		MPI_Barrier(MPI_COMM_WORLD);
		if(myRank==0) cout <<"\nData analysis complete." <<endl;
		MPI_File_close(&file_speed);
		MPI_File_close(&file_rate);

	//Create ASCII Output
		if(myRank==0)
		{
			cout <<"Creating ASCII output." <<endl;
			//Binary Input files
			//Speed
				MPI_File speedfile;
				int rc = MPI_File_open(MPI_COMM_SELF, const_cast<char*>(SpeedFilename.c_str()), MPI_MODE_RDONLY, MPI_INFO_NULL, &speedfile );
				if (rc) cout <<"Unable to read file " <<SpeedFilename <<"."<<endl;
				//ASCII Output files
					ofstream f;
					f.open("../results/"+SimID+".speed");
				//Write results in ascii files.
					for(int ring=0;ring<Isodetection_Rings;ring++)
					{
						std::vector<double> speedbuffer (3);
						MPI_File_read(speedfile,speedbuffer.data(),speedbuffer.size(),MPI_DOUBLE,&status);
						f <<180.0 / Isodetection_Rings * ring <<"\t" <<speedbuffer[1]<<"\t"<<speedbuffer[2]<<endl;
					}
				//Close file
					f.close();
					MPI_File_close(&speedfile);
			//Rate
				if(experiment!="None")
				{
					MPI_File ratefile;
					rc = MPI_File_open(MPI_COMM_SELF, const_cast<char*>(RateFilename.c_str()), MPI_MODE_RDONLY, MPI_INFO_NULL, &ratefile );
					if (rc) cout <<"Unable to read file " <<RateFilename <<"."<<endl;
					//ASCII Output files
						ofstream g;
						g.open("../results/"+SimID+"."+experiment);
					//Write results in ascii files.
						for(int ring=0;ring<Isodetection_Rings;ring++)
						{
							std::vector<double> ratebuffer (4);
							MPI_File_read(ratefile,ratebuffer.data(),ratebuffer.size(),MPI_DOUBLE,&status);
							g <<180.0 / Isodetection_Rings * ring <<"\t" <<ratebuffer[1]<<"\t"<<ratebuffer[2]<<"\t"<<ratebuffer[3]<<endl;
						}
					//Close files
						g.close();
						MPI_File_close(&ratefile);
				}
		}
		MPI_Barrier(MPI_COMM_WORLD);

//FINALIZE
	//Master process finishes logfile
		if(myRank==0)
		{
			//Delete temporary files
				cout <<"Delete temporary files and finish." <<endl;
				MPI_File_delete(const_cast<char*>(SpeedFilename.c_str()),MPI_INFO_NULL);
				MPI_File_delete(const_cast<char*>(RateFilename.c_str()),MPI_INFO_NULL);
			//Ending time and computing time
				tEnd = high_resolution_clock::now();
				double durationTotal =1e-6*duration_cast<microseconds>( tEnd - tStart ).count();
				cout <<"\nProcessing Time:\t"<< durationTotal<<"s ("<< floor(durationTotal/3600.0)<<":"<<floor(fmod(durationTotal/60.0,60.0))<<":"<<floor(fmod(durationTotal,60.0))<<":"<<floor(fmod(1000*durationTotal,1000.0))<<")."<<endl
				<<"##############################"<<endl;
			//LogFile
				LogFile_Analysis(durationTotal,numprocs);

		}
	// Finalize the MPI environment.
	    MPI_Finalize();

	return 0;
}


/*
*
COPYRIGHT AND LICENSE

Copyright (c) 2011 The Regents of the University of California.
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. All advertising materials mentioning features or use of this
software must display the following acknowledgement: This product
includes software developed by the San Diego Supercomputer Center.

4. Neither the names of the Centers nor the names of the contributors
may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS
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.
*
*
* Based on the notes by Prof. Ramon Arrowsmith(ramon.arrowsmith@asu.edu)
* Authors: Han S Kim (hskim@cs.ucsd.edu), Sriram Krishnan (sriram@sdsc.edu)
*
*/

#include <points2grid/config.h>
#include <points2grid/Interpolation.hpp>
#include <points2grid/Global.hpp>

#include <points2grid/OutCoreInterp.hpp>
#include <points2grid/InCoreInterp.hpp>
#include <points2grid/MpiInterp.hpp>

#include <string.h>
#include <math.h>
#include <float.h>
#include <stddef.h>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>
#include <assert.h>

#include <points2grid/lasfile.hpp>
#include <points2grid/debug.hpp>

#include <boost/scoped_ptr.hpp>

#include <fstream>  // std::ifstream
#include <iostream> // std::cerr
#include <string.h>

#include "mpi.h"
#include "signal.h"

/////////////////////////////////////////////////////////////
// Public Methods
/////////////////////////////////////////////////////////////

Interpolation::Interpolation(double x_dist, double y_dist, double radius,
                             int _window_size, int _interpolation_mode = INTERP_AUTO,
                             int _rank = 0, int _process_count = 1, int _reader_count = 1, long _buffer_size = 10000, mpi_times *_timer = NULL) : GRID_DIST_X (x_dist), GRID_DIST_Y(y_dist)
{
    rank = _rank;
    process_count = _process_count;
    reader_count = _reader_count;
    buffer_size = _buffer_size;
    timer = _timer;

    radius_sqr = radius * radius;
    window_size = _window_size;
    interpolation_mode = _interpolation_mode;
    input_files = NULL;
    input_file_count = 0;

    min_x = DBL_MAX;
    min_y = DBL_MAX;

    max_x = -DBL_MAX;
    max_y = -DBL_MAX;

    shapes = NULL;
    shape_count = 0;

}

Interpolation::~Interpolation()
{
    delete interp;
}

int Interpolation::init(char **inputNames, int inputNamesSize, int inputFormat, int bigtiff, int epsg_code, double *bbox,
                        int *_classifications, int _classification_count, int _first_returns, int _last_returns, SHPHandle _shape_filter, int _fill_empty)
{

    clock_t t0, t1;

    t0 = clock();

    char *inputName = inputNames[0];
    classifications = _classifications;
    classification_count = _classification_count;
    first_returns = _first_returns;
    last_returns = _last_returns;
    shape_filter = _shape_filter;
    fill_empty = _fill_empty;

    if(inputName == NULL)
    {
        cerr << "Wrong Input File Name" << endl;
        return -1;
    }

    if (interpolation_mode != INTERP_MPI)
    {
        // Only the first file is used for non mpi runs, set input_files[0].name is needed during interpolation
        // set it now...
        input_files = (input_file_info *) malloc (sizeof(input_file_info));
        input_file_count = 1;
        input_files[0].name = (char *) malloc ((strlen(inputNames[0]) + 1) * sizeof(char));
        strcpy (input_files[0].name, inputNames[0]);

        if (inputFormat == INPUT_ASCII)
        {
            FILE *fp;
            char line[1024];
            double data_x, data_y;

            if ((fp = fopen (inputName, "r")) == NULL)
            {
                cerr << "file open error" << endl;
                return -1;
            }

            // throw the first line away - it contains the header
            fgets (line, sizeof(line), fp);

            // read the data points to find min and max values
            while (fgets (line, sizeof(line), fp) != NULL)
            {
                data_x = atof (strtok (line, ",\n"));
                if (min_x > data_x)
                    min_x = data_x;
                if (max_x < data_x)
                    max_x = data_x;

                data_y = atof (strtok (NULL, ",\n"));
                if (min_y > data_y)
                    min_y = data_y;
                if (max_y < data_y)
                    max_y = data_y;

            }

            fclose (fp);
        }
        else
        { // las input

            las_file las;
            las.open (inputName);

            min_x = las.minimums ()[0];
            min_y = las.minimums ()[1];
            max_x = las.maximums ()[0];
            max_y = las.maximums ()[1];

            las.close ();

        }
    }
    else if (interpolation_mode == INTERP_MPI)
    {
        if (rank < reader_count)
        {
            input_files = (input_file_info *) malloc (
                    inputNamesSize * sizeof(input_file_info));
            input_file_count = inputNamesSize;
            for (int i = 0; i < input_file_count; i++)
            {
                input_files[i].name = (char *) malloc (
                        (strlen (inputNames[i]) + 1) * sizeof(char));
                strcpy (input_files[i].name, inputNames[i]);

                input_files[i].min_x = 0;
                input_files[i].min_y = 0;
                input_files[i].max_x = 0;
                input_files[i].max_y = 0;
                input_files[i].point_count = 0;
                input_files[i].peek_rank = -1;
            }

            for (int i = 0; i < input_file_count; i++)
            {
                if (rank == i % reader_count)
                {
                    las_file las;
                    las.open (input_files[i].name);
                    input_files[i].min_x = las.minimums ()[0];
                    input_files[i].min_y = las.minimums ()[1];
                    input_files[i].max_x = las.maximums ()[0];
                    input_files[i].max_y = las.maximums ()[1];
                    input_files[i].point_count = las.points_count ();
                    input_files[i].peek_rank = rank;
                    las.close ();
//                    printf("%s (%i of %i), point_count = %li\n",
//                            input_files[i].name, i, input_file_count,
//                            input_files[i].point_count);

                }
            }
            for (int i = 0; i < input_file_count; i++)
            {
                if (input_files[i].peek_rank != -1)
                {
                    for (int j = 0; j < reader_count; j++)
                    {
                        if (rank != j)
                        {

                            MPI_Send (input_files[i].name, strlen(input_files[i].name)+1, MPI_CHAR, j, 1, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].min_x), 1, MPI_DOUBLE, j, 2, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].min_y), 1, MPI_DOUBLE, j, 3, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].max_x), 1, MPI_DOUBLE, j, 4, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].max_y), 1, MPI_DOUBLE, j, 5, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].point_count), 1, MPI_LONG, j, 6, MPI_COMM_WORLD);
                            MPI_Send (&(input_files[i].peek_rank), 1, MPI_INT, j, 7, MPI_COMM_WORLD);
                        }
                    }

                }
            }

            for (int i = 0; i < input_file_count; i++)
            {
                if (input_files[i].peek_rank == -1)
                {
                    MPI_Recv (input_files[i].name, strlen(input_files[i].name)+1, MPI_CHAR, i % reader_count, 1, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].min_x), 1, MPI_DOUBLE, i % reader_count, 2, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].min_y), 1, MPI_DOUBLE, i % reader_count, 3, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].max_x), 1, MPI_DOUBLE, i % reader_count, 4, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].max_y), 1, MPI_DOUBLE, i % reader_count, 5, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].point_count), 1, MPI_LONG, i % reader_count, 6, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
                    MPI_Recv (&(input_files[i].peek_rank), 1, MPI_INT, i % reader_count, 7, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
//                    printf("received %s (%i of %i)\n", input_files[i].name,
//                            i, input_file_count);
                }
            }
            min_x = min_y = DBL_MAX;
            max_x = max_y = DBL_MIN;

            for (int i = 0; i < input_file_count; i++)
            {
                if (input_files[i].min_x < min_x)
                {
                    min_x = input_files[i].min_x;
                }
                if (input_files[i].min_y < min_y)
                {
                    min_y = input_files[i].min_y;
                }
                if (input_files[i].max_x > max_x)
                {
                    max_x = input_files[i].max_x;
                }
                if (input_files[i].max_y > max_y)
                {
                    max_y = input_files[i].max_y;
                }
            }
        }
//        if (rank == 0) {
//            printf("region = (%8.4f, %8.4f), (%8.4f, %8.4f)\n",
//                    min_x, min_y, max_x, max_y);
//        }
    }

    t1 = clock();

    //////////////////////////////////////////////////////////////////////
    // Intialization Step excluding min/max searching
    //////////////////////////////////////////////////////////////////////

    // send input file info collected above by the readers to the
    // writers so all have it
    if(rank == 0){
        for(int i = reader_count; i < process_count; i++){
            MPI_Send (&min_x, 1, MPI_DOUBLE, i, 1, MPI_COMM_WORLD);
            MPI_Send (&min_y, 1, MPI_DOUBLE, i, 1, MPI_COMM_WORLD);
            MPI_Send (&max_x, 1, MPI_DOUBLE, i, 1, MPI_COMM_WORLD);
            MPI_Send (&max_y, 1, MPI_DOUBLE, i, 1, MPI_COMM_WORLD);
        }
    }
    if(rank>=reader_count){
        MPI_Recv(&min_x, 1, MPI_DOUBLE, 0, 1, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
        MPI_Recv(&min_y, 1, MPI_DOUBLE, 0, 1, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
        MPI_Recv(&max_x, 1, MPI_DOUBLE, 0, 1, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
        MPI_Recv(&max_y, 1, MPI_DOUBLE, 0, 1, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
    }

    // assign min max values to bbox, return if there is no overlap
    if (bbox != NULL)
    {
        if (rectangles_overlap (min_x, min_y, max_x, max_y, bbox[0], bbox[1],
                                bbox[2], bbox[3]))
        {
            min_x = bbox[0];
            min_y = bbox[1];
            max_x = bbox[2];
            max_y = bbox[3];
        }
        else
        {
            cerr << "BBOX and LAS input data do not overlap" << endl;
            return -1;
        }

    }

    // all processes set raster size
    GRID_SIZE_X = (int)(ceil((max_x - min_x)/GRID_DIST_X)) + 1;
    GRID_SIZE_Y = (int)(ceil((max_y - min_y)/GRID_DIST_Y)) + 1;

    //dbg(3, ("GRID_SIZE_x %i, grid_size y %i, rank %i\n", GRID_SIZE_X, GRID_SIZE_Y, rank);

    // Construct an Interp based on the mode
    if (interpolation_mode == INTERP_AUTO) {
        // if the size is too big to fit in memory,
        // then construct out-of-core structure
        if(GRID_SIZE_X * GRID_SIZE_Y > MEM_LIMIT) {
            interpolation_mode= INTERP_OUTCORE;
        } else {
            interpolation_mode = INTERP_INCORE;
        }
    }

    if (interpolation_mode == INTERP_OUTCORE) {

        // this is untested and remains for historical reasons
        cerr << "Using out of core interp code" << endl;
        interp = new OutCoreInterp(GRID_DIST_X, GRID_DIST_Y, GRID_SIZE_X, GRID_SIZE_Y, radius_sqr, min_x, max_x, min_y, max_y, window_size);
        if(interp == NULL)
        {
            cerr << "OutCoreInterp construction error" << endl;
            return -1;
        }
        cerr << "Interpolation uses out-of-core algorithm" << endl;

    } else if (interpolation_mode == INTERP_MPI){

        interp = new MpiInterp(this, GRID_DIST_X, GRID_DIST_Y, GRID_SIZE_X, GRID_SIZE_Y, radius_sqr, min_x, max_x, min_y, max_y, window_size, rank, process_count, reader_count, buffer_size, timer);

    } else {

        interp = new InCoreInterp(GRID_DIST_X, GRID_DIST_Y, GRID_SIZE_X, GRID_SIZE_Y, radius_sqr, min_x, max_x, min_y, max_y, window_size);

    }

    interp->set_bigtiff(bigtiff);
    interp->set_epsg_code(epsg_code);
    if(interp->init() < 0)
    {
        cerr << "inter->init() error" << endl;
        return -1;
    }

    return 0;
}

int Interpolation::interpolation(char *outputName,
                                 int inputFormat,
                                 int outputFormat,
                                 unsigned int outputType)
{
    if(timer)
    {
        if(rank == reader_count)printf("Readers sending points to writers...\n");
        timer->interp_start = time(NULL);
    }

    int rc;

    double data_x, data_y;
    double data_z;
    // This case is for historical preservation only, not used or tested
    if (inputFormat == INPUT_ASCII && interpolation_mode != INTERP_MPI) {
        FILE *fp;
        char line[1024];

        if((fp = fopen(input_files[0].name, "r")) == NULL)
        {
            printf("file open error\n");
            return -1;
        }

        // throw the first line away - it contains the header
        fgets(line, sizeof(line), fp);

        // read every point and generate DEM
        while(fgets(line, sizeof(line), fp) != NULL)
        {
            data_x = atof(strtok(line, ",\n"));
            data_y = atof(strtok(NULL, ",\n"));
            data_z = atof(strtok(NULL, ",\n"));

            data_x -= min_x;
            data_y -= min_y;

            //if((rc = interp->update(arrX[i], arrY[i], arrZ[i])) < 0)
            if((rc = interp->update(data_x, data_y, data_z)) < 0)
            {
                cerr << "interp->update() error while processing " << endl;
                return -1;
            }
        }

        fclose(fp);
    }

    else
    { // input format is LAS
        if (interpolation_mode == INTERP_MPI)
        {
            if (interp->getIsReader ())
            {
                if (input_file_count == 1) // Single input file
                {
                    if (rectangles_overlap (min_x, min_y, max_x, max_y,
                                            input_files[0].min_x,
                                            input_files[0].min_y,
                                            input_files[0].max_x,
                                            input_files[0].max_y))
                    {
                        las_file las;
                        las.open (input_files[0].name);

                        size_t count = las.points_count ();
                        size_t index (0);
                        size_t stride = count / interp->getReaderCount ();
                        size_t left_over_count = count
                                % interp->getReaderCount ();
                        index = rank * stride;
                        count = (rank + 1) * stride;
                        if (rank == interp->getReaderCount () - 1)
                        {
                            count += left_over_count;
                        }
                        size_t points_processed(0);
                        while (index < count)
                        {
                            data_x = las.getX (index);
                            data_y = las.getY (index);
                            data_z = las.getZ (index);
                            if (point_contained (data_x, data_y, min_x, min_y,
                                                 max_x, max_y))
                            {
                                if (pass_filter(las, index))
                                {
                                    points_processed ++;
                                    data_x -= min_x;
                                    data_y -= min_y;
                                    //
                                    //cerr << "calling update rank "<< rank << endl;
                                    if ((rc = interp->update (data_x, data_y,
                                                              data_z)) < 0)
                                    {
                                        cerr
                                                << "interp->update() error while processing "
                                                << endl;
                                        return -1;
                                    }
                                }
                            }
                            index++;
                        }
                        dbg(5, "rank = %i, index = %li, points_processed = %li", rank, index, points_processed);
                    }
                    interp->getReadDone ()[rank] = 1;

                    for (int i = 0; i < process_count; i++)
                    {
                        if (interp->getWriters ()[i])
                        {
                            interp->updateGridPointSend (i, 1, 1, 1, 1);
                        }
                    }
                }

                else // (input_file_count > 1)
                {

                    for (int i = 0; i < input_file_count; i++)
                    {
                        if (input_files[i].peek_rank == rank)
                        {
                            if (rectangles_overlap (min_x, min_y, max_x, max_y,
                                                    input_files[i].min_x,
                                                    input_files[i].min_y,
                                                    input_files[i].max_x,
                                                    input_files[i].max_y))
                            {
                                dbg(5,
                                    "start read and send rank %i, peek_rank %i, name %s, point_count %li, global and file min_x, miny %lf, %lf, %lf, %lf\n",
                                    rank, input_files[i].peek_rank,
                                    input_files[i].name,
                                    input_files[i].point_count, min_x,
                                    input_files[i].min_x, min_y,
                                    input_files[i].min_y);

                                las_file las;
                                las.open (input_files[i].name);

                                size_t count = input_files[i].point_count;
                                size_t index (0);
                                size_t points_processed (0);
                                while (index < count)
                                {
                                    data_x = las.getX (index);
                                    data_y = las.getY (index);
                                    data_z = las.getZ (index);

                                    if (point_contained (data_x, data_y, min_x,
                                                         min_y, max_x, max_y))
                                    {
                                        if (pass_filter (las, index))
                                        {
                                            points_processed++;
                                            data_x -= min_x;
                                            data_y -= min_y;
                                            if ((rc = interp->update (data_x,
                                                                      data_y,
                                                                      data_z))
                                                    < 0)
                                            {
                                                cerr
                                                        << "interp->update() error while processing "
                                                        << endl;
                                                return -1;
                                            }
                                        }
                                    }
                                    index++;
                                }
                                dbg(3, "rank = %i, index = %li, points_processed = %li", rank, index, points_processed);
                                las.close ();
                            }
                            dbg(2, "*** rank %i, input file %s done with read and send ***", rank, input_files[i].name);
                        }
                    }

                    interp->getReadDone ()[rank] = 1;

                    for (int i = 0; i < process_count; i++)
                    {
                        if (interp->getWriters ()[i])
                        {
                            interp->updateGridPointSend (i, 1, 1, 1, 1);
                        }
                    }

                }
            }
            else if (interp->getIsWriter ())            // rank is a writer
            {
                interp->updateGridPointRecv ();
            }
            //MPI_Barrier (MPI_COMM_WORLD);
        }
        else // interpolation_mode is other than INTERP_MPI,
             // single input file only supported, bbox supported, filters supported

        {
            if (rectangles_overlap (min_x, min_y, max_x, max_y,
                                    input_files[0].min_x, input_files[0].min_y,
                                    input_files[0].max_x, input_files[0].max_y))
            {

                las_file las;
                las.open (input_files[0].name);

                size_t count = las.points_count ();
                size_t index (0);
                size_t points_processed(0);
                while (index < count)
                {
                    data_x = las.getX (index);
                    data_y = las.getY (index);
                    data_z = las.getZ (index);
                    if (point_contained (data_x, data_y, min_x, min_y, max_x,
                                         max_y))
                    {
                        if (pass_filter (las, index))
                        {
                            points_processed++;
                            data_x -= min_x;
                            data_y -= min_y;

                            if ((rc = interp->update (data_x, data_y, data_z))
                                    < 0)
                            {
                                cerr
                                        << "interp->update() error while processing "
                                        << endl;
                                return -1;
                            }
                        }
                    }
                    index++;
                }
                dbg(3, "index = %li, points_processed = %li", index, points_processed);
            }
        }
    }
    if(timer)timer->interp_end = time(NULL);

    if((rc = interp->finish(outputName, outputFormat, outputType)) < 0)
    {
        cerr << "interp->finish() error" << endl;
        return -1;
    }

    return 0;
}

void Interpolation::setRadius(double r)
{
    radius_sqr = r * r;
}

unsigned int Interpolation::getGridSizeX()
{
    return GRID_SIZE_X;
}

unsigned int Interpolation::getGridSizeY()
{
    return GRID_SIZE_Y;
}

// rx1, ry1 = lower left, rx2, ry2 = upper right
int Interpolation::point_contained(double x, double y, double rx1, double ry1, double rx2, double ry2)
{
    return (x >= rx1 && x <= rx2 && y >= ry1 && y <= ry2);
}
// rx1, ry1 = lower left, rx2, ry2 = upper right of first rectangle, sx1, sy1 = lower left, sx2, sy2 = upper right of second rectangle
int Interpolation::rectangles_overlap (double rx1, double ry1, double rx2, double ry2, double sx1, double sy1, double sx2, double sy2)
{
    return sx1 < rx2 && sx2 > rx1 && sy1 < ry2 && sy2 > ry1;
}

int Interpolation::pass_filter (las_file &las, size_t index)
{

    if(!in_shape(las.getX(index), las.getY(index)))
    {
        return 0;
    }

    if(classification_count == 0 && first_returns == 0 && last_returns == 0)
    {
        return 1;
    }
    // At this point there either is no shape_filter shape_filter has been passed, check the point attr filters
    int point_classification = las.getClassification(index);
    for(int i=0; i < classification_count; i++)
    {
        if(point_classification == classifications[i])
        {
            return 1;
        }
    }
    if (first_returns && las.getReturnNumber(index)==1)
    {
        return 1;
    }
    if (last_returns && las.getReturnNumber(index) == las.getReturnCount(index))
    {
        return 1;
    }

    return 0;

}

//static int in_shape_count = 0;

int Interpolation::in_shape(double x, double y)
{
//    in_shape_count++;
//    if(in_shape_count % 100000 == 0){
//        printf("in_shape %lf %lf %i %i\n", x, y, rank, in_shape_count);
//    }
    if(shape_filter == NULL)
    {
        return 1;
    }

    if(shapes == NULL)
    {
        init_shape_filter_index();
    }

    ShapeItemVisitor visitor;
    shape_filter_index.query(y, y, &visitor);
    std::vector<void*> found_segments;
    found_segments = visitor.items;

    geos::geom::Coordinate las_point;
    las_point.x = x;
    las_point.y = y;
    int cross_count = 0;

    for (std::size_t i = 0; i < found_segments.size (); i++)
    {

        ShapeSegment *segment = (ShapeSegment *)(found_segments[i]);
        double x1 =
                shapes[segment->shape]->padfX[segment->vertex_1];
        double y1 =
                shapes[segment->shape]->padfY[segment->vertex_1];
        double x2 =
                shapes[segment->shape]->padfX[segment->vertex_1 + 1];
        double y2 =
                shapes[segment->shape]->padfY[segment->vertex_1 + 1];

        geos::geom::Coordinate p1 (x1, y1);
        geos::geom::Coordinate p2 (x2, y2);

        cross_count += countSegment (p1, p2, las_point);
        //printf("%i %li %li  %lf    %lf %lf   %lf     %lf %lf\n", cross_count, i, found_segments.size (), x, x1,x2, y ,  y1, y2);

    }

    return cross_count % 2;
}

int Interpolation::init_shape_filter_index()
{
    shape_count = shape_filter->nRecords;
    shapes = (SHPObject **) malloc(shape_count * sizeof(SHPObject *));

    for(int i=0; i<shape_count; i++)
    {
        shapes[i] =  SHPReadObject(shape_filter, i);
        int part_cnt = shapes[i]->nParts;
        for(int j=0; j<part_cnt; j++)
        {
            int start_vertex = shapes[i]->panPartStart[j];
            int end_vertex;
            if(j < part_cnt - 1)
            {
                end_vertex = shapes[i]->panPartStart[j+1] - 1;
            }
            else
            {
                end_vertex =  shapes[i]->nVertices - 1;
            }
            for(int k = start_vertex; k < end_vertex; k++)
            {
                double min =shapes[i]->padfY[k];
                double max =shapes[i]->padfY[k+1];

                if(max<min){
                    double tmp = min;
                    min = max;
                    max = tmp;
                }

                ShapeSegment *segment = (ShapeSegment *)malloc(sizeof(ShapeSegment));
                segment->shape = i;
                segment->vertex_1 = k;
                shape_filter_index.insert(min, max, segment);

            }
        }
    }

    return 1;
}

int
Interpolation::countSegment(const geos::geom::Coordinate& p1,
                                 const geos::geom::Coordinate& p2, const geos::geom::Coordinate& point)
{
        // This code is from geos v3.6 RayCrossingCounter.cpp
        // and adapted for this use
        int cross = 0;
        // For each segment, check if it crosses
        // a horizontal ray running from the test point in
        // the positive x direction.

        // check if the segment is strictly to the left of the test point
        if (p1.x < point.x && p2.x < point.x)
                return 0;

        // check if the point is equal to the current ring vertex
        if (point.x == p2.x && point.y == p2.y)
        {
                //isPointOnSegment = true;
                return 0;
        }

        // For horizontal segments, check if the point is on the segment.
        // Otherwise, horizontal segments are not counted.
        if (p1.y == point.y && p2.y == point.y)
        {
                //double minx = p1.x;
                //double maxx = p2.x;

                //if (minx > maxx)
                //{
                //        minx = p2.x;
                //        maxx = p1.x;
                //}

                //if (point.x >= minx && point.x <= maxx)
                        //isPointOnSegment = true;

                return 0;
        }

        // Evaluate all non-horizontal segments which cross a horizontal ray
        // to the right of the test pt.
        // To avoid double-counting shared vertices, we use the convention that
        // - an upward edge includes its starting endpoint, and excludes its
        //   final endpoint
        // - a downward edge excludes its starting endpoint, and includes its
        //   final endpoint
        if (((p1.y > point.y) && (p2.y <= point.y)) ||
                ((p2.y > point.y) && (p1.y <= point.y)) )
        {
                // For an upward edge, orientationIndex will be positive when p1->p2
                // crosses ray. Conversely, downward edges should have negative sign.

                int sign = orientationIndex(p1, p2, point);
                if (sign == 0)
                {
                        //isPointOnSegment = true;
                        return 0;
                }

                if (p2.y < p1.y)
                        sign = -sign;

                // The segment crosses the ray if the sign is strictly positive.
                if (sign > 0)
                        cross++;
        }
        return cross;
}

// This code is from geos v3.6 RayCrossingCounter.cpp
// and called from the countSegment method
// it avoids counting common segment vertex crossings
int
Interpolation::orientationIndex(const geos::geom::Coordinate& p1,
         const geos::geom::Coordinate& p2, const geos::geom::Coordinate& q)
{
        // travelling along p1->p2, turn counter clockwise to get to q return 1,
        // travelling along p1->p2, turn clockwise to get to q return -1,
        // p1, p2 and q are colinear return 0.
        double dx1=p2.x-p1.x;
        double dy1=p2.y-p1.y;
        double dx2=q.x-p2.x;
        double dy2=q.y-p2.y;
        return geos::algorithm::RobustDeterminant::signOfDet2x2(dx1,dy1,dx2,dy2);
}