#ifndef RECONST3D_CREATE_OPTIMIZER
#define RECONST3D_CREATE_OPTIMIZER

#include <Eigen/Geometry>

#include <g2o/core/block_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/solver.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/robust_kernel_impl.h>

#include <opencv2/core/core.hpp>
#include <opencv2/rgbd/rgbd.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv_candidate_reconst3d/reconst3d.hpp>

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> >  G2OBlockSolver;
typedef g2o::LinearSolver< G2OBlockSolver::PoseMatrixType> G2OLinearSolver;
typedef g2o::LinearSolverCholmod<G2OBlockSolver::PoseMatrixType> G2OLinearCholmodSolver;
typedef g2o::LinearSolverCSparse<G2OBlockSolver::PoseMatrixType> G2OLinearCSparseSolver;

const std::string DEFAULT_LINEAR_SOLVER_TYPE = "cholmod"; //"csparse";
const std::string DEFAULT_NON_LINEAR_SOLVER_TYPE  = "GN";

// get norms
//
static inline
float tvecNorm(const cv::Mat& Rt)
{
    return cv::norm(Rt(cv::Rect(3,0,1,3)));
}

static inline
float rvecNormDegrees(const cv::Mat& Rt)
{
    cv::Mat rvec;
    cv::Rodrigues(Rt(cv::Rect(0,0,3,3)), rvec);
    return cv::norm(rvec) * 180. / CV_PI;
}

// convertions
//
inline
Eigen::Vector3d cvtPoint_ocv2egn(const cv::Point3f& ocv_p)
{
    Eigen::Vector3d egn_p;
    egn_p[0] = ocv_p.x;
    egn_p[1] = ocv_p.y;
    egn_p[2] = ocv_p.z;

    return egn_p;
}

inline
cv::Mat cvtIsometry_egn2ocv(const Eigen::Isometry3d& egn_o)
{
    Eigen::Matrix3d eigenRotation = egn_o.rotation();
    Eigen::Matrix<double,3,1> eigenTranslation = egn_o.translation();

    cv::Mat R, t;
    eigen2cv(eigenRotation, R);
    eigen2cv(eigenTranslation, t);
    t = t.reshape(1,3);

    cv::Mat ocv_o = cv::Mat::eye(4,4,CV_64FC1);
    R.copyTo(ocv_o(cv::Rect(0,0,3,3)));
    t.copyTo(ocv_o(cv::Rect(3,0,1,3)));

    return ocv_o;
}

// corresps
inline
void set2shorts(int& dst, int short_v1, int short_v2)
{
    unsigned short* ptr = reinterpret_cast<unsigned short*>(&dst);
    ptr[0] = static_cast<unsigned short>(short_v1);
    ptr[1] = static_cast<unsigned short>(short_v2);
}

inline
void get2shorts(int src, int& short_v1, int& short_v2)
{
    typedef union { int vint32; unsigned short vuint16[2]; } s32tou16;
    const unsigned short* ptr = (reinterpret_cast<s32tou16*>(&src))->vuint16;
    short_v1 = ptr[0];
    short_v2 = ptr[1];
}

int computeCorrespsFiltered(const cv::Mat& K, const cv::Mat& K_inv, const cv::Mat& Rt,
                            const cv::Mat& depth0, const cv::Mat& validMask0,
                            const cv::Mat& depth1, const cv::Mat& selectMask1, float maxDepthDiff,
                            cv::Mat& corresps,
                            const cv::Mat& normals0, const cv::Mat& normals1,
                            const cv::Mat& image0, const cv::Mat& image1,
                            float maxColorDiff = FLT_MAX);

void selectPosesSubset(const std::vector<cv::Mat>& poses,
                       const std::vector<int>& indices,
                       std::vector<int>& selectedIndices, size_t count);

// create solver
//
inline
G2OLinearSolver* createLinearSolver(const std::string& type)
{
    G2OLinearSolver* solver = 0;
    if(type == "cholmod")
        solver = new G2OLinearCholmodSolver();
    else if(type == "csparse")
    {
        solver = new G2OLinearCSparseSolver();
    }
    else
    {
        CV_Assert(0);
    }

    return solver;
}

inline
G2OBlockSolver* createBlockSolver(G2OLinearSolver* linearSolver)
{
    return new G2OBlockSolver(linearSolver);
}

inline
g2o::OptimizationAlgorithm* createNonLinearSolver(const std::string& type, G2OBlockSolver* blockSolver)
{
    g2o::OptimizationAlgorithm* solver = 0;
    if(type == "GN")
        solver = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
    else if(type == "LM")
        solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
    else
        CV_Assert(0);

    return solver;
}

inline
g2o::SparseOptimizer* createOptimizer(g2o::OptimizationAlgorithm* solver)
{
    g2o::SparseOptimizer* optimizer = new g2o::SparseOptimizer();
    optimizer->setAlgorithm(solver);
    optimizer->setVerbose(true);
    return optimizer;
}

// graph opt

// Restore refined camera poses from the graph.
void getSE3Poses(g2o::SparseOptimizer* optimizer, const std::vector<int>& frameIndices, std::vector<cv::Mat>& poses);

// Fill the given graph by vertices and edges for the camera pose refinement geometrically.
// Each vertex is a camera pose. Each edge constraint is the odometry between linked vertices.
void fillGraphSE3(g2o::SparseOptimizer* optimizer,
                  const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks,
                  std::vector<int>& frameIndices);

