// ************************  Prize-collecting Steiner Forest (PCSF) *********************//
// This is a C++ implementation of our heuristic algorithm for the PCSF (Akhmedov et al 2017).
// The heuristic is developed for functional analyses of large biological networks in a reasonable time.

// Loading the required libraries
#include <Rcpp.h>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/random.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <time.h>
#include <stdio.h>
#include <boost/graph/graphviz.hpp>
#include <fstream>
#include <vector>
#include <utility>
#include <string>
#include <math.h>
#include <iomanip>
#include <boost/limits.hpp>
#include <queue>

using namespace boost;
using namespace Rcpp;
using namespace std;

// Node class
class Node
{  public:
  vector <int> children;
  int father;
  int size;
  double price;
  Node(){};
  Node(const Node & other){
    father=other.father;
    children= other.children;}

  Node & operator= (const Node & other){
    father=other.father;
    children= other.children;
    return *this;
  }

};

// Properties of Vertices in the network
struct VertexProperties  {
  VertexProperties() :c(0) {}
  VertexProperties(string const & name) : name(name),c(0){}
  string name;
  double c;
};

// Properties of the graph that is used within the BOOST
typedef adjacency_list<vecS, vecS, undirectedS, VertexProperties, property < edge_weight_t, double > > GraphBase;
typedef graph_traits<GraphBase>::vertex_iterator vertex_iterator;
typedef graph_traits<GraphBase>::out_edge_iterator edge_iterator;
typedef graph_traits<GraphBase>::edge_iterator graph_edge_iterator;
typedef graph_traits<GraphBase>::edge_descriptor Edge;
typedef graph_traits<GraphBase>::vertex_descriptor Vertex;

GraphBase g, g_adjusted; GraphBase G; GraphBase G_pruned;
property_map<GraphBase, edge_weight_t>::type weight_g;
property_map<GraphBase, edge_weight_t>::type weight_g_adjusted;
property_map<GraphBase, edge_weight_t>::type weight_G;
property_map<GraphBase, edge_weight_t>::type weight_G_pruned;

int Root = -1;
static map <string, int> g_map;
static map <string, int> G_map;
static map <string, int> G_pruned_map;

void clear_variables(){
  g.clear();
  g_adjusted.clear();
  G.clear();
  G_pruned.clear();
  g_map.clear();
  G_map.clear();
  G_pruned_map.clear();
}

// Map
int idx_g(String const & id)
{
  map<string, int>::iterator mit = g_map.find(id);
  if (mit == g_map.end())
    return g_map[id] = add_vertex(VertexProperties(id), g);
  return mit->second;
}
int idx_G(string const & id)
{
  map<string, int>::iterator mit = G_map.find(id);
  if (mit == G_map.end())
    return G_map[id] = add_vertex(VertexProperties(id), G);
  return mit->second;
}
int idx_G_pruned(string const & id)
{
  map<string, int>::iterator mit = G_pruned_map.find(id);
  if (mit == G_pruned_map.end())
    return G_pruned_map[id] = add_vertex(VertexProperties(id), G_pruned);
  return mit->second;
}

// Reading the input network
void read_input_graph(CharacterVector from, CharacterVector to,  NumericVector cost, CharacterVector prize, NumericVector prize_v)
{
  for(int i=0; i < from.size(); i++){
    add_edge(vertex(idx_g(from[i]), g), vertex(idx_g(to[i]), g), cost[i], g);
  }
  for(int i=0; i<prize.size(); i++){
    g[idx_g(prize[i])].c = prize_v[i];
  }

  // cerr << num_edges(g) << " edges, " << num_vertices(g) << " vertices" << endl;

  g_adjusted = g;
  weight_g_adjusted = get(edge_weight, g_adjusted);
  graph_edge_iterator ei, ei_end; double penalty; Vertex sour, tar;
  for(tie(ei, ei_end) = edges(g_adjusted); ei != ei_end; ++ei){
    sour = source(*ei,g_adjusted); tar = target(*ei,g_adjusted); penalty = 0;
    if(g_adjusted[sour].c < 0 && g_adjusted[tar].c < 0){
      penalty = g_adjusted[sour].c + g_adjusted[tar].c;
    } else if( g_adjusted[sour].c < 0 ){
      penalty = g_adjusted[sour].c;
    } else if( g_adjusted[tar].c < 0 ){
      penalty = g_adjusted[tar].c;
    }
    weight_g_adjusted[*ei] = weight_g_adjusted[*ei] + abs(penalty);
  }
}

// A function to dynamically remove the leaf node if its prize smaller than connection cost.
// It is used called within the process_leafs() function.
void clear(vector <Node> & predecessor, int & current_node){
  int node=current_node;
  for(unsigned int j=0; j<predecessor[node].children.size(); j++){
    clear(predecessor, predecessor[node].children[j]);
  }
  predecessor[node].father=-1;
}

// A function to dynamically remove the leaf node if its prize smaller than connection cost.
void process_leafs(vector <Node> & predecessor, int & current_node, Edge &e, bool &found){
  for(unsigned int i=0; i<predecessor[current_node].children.size(); i++){
    process_leafs(predecessor, predecessor[current_node].children[i], e, found);
  }
  int node = current_node;
  if(node != predecessor[node].father){
    boost::tuples::tie(e,found) = edge( vertex(predecessor[node].father, G_pruned),vertex(node, G_pruned) , G_pruned);
    if(predecessor[node].price - weight_G_pruned[e] <= 0) {
      clear(predecessor, node);
    }
  }

}

// A function to dynamically sum up the prizes of vertices.
void price_collect(vector <Node> & predecessor, int & current_node, Edge &e, bool &found){
  for(unsigned int i=0; i<predecessor[current_node].children.size(); i++){
    price_collect(predecessor, predecessor[current_node].children[i], e, found);
  }
  int node = current_node;
  if(node != predecessor[node].father){
    boost::tuples::tie(e,found) = edge(vertex(predecessor[node].father, G_pruned),vertex(node, G_pruned) , G_pruned);
    if(predecessor[node].price - weight_G_pruned[e] > 0)
      predecessor[predecessor[node].father].price = predecessor[predecessor[node].father].price + predecessor[node].price - weight_G_pruned[e];
  }
}