// Refine camera poses geometrically
void refineGraphSE3(const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks,
                    std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

void refineGraphSE3Segment(const std::vector<cv::Mat>& odometryPoses,
                           const std::vector<cv::Mat>& partiallyRefinedPoses,
                           const std::vector<int>& refinedFrameIndices,
                           std::vector<cv::Mat>& refinedAllPoses);

// Graph with 2 types of edges: odometry and Rgbd+ICP for correspondences.
// TODO we need next iteration of the code refactoring
void fillGraphSE3RgbdICP(g2o::SparseOptimizer* optimizer, int pyramidLevel,
                         const std::vector<cv::Ptr<cv::OdometryFrame> >& frames,
                         const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                         std::vector<int>& frameIndices,
                         double maxTranslation, double maxRotation, double maxDepthDiff);

// Refine camera poses by graph with odometry edges and Rgbd+ICP edges
void refineGraphSE3RgbdICP(const std::vector<cv::Ptr<cv::RgbdFrame> >& frames,
                           const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                           float pointsPart, std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

void refineGraphSE3RgbdICPModel(std::vector<cv::Ptr<cv::RgbdFrame> >& frames,
                                const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                                std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

#endif


#include <ros/ros.h>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <laser_node/Obstacles.h>
#include <gvg_mapper/GVGNode.h>
#include <gvg_mapper/GVGEdgeMsg.h>
#include "GVGGraph.h"
#include <Eigen/Dense>
#include <iostream>
#include <string>
#include <sstream>

using namespace Eigen;
using namespace std;

double pow2(double x) { return((x)*(x)); }

/*
 * Checks if the point lies in the ellipse.
 */
bool UncertaintyEllipse::contains(geometry_msgs::Point32& p) {
  double dx = (center.x - p.x)*cos(-angle) + (center.y - p.y)*sin(-angle);
  double dy = (center.x - p.x)*-sin(-angle) + (center.y - p.y)*cos(-angle);
  double result = (dx*dx)/(halfaxis1*halfaxis1) + (dy*dy)/(halfaxis2*halfaxis2);
  return result <= 1.0;
}

/*
 * edge_angle_diffs1 should be the current vertex we are trying to match, and edge_angle_diffs2 should iterate over all the vertices in our current GVG Graph.
 * Returns bearing ID which minimizes the error that matches the two vertices. -1 if the # of bearings differ (not same vertex).
 */
vector<double> GVGGraph::edge_angle_diffs_error(std::vector<double>& edge_angle_diffs1,
			   std::vector<double>& edge_angle_diffs2) {

  vector<double> errors;
  for (int i = 0; i < (int) edge_angle_diffs1.size(); i++) {
    double error = 0;
    for (int j = 0; j < (int) edge_angle_diffs2.size(); j++) {
      int index1 = j % ((int) edge_angle_diffs1.size());
      int index2 = (i + j) % ((int) edge_angle_diffs2.size());
      double diff = edge_angle_diffs2.at(index2) - edge_angle_diffs1.at(index1);
      error += diff*diff;
    }
    errors.push_back(error);
  }
  return errors;
}

/*
 * Returns true iff a vertex is deemed to be similar to another vertex
 * based on their local signatures.
 */
bool GVGVertex::similar_vertex(GVGVertex& v) {
  if (norm(p, v.p) > SAME_VERTEX_RADIUS) {
    return false;
  }
  return true;
}

Vertex GVGGraph::nullVertex() {
  return boost::graph_traits<Graph>::null_vertex();
}

/*
 * GVG Graph Default constructor
 */
GVGGraph::GVGGraph() {
  this->next_node_id = 0;
  this->next_edge_id = 0;
  this->closest_unexplored_vertex = -1;
  this->relocalize = false;
  this->relocalize_epsilon = 0.0;
  this->robot_angle_correction = 0.0;
  this->mapLoadLocalization = true;
  this->add_meetpoint_srv = nh.advertiseService("add_meetpoint", &GVGGraph::addMeetpoint, this);
  this->add_endpoint_srv = nh.advertiseService("add_endpoint", &GVGGraph::addEndpoint, this);
  this->extend_edge_srv = nh.advertiseService("extend_edge", &GVGGraph::extendGVGEdge, this);
  this->retrieve_bearings_srv = nh.advertiseService("retrieve_bearings", &GVGGraph::retrieveBearings, this);
  this->retrieve_path_srv = nh.advertiseService("retrieve_path", &GVGGraph::retrievePath, this);
  this->check_relocalize_srv = nh.advertiseService("check_relocalize", &GVGGraph::checkRelocalize, this);
  this->retrieve_edges_srv = nh.advertiseService("retrieve_edges", &GVGGraph::retrieveEdges, this);
  this->process_meetpoint_cln = nh.serviceClient<localizer::UpdateFilter>("/indoor/gvg/UpdateFilter");
  this->gvg_node_pub = nh.advertise<gvg_mapper::GVGNode>("node", 1);
  this->gvg_edge_pub = nh.advertise<gvg_mapper::GVGEdgeMsg>("edge", 1);
  this->lastVertex = nullVertex();
  this->load_map_srv = nh.advertiseService("load_saved_map", &GVGGraph::loadSavedMap, this);
  this->init_load_map_transform_cln = nh.serviceClient<localizer::InitLoadMapTransform>("/indoor/gvg/InitLoadMapTransform");
  nh.getParam("/indoor/gvg/agent/meetpoint_threshold", this->MEETPOINT_THRESHOLD);
  nh.getParam("/gvg_mapper/laser_distance_x", this->laser_offset_x);
  nh.getParam("/gvg_mapper/laser_distance_y", this->laser_offset_y);
  nh.getParam("/gvg_mapper/relocalize_epsilon", this->relocalize_epsilon);
  nh.getParam("/gvg_mapper/exploration_policy", this->exploration_policy);
  nh.getParam("/gvg_mapper/vertex_matching_policy", this->vertex_matching_policy);
  nh.getParam("/gvg_mapper/closest_distance_threshold", this->closest_distance_threshold);
  nh.getParam("/gvg_mapper/edge_length_threshold", this->edge_length_threshold);
}

/*
 * Adds a new meetpoint or endpoint to the GVG,
 * unless the vertex can be matched to an existing
 * vertex in the graph. v has no node_id before its
 * signature is searched in the graph.
 */
Vertex GVGGraph::addVertex(GVGVertex& v, double robot_angle, geometry_msgs::PoseWithCovariance& pose) {
  Vertex target = nullVertex();
  arriving_bearing = -1;

  if (vertex_matching_policy == 0 && mapLoadLocalization) {
    double closestDistance = SAME_VERTEX_RADIUS;
    Vertex_iter vertex, vertex_end;
    for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
      double dist = norm(G[*vertex].p, v.p);
      if (dist <= closestDistance && G[*vertex].similar_vertex(v)) {
        closestDistance = dist;
        target = *vertex;
      }
    }
    if (target != nullVertex()) {
      for (int i = 0; i < (int) G[target].possible_bearings.size(); i++) {
        double angle = G[target].possible_bearings.at(i).edge_angle_diff;
        if (G[target].possible_bearings.at(i).edge_angle_diff > M_PI) angle = G[target].possible_bearings.at(i).edge_angle_diff - 2*M_PI;
        double relative_angle = abs(abs_angle(G[target].vertex_angle, angle) - abs_angle(robot_angle, M_PI));
        if ((min(relative_angle, 2*M_PI-relative_angle) < M_PI/4.0)&&(num_vertices(G) > 1)) {
          arriving_bearing = G[target].possible_bearings.at(i).bearing_id;
        }
      }
    }
  }
  // Partial vertex matching with full oracle or partial oracle
  else {
    // Find the vertex with the smallest least squares error
    if (num_vertices(G) > 0) {
      // Create uncertainty ellipse centered at robot position. Any meetpoint outside of the ellipse won't be considered for the least squares error.
      ellipse.center = v.p;
      Matrix2d cov;
      cov << pose.covariance[0], pose.covariance[1], pose.covariance[6], pose.covariance[7];
      EigenSolver<MatrixXd> es(cov);
      Vector2d V1= Vector2d(std::real(es.eigenvectors().col(0)[0]),std::real(es.eigenvectors().col(0)[1]));
      Vector2d V2= Vector2d(std::real(es.eigenvectors().col(1)[0]),std::real(es.eigenvectors().col(1)[1]));
      double l1=std::real(es.eigenvalues()[0]);
      double l2=std::real(es.eigenvalues()[1]);

      if((l1 > 0.0) && (l2 > 0.0)) {
        double k = 1.0;
        ellipse.halfaxis1 = sqrt(k*(l1))*3;
        ellipse.halfaxis2 = sqrt(k*(l2))*3;
        double angle = -atan2(V1[1],V1[0]);
        if(l1 < l2) {
          double tmp = l1; l1 = l2; l2 = tmp;
          angle = -atan2(V2[1],V2[0]);
        }
        ellipse.angle = angle;
      }
      else ROS_WARN("Degenerate covariance");

      double min_error = INFINITY;
      Vertex_iter    vertex, vertex_end;
      for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
        if ((ellipse.contains(G[*vertex].p) || !mapLoadLocalization) && (abs(v.closest_distance - G[*vertex].closest_distance) <= closest_distance_threshold) && (v.expected_degree == G[*vertex].expected_degree)) {

          // Check if we have already traveled this edge by looking at edge signatures
          vector<int> dismissed_edges;
          vector<EdgeBearing> bearings = G[*vertex].possible_bearings;
          for (int i = 0; i < (int) bearings.size(); i++) {
            if (bearings.at(i).visited) {
              for (int j = 0; j < (int) edges_list.size(); j++) {
                if (bearings.at(i).edge_id == edges_list.at(j).edge_id) {
                  bool edge_endpoint = (lastVertex == edges_list.at(j).source && *vertex == edges_list.at(j).target) || (*vertex == edges_list.at(j).source && lastVertex == edges_list.at(j).target);
                  // make sure our last vertex is one of this edge's endpoints. also look at edge length to determine if this edge is correct
                  if (edge_endpoint && (abs(edges_list.at(j).length - currentEdge.length) < edge_length_threshold)) {
                    arriving_bearing = bearings.at(i).bearing_id;
                    target = *vertex;
                    min_error = 0;
                    break;
                  }
                  // incorrect edge
                  else dismissed_edges.push_back(bearings.at(i).bearing_id);
                }
              }
            }
            if (arriving_bearing != -1) break;
          }

          if (arriving_bearing == -1) {
            // Calculate the errors for each edge angle diffs configuration
            vector<double> errors = edge_angle_diffs_error(v.edge_angle_diffs, G[*vertex].edge_angle_diffs);
            for (int i = 0; i < (int) errors.size(); i++) {
              bool dismissed = false;
              for (int j = 0; j < (int) dismissed_edges.size(); j++) {
                if (dismissed_edges.at(j) == i) {
                  dismissed = true;
                  break;
                }
              }
              // Keep the vertex with the smallest error
              if (!dismissed && errors.at(i) < min_error) {
                target = *vertex;
                arriving_bearing = i;
                min_error = errors.at(i);
              }
            }
          }
        }
      }

      if (vertex_matching_policy == 1) {
        // Confirm the correct vertex
        if (target == nullVertex() || arriving_bearing == -1) {
          ROS_WARN("No matching meetpoint found");
          target = nullVertex();
          arriving_bearing = -1;
        }
        else ROS_WARN("Vertex matching: Found vertex %d from bearing %d with least squares error %f", (int)target, arriving_bearing, min_error);
        string str = "";
        string str2 = "";

        cout << "Is this an existing vertex? (Type 'y' for yes, otherwise type anything)\n";
        getline(cin, str);
        if (str == "y") {
          cout << "Is this information correct? (Type 'n' for no, otherwise type anything)\n";
          getline(cin, str);
          // User input vertex already exists
          if (str == "n") {
            cout << "Node ID of matching vertex?\n";
            getline(cin, str);
            stringstream stream(str);
            stream >> target;
            cout << "Bearing we arrived from?\n";
            getline(cin, str2);
            stringstream stream2(str2);
            stream2 >> arriving_bearing;
          }
        }
        else target = nullVertex();
      }
    }
  }

  if (num_vertices(G) > 0) {
    srand(time(NULL));
    float r = (float)rand()/(float)RAND_MAX;
    if (r <= this->relocalize_epsilon) relocalize = true;
    cout << "Current robot orientation: " << robot_angle * 180.0/M_PI << endl;
  }

  if (target == nullVertex()) {
    v.node_id = next_node_id++;
    target = add_vertex(v, G);
    arriving_bearing = 0;
    ROS_INFO("Added new vertex %d", v.node_id);
    unexploredEdgesCount.push_back(v.expected_degree);
  } else {
    ROS_INFO("Found matching vertex %d", G[target].node_id);
    mapLoadLocalization = true;

    localizer::InitLoadMapTransform srv;
    srv.request.bearing_angle = abs_angle(G[target].vertex_angle, G[target].possible_bearings.at(arriving_bearing).edge_angle_diff);
    srv.request.bearing_angle = abs_angle(srv.request.bearing_angle, M_PI);

    if (!init_load_map_transform_cln.call(srv)) {
      ROS_WARN("Could not call localizer to pass init load map bearing information!");
    }

  }

  // TODO: Uncertain if code is useful when we have robot_pose_ekf. Fix the robot orientation relative to the arriving bearing by looking at the possible bearings angle differences
  /*if (num_vertices(G) > 0) {
    robot_angle_correction = 0;
    for (int i = 0; i < (int) v.possible_bearings.size(); i++) {
      robot_angle_correction += G[target].possible_bearings.at((i + arriving_bearing) % G[target].possible_bearings.size()).edge_angle_diff - v.possible_bearings.at(i).edge_angle_diff;
    }
    robot_angle_correction = robot_angle_correction/v.possible_bearings.size();
  }*/

  if (num_vertices(G) > 1 && lastVertex != nullVertex()) {
    G[target].possible_bearings.at(arriving_bearing).visited = true;
    // Update the vertex bearings to the newly added edge
    if (G[target].possible_bearings.at(arriving_bearing).edge_id == -1) {

      // Update the old vertex bearing information
      G[lastVertex].possible_bearings.at(lastVertexBearing).edge_id = currentEdge.edge_id;
      G[lastVertex].possible_bearings.at(lastVertexBearing).visited = true;
      unexploredEdgesCount.at(G[lastVertex].node_id) -= 1;

      G[target].possible_bearings.at(arriving_bearing).edge_id = currentEdge.edge_id;
      unexploredEdgesCount.at(G[target].node_id) -= 1;
      // Add edge to graph
      EdgeWeightProperty weight(currentEdge.length);
      add_edge(lastVertex, target, weight, G);
      add_edge(target, lastVertex, weight, G);
      currentEdge.source = G[lastVertex].node_id;
      currentEdge.target = G[target].node_id;
      edges_list.push_back(currentEdge);

      // Publish edge information for visualization purposes
      gvg_mapper::GVGEdgeMsg msg;
      msg.header = header;
      msg.edge_id = currentEdge.edge_id;
      msg.source = currentEdge.source;
      msg.target = currentEdge.target;
      msg.line.assign(currentEdge.line.begin(), currentEdge.line.end());
      msg.length = currentEdge.length;

      gvg_edge_pub.publish(msg);
    }
  }

  // Create new edge between this vertex and next visited vertex
  last_edge_length = currentEdge.length;
  GVGEdge edge;
  edge.edge_id = next_edge_id++;
  edge.line.clear();
  currentEdge = edge;

  lastVertex = target;
  return target;
}

/*
 * Adds a new meetpoint to the GVG graph. p is the location of the meetpoint in world
 * coordinates, closest_distance is the distance to the (at least) three closest obstacles
 * that define the meetpoint. possible_bearings are the angles that the robot can turn by
 * to choose another GVG edge. These angles are given in laser coordinates, i.e. with respect
 * to the x-axis, positive angles on its right, negative on its left. The angles are in rads.
 */
bool GVGGraph::addMeetpoint(gvg_mapper::AddMeetpoint::Request& req, gvg_mapper::AddMeetpoint::Response& res) {
  GVGVertex v;
  v.p.x = req.pose.pose.position.x;
  v.p.y = req.pose.pose.position.y;
  v.expected_degree = (int) req.possible_bearings.size();
  std::sort(req.possible_bearings.begin(), req.possible_bearings.end());
  assert(req.possible_bearings.at(0) <= req.possible_bearings.at(1));
  assert(req.meetpoint_obstacles.collection.size() >= 3);

  local_possible_bearings.clear();
  local_possible_bearings = req.possible_bearings;

  header = req.header;

  std::vector<double> edge_angle_diffs;

  if ( (M_PI - req.possible_bearings.back()) < M_PI - abs(req.possible_bearings.front()) ) {
    v.vertex_angle = abs_angle(req.robot_angle, req.possible_bearings.back());
    edge_angle_diffs.push_back(2*M_PI - req.possible_bearings.back() + req.possible_bearings.front());
    for (int i = 1; i < (int) req.possible_bearings.size(); i++) {
      edge_angle_diffs.push_back(std::abs(req.possible_bearings.at(i) - req.possible_bearings.at(i-1)));
    }
  }
  else {
    v.vertex_angle = abs_angle(req.robot_angle, req.possible_bearings.front());
    edge_angle_diffs.push_back(abs(req.possible_bearings.front()) - abs(req.possible_bearings.at(1)));
    for (int i = 2; i < (int) req.possible_bearings.size(); i++) {
      edge_angle_diffs.push_back(std::abs(req.possible_bearings.at(i) - req.possible_bearings.at(i-1)));
    }
    edge_angle_diffs.push_back(2*M_PI - req.possible_bearings.back() + req.possible_bearings.front());
  }

  v.edge_angle_diffs.assign(edge_angle_diffs.begin(), edge_angle_diffs.end());

  // Add the bearings to GVGVertex::possible_bearings (in relative angle to vertex_angle)
  EdgeBearing bearing;
  bearing.bearing_id = 0;
  bearing.edge_id = -1;
  bearing.edge_angle_diff = 0;
  bearing.visited = false;
  v.possible_bearings.push_back(bearing);

  for (int i = 0; i < (int) req.possible_bearings.size() - 1; i++) {
    bearing.edge_angle_diff = 0;
    bearing.bearing_id = i+1;
    bearing.edge_id = -1;
    for (int j = 0; j <= i; j++) {
      bearing.edge_angle_diff = bearing.edge_angle_diff + edge_angle_diffs.at(j);
    }
    bearing.visited = false;
    v.possible_bearings.push_back(bearing);
  }

  v.surrounding_obstacles = req.meetpoint_obstacles;
  v.closest_distance = req.closest_distance;

  v = G[addVertex(v, req.robot_angle, req.pose)];

  // Service call to localizer for process meetpoint
  localizer::UpdateFilter srv;
  srv.request.id = v.node_id;
  srv.request.x = laser_offset_x;
  srv.request.y = laser_offset_y;

  double angle = v.possible_bearings.at(0).edge_angle_diff;
  if (angle > M_PI) angle -= 2*M_PI;
  double absolute = abs_angle(v.vertex_angle, angle);
  double robot_angle = v.possible_bearings.at(arriving_bearing).edge_angle_diff;
  if (robot_angle > M_PI) robot_angle -= 2*M_PI;
  double absolute_robot_angle = abs_angle(v.vertex_angle, robot_angle);
  absolute_robot_angle += M_PI;
  if (absolute_robot_angle > M_PI) absolute_robot_angle -= 2*M_PI;
  double relative_bearing = absoluteToRelativeAngle(absolute_robot_angle, absolute_robot_angle - absolute);
  relative_bearing += robot_angle_correction;
  if (relative_bearing > M_PI) relative_bearing -= 2*M_PI;
  else if (relative_bearing < -M_PI) relative_bearing += 2*M_PI;

  srv.request.yaw = relative_bearing;

  if (!process_meetpoint_cln.call(srv)) {
    ROS_WARN("Could not call process meetpoint from localizer!");
  }
  std::vector<double> X;
  X = srv.response.X;
  // Update meetpoint locations with localizer data
  for (int i = 3; i < (int) X.size(); i += 3) {
    int meetpoint_id = (i - 3)/3;
    geometry_msgs::Point32 p;
    p.x = X.at(i);
    p.y = X.at(i+1);
    G[meetpoint_id].p = p;
    G[meetpoint_id].vertex_angle = X.at(i+2);
  }

  Vertex_iter    vertex, vertex_end;
  for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    gvg_mapper::GVGNode msg;
    msg.node_id = G[*vertex].node_id;
    msg.p       = G[*vertex].p;
    msg.degree  = G[*vertex].expected_degree;
    std::vector<gvg_mapper::EdgeBearingMsg> bearings;
    for (int i = 0; i < G[*vertex].possible_bearings.size(); i++) {
      EdgeBearing current = G[*vertex].possible_bearings.at(i);
      gvg_mapper::EdgeBearingMsg eb;
      eb.bearing_id = current.bearing_id;
      eb.edge_id = current.edge_id;
      eb.edge_angle_diff = current.edge_angle_diff;
      eb.visited = current.visited;
      bearings.push_back(eb);
    }
    msg.possible_bearings.assign(bearings.begin(), bearings.end());
    msg.edge_angle_diffs.assign(G[*vertex].edge_angle_diffs.begin(), G[*vertex].edge_angle_diffs.end());
    msg.closest_distance = G[*vertex].closest_distance;
    msg.vertex_angle = G[*vertex].vertex_angle;
    msg.surrounding_obstacles = G[*vertex].surrounding_obstacles;
    msg.header = req.header;

    gvg_node_pub.publish(msg);

    if (msg.node_id == v.node_id) res.node = msg;

    ros::Rate r(50);
    r.sleep();
  }

  return true;
}

/*
 * Adds a new endpoint to the GVG graph. p is the location of the endpoint in world
 * coordinates, closest_distance is the distance to the closest obstacle that defines the
 * endpoint.
 */
bool GVGGraph::addEndpoint(gvg_mapper::AddEndpoint::Request& req, gvg_mapper::AddEndpoint::Response& res) {
  GVGVertex v;
  v.p.x = req.pose.pose.position.x;
  v.p.y = req.pose.pose.position.y;

  v.vertex_angle = abs_angle(req.robot_angle, M_PI);
  v.expected_degree = 1;
  v.closest_distance = req.closest_distance;
  v.surrounding_obstacles = req.meetpoint_obstacles;

  EdgeBearing bearing;
  // Add the direction that the Robot came from to GVGVertex list of possible bearings
  bearing.bearing_id = 0;
  bearing.edge_id = -1;
  bearing.edge_angle_diff = 2*M_PI;
  bearing.visited = false;
  v.possible_bearings.push_back(bearing);

  v.edge_angle_diffs.push_back(2*M_PI);

  v = G[addVertex(v, req.robot_angle, req.pose)];

  // Service call to localizer for process meetpoint
  localizer::UpdateFilter srv;
  srv.request.id = v.node_id;
  srv.request.x = laser_offset_x;
  srv.request.y = laser_offset_y;

  double angle = v.possible_bearings.at(0).edge_angle_diff;
  if (angle > M_PI) angle -= 2*M_PI;
  double absolute = abs_angle(v.vertex_angle, angle);
  double robot_angle = v.possible_bearings.at(arriving_bearing).edge_angle_diff;
  if (robot_angle > M_PI) robot_angle -= 2*M_PI;
  double absolute_robot_angle = abs_angle(v.vertex_angle, robot_angle);
  absolute_robot_angle += M_PI;
  if (absolute_robot_angle > M_PI) absolute_robot_angle -= 2*M_PI;
  double relative_bearing = absoluteToRelativeAngle(absolute_robot_angle, absolute_robot_angle - absolute);
  relative_bearing += robot_angle_correction;
  if (relative_bearing > M_PI) relative_bearing -= 2*M_PI;
  else if (relative_bearing < -M_PI) relative_bearing += 2*M_PI;

  srv.request.yaw = relative_bearing;

  if (!process_meetpoint_cln.call(srv)) {
    ROS_WARN("Could not call process meetpoint from localizer!");
  }
  std::vector<double> X;
  X = srv.response.X;
  // Update meetpoint locations with localizer data
  for (int i = 3; i < (int) X.size(); i += 3) {
    int meetpoint_id = (i - 3)/3;
    geometry_msgs::Point32 p;
    p.x = X.at(i);
    p.y = X.at(i+1);
    G[meetpoint_id].p = p;
    G[meetpoint_id].vertex_angle = X.at(i+2);
  }

  Vertex_iter    vertex, vertex_end;
  for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    gvg_mapper::GVGNode msg;
    msg.node_id = G[*vertex].node_id;
    msg.p       = G[*vertex].p;
    msg.degree  = G[*vertex].expected_degree;
    std::vector<gvg_mapper::EdgeBearingMsg> bearings;
    for (int i = 0; i < G[*vertex].possible_bearings.size(); i++) {
      EdgeBearing current = G[*vertex].possible_bearings.at(i);
      gvg_mapper::EdgeBearingMsg eb;
      eb.bearing_id = current.bearing_id;
      eb.edge_id = current.edge_id;
      eb.edge_angle_diff = current.edge_angle_diff;
      eb.visited = current.visited;
      bearings.push_back(eb);
    }
    msg.possible_bearings.assign(bearings.begin(), bearings.end());
    msg.edge_angle_diffs.assign(G[*vertex].edge_angle_diffs.begin(), G[*vertex].edge_angle_diffs.end());
    msg.closest_distance = G[*vertex].closest_distance;
    msg.vertex_angle = G[*vertex].vertex_angle;
    msg.surrounding_obstacles = G[*vertex].surrounding_obstacles;
    msg.header = req.header;

    gvg_node_pub.publish(msg);

    if (msg.node_id == v.node_id) res.node = msg;

    ros::Rate r(50);
    r.sleep();
  }

  return true;
}