// After reading the input network information from the input file, the algorithm constructs a
vector< Vertex > constructG(vector<int> & terminals, int &Root){

  // Distance: all-pairs-shortest-path distance matrix
  // perPath: List of arcs in all-pairs-shortest-path distance matrix
  vector <vector <vector<int> > > perPath;
  vector <vector<double> > Distance;
  perPath.resize (terminals.size());
  Distance.resize (terminals.size());
  for (unsigned int i = 0; i < terminals.size(); ++i) {
    perPath [i].resize(terminals.size());
    Distance [i].resize(terminals.size());
  }

  // Computing all-pairs-shortest-path distance matrix
  Vertex from; int current, pred, outer = 0, inner;
  std::vector<Vertex> p = vector<Vertex> (num_vertices(g));
  std::vector<double> d = vector<double> (num_vertices(g));

  for (std::vector<int>::iterator first=terminals.begin(); first!=terminals.end(); ++first){

    from = vertex(*first, g_adjusted);
    dijkstra_shortest_paths(g_adjusted, from,
                            predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g_adjusted))).
                              distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g_adjusted))));

    inner = outer + 1;
    for (std::vector<int>::iterator second = first+1; second!=terminals.end(); ++second) {
      current=*second; pred=p[current];
      Distance[outer][inner] = d[current];
      Distance[inner][outer] = d[current];
      while(pred!=current){perPath[outer][inner].push_back(current); current=pred; pred=p[current]; } perPath[outer][inner].push_back(current);
      inner++;
    }
    outer++;
  }

  // Heuristic Clustering, given large input network, the algorithm clusters input network into
  // smaller clusters, and solves the MST afterwards
  set<int> V;
  set<int> D;
  unsigned int root_index = -1;
  vector<int> node_labels;
  node_labels.resize(terminals.size());
  for(unsigned int i=0; i< node_labels.size(); i++){
    node_labels[i] = 0;}
  for(unsigned int i=0; i<terminals.size(); i++){
    if(terminals[i] == Root){
      root_index = i;}
    else{
      V.insert(i);}
  }
  node_labels[root_index] = INT_MAX;
  int clusterID=0; int targ;

  while(!V.empty()){
    clusterID++;  current = *V.begin();
    node_labels[current] =clusterID;
    V.erase(current);

    D.clear();
    for (unsigned int i=0; i < terminals.size(); i++) {
      targ = i;
      if(node_labels[targ] == 0 && current != targ && i != root_index){
        if(g[terminals[current]].c >= Distance[current][targ] && g[terminals[targ]].c >= Distance[current][targ]){
          if(g[terminals[targ]].c > 0){
            D.insert(targ); V.erase(targ);
          }
          node_labels[targ]=clusterID;
        }
      }
    }

    while(!D.empty()){
      current = *D.begin();  D.erase(current);
      for (unsigned int i=0; i < terminals.size(); i++) {
        targ = i;
        if(node_labels[targ] == 0 && current != targ && i != root_index){
          if(g[terminals[current]].c >= Distance[current][targ] && g[terminals[targ]].c >= Distance[current][targ]){
            if(g[terminals[targ]].c > 0){ D.insert(targ); V.erase(targ);}
            node_labels[targ]=clusterID;
          }
        }
      }
    }

  }

  // Identfying the vertex membership with respect to clusters
  vector<vector <int> > clusters(clusterID+1);
  for(unsigned int i=0; i< node_labels.size(); i++){
    for(int j=0; j<=clusterID; j++){
      if(node_labels[i] == j){
        clusters[j].push_back(i);
      }
    }
  }

  std::vector<int>::iterator it, itt; int num_clusters=0;
  for(unsigned int i=1; i<clusters.size(); i++){
    if(clusters[i].size() > 1) num_clusters++;
  }

  if(num_clusters == 0){
    //cout<<"There is no tree in construct G ()"<<endl;
    //return 0;
  }

  // Regrouping the singletone and dobletone clusters after clustering
  unsigned int threshold_num = 2; int min_index; double min_distance;
  for(unsigned int i=1; i<clusters.size(); i++){
    if( clusters[i].size() <= threshold_num){
      for (it=clusters[i].begin(); it!=clusters[i].end(); ++it){
        min_index = -1; min_distance = DBL_MAX;
        for(unsigned int j=1; j<clusters.size(); j++){
          if( clusters[j].size() > threshold_num){
            for (itt=clusters[j].begin(); itt!=clusters[j].end(); ++itt){
              if(min_distance > Distance[*it][*itt] -g[terminals[*it]].c - g[terminals[*itt]].c ){
                min_distance = Distance[*it][*itt] -g[terminals[*it]].c - g[terminals[*itt]].c; min_index = j;
              }
            }
          }
        }

        if (min_index != -1){
          clusters[min_index].push_back(*it);
          *it = -1;
        }
      }
    }

  }

  // Construct an artificial graph G, which is composed of all clusters determined
  // from Heuristic Clustering phase

  string str; unsigned int index1=-1, index2=-1;

  for (unsigned int l = 0; l<terminals.size(); l++){
    str=to_string(l);
    index1=idx_G(str);
    G[index1].c = g[terminals[l]].c;
    G[index1].name = g[terminals[l]].name;
  }

  for (unsigned int l = 0; l<terminals.size(); l++){
    if(l != root_index){
      str=to_string(l);
      index1=idx_G(str);
      add_edge(root_index, index1, Distance[root_index][index1], G);
    }
  }

  for(unsigned int i = 1; i < clusters.size(); i++){
    for (it=clusters[i].begin(); it!=clusters[i].end(); ++it){
      if(*it != -1){
        str=to_string(*it); index1=idx_G(str);
        for (itt=it+1; itt!=clusters[i].end(); ++itt){
          if(*itt != -1){
            str=to_string(*itt); index2=idx_G(str);
            add_edge(index1, index2, Distance[*it][*itt], G);
          }
        }
      }
    }
  }

  weight_G = get(edge_weight, G);
  vector < Vertex > spanning_tree_G(num_vertices(G));
  prim_minimum_spanning_tree(G, & spanning_tree_G[0]);

  Edge beg; Vertex sour, tar; double cost;
  Edge e; bool found;

  weight_g = get(edge_weight, g);

  vector<int> path; index1=0; index2=0;
  edge_iterator out_i, out_end; int add=0;

  // Solving the Minimum Spanning Tree on G
  for(unsigned int i = 0; i < spanning_tree_G.size(); ++i ){

    if(spanning_tree_G[i]!=i ){

      if(i> spanning_tree_G[i]) path=perPath[spanning_tree_G[i]][i];
      else path=perPath[i][spanning_tree_G[i]];

      for(unsigned int j=0; j<path.size()-1; j++){

        sour=vertex(path[j], g); tar= vertex(path[j+1], g);
        boost::tuples::tie(beg, found) = edge(sour, tar,g);
        cost=get(weight_g, beg);

        index1=idx_G_pruned(to_string(path[j])); index2=idx_G_pruned(to_string(path[j+1]));
        add=0;
        for (boost::tuples::tie(out_i, out_end) = out_edges(vertex(index1,G_pruned), G_pruned); out_i != out_end; ++out_i) {
          if(target(*out_i, G_pruned)==index2) add++;
        }
        if(!add) add_edge(index1, index2, cost, G_pruned);
      }
  }
  }

  weight_G_pruned = get(edge_weight, G_pruned);

  vector< Vertex >spanning_tree_G_pruned(num_vertices(G_pruned));
  prim_minimum_spanning_tree(G_pruned, &spanning_tree_G_pruned[0]);

  double total1=0;
  for(unsigned int i = 0; i < spanning_tree_G_pruned.size(); ++i ){
    if(spanning_tree_G_pruned[i] != i){
      sour= vertex(i,G_pruned); tar=vertex(spanning_tree_G_pruned[i], G_pruned);
      boost::tuples::tie(beg, found) = edge(sour, tar,G_pruned);
      total1+=get(weight_G_pruned, beg);
    }
  }

  return spanning_tree_G_pruned;

  }