/*
 * The robot is currently traversing an edge, so just extend that edge in the
 * internal representation.
 */
bool GVGGraph::extendGVGEdge(gvg_mapper::ExtendEdge::Request& req, gvg_mapper::ExtendEdge::Response& res) {
  if ((int) currentEdge.line.size() == 0) {
    currentEdge.length = 0;
  } else {
    geometry_msgs::Point32 p1;
    p1.x = req.p_stamped.point.x;
    p1.y = req.p_stamped.point.y;
    p1.z = 0.0;
    geometry_msgs::Point32 p2;
    p2.x = currentEdge.line.back().point.x;
    p2.y = currentEdge.line.back().point.y;
    p2.z = 0.0;
    currentEdge.length += norm(p1, p2);
  }

  currentEdge.line.push_back(req.p_stamped);

  res.success = true;
  return true;
}

/*
 * Service call for Planner, returns whether robot has random relocalize or edge length relocalize.
 */
bool GVGGraph::checkRelocalize(gvg_mapper::CheckRelocalize::Request& req, gvg_mapper::CheckRelocalize::Response& res) {
  // Check if the robot's uncertainty is too large, then relocalize.
  if (vertex_matching_policy != 0 && num_vertices(G) > 0) {
    if (relocalize) {
      relocalize = false;
      string str = "";
      cout << "relocalize (random relocalize)?\n";
      getline(cin, str);
      if (str == "y") {
        res.relocalize = 2;
        return true;
      }
    }
    if (last_edge_length >= 3.0 && last_edge_length <= max(ellipse.halfaxis1, ellipse.halfaxis2)) {
      string str = "";
      cout << "relocalize?\n";
      getline(cin, str);
      if (str == "y") {
        res.relocalize = 1;
        return true;
      }
    }
  }
  else res.relocalize = 0;
  return true;
}

/*
 * Service call for Planner, returns the list of possible bearings at vertex node_id.
 */
bool GVGGraph::retrieveBearings(gvg_mapper::RetrieveBearings::Request& req, gvg_mapper::RetrieveBearings::Response& res) {
  int node_id = req.node_id;
  // No target, so exploration mode
  if (req.target == -1) {

    Vertex currentVertex = nullVertex();
    Vertex_iter    vertex, vertex_end;
    for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    /* Find the vertex with node_id */
      if (G[*vertex].node_id == node_id) {
        currentVertex = *vertex;
      }
    }

    std::vector<double> bearings;
    for (int i = 0; i < (int) G[currentVertex].possible_bearings.size(); i++) {
      if (!G[currentVertex].possible_bearings.at(i).visited) {
        int index = (i - arriving_bearing);
        if (index < 0) index += local_possible_bearings.size();
        if ( (M_PI - local_possible_bearings.back()) < M_PI - abs(local_possible_bearings.front()) ) {
          index -= 1;
          if (index < 0) index += local_possible_bearings.size();
          bearings.push_back(local_possible_bearings.at(index));
        }
        else bearings.push_back(local_possible_bearings.at(index));
      }
    }

    if (bearings.size() > 0) {
      double chosen_bearing;

      /* Modify this to determine chosen bearing */
      switch (exploration_policy) {
        case 0: {
          srand (time(NULL));
          int choice = rand() % bearings.size();
          if (bearings.size() == 1) choice = 0;
          chosen_bearing = bearings[choice];
          break;
        }
        case 1: {
          chosen_bearing = bearings[bearings.size() - 1];
          break;
        }
        case 2: {
          cout << "Choose a direction for robot to explore next: ";
          string str = "";
          getline(cin, str);
          chosen_bearing = atof(str.c_str());
          break;
        }
        default: {
          chosen_bearing = bearings[0];
          break;
        }
      }
      res.chosen_bearing = chosen_bearing;
    } else res.chosen_bearing = NULL;

    /* Update the chosen bearing as visited */

    for (int i = 0; i < (int) G[currentVertex].possible_bearings.size(); i++) {
      double relative_angle;
      int index = (i - arriving_bearing);
      if (index < 0) index += local_possible_bearings.size();
      if ( (M_PI - local_possible_bearings.back()) < M_PI - abs(local_possible_bearings.front()) ) {
        index -= 1;
        if (index < 0) index += local_possible_bearings.size();
        relative_angle = local_possible_bearings.at(index);
      }
      else relative_angle = local_possible_bearings.at(index);
      if (relative_angle == res.chosen_bearing) {
        if (!G[currentVertex].possible_bearings.at(i).visited) {
          lastVertexBearing = i;
        }
        break;
      }
    }

    for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
      cout << vertex_state(G[*vertex].node_id);
    }

    return true;

  // Pathplanning mode, need to find bearing connecting node_id and target
  } else {
    int target;
    target = req.target;
    int edge_id = -1;
    double min_length = INFINITY;
    for (int i = 0; i < (int) edges_list.size(); i++) {
      bool correct_edge = false;
      if (((edges_list.at(i).source == node_id) && (edges_list.at(i).target == target)) || ((edges_list.at(i).source == target) && (edges_list.at(i).target == node_id))) correct_edge = true;
      if (correct_edge) {
        if (edges_list.at(i).length < min_length) {
          edge_id = edges_list.at(i).edge_id;
          min_length = edges_list.at(i).length;
        }
      }
    }
    // No valid edge found, meaning we have the case of endpoint as first vertex encountered.
    Vertex currentVertex = nullVertex();
    Vertex_iter vertex, vertex_end;
    for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    /* Find the vertex with node_id */
      if (G[*vertex].node_id == node_id) {
        currentVertex = *vertex;
      }
    }
    if (edge_id == -1) {
      int index = (0 - arriving_bearing);
      if (index < 0) index += local_possible_bearings.size();
      if ( (M_PI - local_possible_bearings.back()) < M_PI - abs(local_possible_bearings.front()) ) {
        index -= 1;
        if (index < 0) index += local_possible_bearings.size();
        res.chosen_bearing = local_possible_bearings.at(index);
      }
      else res.chosen_bearing = local_possible_bearings.at(index);
    }
    else {
      ROS_INFO("Edge ID between current vertex and next vertex: %d", edge_id);
      for (int i = 0; i < (int) G[currentVertex].possible_bearings.size(); i++) {
        if (G[currentVertex].possible_bearings.at(i).edge_id == edge_id) {
          double bearing;
          int index = (i - arriving_bearing);
          if (index < 0) index += local_possible_bearings.size();
          if ( (M_PI - local_possible_bearings.back()) < M_PI - abs(local_possible_bearings.front()) ) {
            index -= 1;
            if (index < 0) index += local_possible_bearings.size();
            bearing = local_possible_bearings.at(index);
          }
          else bearing = local_possible_bearings.at(index);
          res.chosen_bearing = bearing;
          break;
        }
      }
    }
    return true;
  }
}

bool GVGGraph::retrieveEdges(gvg_mapper::RetrieveEdges::Request& req, gvg_mapper::RetrieveEdges::Response& res) {

  for (int i = 0; i < edges_list.size(); i++) {
    GVGEdge e = edges_list.at(i);
    if (e.source == req.node_id || e.target == req.node_id) {
      gvg_mapper::GVGEdgeMsg msg;
      msg.edge_id = e.edge_id;
      msg.source = e.source;
      msg.target = e.target;
      msg.length = e.length;
      msg.line.assign(e.line.begin(), e.line.end());

      res.edges.push_back(msg);
    }
  }

  return true;
}

/*
 * Path begins from the end of the list.
 */