// After obtaining MST tree, the algorithm prunes the leaf nodes
// which have prizes smaller than connection cost
double cut(int &Root, vector< Vertex > &spanning_tree_G_pruned,  vector< string > &tree_from,  vector< string > &tree_to,  vector< double > &tree_cost, map < string, double > &tree_terminals){

  weight_G_pruned = get(edge_weight, G_pruned);

  Edge e; bool found;

  Edge beg; Vertex sour, tar; int ancestor=-1; bool select=false;

  int root= -1;
  if(Root == -1){
    double max=0;
    vertex_iterator ei, ef;
    for(tie(ei, ef)= vertices(G_pruned); ei!=ef; ei++){
      if(g[boost::lexical_cast<int>(G_pruned[*ei].name)].c > max){
        root = *ei;
        max = g[boost::lexical_cast<int>(G_pruned[*ei].name)].c;
      }
    }
  } else {root = idx_G_pruned(to_string(Root));}

  select = true;

  bool ancestor_changed=true; unsigned int father, temp; ancestor = root;
  if (select){
    father=spanning_tree_G_pruned[ancestor];
    spanning_tree_G_pruned[ancestor]=ancestor;
    while(ancestor_changed){
      if(spanning_tree_G_pruned[father]==father){
        ancestor_changed=false;
        spanning_tree_G_pruned[father]=ancestor;
      }else{
        temp=spanning_tree_G_pruned[father];
        spanning_tree_G_pruned[father]=ancestor;
        ancestor=father; father=temp;
      }

    }

  }else{
    //cout <<"There is no tree"<<endl;
    return 0;
  }

  vector<Node> predecessor(num_vertices(G_pruned));
  if(select){
    for(unsigned int i = 0; i < spanning_tree_G_pruned.size(); ++i ){
      if(spanning_tree_G_pruned[i]!=i){
        predecessor[i].father=spanning_tree_G_pruned[i];
        predecessor[spanning_tree_G_pruned[i]].children.push_back(i);
      }else{predecessor[i].father=i;}
    }
  }

  for(unsigned int i = 0; i < predecessor.size(); ++i ){
    predecessor[i].size=predecessor[i].children.size();
    predecessor[i].price=g[boost::lexical_cast<int>(G_pruned[i].name)].c;

  }

  price_collect(predecessor, root, e, found);

  process_leafs(predecessor, root, e, found);

  weight_g = get(edge_weight, g);

  // Tree
  for(unsigned int i = 0; i < predecessor.size(); ++i ){
    if(predecessor[i].father != -1 &&  predecessor[i].father != (int) i ){
      sour= vertex(boost::lexical_cast<int>(G_pruned[i].name),g); tar = vertex(boost::lexical_cast<int>(G_pruned[predecessor[i].father].name),g);
      boost::tuples::tie(beg, found) = edge(sour, tar,g);
      tree_from.push_back(g[boost::lexical_cast<int>(G_pruned[i].name)].name);
      tree_to.push_back(g[boost::lexical_cast<int>(G_pruned[predecessor[i].father].name)].name);
      tree_cost.push_back(weight_g[beg]);
      tree_terminals[g[boost::lexical_cast<int>(G_pruned[i].name)].name] = g[boost::lexical_cast<int>(G_pruned[i].name)].c;
      tree_terminals[g[boost::lexical_cast<int>(G_pruned[predecessor[i].father].name)].name] = g[boost::lexical_cast<int>(G_pruned[predecessor[i].father].name)].c;
    }
  }

  double total = 0, lostPrice =0;
  int uncovered_nodes = 0;
  for(unsigned int i = 0; i < predecessor.size(); ++i ){
    if(predecessor[i].father != -1){
      if(predecessor[i].father != (int) i){
        sour= vertex(i,G_pruned); tar = vertex(predecessor[i].father,G_pruned);
        boost::tuples::tie(beg, found) = edge(sour, tar,G_pruned);
        total+=get(weight_G_pruned, beg);
      }
    }
  }

  // Lsit of nodes that are outside of final tree
  vector<int> calculatecost(num_vertices(g));
  for(unsigned int i = 0; i < predecessor.size(); ++i ){
    if(predecessor[i].father != -1 && predecessor[i].father != (int) i){
      sour= vertex(boost::lexical_cast<int>(G_pruned[i].name),g); tar = vertex(boost::lexical_cast<int>(G_pruned[predecessor[i].father].name),g);
      calculatecost[sour]=1; calculatecost[tar]=1;
    }
  }

  // Uncovered nodes
  for(unsigned int i = 0; i < num_vertices(g); ++i ){
    if(calculatecost[i] == 0 && (int) i != root ){
      lostPrice += g[i].c;
      uncovered_nodes++;
    }
  }

  // The list of Nodes in the final Tree
  for(unsigned int i = 0; i < num_vertices(g); ++i ){
    if(calculatecost[i] == 1){
    }
  }

  // Objective value
  return total + lostPrice;

}

//' Internal function \code{call_sr}
//'
//' This function is internally used to solve the PCST.
//'
//' @keywords internal
//'
//' @param from  A \code{CharacterVector} that corresponds to \code{head} nodes of the edges.
//' @param to A \code{CharacterVector} that corresponds the \code{tail} nodes of the edges.
//' @param cost A \code{NumericVector} which represents the edge weights.
//' @param node_names A \code{CharacterVector} demonstrates the names of the nodes.
//' @param node_prizes A \code{NumericVector} which corresponds to the node prizes.
//' @author Murodzhon Akhmedov
//'
// [[Rcpp::export]]
List call_sr(CharacterVector from, CharacterVector to,  NumericVector cost, CharacterVector node_names, NumericVector node_prizes)
{
  clear_variables();

  vector <int> terminals;

  read_input_graph(from, to, cost, node_names, node_prizes);

  Root = idx_g("DUMMY");

  double max_price=0; int max_price_index = -1;
  for(unsigned int i=0; i<num_vertices(g); i++){
    if (g[i].c > max_price){
      max_price = g[i].c;
      max_price_index = i;
    }
  }

  if(Root != -1){
    for(unsigned int i=0; i<num_vertices(g); i++){
      if( (int) i != Root && g[i].c >0){
        terminals.push_back(i);
      }
    }
  }else{
    Root = max_price_index;
    for(unsigned int i=0; i<num_vertices(g); i++){
      if( (int) i != Root && g[i].c >0){
        terminals.push_back(i);
      }
    }
  }

  terminals.push_back(Root);

  if(terminals.size() <=1){
    // There is no tree
    //return 0;
  }

  vector< Vertex > spanning_tree;
  spanning_tree = constructG(terminals, Root);

  vector< string > tree_from;
  vector< string > tree_to;
  vector< double > tree_cost;
  map < string, double > tree_terminals;
  double obj = cut(Root, spanning_tree, tree_from, tree_to, tree_cost, tree_terminals); if (obj == 0) return 0;

  CharacterVector tree_f(tree_from.size());
  CharacterVector tree_t(tree_to.size());
  NumericVector tree_c(tree_cost.size());
  CharacterVector tree_ter(tree_terminals.size());
  NumericVector tree_ter_p(tree_terminals.size());

  for(unsigned int i=0; i<tree_from.size(); i++){
    tree_f[i]=tree_from[i];
    tree_t[i]=tree_to[i];
    tree_c[i]=tree_cost[i];
  }

  int counter = 0;
  for (std::map<string, double>::iterator it=tree_terminals.begin(); it!=tree_terminals.end(); ++it){
    tree_ter[counter] = it->first;
    tree_ter_p[counter] = it->second;
    counter++;
  }

  List tree = List::create(tree_from, tree_to, tree_cost, tree_ter, tree_ter_p);

  return tree;
}


//  Copyright (c) 2007-2013 Hartmut Kaiser
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(HPX_PERFORMANCE_COUNTERS_SERVER_AVERAGE_COUNT_COUNTER_SEP_30_2011_1045AM)
#define HPX_PERFORMANCE_COUNTERS_SERVER_AVERAGE_COUNT_COUNTER_SEP_30_2011_1045AM

#include <hpx/hpx_fwd.hpp>
#include <hpx/performance_counters/server/base_performance_counter.hpp>
#include <hpx/util/interval_timer.hpp>
#include <hpx/lcos/local/spinlock.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>

#if defined(BOOST_MSVC)
#  pragma warning(push)
#  pragma warning(disable: 4244)
#endif
#include <boost/accumulators/statistics/median.hpp>
#if defined(BOOST_MSVC)
#  pragma warning(pop)
#endif

///////////////////////////////////////////////////////////////////////////////
namespace hpx { namespace performance_counters { namespace server
{
    namespace detail
    {
        template <typename Statistic>
        struct counter_type_from_statistic;

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::mean>
        {
            typedef boost::accumulators::tag::mean aggregating_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type();
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    boost::accumulators::mean(accum));
            }

            typedef boost::mpl::false_ need_reset;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::variance>
        {
            typedef boost::accumulators::tag::variance aggregating_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type();
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    sqrt(boost::accumulators::variance(accum)));
            }

            typedef boost::mpl::false_ need_reset;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::median>
        {
            typedef boost::accumulators::tag::median aggregating_tag;
            typedef boost::accumulators::with_p_square_quantile aggregating_type_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag(aggregating_type_tag)>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type();
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    boost::accumulators::median(accum));
            }

            typedef boost::mpl::false_ need_reset;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::rolling_mean>
        {
            typedef boost::accumulators::tag::rolling_mean aggregating_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type(
                    boost::accumulators::tag::rolling_window::window_size = parameter2
                );
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    boost::accumulators::rolling_mean(accum));
            }

            typedef boost::mpl::false_ need_reset;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::max>
        {
            typedef boost::accumulators::tag::max aggregating_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type();
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    (boost::accumulators::max)(accum));
            }

            typedef boost::mpl::true_ need_reset;
        };

        template <>
        struct counter_type_from_statistic<boost::accumulators::tag::min>
        {
            typedef boost::accumulators::tag::min aggregating_tag;
            typedef boost::accumulators::accumulator_set<
                double, boost::accumulators::stats<aggregating_tag>
            > accumulator_type;

            static accumulator_type* create(boost::uint64_t parameter2)
            {
                return new accumulator_type();
            }

            static boost::int64_t call(accumulator_type& accum)
            {
                return static_cast<boost::int64_t>(
                    (boost::accumulators::min)(accum));
            }

            typedef boost::mpl::true_ need_reset;
        };
    }

    ///////////////////////////////////////////////////////////////////////////
    // This counter exposes the average count of items processed during the
    // given base time interval. The counter relies on querying a steadily
    // growing counter value.
    template <typename Statistic>
    class statistics_counter
      : public base_performance_counter,
        public components::managed_component_base<
            statistics_counter<Statistic> >
    {
        typedef components::managed_component_base<
            statistics_counter<Statistic> > base_type;

        // avoid warnings about using this in member initializer list
        statistics_counter* this_() { return this; }

    public:
        typedef statistics_counter type_holder;
        typedef base_performance_counter base_type_holder;

        statistics_counter() {}

        statistics_counter(counter_info const& info,
            std::string const& base_counter_name,
            boost::uint64_t parameter1, boost::uint64_t parameter2);

        /// Overloads from the base_counter base class.
        hpx::performance_counters::counter_value
            get_counter_value(bool reset = false);

        bool start();

        bool stop();

        void reset_counter_value();

        void on_terminate() {}

        /// \brief finalize() will be called just before the instance gets
        ///        destructed
        void finalize()
        {
            base_performance_counter::finalize();
            base_type::finalize();
        }

        static components::component_type get_component_type()
        {
            return base_type::get_component_type();
        }
        static void set_component_type(components::component_type t)
        {
            base_type::set_component_type(t);
        }

    protected:
        bool evaluate_base_counter(counter_value& value);
        bool evaluate();
        bool ensure_base_counter();

    private:
        typedef lcos::local::spinlock mutex_type;
        mutable mutex_type mtx_;

        hpx::util::interval_timer timer_; ///< base time interval in milliseconds
        std::string base_counter_name_;   ///< name of base counter to be queried
        naming::id_type base_counter_id_;

        typedef typename
            detail::counter_type_from_statistic<Statistic>::accumulator_type
        accumulator_type;

        boost::scoped_ptr<accumulator_type> value_;
        counter_value prev_value_;

        boost::uint64_t parameter1_, parameter2_;
    };
}}}

#endif


//  ratio_test.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

#include <boost/ratio/ratio.hpp>
#include <iostream>

typedef boost::ratio<5, 3>   five_thirds;       // five_thirds::num == 5, five_thirds::den == 3
typedef boost::ratio<25, 15> also_five_thirds;  // also_five_thirds::num == 5, also_five_thirds::den == 3
typedef boost::ratio_divide<five_thirds, also_five_thirds>::type one;  // one::num == 1, one::den == 1

typedef boost::ratio_multiply<boost::ratio<5>, boost::giga>::type _5giga;  // _5giga::num == 5000000000, _5giga::den == 1
typedef boost::ratio_multiply<boost::ratio<5>, boost::nano>::type _5nano;  // _5nano::num == 1, _5nano::den == 200000000

//  Test the case described in library working group issue 948.

typedef boost::ratio<BOOST_RATIO_INTMAX_C(0x7FFFFFFFFFFFFFFF), BOOST_RATIO_INTMAX_C(0x7FFFFFFFFFFFFFF0)> R1;
typedef boost::ratio<8, 7> R2;
typedef boost::ratio_multiply<R1, R2>::type RT;