bool GVGGraph::retrievePath(gvg_mapper::RetrievePath::Request& req, gvg_mapper::RetrievePath::Response& res) {
  vector<int> parents;
  vector<double> distances(num_vertices(G));
  res.distance = 0.0;

  Vertex currentVertex = nullVertex();
  Vertex_iter vertex, vertex_end;
  for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    if (G[*vertex].node_id == req.source) {
      currentVertex = *vertex;
    }
  }

  parents = computeDijkstra(currentVertex, distances);

  // Choose the next target, depending on the policy.
  int target = req.target;
  if (req.target == -1) target = closest_unexplored_vertex;

  // Entire map has been explored.
  if (target == -1) {
    target = closest_unexplored_vertex;
    if (num_vertices(G) > 1) {
      vector<int> vertexList;
      res.vertex_list = vertexList;
    }
  }
  // Still exploring, path planning to return to target.
  else {
    res.distance = distances[target];
    if (unexploredEdgesCount.at(target) > 0) res.frontier = true;
    else res.frontier = false;
    vector<int> vertexList;
    vertexList.push_back(target);
    int current = parents[target];
    while (current != req.source) {
      vertexList.push_back(current);
      current = parents[current];
    }
    res.vertex_list = vertexList;
  }
  return true;
}

/*
 * Returns Dijkstra parents list for each vertex.
 */
vector<int> GVGGraph::computeDijkstra(Vertex v, vector<double> &distances) {
  // Create things for Dijkstra
  vector<Vertex> parents(num_vertices(G)); // To store parents

  boost::dijkstra_shortest_paths(G, v, boost::predecessor_map(&parents[0]).distance_map(&distances[0]));
  vector<int> result;
  // Output results
  closest_unexplored_vertex = -1;
  double distance = INFINITY;
  boost::graph_traits < Graph >::vertex_iterator vertexIterator, vend;
  for (boost::tie(vertexIterator, vend) = boost::vertices(G); vertexIterator != vend; ++vertexIterator)
  {
    if ((distances[*vertexIterator] < distance)&&(unexploredEdgesCount.at(*vertexIterator) > 0)) {
      closest_unexplored_vertex = *vertexIterator;
      distance = distances[*vertexIterator];
    }
    result.push_back(parents[*vertexIterator]);
  }
  return result;
}

bool GVGGraph::loadSavedMap(gvg_mapper::LoadSavedMap::Request& req, gvg_mapper::LoadSavedMap::Response& res) {

  mapLoadLocalization = false;
  int max_node_id = 0;
  for (int i = 0; i < req.nodes.size(); i++) {
    // Create vertex and copy information
    GVGVertex v;
    gvg_mapper::GVGNode node = req.nodes.at(i);
    v.node_id = node.node_id;
    if (v.node_id > max_node_id) max_node_id = v.node_id;
    v.p = node.p;
    v.expected_degree = node.degree;
    v.closest_distance = node.closest_distance;
    int num_unexplored = 0;
    for (int j = 0; j < node.possible_bearings.size(); j++) {
      EdgeBearing eb;
      gvg_mapper::EdgeBearingMsg ebm = node.possible_bearings.at(j);
      eb.bearing_id = ebm.bearing_id;
      eb.edge_id = ebm.edge_id;
      eb.edge_angle_diff = ebm.edge_angle_diff;
      eb.visited = ebm.visited;
      // Increment num of unexplored edges
      if (!eb.visited) num_unexplored++;
      v.possible_bearings.push_back(eb);
    }
    v.edge_angle_diffs.assign(node.edge_angle_diffs.begin(), node.edge_angle_diffs.end());
    v.vertex_angle = node.vertex_angle;

    // Add vertex to graph
    unexploredEdgesCount.push_back(num_unexplored);
    add_vertex(v, G);
  }

  int max_edge_id = 0;
  for (int i = 0; i < req.edges.size(); i++) {
    GVGEdge e;
    gvg_mapper::GVGEdgeMsg em = req.edges.at(i);
    e.edge_id = em.edge_id;
    if (e.edge_id > max_edge_id) max_edge_id = e.edge_id;
    e.source = em.source;
    e.target = em.target;
    e.length = em.length;
    e.line.assign(em.line.begin(), em.line.end());

    // Create EWP and add edge to graph G
    EdgeWeightProperty weight(e.length);
    add_edge(e.source, e.target, weight, G);
    add_edge(e.target, e.source, weight, G);
    edges_list.push_back(e);
  }

  next_node_id = max_node_id + 1;
  next_edge_id = max_edge_id + 1;

  return true;
}

/*
 * Takes an absolute angle and a relative angle to convert to an absolute angle (within -180 to 180 degrees)
 */
double abs_angle(double theta, double phi) {
  double result;
  result = theta + phi + M_PI;
  if (result < 0) result = result + 2*M_PI;
  else if (result > 2*M_PI) result = result - 2*M_PI;
  result = result - M_PI;
  return result;
}

double absoluteToRelativeAngle(double robot_angle, double absolute_angle) {
  double relative_angle = absolute_angle;
  if (robot_angle > 0) {
    if (relative_angle < 0) relative_angle = -relative_angle;
    else {
      if (relative_angle > M_PI) relative_angle = 2*M_PI - relative_angle;
      else relative_angle = -relative_angle;
    }
  } else {
    if (relative_angle > 0) relative_angle = -relative_angle;
    else {
      if (relative_angle < -M_PI) relative_angle = -2*M_PI - relative_angle;
      else relative_angle = -relative_angle;
    }
  }
  return relative_angle;
}

/*
 * Debugging purposes. Prints out the state of a vertex in the Graph G.
 */
string GVGGraph::vertex_state(int node_id) {
  stringstream ss;
  Vertex currentVertex = nullVertex();
  Vertex_iter vertex, vertex_end;
  for (tie(vertex, vertex_end) = vertices(G); vertex != vertex_end; vertex++) {
    /* Find the vertex with node_id */
    if (G[*vertex].node_id == node_id) {
      currentVertex = *vertex;
    }
  }
  GVGVertex current = G[currentVertex];
  ss << "Vertex state for node " << node_id << " : " << "edge 0 orientation: " << current.vertex_angle*180.0/M_PI << " ";
  ss << "Unexplored Edges Count: " << unexploredEdgesCount.at(node_id) << endl;
  ss << "Angle\tBearing\tEdge\tVisited" << endl;
  for (int i = 0; i < (int) current.possible_bearings.size(); i++) {
    ss << current.possible_bearings.at(i).edge_angle_diff*180.0/M_PI << "\t" << current.possible_bearings.at(i).bearing_id << "\t" << current.possible_bearings.at(i).edge_id << "\t" << current.possible_bearings.at(i).visited << endl;
  }
  return ss.str();
}


#ifndef PAINTER_H
#define PAINTER_H

#include <set>
#include <algorithm>

#include <boost/make_shared.hpp>

#include "point.hpp"
#include "shapeGenerator.hpp"
#include "pheromoneMap.hpp"

/**
 * Utility class for painting simulation objects onto the map using mouse
 * pointer (wrapper for external Python GUI)
 */
class Painter
{
public:

    /// Spawns entities of specified type between two supplied positions.
    /// Doesn't overwrite spaces in which something already exists.
    template<class T>
    inline static void drawEntities(World* world, const Point& from,
        const Point& to, const float radius, const float strength)
    {
        auto points_set = ShapeGenerator::GenerateLine(from, to, radius);

        // copy from set to vector, because it's easier to remove elements
        // from a vector.
        std::vector<Point> candidate_points;
        std::copy(points_set.begin(), points_set.end(),
            std::back_inserter(candidate_points));

        // remove points outside of the map
        candidate_points.erase(
            std::remove_if(candidate_points.begin(), candidate_points.end(),
                [world] (auto candidate_pt) -> bool {
                return !(candidate_pt.isInBounds(world->getDimensions()));
            }),
            candidate_points.end()
        );

        const int leftmost = std::min(from.posX(), to.posX()) - radius,
            rightmost = std::max(from.posX(), to.posX()) + radius,
            bottommost = std::min(from.posY(), to.posY()) - radius,
            topmost = std::max(from.posY(), to.posY()) + radius;
        const Point botleft(leftmost, bottommost), topright(rightmost, topmost);

        const auto& entities = world->getEntityMap().lock()->
            getEntitiesInSquare(botleft, topright);

        // remove painted points for which their space is already occupied
        candidate_points.erase(
            std::remove_if(candidate_points.begin(), candidate_points.end(),
                [&entities] (auto candidate_pt) -> bool {
                    for(auto& e : entities)
                        if(candidate_pt == e.lock()->getPos())
                            return true;
                    return false;
                }),
            candidate_points.end()
        );

        // make selected entities at valid locations
        for(const auto& valid_point : candidate_points)
            world->addSimulationObject<T>(boost::make_shared<T>(
                world, valid_point));
    }

    /// Increases strength of specified pheromone in blobs of specified radius
    /// and of specified central strength, in a line specified
    /// by params "from" and "to".
    inline static void drawPheromone(World* world, const Point& from,
        const Point& to, PheromoneMap::Type type, const float radius,
        const float strength)
    {
        auto& pheromone_maps = world->getSimulationObjects<PheromoneMap>();

        boost::shared_ptr<PheromoneMap> our_map;
        for(const auto& map : pheromone_maps)
            if(map->getType() == type)
            {
                our_map = map;
                break;
            }

        const int thick = 1;
        auto points_set = ShapeGenerator::GenerateLine(from, to, thick);

        // copy from set to vector, because it's easier to remove elements
        // from a vector.
        std::vector<Point> candidate_points;
        std::copy(points_set.begin(), points_set.end(),
            std::back_inserter(candidate_points));

        // remove points outside of the map
        candidate_points.erase(
            std::remove_if(candidate_points.begin(), candidate_points.end(),
                [world] (auto candidate_pt) -> bool {
                return !(candidate_pt.isInBounds(world->getDimensions()));
            }),
            candidate_points.end()
        );

        // create blobs in valid points
        for(const auto& valid_point : candidate_points)
            our_map->createBlob(valid_point, radius, strength);
    }

    /// Wrapper for drawPheromone (for easier Python export)
    inline static void drawPheromoneToFoodWrapper(World* world,
        const int from_x, const int from_y, const int to_x, const int to_y,
        const float radius, const float strength)
    {
        drawPheromone(world, Point(from_x, from_y), Point(to_x, to_y),
            PheromoneMap::Type::ToFood, radius, strength);
    }

    /// Wrapper for drawPheromone (for easier Python export)
    inline static void drawPheromoneFromFoodWrapper(World* world,
        const int from_x, const int from_y, const int to_x, const int to_y,
        const float radius, const float strength)
    {
        drawPheromone(world, Point(from_x, from_y), Point(to_x, to_y),
            PheromoneMap::Type::FromFood, radius, strength);
    }

    /// Wrapper for drawEntities (for easier Python export)
    template<class T>
    inline static void drawEntitiesWrapper(World* world, const int from_x,
        const int from_y, const int to_x, const int to_y,
        const float radius, const float strength = 0)
    {
        Painter::drawEntities<T>(world, Point(from_x, from_y),
            Point(to_x, to_y), radius, strength);
    }
};

#endif // PAINTER_H


/*
 ___ ___ __     __ ____________
|   |   |  |   |__|__|__   ___/  Ubiquitous Internet @ IIT-CNR
|   |   |  |  /__/  /  /  /      Tiny Markov simulator
|   |   |  |/__/  /   /  /       https://github.com/ccicconetti/markovsim/
|_______|__|__/__/   /__/

Licensed under the MIT License <http://opensource.org/licenses/MIT>.
Copyright (c) 2018 Claudio Cicconetti <https://ccicconetti.github.io/>

Permission is hereby  granted, free of charge, to any  person obtaining a copy
of this software and associated  documentation files (the "Software"), to deal
in the Software  without restriction, including without  limitation the rights
to  use, copy,  modify, merge,  publish, distribute,  sublicense, and/or  sell
copies  of  the Software,  and  to  permit persons  to  whom  the Software  is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE  IS PROVIDED "AS  IS", WITHOUT WARRANTY  OF ANY KIND,  EXPRESS OR
IMPLIED,  INCLUDING BUT  NOT  LIMITED TO  THE  WARRANTIES OF  MERCHANTABILITY,
FITNESS FOR  A PARTICULAR PURPOSE AND  NONINFRINGEMENT. IN NO EVENT  SHALL THE
AUTHORS  OR COPYRIGHT  HOLDERS  BE  LIABLE FOR  ANY  CLAIM,  DAMAGES OR  OTHER
LIABILITY, WHETHER IN AN ACTION OF  CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE  OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "Markov/mmc.h"
#include "Support/chrono.h"
#include "Support/glograii.h"

#include <boost/program_options.hpp>

#include <glog/logging.h>

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <limits>

namespace po = boost::program_options;

int main(int argc, char* argv[]) {
  uiiit::support::GlogRaii myGlogRaii(argv[0]);

  size_t       mySeed;
  double       lambda;
  double       mu;
  unsigned int c;
  double       myWarmup;
  double       myDuration;
  double       myThreshold;
  std::string  myDistFile;

  po::options_description myDesc("Allowed options");
  // clang-format off
  myDesc.add_options()
    ("help,h", "produce help message")
    ("seed",
     po::value<size_t>(&mySeed)->default_value(std::chrono::system_clock::now().time_since_epoch().count()),
     "Seed generator. If not specified use the number of seconds since Jan 1, 1970.")
    ("lambda",
     po::value<double>(&lambda)->default_value(1.0),
     "Arrival rate.")
    ("mu",
     po::value<double>(&mu)->default_value(1.0),
     "Serving rate.")
    ("c",
     po::value<unsigned int>(&c)->default_value(2),
     "Number of servers.")
    ("warmup",
     po::value<double>(&myWarmup)->default_value(30),
     "The warm-up duration, in s.")
    ("duration",
     po::value<double>(&myDuration)->default_value(60),
     "Simulation duration step, in s.")
    ("threshold",
     po::value<double>(&myThreshold)->default_value(0.001),
     "Convergence threshold, in s.")
    ("distributions",
     po::value<std::string>(&myDistFile)->default_value(""),
     "Write the full distributions on file with the given base name.")
    ;
  // clang-format on

  try {
    po::variables_map myVarMap;
    po::store(po::parse_command_line(argc, argv, myDesc), myVarMap);
    po::notify(myVarMap);

    if (myVarMap.count("help")) {
      std::cout << myDesc << std::endl;
      return EXIT_FAILURE;
    }

    uiiit::markov::MMC     myMMC(lambda, mu, c, mySeed);
    const auto             myTheoDelay = myMMC.averageResponseTime();
    auto                   myMeasDelay = std::numeric_limits<double>::max();
    uiiit::support::Chrono myChrono(true);
    LOG(INFO) << "starting simulation";
    myMMC.run(myWarmup);
    myMMC.statEnable();
    auto mySimulTime = myWarmup;
    while (fabs(myTheoDelay - myMeasDelay) > myThreshold) {
      mySimulTime += myDuration;
      myMMC.run(myDuration);
      myMeasDelay = myMMC.averageDelay();
      VLOG(1) << "avg response time: theo " << myTheoDelay
              << " vs. meas: " << myMeasDelay;
    }
    LOG(INFO) << "done in " << mySimulTime << " s (" << myChrono.stop()
              << " s real time)";

    if (not myDistFile.empty()) {
      myMMC.sortDelays();
      size_t i = 0;
      for (const auto& myDeque : myMMC.delays()) {
        ++i;
        if (myDeque.empty()) {
          continue;
        }
        std::ofstream myOut(myDistFile +
                            std::to_string(i * 100 / myMMC.delays().size()) +
                            ".dat");
        for (const auto& myDelay : myDeque) {
          myOut << myDelay << '\n';
        }
      }
    }

    return EXIT_SUCCESS;

  } catch (const std::exception& aErr) {
    LOG(ERROR) << "Exception caught: " << aErr.what();

  } catch (...) {
    LOG(ERROR) << "Unknown exception caught";
  }

  return EXIT_FAILURE;
}


// Boost.uBLAS
//
// Copyright (c) 2018 Fady Essam
// Copyright (c) 2018 Stefan Seefeld
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef boost_numeric_ublas_opencl_misc_hpp_
#define boost_numeric_ublas_opencl_misc_hpp_

#include <boost/numeric/ublas/opencl/library.hpp>
#include <boost/numeric/ublas/opencl/vector.hpp>
#include <boost/numeric/ublas/opencl/matrix.hpp>

namespace boost { namespace numeric { namespace ublas { namespace opencl {

template <typename T>
typename std::enable_if<is_numeric<T>::value, T>::type
a_sum(ublas::vector<T, opencl::storage> const &v, compute::command_queue& queue)
{
  compute::vector<T> scratch_buffer(v.size(), queue.get_context());
  compute::vector<T> result_buffer(1, queue.get_context());
  cl_event event;
  if (std::is_same<T, float>::value)
    clblasSasum(v.size(),
		result_buffer.begin().get_buffer().get(), //result buffer
		0, //offset in result buffer
		v.begin().get_buffer().get(), //input buffer
		0, //offset in input buffer
		1, //increment in input buffer
		scratch_buffer.begin().get_buffer().get(),
		1, //number of command queues
		&(queue.get()), //queue
		0, // number of events waiting list
		NULL, //event waiting list
		&event); //event
  else if (std::is_same<T, double>::value)
    clblasDasum(v.size(),
		result_buffer.begin().get_buffer().get(), //result buffer
		0, //offset in result buffer
		v.begin().get_buffer().get(), //input buffer
		0, //offset in input buffer
		1, //increment in input buffer
		scratch_buffer.begin().get_buffer().get(),
		1, //number of command queues
		&(queue.get()), //queue
		0, // number of events waiting list
		NULL, //event waiting list
		&event); //event
  else if (std::is_same<T, std::complex<float>>::value)
    clblasScasum(v.size(),
		 result_buffer.begin().get_buffer().get(), //result buffer
		 0, //offset in result buffer
		 v.begin().get_buffer().get(), //input buffer
		 0, //offset in input buffer
		 1, //increment in input buffer
		 scratch_buffer.begin().get_buffer().get(),
		 1, //number of command queues
		 &(queue.get()), //queue
		 0, // number of events waiting list
		 NULL, //event waiting list
		 &event); //event
  else if (std::is_same<T, std::complex<double>>::value)
    clblasDzasum(v.size(),
		 result_buffer.begin().get_buffer().get(), //result buffer
		 0, //offset in result buffer
		 v.begin().get_buffer().get(), //input buffer
		 0, //offset in input buffer
		 1, //increment in input buffer
		 scratch_buffer.begin().get_buffer().get(),
		 1, //number of command queues
		 &(queue.get()), //queue
		 0, // number of events waiting list
		 NULL, //event waiting list
		 &event); //event
  clWaitForEvents(1, &event);
  return result_buffer[0];
}

template <typename T, typename A>
typename std::enable_if<is_numeric<T>::value, T>::type
a_sum(ublas::vector<T, A> const &v, compute::command_queue& queue)
{
  ublas::vector<T, opencl::storage> vdev(v, queue);
  return a_sum(vdev, queue);
}

template <typename T>
typename std::enable_if<std::is_same<T, float>::value |
			std::is_same<T, double>::value,
			T>::type
norm_1(ublas::vector<T, opencl::storage> const &v, compute::command_queue& queue)
{
  return a_sum(v, queue);
}

template <typename T, typename A>
typename std::enable_if<std::is_same<T, float>::value |
			std::is_same<T, double>::value,
			T>::type
norm_1(ublas::vector<T, A> const &v, compute::command_queue& queue)
{
  ublas::vector<T, opencl::storage> vdev(v, queue);
  return norm_1(vdev, queue);
}

template <typename T>
typename std::enable_if<is_numeric<T>::value, T>::type
norm_2(ublas::vector<T, opencl::storage> const &v, compute::command_queue& queue)
{
  compute::vector<T> scratch_buffer(2*v.size(), queue.get_context());
  compute::vector<T> result_buffer(1, queue.get_context());
  cl_event event;
  if (std::is_same<T, float>::value)
    clblasSnrm2(v.size(),
		result_buffer.begin().get_buffer().get(), //result buffer
		0, //offset in result buffer
		v.begin().get_buffer().get(), //input buffer
		0, //offset in input buffer
		1, //increment in input buffer
		scratch_buffer.begin().get_buffer().get(),
		1, //number of command queues
		&(queue.get()), //queue
		0, // number of events waiting list
		NULL, //event waiting list
		&event); //event
  else if (std::is_same<T, double>::value)
    clblasDnrm2(v.size(),
		result_buffer.begin().get_buffer().get(), //result buffer
		0, //offset in result buffer
		v.begin().get_buffer().get(), //input buffer
		0, //offset in input buffer
		1, //increment in input buffer
		scratch_buffer.begin().get_buffer().get(),
		1, //number of command queues
		&(queue.get()), //queue
		0, // number of events waiting list
		NULL, //event waiting list
		&event); //event
  else if (std::is_same<T, std::complex<float>>::value)
    clblasScnrm2(v.size(),
		 result_buffer.begin().get_buffer().get(), //result buffer
		 0, //offset in result buffer
		 v.begin().get_buffer().get(), //input buffer
		 0, //offset in input buffer
		 1, //increment in input buffer
		 scratch_buffer.begin().get_buffer().get(),
		 1, //number of command queues
		 &(queue.get()), //queue
		 0, // number of events waiting list
		 NULL, //event waiting list
		 &event); //event
  else if (std::is_same<T, std::complex<double>>::value)
    clblasDznrm2(v.size(),
		 result_buffer.begin().get_buffer().get(), //result buffer
		 0, //offset in result buffer
		 v.begin().get_buffer().get(), //input buffer
		 0, //offset in input buffer
		 1, //increment in input buffer
		 scratch_buffer.begin().get_buffer().get(),
		 1, //number of command queues
		 &(queue.get()), //queue
		 0, // number of events waiting list
		 NULL, //event waiting list
		 &event); //event
  clWaitForEvents(1, &event);
  return result_buffer[0];
}

template <typename T, typename A>
typename std::enable_if<is_numeric<T>::value, T>::type
norm_2(ublas::vector<T, A> const &v, compute::command_queue& queue)
{
  ublas::vector<T, opencl::storage> vdev(v, queue);
  return norm_2(vdev, queue);
}

}}}}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_TIED_NSEIG_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_TIED_NSEIG_HPP_INCLUDED

#include <nt2/linalg/functions/nseig.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/balance.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/gebak.hpp>
#include <nt2/include/functions/gebal.hpp>
#include <nt2/include/functions/geev_w.hpp>
#include <nt2/include/functions/geev_wvr.hpp>
#include <nt2/include/functions/geev_wvrvl.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&, const A2&) const
    {
      return a0;
    }
  };

  //============================================================================
  //Eig computations
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::nseig_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type    child0;
    typedef typename child0::value_type                                     type_t;
    typedef typename nt2::meta::as_real<type_t>::type                      rtype_t;
    typedef typename nt2::meta::as_complex<rtype_t>::type                  ctype_t;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::_2D> desired_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_2(a0, a1, nt2::policy<ext::vector_>());
    }

    /// INTERNAL ONLY - W = NSEIG(A, matrix_/vector_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic, a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, w, boost::proto::child_c<0>(a1));
      w.resize(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::geev_w( boost::proto::value(a)
                                   , boost::proto::value(w)));
      boost::proto::child_c<0>(a1) = w;
    }

    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic, a, boost::proto::child_c<0>(a0), work);
      nt2::container::table <ctype_t, _2D > w(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::geev_w( boost::proto::value(a)
                                  , boost::proto::value(w)
                                  ));
      boost::proto::child_c<0>(a1) = from_diag(w); //from_diag doesnt support aliasing currently
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W]= NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1,nt2::policy<ext::matrix_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W]= NSEIG(A,  matrix_/vector_/balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

   /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      size_t n = height(a);
      w.resize(of_size(n, 1));
      vr.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvr( boost::proto::value(a)
                                     , boost::proto::value(w)
                                     , boost::proto::value(vr)
                                     ));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      size_t n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      vr.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvr( boost::proto::value(a)
                                     , boost::proto::value(w)
                                     , boost::proto::value(vr)
                                     ));
      boost::proto::child_c<1>(a1) = from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::balance_> const &
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::balance_>());
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3( a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A, vector_/matrix_/balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY: 3o 2i
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      size_t n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      vr.resize(of_size(n, n));
      vl.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(w)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      boost::proto::child_c<1>(a1) = from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 2i
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      size_t n = height(a);
      vr.resize(of_size(n, n));
      vl.resize(of_size(n, n));
      w.resize(of_size(n, 1));
      NT2_LAPACK_VERIFY(nt2::geev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(w)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 2i
    template < class T> BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval3_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    /// INTERNAL ONLY: 3o 2i
    template < class T> BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      eval3_3(a0, a1, nt2::policy<ext::matrix_>(), nt2::policy<ext::balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [W]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 1o 3i
    template < class T >
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , T const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval1_2(a0, a1, T());
    }

    /// INTERNAL ONLY: 1o 3i
    template < class T >
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> a = boost::proto::child_c<0>(a0);
      nt2::container::table<type_t> b = balance(a);
      boost::proto::child_c<0>(a1) = nseig(b, nt2::policy<ext::vector_>());
    }
    /// INTERNAL ONLY: 1o 3i
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> a = boost::proto::child_c<0>(a0);
      nt2::container::table<type_t> b = balance(a);
      boost::proto::child_c<0>(a1) = nseig(b); //as_temporary(b));

    }
    //==========================================================================
    /// INTERNAL ONLY - [V, W]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 2o 3i
    template < class T> BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  T  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval2_2 (a0, a1, T());
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic,  v, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));

      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(v, w) = nseig(a, nt2::policy<ext::vector_>());
      NT2_LAPACK_VERIFY(gebak( boost::proto::value(v)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = v;
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic,  v, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal( boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(v, w) = nseig(a, nt2::policy<ext::matrix_>());
      NT2_LAPACK_VERIFY(gebak( boost::proto::value(v)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = v;
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

     /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      tie( boost::proto::child_c<0>(a1)
         , boost::proto::child_c<1>(a1)
         , boost::proto::child_c<2>(a1)) = nseig(a, nt2::policy<ext::vector_>());
    }

     /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      nt2_la_int n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      tie( vr, w, vl) = nseig(a, nt2::policy<ext::vector_>());
      boost::proto::child_c<1>(a1) =  from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(vr, w, vl) = nseig(a, nt2::policy<ext::vector_>(), nt2::policy<ext::no_balance_>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vr),
                              boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vl)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'L'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a), boost::proto::value(scale), ilo, ihi, 'B'));
      tie(vr, w, vl) = nseig(a, nt2::policy<ext::matrix_>(), nt2::policy<ext::no_balance_>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vr),
                              boost::proto::value(scale), ilo, ihi, 'B', 'R'));
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vl),
                              boost::proto::value(scale), ilo, ihi, 'B', 'L'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

  };
} }

#endif


/*
 * File:   PoolLayer.hpp
 * Author: heshan
 *
 * Created on June 7, 2018, 2:05 PM
 */