int main()
{
    typedef boost::ratio<8, BOOST_RATIO_INTMAX_C(0x7FFFFFFFD)> R1;
    typedef boost::ratio<3, BOOST_RATIO_INTMAX_C(0x7FFFFFFFD)> R2;
    typedef boost::ratio_subtract<R1, R2>::type RS;
    std::cout << RS::num << '/' << RS::den << '\n';

  return 0;
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Mhalf Mhalf (function template)

  Generates the constant @c -0.5.

  @headerref{<boost/simd/constant/mhalf.hpp>}

  @par Description

  1.  @code
      template<typename T> T Mhalf();
      @endcode

  2.  @code
      template<typename T> T Mhalf( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T  that evaluates to -0.5.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c that evaluates to `T(-0.5)`.

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/mhalf.hpp>
#include <boost/simd/constant/simd/mhalf.hpp>

#endif


/*    Copyright (c) 2010-2019, Delft University of Technology
*    All rigths reserved
*
*    This file is part of the Tudat. Redistribution and use in source and
*    binary forms, with or without modification, are permitted exclusively
*    under the terms of the Modified BSD license. You should have received
*    a copy of the license with this file. If not, please or visit:
*    http://tudat.tudelft.nl/LICENSE.
*/

#include <iostream>
#include <fstream>

#include <boost/filesystem.hpp>

#include "pagmo/island.hpp"
#include "pagmo/io.hpp"
#include "pagmo/problem.hpp"
#include <pagmo/rng.hpp>
#include "pagmo/algorithms/sade.hpp"
#include "problems/multipleGravityAssist.h"
#include "tudat/astro/ephemerides/approximatePlanetPositions.h"
#include "tudat/astro/ephemerides/constantEphemeris.h"
#include "tudat/astro/gravitation/gravityFieldModel.h"
#include "tudat/astro/mission_segments/createTransferTrajectory.h"
#include "tudat/simulation/environment/body.h"

using namespace pagmo;
using namespace tudat;
using namespace ephemerides;
using namespace gravitation;
using namespace simulation_setup;

simulation_setup::NamedBodyMap getApproximatePlanetBodyMap( )
{

    NamedBodyMap bodyMap;
    bodyMap[ "Sun" ] = std::make_shared< Body >( );
    bodyMap[ "Mercury" ] = std::make_shared< Body >( );
    bodyMap[ "Venus" ] = std::make_shared< Body >( );
    bodyMap[ "Earth" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ] = std::make_shared< Body >( );
    bodyMap[ "Jupiter" ] = std::make_shared< Body >( );
    bodyMap[ "Saturn" ] = std::make_shared< Body >( );

    bodyMap[ "Sun" ]->setEphemeris( std::make_shared< ConstantEphemeris >( Eigen::Vector6d::Zero( ) ) );
    bodyMap[ "Mercury" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                            ApproximatePlanetPositionsBase::BodiesWithEphemerisData::mercury ) );
    bodyMap[ "Venus" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                          ApproximatePlanetPositionsBase::BodiesWithEphemerisData::venus ) );
    bodyMap[ "Earth" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                          ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter ) );
    bodyMap[ "Mars" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                          ApproximatePlanetPositionsBase::BodiesWithEphemerisData::earthMoonBarycenter ) );
    bodyMap[ "Jupiter" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                            ApproximatePlanetPositionsBase::BodiesWithEphemerisData::jupiter ) );
    bodyMap[ "Saturn" ]->setEphemeris( std::make_shared< ApproximatePlanetPositions >(
                                           ApproximatePlanetPositionsBase::BodiesWithEphemerisData::saturn ) );

    bodyMap[ "Sun" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 1.32712428e20 ) );
    bodyMap[ "Mercury" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 2.2321e13 ) );
    bodyMap[ "Venus" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 3.24860e14 ) );
    bodyMap[ "Earth" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 3.9860119e14 ) );
    bodyMap[ "Mars" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 4.282837e13 ) );
    bodyMap[ "Jupiter" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 1.267e17 ) );
    bodyMap[ "Saturn" ]->setGravityFieldModel( std::make_shared< GravityFieldModel >( 3.79e16 ) );

    return bodyMap;
}

namespace std
{
template <typename T>
ostream& operator<<(ostream& output, std::vector<T> const& values)
{
    std::cout<<"[ ";
    for (auto const& value : values)
    {
        output << value <<", ";
    }
    std::cout<<" ]"<<std::endl;
    return output;
}
}
//! Execute  main
int main( )
{
    //Set seed for reproducible results
    pagmo::random_device::set_seed( 123456789 );

    // Set transfer order
    std::vector< std::string > bodyOrder = {
        "Earth", "Venus", "Venus", "Earth", "Jupiter", "Saturn" };
    int numberOfNodes = bodyOrder.size( );

    // Create leg settings (all unpowered)
    std::vector< std::shared_ptr< TransferLegSettings > > transferLegSettings;
    transferLegSettings.resize( numberOfNodes - 1 );
    transferLegSettings[ 0 ] = unpoweredLeg( );
    transferLegSettings[ 1 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 2 ] = dsmVelocityBasedLeg( );
    transferLegSettings[ 3 ] = unpoweredLeg( );
    transferLegSettings[ 4 ] = unpoweredLeg( );

    // Define minimum periapsis altitudes for flybys;
    std::map< std::string, double >minimumPeriapses;
    minimumPeriapses[ "Venus" ] = 6351800.0;
    minimumPeriapses[ "Earth" ] = 6778000.0;
    minimumPeriapses[ "Mars" ] = 3696200.0;
    minimumPeriapses[ "Jupiter" ] =  600000000;

    std::vector< std::shared_ptr< TransferNodeSettings > > transferNodeSettings;
    transferNodeSettings.resize( numberOfNodes );
    transferNodeSettings[ 0 ] = escapeAndCaptureNode( std::numeric_limits< double >::infinity( ), 0.0 );
    transferNodeSettings[ 1 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 1 ) ) );
    transferNodeSettings[ 2 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 2 ) ) );
    transferNodeSettings[ 3 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 3 ) ) );
    transferNodeSettings[ 4 ] = swingbyNode( minimumPeriapses.at( bodyOrder.at( 4 ) ) );
    transferNodeSettings[ 5 ] = captureAndInsertionNode( 1.0895e8 / 0.02, 0.98 );

    printTransferParameterDefinition( transferLegSettings, transferNodeSettings );
    simulation_setup::NamedBodyMap bodyMap = getApproximatePlanetBodyMap( );

    // Define search bounds: first parameter is start date, following parameters are leg durations
    std::vector< std::vector< double > > bounds( 2, std::vector< double >( 14, 0.0 ) );
    bounds[ 0 ][ 0 ] = -2000.0; //MJD2000
    bounds[ 1 ][ 0 ] = 0.0; //MJD2000
    bounds[ 0 ][ 1 ] = 50.0;
    bounds[ 1 ][ 1 ] = 500.0;
    bounds[ 0 ][ 2 ] = 100.0;
    bounds[ 1 ][ 2 ] = 500.0;
    bounds[ 0 ][ 3 ] = 50.0;
    bounds[ 1 ][ 3 ] = 500.0;
    bounds[ 0 ][ 4 ] = 500.0;
    bounds[ 1 ][ 4 ] = 2000.0;
    bounds[ 0 ][ 5 ] = 1000.0;
    bounds[ 1 ][ 5 ] = 10000.0;
//    bounds[ 0 ][ 6 ] = 0.0;
//    bounds[ 1 ][ 6 ] = 2.0E3;;
//    bounds[ 0 ][ 7 ] = 0.0;
//    bounds[ 1 ][ 7 ] = 2.0 * mathematical_constants::PI;

//    bounds[ 0 ][ 8 ] = 0.0;
//    bounds[ 1 ][ 8 ] = 2.0 * mathematical_constants::PI;