#ifndef POOLLAYER_HPP
#define POOLLAYER_HPP

#include <iostream>
#include <Eigen>
#include <string>

class PoolLayer {
public:
    PoolLayer();
    /**
     * Constructor
     *
     * @param dimensions: dimensions of the input matrix (depth, height, width)
     * @param poolW: pool matrix width
     * @param poolH: pool matrix height
     */
    PoolLayer(std::tuple<int, int, int> dimensions, int poolW, int poolH);
    /**
     *
     * @param orig
     */
    PoolLayer(const PoolLayer& orig);
    /**
     *
     */
    virtual ~PoolLayer();
    /**
     * Initialize output matrix
     *
     * @return 0
     */
    int initMat();
    /**
     *
     * @param input: input image or matrix
     * @return
     */
    Eigen::MatrixXd * pool(Eigen::MatrixXd * input);
    /**
     * Return the dimension of the layer output
     *
     * @return a tuple
     */
    std::tuple<int, int, int> getOutputDims();
private:

public:
    int height, width;
    int depth, outHeight, outWidth, poolW, poolH;
    Eigen::MatrixXd ** maxIndices;
    Eigen::MatrixXd * output;

};

#endif /* POOLLAYER_HPP */


#include "RBGL.hpp"
#include <boost/foreach.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/incremental_components.hpp>