//    bounds[ 0 ][ 9 ] = 0.05;
//    bounds[ 1 ][ 9 ] = 0.95;
    int currentIndex = 5;
    for( int i = 1; i < 3; i++ )
    {
        // periapsis bounds
        bounds[ 0 ][ currentIndex + ( i - 1 ) * 4 + 1 ] = minimumPeriapses.at( bodyOrder.at( i ) );
        bounds[ 1 ][ currentIndex + ( i - 1 ) * 4 + 1 ] = 1.5 * minimumPeriapses.at( bodyOrder.at( i ) );

        // orbit orientation bounds
        bounds[ 0 ][ currentIndex + ( i - 1 ) * 4 + 2 ] = 0.0;
        bounds[ 1 ][ currentIndex + ( i - 1 ) * 4 + 2 ] = 2.0 * mathematical_constants::PI;

        // Swingby Delta V bounds
        bounds[ 0 ][ currentIndex + ( i - 1 ) * 4 + 3 ] = 0;
        bounds[ 1 ][ currentIndex + ( i - 1 ) * 4 + 3 ] = 2500.0;

        // DSM TOF fraction
        bounds[ 0 ][ currentIndex + ( i - 1 ) * 4 + 4 ] = 0.05;
        bounds[ 1 ][ currentIndex + ( i - 1 ) * 4 + 4 ] = 0.95;
    }

    // Create object to compute the problem fitness
    problem prob{ MultipleGravityAssist(
                    bodyMap, transferLegSettings, transferNodeSettings, bodyOrder, "Sun", bounds ) };

    // Select NSGA2 algorithm for priblem
    algorithm algo{sade( )};

    // Create an island with 1000 individuals
    island isl{algo, prob, 500 };

    // Evolve for 512 generations
    for( int i = 0 ; i < 10000; i++ )
    {

        isl.evolve( );
        while( isl.status( ) != pagmo::evolve_status::idle &&
               isl.status( ) != pagmo::evolve_status::idle_error )
        {
            isl.wait( );
        }

        isl.wait_check( ); // Raises errors

        if( i% 100 == 0 )
        {
            std::cout<<i<<" "<<isl.get_population().champion_f()[0]<<"***  "<<
                       isl.get_population().champion_x()<<std::endl;        // Write current iteration results to file
        }
//        printPopulationToFile( isl.get_population( ).get_x( ), "mo_mga_EVEEJ_" + std::to_string( i ), false );
//        printPopulationToFile( isl.get_population( ).get_f( ), "mo_mga_EVEEJ_" + std::to_string( i ), true );
    }

    return 0;

}


#include <iostream>
#include <sstream>
#include <fstream>
#include <unistd.h>
#include <termios.h>

#include <ros/ros.h>
#include <gazebo_msgs/GetModelState.h>
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"

#include <Eigen/Geometry>

using std::cout;
using std::endl;

char getch() {
     char buf = 0;
     struct termios old = {0};
     if (tcgetattr(0, &old) < 0)
          perror("tcsetattr()");
     old.c_lflag &= ~ICANON;
     old.c_lflag &= ~ECHO;
     old.c_cc[VMIN] = 1;
     old.c_cc[VTIME] = 0;
     if (tcsetattr(0, TCSANOW, &old) < 0)
          perror("tcsetattr ICANON");
     if (read(0, &buf, 1) < 0)
          perror ("read()");
     old.c_lflag |= ICANON;
     old.c_lflag |= ECHO;
     if (tcsetattr(0, TCSADRAIN, &old) < 0)
          perror ("tcsetattr ~ICANON");
     return (buf);
}

int main(int argc, char **argv)
{
     ros::init(argc, argv, "velocity_control_node");
     ros::NodeHandle n_;

     double linear_mag;
     double angular_mag;
     std::string model_name = "EMPTY_MODEL_NAME";

     n_.param<double>("linear_mag", linear_mag, 1.0);
     n_.param<double>("angular_mag", angular_mag, 1.0);
     n_.param<std::string>("model_name", model_name, "eye");

     // Notify roscore of joystick publication
     ros::Publisher twist_pub_ = n_.advertise<geometry_msgs::Twist>("vel_cmd", 1);

     ros::ServiceClient client = n_.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
     gazebo_msgs::GetModelState getmodelstate;
     getmodelstate.request.model_name=model_name;

     //ros::Rate loop_rate(10);
     bool quit = false;

     Eigen::Quaternion<float> forward(0,0,0,0);

     cout << "Issue TWIST messages with W A S D and the arrow keys." << endl;
     while (ros::ok() && !quit) {
          geometry_msgs::Twist msg;
          msg.linear.x = 0.0;
          msg.linear.y = 0.0;
          msg.linear.z = 0.0;

          msg.angular.x = 0.0;
          msg.angular.y = 0.0;
          msg.angular.z = 0.0;

          twist_pub_.publish(msg);

          Eigen::Quaternion<float> attitude;
          if(client.call(getmodelstate)) {
               //std::cout << getmodelstate.response.pose << std::endl;
               attitude = Eigen::Quaternion<float>(getmodelstate.response.pose.orientation.w,
                                                   getmodelstate.response.pose.orientation.x,
                                                   getmodelstate.response.pose.orientation.y,
                                                   getmodelstate.response.pose.orientation.z);
          } else {
               cout << "Error getting model state." << endl;
               attitude = forward;
          }

          //Eigen::Quaternion<float> direction;

          Eigen::Quaternion<float>::Vector3 direction(0,0,0);
          Eigen::Quaternion<float>::Vector3 rotation(0,0,0);

          char c, c2;
          c = getch();
          switch(c) {
          case 'q':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,0,linear_mag));
               break;
          case 'z':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,0,-linear_mag));
               break;
          case 'w':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(linear_mag,0,0));

               //q = Eigen::AngleAxis<float>(,);
//msg.linear.x = linear_mag;
               break;
          case 'a':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,linear_mag,0));

               //msg.linear.y = linear_mag;
               break;
          case 's':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(-linear_mag,0,0));

               //msg.linear.x = -linear_mag;
               break;
          case 'd':
               direction = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,-linear_mag,0));

               //msg.linear.y = -linear_mag;
               break;
          //case 'q':
          //     rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(angular_mag,0,0));
          //     break;
          //case 'e':
          //     rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(-angular_mag,0,0));
          //     break;
          case 27:
               if (getch() == '[') {
                    c2 = getch();
                    switch(c2) {
                    case 'D':
                         // LEFT ARROW
                         //msg.angular.z = angular_mag;
                         rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,0,angular_mag));
                         break;
                    case 'C':
                         // RIGHT ARROW
                         //msg.angular.z = -angular_mag;
                         rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,0,-angular_mag));
                         break;
                    case 'A':
                         //msg.angular.y = angular_mag;
                         rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,angular_mag,0));
                         // UP ARROW
                         break;
                    case 'B':
                         //msg.angular.y = -angular_mag;
                         rotation = attitude._transformVector(Eigen::Quaternion<float>::Vector3(0,-angular_mag,0));
                         // DOWN ARROW
                         break;
                    default:
                         break;
                    }
               }
               break;
          default:
               break;
          }

          msg.linear.x = direction.x();
          msg.linear.y = direction.y();
          msg.linear.z = direction.z();

          msg.angular.x = rotation.x();
          msg.angular.y = rotation.y();
          msg.angular.z = rotation.z();
          //printf("Got: %c=%d\n",c,c);

          twist_pub_.publish(msg);
          usleep(1000);

          //ros::spinOnce();
          //loop_rate.sleep();
     }

     return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <algorithm>
#include <boost/iterator/transform_iterator.hpp>
#include <cstddef>
#include <cstdint>
#include <memory>
#include <string>
#include <vector>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/TensorData.hpp"
#include "IO/H5/AccessType.hpp"
#include "IO/H5/File.hpp"
#include "IO/H5/VolumeData.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/Algorithm.hpp"
#include "Utilities/ErrorHandling/Error.hpp"
#include "Utilities/FileSystem.hpp"
#include "Utilities/MakeString.hpp"
#include "Utilities/Numeric.hpp"

namespace {
template <typename T>
T multiply(const double obs_value, const T& component) {
  T result = component;
  for (auto& t : result) {
    t *= obs_value;
  }
  return result;
}

template <typename DataType>
void test() {
  const std::string h5_file_name("Unit.IO.H5.VolumeData.h5");
  const uint32_t version_number = 4;
  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }

  h5::H5File<h5::AccessType::ReadWrite> my_file(h5_file_name);
  const std::vector<DataType> tensor_components_and_coords{
      {8.9, 7.6, 3.9, 2.1, 18.9, 17.6, 13.9, 12.1},
      {0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0},
      {0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0},
      {0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0},
      {-78.9, -7.6, -1.9, 8.1, 6.3, 8.7, 9.8, 0.2},
      {-7.9, 7.6, 1.9, -8.1, -6.3, 2.7, 6.8, -0.2},
      {17.9, 27.6, 21.9, -28.1, -26.3, 32.7, 26.8, -30.2}};
  const std::vector<size_t> observation_ids{8435087234, size_t(-1)};
  const std::vector<double> observation_values{8.0, 2.3};
  const std::vector<std::string> grid_names{"[[2,3,4]]", "[[5,6,7]]"};
  const std::vector<std::vector<Spectral::Basis>> bases{
      {3, Spectral::Basis::Chebyshev}, {3, Spectral::Basis::Legendre}};
  const std::vector<std::vector<Spectral::Quadrature>> quadratures{
      {3, Spectral::Quadrature::Gauss},
      {3, Spectral::Quadrature::GaussLobatto}};
  {
    auto& volume_file =
        my_file.insert<h5::VolumeData>("/element_data", version_number);
    const auto write_to_file = [&volume_file, &tensor_components_and_coords,
                                &grid_names, &bases,
                                &quadratures](const size_t observation_id,
                                              const double observation_value) {
      std::string first_grid = grid_names.front();
      std::string last_grid = grid_names.back();
      volume_file.write_volume_data(
          observation_id, observation_value,
          std::vector<ElementVolumeData>{
              {{2, 2, 2},
               {TensorComponent{first_grid + "/S",
                                multiply(observation_value,
                                         tensor_components_and_coords[0])},
                TensorComponent{first_grid + "/x-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[1])},
                TensorComponent{first_grid + "/y-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[2])},
                TensorComponent{first_grid + "/z-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[3])},
                TensorComponent{first_grid + "/T_x",
                                multiply(observation_value,
                                         tensor_components_and_coords[4])},
                TensorComponent{first_grid + "/T_y",
                                multiply(observation_value,
                                         tensor_components_and_coords[5])},
                TensorComponent{first_grid + "/T_z",
                                multiply(observation_value,
                                         tensor_components_and_coords[6])}},
               bases.front(),
               quadratures.front()},
              // Second Element Data
              {{2, 2, 2},
               {TensorComponent{last_grid + "/S",
                                multiply(observation_value,
                                         tensor_components_and_coords[1])},
                TensorComponent{last_grid + "/x-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[0])},
                TensorComponent{last_grid + "/y-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[5])},
                TensorComponent{last_grid + "/z-coord",
                                multiply(observation_value,
                                         tensor_components_and_coords[3])},
                TensorComponent{last_grid + "/T_x",
                                multiply(observation_value,
                                         tensor_components_and_coords[6])},
                TensorComponent{last_grid + "/T_y",
                                multiply(observation_value,
                                         tensor_components_and_coords[4])},
                TensorComponent{last_grid + "/T_z",
                                multiply(observation_value,
                                         tensor_components_and_coords[2])}},
               bases.back(),
               quadratures.back()}});
    };
    for (size_t i = 0; i < observation_ids.size(); ++i) {
      write_to_file(observation_ids[i], observation_values[i]);
    }
  }
  // Open the read volume file and check that the observation id and values are
  // correct.
  const auto& volume_file =
      my_file.get<h5::VolumeData>("/element_data", version_number);
  const auto read_observation_ids = volume_file.list_observation_ids();
  CHECK(alg::all_of(read_observation_ids, [&observation_ids](const size_t id) {
    return alg::found(observation_ids, id);
  }));
  {
    INFO("Test find_observation_id");
    std::vector<size_t> found_observation_ids(observation_values.size());
    std::transform(observation_values.begin(), observation_values.end(),
                   found_observation_ids.begin(),
                   [&volume_file](const double observation_value) {
                     return volume_file.find_observation_id(observation_value);
                   });
    CHECK(found_observation_ids == observation_ids);
  }
  // Check that the volume data is correct
  const auto check_time = [&volume_file, &tensor_components_and_coords,
                           &grid_names, &bases,
                           &quadratures](const size_t observation_id,
                                         const double observation_value) {
    CHECK(std::vector<std::vector<size_t>>{{2, 2, 2}, {2, 2, 2}} ==
          volume_file.get_extents(observation_id));
    CHECK(volume_file.get_observation_value(observation_id) ==
          observation_value);
    // Check that all of the grid names were written correctly by checking their
    // equality of elements
    const std::vector<std::string> read_grid_names =
        volume_file.get_grid_names(observation_id);
    [&read_grid_names, &grid_names]() {
      auto sortable_grid_names = grid_names;
      auto sortable_read_grid_names = read_grid_names;
      std::sort(sortable_grid_names.begin(), sortable_grid_names.end(),
                std::less<>{});
      std::sort(sortable_read_grid_names.begin(),
                sortable_read_grid_names.end(), std::less<>{});
      REQUIRE(sortable_read_grid_names == sortable_grid_names);
    }();
    // Find the order the grids were written in
    std::vector<size_t> grid_positions(read_grid_names.size());
    for (size_t i = 0; i < grid_positions.size(); i++) {
      auto grid_name = grid_names[i];
      auto position =
          std::find(read_grid_names.begin(), read_grid_names.end(), grid_name);
      // We know the grid name is in the read_grid_names because of the previous
      // so we know `position` is an actual pointer to an element
      grid_positions[i] =
          static_cast<size_t>(std::distance(read_grid_names.begin(), position));
    }
    auto read_bases = volume_file.get_bases(observation_id);
    alg::sort(read_bases, std::less<>{});
    auto read_quadratures = volume_file.get_quadratures(observation_id);
    alg::sort(read_quadratures, std::less<>{});
    // We need non-const bases and quadratures in order to sort them, and we
    // need them in their string form,
    const auto& stringify = [](const auto& bases_or_quadratures) {
      std::vector<std::vector<std::string>> local_target_data{};
      local_target_data.reserve(bases_or_quadratures.size() + 1);
      for (const auto& element_data : bases_or_quadratures) {
        std::vector<std::string> target_axis_data{};
        target_axis_data.reserve(element_data.size() + 1);
        for (const auto& axis_datum : element_data) {
          target_axis_data.emplace_back(MakeString{} << axis_datum);
        }
        local_target_data.push_back(target_axis_data);
      }
      return local_target_data;
    };
    auto target_bases = stringify(bases);
    alg::sort(target_bases, std::less<>{});
    auto target_quadratures = stringify(quadratures);
    alg::sort(target_quadratures, std::less<>{});
    CHECK(target_bases == read_bases);
    CHECK(target_quadratures == read_quadratures);
    const std::vector<std::string> expected_components{
        "S", "x-coord", "y-coord", "z-coord", "T_x", "T_y", "T_z",
    };
    const auto read_components =
        volume_file.list_tensor_components(observation_id);
    CHECK(alg::all_of(read_components,
                      [&expected_components](const std::string& id) {
                        return alg::found(expected_components, id);
                      }));
    // Helper Function to get number of points on a particular grid
    const auto accumulate_extents =
        [](const std::vector<size_t>& grid_extents) {
          return alg::accumulate(grid_extents, 1, std::multiplies<>{});
        };

    const auto read_extents = volume_file.get_extents(observation_id);
    std::vector<size_t> element_num_points(
        boost::make_transform_iterator(read_extents.begin(),
                                       accumulate_extents),
        boost::make_transform_iterator(read_extents.end(), accumulate_extents));
    const auto read_points_by_element = [&element_num_points]() {
      std::vector<size_t> read_points(element_num_points.size());
      read_points[0] = 0;
      for (size_t index = 1; index < element_num_points.size(); index++) {
        read_points[index] =
            read_points[index - 1] + element_num_points[index - 1];
      }
      return read_points;
    }();
    // Given a DataType, corresponding to contiguous data read out of a
    // file, find the data which was written by the grid whose extents are
    // found at position `grid_index` in the vector of extents.
    const auto get_grid_data = [&element_num_points, &read_points_by_element](
                                   const DataVector& all_data,
                                   const size_t grid_index) {
      DataType result(element_num_points[grid_index]);
      // clang-tidy: do not use pointer arithmetic
      std::copy(&all_data[read_points_by_element[grid_index]],
                &all_data[read_points_by_element[grid_index]] +  // NOLINT
                    element_num_points[grid_index],
                result.begin());
      return result;
    };
    // The order the fake tensor data was assigned to the tensor components
    std::vector<std::vector<size_t>> grid_data_orders{{0, 1, 2, 3, 4, 5, 6},
                                                      {1, 0, 5, 3, 6, 4, 2}};
    // The tensor components can be written in any order to the file, we loop
    // over the expected components rather than the read components because they
    // are in a particular order.
    for (size_t i = 0; i < expected_components.size(); i++) {
      const auto& component = expected_components[i];
      // for each grid
      for (size_t j = 0; j < grid_names.size(); j++) {
        CHECK(get_grid_data(
                  volume_file.get_tensor_component(observation_id, component),
                  grid_positions[j]) ==
              multiply(observation_value,
                       tensor_components_and_coords[grid_data_orders[j][i]]));
      }
    }
  };
  for (size_t i = 0; i < observation_ids.size(); ++i) {
    check_time(observation_ids[i], observation_values[i]);
  }

  {
    INFO("offset_and_length_for_grid");
    const size_t observation_id = observation_ids.front();
    // [find_offset]
    const auto all_grid_names = volume_file.get_grid_names(observation_id);
    const auto all_extents = volume_file.get_extents(observation_id);
    const auto first_grid_offset_and_length = h5::offset_and_length_for_grid(
        grid_names.front(), all_grid_names, all_extents);
    // [find_offset]
    CHECK(first_grid_offset_and_length.first == 0);
    CHECK(first_grid_offset_and_length.second == 8);
    const auto last_grid_offset_and_length = h5::offset_and_length_for_grid(
        grid_names.back(), all_grid_names, all_extents);
    CHECK(last_grid_offset_and_length.first == 8);
    CHECK(last_grid_offset_and_length.second == 8);
  }

  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }
}
}  // namespace

SPECTRE_TEST_CASE("Unit.IO.H5.VolumeData", "[Unit][IO][H5]") {
  test<DataVector>();
  test<std::vector<float>>();
}

// [[OutputRegex, The expected format of the tensor component names is
// 'GROUP_NAME/COMPONENT_NAME' but could not find a '/' in]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.IO.H5.VolumeData.ComponentFormat0",
                               "[Unit][IO][H5]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::string h5_file_name("Unit.IO.H5.VolumeData.ComponentFormat.h5");
  const uint32_t version_number = 4;
  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }
  h5::H5File<h5::AccessType::ReadWrite> my_file(h5_file_name);
  auto& volume_file =
      my_file.insert<h5::VolumeData>("/element_data", version_number);
  volume_file.write_volume_data(100, 10.0,
                                {{{2},
                                  {TensorComponent{"S", DataVector{1.0, 2.0}}},
                                  {Spectral::Basis::Legendre},
                                  {Spectral::Quadrature::Gauss}}});
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
  // clang-format off
}

// [[OutputRegex, The expected format of the tensor component names is
// 'GROUP_NAME/COMPONENT_NAME' but could not find a '/' in]]
[[noreturn]]
SPECTRE_TEST_CASE("Unit.IO.H5.VolumeData.ComponentFormat1",
                               "[Unit][IO][H5]") {
  // clang-format on
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::string h5_file_name("Unit.IO.H5.VolumeData.ComponentFormat1.h5");
  const uint32_t version_number = 4;
  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }
  h5::H5File<h5::AccessType::ReadWrite> my_file(h5_file_name);
  auto& volume_file =
      my_file.insert<h5::VolumeData>("/element_data", version_number);
  volume_file.write_volume_data(100, 10.0,
                                {{{2},
                                  {TensorComponent{"A/S", DataVector{1.0, 2.0}},
                                   TensorComponent{"S", DataVector{1.0, 2.0}}},
                                  {Spectral::Basis::Legendre},
                                  {Spectral::Quadrature::Gauss}}});
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
  // clang-format off
}