extern "C"
{
	using namespace boost;
	using namespace std;

        typedef graph_traits<Graph_ud>::vertex_descriptor Vertex;
        typedef graph_traits<Graph_ud>::vertices_size_type size_type;

  typedef graph_traits<Graph_ud>::vertices_size_type VertexIndex;

	typedef Vertex* Parent;
	typedef size_type* Rank;
	typedef VertexIndex* Rank;

	static vector<size_type> rrank(1);
	static vector<Vertex> parent(1);
	static disjoint_sets<Rank, Parent> ds( &rrank[0], &parent[0]);
	static bool initialized = false;

	typedef component_index<unsigned int> Components;

	typedef enum {
		E_IC_INIT_INCREMENTAL_COMPONENT,
		E_IC_INCREMENTAL_COMPONENT
	} E_IC_METHOD;

    static SEXP BGL_incr_comp_internal(SEXP num_verts_in,
                SEXP num_edges_in, SEXP R_edges_in, E_IC_METHOD method)
    {
	using namespace boost;
        int NV = INTEGER(num_verts_in)[0] ;

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

	if ( !initialized || method == E_IC_INIT_INCREMENTAL_COMPONENT )
	{
	    rrank.clear();   rrank.resize(NV, 0);
	    parent.clear(); parent.resize(NV, 0);

  	    disjoint_sets<Rank, Parent>  ds1(&rrank[0], &parent[0]);
  	    ds = ds1;
	    initialize_incremental_components(g, ds);

	    initialized = true;
	}

	if ( method == E_IC_INCREMENTAL_COMPONENT )
	{
	    incremental_components(g, ds);
	}

        Components components(parent.begin(), parent.end());
	unsigned int NC = components.size();

        SEXP anslst, conn, outvec;
        PROTECT(anslst = Rf_allocVector(VECSXP,NC+1));
	PROTECT(conn = Rf_allocVector(INTSXP, 1));

	INTEGER(conn)[0] = NC;
	SET_VECTOR_ELT(anslst,0,conn);

	int l;

        int k = 0;
	BOOST_FOREACH(VertexIndex current_index, components) {
            l = 0;
	    // Iterate through the child vertex indices for [current_index]
	    BOOST_FOREACH(VertexIndex child_index, components[current_index]) {
                l++;  // determine size of component k
            }
            PROTECT(outvec = Rf_allocMatrix(INTSXP, 1, l));  // storage for names of component k
            l = 0;
	    BOOST_FOREACH(VertexIndex child_index, components[current_index]) {
                INTEGER(outvec)[l++] =(int) child_index;
            }
	    SET_VECTOR_ELT(anslst,k+1,outvec);   // use offset into anslst, top element is NC
            k = k+1;  // was l+1 seems to be bug
        }

        UNPROTECT(NC+2);
        return(anslst);
    }
    SEXP BGL_init_incremental_components(SEXP num_verts_in,
		SEXP num_edges_in, SEXP R_edges_in)
    {
        SEXP ans = BGL_incr_comp_internal(num_verts_in,
                num_edges_in, R_edges_in,
		E_IC_INIT_INCREMENTAL_COMPONENT);
	return(ans);
    }

    SEXP BGL_incremental_components(SEXP num_verts_in,
             SEXP num_edges_in, SEXP R_edges_in)
    {
        SEXP ans = BGL_incr_comp_internal(num_verts_in,
                num_edges_in, R_edges_in,
		E_IC_INCREMENTAL_COMPONENT);
	return(ans);
    }

    SEXP BGL_same_component(SEXP num_verts_in,
		    SEXP num_edges_in, SEXP R_edges_in,
		    SEXP vert_1, SEXP vert_2)
    {
        if ( !initialized )
          Rf_error("graph is not prepared to handle incremental components.");

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in);

        int NV = INTEGER(num_verts_in)[0];
	int v1 = INTEGER(vert_1)[0];
	int v2 = INTEGER(vert_2)[0];

	bool r = FALSE;

	if ( 0 <= v1 && v1 < NV && 0 <= v2 && v2 < NV )
	    r = same_component(vertex(v1, g), vertex(v2, g), ds);

	SEXP conn;
	PROTECT(conn = Rf_allocVector(LGLSXP, 1));
	LOGICAL(conn)[0] = r;
	UNPROTECT(1);
	return(conn);
    }

}


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <ArborX.hpp>
#include <ArborX_Ray.hpp>
#include <ArborX_Version.hpp>

#include <Kokkos_Core.hpp>

#include <boost/program_options.hpp>

#include <iostream>
#include <random>

template <typename MemorySpace>
struct SpheresToBoxes
{
  Kokkos::View<ArborX::Sphere *, MemorySpace> _spheres;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<SpheresToBoxes<MemorySpace>, ArborX::PrimitivesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t
  size(const SpheresToBoxes<MemorySpace> &stob)
  {
    return stob._spheres.extent(0);
  }
  KOKKOS_FUNCTION static ArborX::Box
  get(SpheresToBoxes<MemorySpace> const &stobs, std::size_t const i)
  {
    auto const &sphere = stobs._spheres(i);
    auto const &c = sphere.centroid();
    auto const r = sphere.radius();
    return {{c[0] - r, c[1] - r, c[2] - r}, {c[0] + r, c[1] + r, c[2] + r}};
  }
};

template <typename MemorySpace>
struct Rays
{
  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> _rays;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<Rays<MemorySpace>, ArborX::PredicatesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t size(const Rays<MemorySpace> &rays)
  {
    return rays._rays.extent(0);
  }
  KOKKOS_FUNCTION static auto get(Rays<MemorySpace> const &rays, std::size_t i)
  {
    return attach(intersects(rays._rays(i)), (int)i);
  }
};

template <typename MemorySpace>
struct AccumRaySphereInterDist
{
  Kokkos::View<ArborX::Sphere *, MemorySpace> _spheres;
  Kokkos::View<float *, MemorySpace> _accumulator;

  template <typename Predicate>
  KOKKOS_FUNCTION void operator()(Predicate const &predicate,
                                  int const primitive_index) const
  {
    auto const &ray = ArborX::getGeometry(predicate);
    auto const &sphere = _spheres(primitive_index);

    float const length = overlapDistance(ray, sphere);
    int const i = getData(predicate);

    Kokkos::atomic_add(&_accumulator(i), length);
  }
};

int main(int argc, char *argv[])
{
  using ExecutionSpace = Kokkos::DefaultExecutionSpace;
  using MemorySpace = ExecutionSpace::memory_space;

  Kokkos::ScopeGuard guard(argc, argv);

  namespace bpo = boost::program_options;

  int num_spheres;
  int num_rays;
  float L;

  bpo::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
    ( "help", "help message" )
    ("spheres", bpo::value<int>(&num_spheres)->default_value(100), "number of spheres")
    ("rays", bpo::value<int>(&num_rays)->default_value(10000), "number of rays")
    ("L", bpo::value<float>(&L)->default_value(100.0), "size of the domain")
    ;
  // clang-format on
  bpo::variables_map vm;
  bpo::store(bpo::command_line_parser(argc, argv).options(desc).run(), vm);
  bpo::notify(vm);

  if (vm.count("help") > 0)
  {
    std::cout << desc << '\n';
    return 1;
  }

  std::cout << "ArborX version: " << ArborX::version() << std::endl;
  std::cout << "ArborX hash   : " << ArborX::gitCommitHash() << std::endl;
  std::cout << "Kokkos version: " << KokkosExt::version() << std::endl;

  std::uniform_real_distribution<float> uniform{0.0, 1.0};
  std::default_random_engine gen;
  auto rand_uniform = [&]() { return uniform(gen); };

  // Random parameters for Gaussian distribution of radii
  const float mu_R = 1.0;
  const float sigma_R = mu_R / 3.0;

  std::normal_distribution<> normal{mu_R, sigma_R};
  auto rand_normal = [&]() { return std::max(normal(gen), 0.0); };

  // Construct spheres
  //
  // The centers of spheres are uniformly sampling the domain. The radii of
  // spheres have Gaussian (mu_R, sigma_R) sampling.
  Kokkos::View<ArborX::Sphere *, MemorySpace> spheres(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "spheres"), num_spheres);
  auto spheres_host = Kokkos::create_mirror_view(spheres);
  for (int i = 0; i < num_spheres; ++i)
  {
    spheres_host(i) = {
        {rand_uniform() * L, rand_uniform() * L, rand_uniform() * L},
        rand_normal()};
  }
  Kokkos::deep_copy(spheres, spheres_host);

  // Construct rays
  //
  // The origins of rays are uniformly sampling the bottom surface of the
  // domain. The direction vectors are uniformly sampling of a cosine-weighted
  // hemisphere, It requires expressing the direction vector in the spherical
  // coordinates as:
  //    {sinpolar * cosazimuth, sinpolar * sinazimuth, cospolar}
  // A detailed description can be found in the slides here (slide 47):
  // https://cg.informatik.uni-freiburg.de/course_notes/graphics2_08_renderingEquation.pdf
  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> rays(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "rays"), num_rays);
  auto rays_host = Kokkos::create_mirror_view(rays);

  for (int i = 0; i < num_rays; ++i)
  {
    float xi_1 = rand_uniform();
    float xi_2 = rand_uniform();

    rays_host(i) = {ArborX::Point{rand_uniform() * L, rand_uniform() * L, 0.f},
                    ArborX::Experimental::Vector{
                        float(std::cos(2 * M_PI * xi_2) * std::sqrt(xi_1)),
                        float(std::sin(2 * M_PI * xi_2) * std::sqrt(xi_1)),
                        std::sqrt(1.f - xi_1)}};
  }
  Kokkos::deep_copy(rays, rays_host);

  Kokkos::Timer timer;

  ExecutionSpace exec_space{};

  exec_space.fence();
  timer.reset();
  ArborX::BVH<MemorySpace> bvh{exec_space,
                               SpheresToBoxes<MemorySpace>{spheres}};

  Kokkos::View<float *, MemorySpace> accumulator("accumulator", num_rays);
  bvh.query(exec_space, Rays<MemorySpace>{rays},
            AccumRaySphereInterDist<MemorySpace>{spheres, accumulator});
  exec_space.fence();
  auto time = timer.seconds();

  auto accumulator_avg =
      ArborX::accumulate(exec_space, accumulator, 0.f) / num_rays;

  printf("time          : %.3f   [%.3fM ray/sec]\n", time,
         num_rays / (1000000 * time));
  printf("ray avg       : %.3f\n", accumulator_avg);

  return EXIT_SUCCESS;
}