// [[OutputRegex, Trying to write tensor component 'S' which already exists
// in HDF5 file in group 'element_data.vol/ObservationId100']]
[[noreturn]] SPECTRE_TEST_CASE("Unit.IO.H5.VolumeData.WriteTwice",
                               "[Unit][IO][H5]") {
  // clang-format on
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::string h5_file_name("Unit.IO.H5.VolumeData.WriteTwice.h5");
  const uint32_t version_number = 4;
  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }
  h5::H5File<h5::AccessType::ReadWrite> my_file(h5_file_name);
  auto& volume_file =
      my_file.insert<h5::VolumeData>("/element_data", version_number);
  volume_file.write_volume_data(
      100, 10.0,
      {{{2},
        {TensorComponent{"A/S", DataVector{1.0, 2.0}},
         TensorComponent{"A/S", DataVector{1.0, 2.0}}},
        {Spectral::Basis::Legendre},
        {Spectral::Quadrature::Gauss}}});
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
  // clang-format off
}

// [[OutputRegex, No observation with value]]
SPECTRE_TEST_CASE("Unit.IO.H5.VolumeData.FindNoObservationId",
                  "[Unit][IO][H5]") {
  // clang-format on
  ERROR_TEST();
  const std::string h5_file_name(
      "Unit.IO.H5.VolumeData.FindNoObservationId.h5");
  const uint32_t version_number = 4;
  if (file_system::check_if_file_exists(h5_file_name)) {
    file_system::rm(h5_file_name, true);
  }
  h5::H5File<h5::AccessType::ReadWrite> h5_file(h5_file_name);
  auto& volume_file =
      h5_file.insert<h5::VolumeData>("/element_data", version_number);
  volume_file.write_volume_data(
      100, 10.0,
      {{{2},
        {TensorComponent{"A/S", DataVector{1.0, 2.0}}},
        {Spectral::Basis::Legendre},
        {Spectral::Quadrature::Gauss}}});
  volume_file.find_observation_id(11.0);
}