/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#include "panther_ros.hpp"

#include <nav_msgs/Path.h>

#include <decomp_ros_msgs/PolyhedronArray.h>
#include <decomp_ros_utils/data_ros_utils.h>  //For DecompROS::polyhedron_array_to_ros
#include <decomp_geometry/polyhedron.h>       //For hyperplane
#include <Eigen/Geometry>

#include <panther_msgs/Log.h>

// #include <assert.h> /* assert */

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

typedef PANTHER_timers::Timer MyTimer;

// this object is created in the panther_ros_node
PantherRos::PantherRos(ros::NodeHandle nh1, ros::NodeHandle nh2, ros::NodeHandle nh3) : nh1_(nh1), nh2_(nh2), nh3_(nh3)
{
  name_drone_ = ros::this_node::getNamespace();  // This returns also the slashes (2 in Kinetic, 1 in Melodic)
  name_drone_.erase(std::remove(name_drone_.begin(), name_drone_.end(), '/'), name_drone_.end());  // Remove the slashes

  std::string id = name_drone_;
  id.erase(0, 2);  // Erase SQ or HX i.e. SQ12 --> 12  HX8621 --> 8621 # TODO Hard-coded for this this convention
  id_ = std::stoi(id);

  // wait for body transform to be published before initializing.  This has to be done before creating a new PANTHER
  // object
  name_camera_depth_optical_frame_tf_ = name_drone_ + "/camera_depth_optical_frame";

  while (true)
  {
    tf2_ros::Buffer tf_buffer;
    std::unique_ptr<tf2_ros::TransformListener> tfListener{ new tf2_ros::TransformListener(tf_buffer) };
    geometry_msgs::TransformStamped transform_stamped;
    try
    {
      transform_stamped = tf_buffer.lookupTransform(name_drone_, name_camera_depth_optical_frame_tf_, ros::Time(0),
                                                    ros::Duration(0.5));  // Note that ros::Time(0) will just get us the
                                                                          // latest available transform.
      par_.b_T_c = tf2::transformToEigen(transform_stamped);              // Body to camera_depth_optical_frame
      par_.c_T_b = (par_.b_T_c).inverse();
      break;
    }
    catch (tf2::TransformException& ex)
    {
      ROS_WARN_THROTTLE(1.0, "Trying to find transform %s --> %s", name_drone_.c_str(),
                        name_camera_depth_optical_frame_tf_.c_str());
    }
  }
  ROS_INFO("Found transform %s --> %s", name_drone_.c_str(), name_camera_depth_optical_frame_tf_.c_str());
  // std::cout << "par_.b_T_c.matrix()= " << par_.b_T_c.matrix() << std::endl;

  safeGetParam(nh1_, "use_ff", par_.use_ff);
  safeGetParam(nh1_, "visual", par_.visual);
  safeGetParam(nh1_, "color_type", par_.color_type);
  safeGetParam(nh1_, "n_agents", par_.n_agents);

  safeGetParam(nh1_, "dc", par_.dc);
  safeGetParam(nh1_, "goal_radius", par_.goal_radius);
  safeGetParam(nh1_, "drone_radius", par_.drone_radius);
  double drone_radius_extra_safety;
  safeGetParam(nh1_, "drone_radius_extra_safety", drone_radius_extra_safety);
  par_.drone_radius = par_.drone_radius + drone_radius_extra_safety;

  safeGetParam(nh1_, "Ra", par_.Ra);
  safeGetParam(nh1_, "impose_FOV_in_trajCB", par_.impose_FOV_in_trajCB);

  safeGetParam(nh1_, "ydot_max", par_.ydot_max);

  safeGetParam(nh1_, "x_min", par_.x_min);
  safeGetParam(nh1_, "x_max", par_.x_max);

  safeGetParam(nh1_, "y_min", par_.y_min);
  safeGetParam(nh1_, "y_max", par_.y_max);

  safeGetParam(nh1_, "z_min", par_.z_min);
  safeGetParam(nh1_, "z_max", par_.z_max);

  std::vector<double> v_max_tmp;
  std::vector<double> a_max_tmp;
  std::vector<double> j_max_tmp;

  safeGetParam(nh1_, "v_max", v_max_tmp);
  safeGetParam(nh1_, "a_max", a_max_tmp);
  safeGetParam(nh1_, "j_max", j_max_tmp);

  par_.v_max << v_max_tmp[0], v_max_tmp[1], v_max_tmp[2];
  par_.a_max << a_max_tmp[0], a_max_tmp[1], a_max_tmp[2];
  par_.j_max << j_max_tmp[0], j_max_tmp[1], j_max_tmp[2];

  safeGetParam(nh1_, "factor_alloc", par_.factor_alloc);

  safeGetParam(nh1_, "factor_alpha", par_.factor_alpha);

  safeGetParam(nh1_, "num_seg", par_.num_seg);
  safeGetParam(nh1_, "deg_pos", par_.deg_pos);
  safeGetParam(nh1_, "deg_yaw", par_.deg_yaw);
  safeGetParam(nh1_, "num_max_of_obst", par_.num_max_of_obst);
  safeGetParam(nh1_, "num_samples_simpson", par_.num_samples_simpson);

  safeGetParam(nh1_, "upper_bound_runtime_snlopt", par_.upper_bound_runtime_snlopt);
  safeGetParam(nh1_, "lower_bound_runtime_snlopt", par_.lower_bound_runtime_snlopt);
  safeGetParam(nh1_, "kappa", par_.kappa);
  safeGetParam(nh1_, "mu", par_.mu);

  safeGetParam(nh1_, "max_seconds_keeping_traj", par_.max_seconds_keeping_traj);

  safeGetParam(nh1_, "a_star_samp_x", par_.a_star_samp_x);
  safeGetParam(nh1_, "a_star_samp_y", par_.a_star_samp_y);
  safeGetParam(nh1_, "a_star_samp_z", par_.a_star_samp_z);
  safeGetParam(nh1_, "a_star_fraction_voxel_size", par_.a_star_fraction_voxel_size);
  safeGetParam(nh1_, "allow_infeasible_guess", par_.allow_infeasible_guess);

  safeGetParam(nh1_, "a_star_bias", par_.a_star_bias);

  safeGetParam(nh1_, "basis", par_.basis);

  safeGetParam(nh1_, "mode", par_.mode);

  safeGetParam(nh1_, "angle_deg_focus_front", par_.angle_deg_focus_front);

  safeGetParam(nh1_, "res_plot_traj", par_.res_plot_traj);

  safeGetParam(nh1_, "norminv_prob", par_.norminv_prob);
  safeGetParam(nh1_, "gamma", par_.gamma);

  safeGetParam(nh1_, "alpha_shrink", par_.alpha_shrink);

  safeGetParam(nh1_, "fov_x_deg", par_.fov_x_deg);
  safeGetParam(nh1_, "fov_y_deg", par_.fov_y_deg);
  safeGetParam(nh1_, "fov_depth", par_.fov_depth);

  safeGetParam(nh1_, "c_smooth_yaw_search", par_.c_smooth_yaw_search);
  safeGetParam(nh1_, "c_visibility_yaw_search", par_.c_visibility_yaw_search);
  // safeGetParam(nh1_, "num_of_layers", par_.num_of_layers); //This one is the same as num_samples_simpson
  safeGetParam(nh1_, "num_of_yaw_per_layer", par_.num_of_yaw_per_layer);

  safeGetParam(nh1_, "c_pos_smooth", par_.c_pos_smooth);
  safeGetParam(nh1_, "c_yaw_smooth", par_.c_yaw_smooth);
  safeGetParam(nh1_, "c_fov", par_.c_fov);
  safeGetParam(nh1_, "c_final_pos", par_.c_final_pos);
  safeGetParam(nh1_, "c_final_yaw", par_.c_final_yaw);
  safeGetParam(nh1_, "print_graph_yaw_info", par_.print_graph_yaw_info);

  bool perfect_prediction;  // use_ground_truth_prediction
  safeGetParam(nh1_, "perfect_prediction", perfect_prediction);

  // safeGetParam(nh1_, "distance_to_force_final_pos", par_.distance_to_force_final_pos);
  // safeGetParam(nh1_, "factor_alloc_when_forcing_final_pos", par_.factor_alloc_when_forcing_final_pos);
  // par_.force_final_pos = false;

  if ((par_.basis != "B_SPLINE" || par_.basis != "BEZIER" || par_.basis != "MINVO") == false)
  {
    std::cout << red << bold << "Basis " << par_.basis << " not implemented yet, aborting" << reset << std::endl;
    abort();
  }
  else
  {
    std::cout << bold << green << "Basis chosen: " << par_.basis << reset << std::endl;
  }

  // CHECK parameters
  std::cout << bold << "Parameters obtained, checking them..." << reset << std::endl;

  verify((par_.c_smooth_yaw_search >= 0), "par_.c_smooth_yaw_search>=0 must hold");
  verify((par_.c_visibility_yaw_search >= 0), "par_.c_visibility_yaw_search>=0 must hold");
  verify((par_.num_of_yaw_per_layer >= 1), "par_.num_of_yaw_per_layer>=1 must hold");

  verify((par_.c_pos_smooth >= 0), "par_.c_pos_smooth>=0 must hold");
  verify((par_.c_yaw_smooth >= 0), "par_.c_yaw_smooth>=0 must hold");
  verify((par_.c_fov >= 0), "par_.c_fov>=0 must hold");
  verify((par_.c_final_pos >= 0), "par_.c_final_pos>=0 must hold");
  verify((par_.c_final_yaw >= 0), "par_.c_final_yaw>=0 must hold");

  verify((par_.ydot_max >= 0), "ydot_max>=0 must hold");
  verify((par_.gamma >= 0), "par_.gamma >= 0 must hold");
  // verify((par_.beta < 0 || par_.alpha < 0), " ");
  // verify((par_.a_max.z() <= 9.81), "par_.a_max.z() >= 9.81, the drone will flip");
  verify((par_.factor_alloc >= 1.0), "Needed: factor_alloc>=1");
  verify((par_.kappa >= 0 && par_.mu >= 0), "Needed: kappa and mu >= 0");
  verify(((par_.kappa + par_.mu) <= 1), "Needed: (par_.kappa + par_.mu) <= 1");
  verify((par_.a_star_fraction_voxel_size >= 0.0 && par_.a_star_fraction_voxel_size <= 1.0), "a_star_fraction_voxel_"
                                                                                "size is not in [0,1] ");
  verify((par_.deg_pos == 3), "PANTHER needs deg_pos==3");
  verify((par_.deg_yaw == 2), "PANTHER needs deg_yaw==2");
  verify((par_.num_max_of_obst >= 0), "num_max_of_obst>=0 must hold");
  verify((par_.num_seg >= 1), "num_seg>=1 must hold");

  verify((par_.fov_x_deg >= 0), "fov_x_deg>=0 must hold");
  verify((par_.fov_y_deg >= 0), "fov_y_deg>=0 must hold");

  verify((par_.fov_y_deg == par_.fov_x_deg), "par_.fov_y_deg == par_.fov_x_deg must hold");

  if (par_.impose_FOV_in_trajCB)
  {
    std::cout<<"par_.fov_depth= "<<par_.fov_depth<<std::endl;
    std::cout<<"par_.Ra= "<<par_.Ra<<std::endl;
    std::cout<<"par_.drone_radius= "<<par_.drone_radius<<std::endl;
    verify((par_.fov_depth > (par_.Ra + par_.drone_radius)), "(par_.fov_depth > (par_.Ra + par_.drone_radius) must "
                                                             "hold");
  }

  std::cout << bold << "Parameters checked" << reset << std::endl;

  panther_ptr_ = std::unique_ptr<Panther>(new Panther(par_));

  // Publishers
  pub_goal_ = nh1_.advertise<snapstack_msgs::Goal>("goal", 1);
  pub_setpoint_ = nh1_.advertise<visualization_msgs::Marker>("setpoint", 1);
  pub_point_G_ = nh1_.advertise<geometry_msgs::PointStamped>("point_G", 1);
  pub_point_G_term_ = nh1_.advertise<geometry_msgs::PointStamped>("point_G_term", 1);
  pub_point_A_ = nh1_.advertise<visualization_msgs::Marker>("point_A", 1);
  pub_actual_traj_ = nh1_.advertise<visualization_msgs::Marker>("actual_traj", 1);
  poly_safe_pub_ = nh1_.advertise<decomp_ros_msgs::PolyhedronArray>("polys", 1, true);
  pub_traj_safe_colored_ = nh1_.advertise<visualization_msgs::MarkerArray>("traj_obtained", 1);
  pub_traj_ = nh1_.advertise<panther_msgs::DynTraj>("/trajs", 1, true);  // The last boolean is latched or not
  pub_fov_ = nh1_.advertise<visualization_msgs::Marker>("fov", 1);
  pub_obstacles_ = nh1_.advertise<visualization_msgs::Marker>("obstacles", 1);
  pub_log_ = nh1_.advertise<panther_msgs::Log>("log", 1);

  // Subscribers
  sub_term_goal_ = nh1_.subscribe("term_goal", 1, &PantherRos::terminalGoalCB, this);
  sub_whoplans_ = nh1_.subscribe("who_plans", 1, &PantherRos::whoPlansCB, this);
  sub_state_ = nh1_.subscribe("state", 1, &PantherRos::stateCB, this);

  ///
  if (perfect_prediction == false)
  {
    ROS_INFO("NOT using ground truth trajectories (subscribed to trajs_predicted)");
    sub_traj_ = nh1_.subscribe("trajs_predicted", 20, &PantherRos::trajCB, this);  // number is queue size

    // obstacles --> topic trajs_predicted
    // agents -->  //Not implemented yet  (solve the common frame problem)
  }
  else
  {
    ROS_INFO("Using ground truth trajectories (subscribed to /trajs)");

    sub_traj_ = nh1_.subscribe("/trajs", 20, &PantherRos::trajCB, this);
    // sub_traj_ = nh1_.subscribe("trajs_zhejiang", 20, &PantherRos::trajCB,
    //                            this);  // Uncomment ONLY FOR THE BENCHMARK WITH zhejiang CODE
    // obstacles --> topic /trajs
    // agents --> topic /trajs
    // Everything in the same world frame
  }
  //

  // Timers
  pubCBTimer_ = nh2_.createTimer(ros::Duration(par_.dc), &PantherRos::pubCB, this);
  replanCBTimer_ = nh3_.createTimer(ros::Duration(par_.dc), &PantherRos::replanCB, this);

  // For now stop all these subscribers/timers until we receive GO
  sub_state_.shutdown();
  sub_term_goal_.shutdown();
  pubCBTimer_.stop();
  replanCBTimer_.stop();

  // Rviz_Visual_Tools
  visual_tools_.reset(new rvt::RvizVisualTools("world", "/rviz_visual_tools"));
  visual_tools_->loadMarkerPub();  // create publisher before waiting
  ros::Duration(0.5).sleep();
  visual_tools_->deleteAllMarkers();
  visual_tools_->enableBatchPublishing();

  // Markers
  setpoint_ = getMarkerSphere(0.35, ORANGE_TRANS);
  E_ = getMarkerSphere(0.35, RED_NORMAL);
  A_ = getMarkerSphere(0.35, RED_NORMAL);

  timer_stop_.reset();

  clearMarkerActualTraj();

  bool gui_mission;
  safeGetParam(nh1_, "gui_mission", gui_mission);

  std::cout << yellow << bold << "gui_mission= " << gui_mission << reset << std::endl;
  std::cout << yellow << bold << "perfect_prediction= " << perfect_prediction << reset << std::endl;
  std::cout << yellow << bold << "mode= " << par_.mode << reset << std::endl;
  // To avoid having to click on the GUI
  if (gui_mission == false)
  {
    panther_msgs::WhoPlans tmp;
    tmp.value = panther_msgs::WhoPlans::PANTHER;
    whoPlansCB(tmp);
  }
  constructFOVMarker();  // only needed once

  ROS_INFO("Planner initialized");
}

PantherRos::~PantherRos()
{
  sub_state_.shutdown();
  sub_term_goal_.shutdown();
  pubCBTimer_.stop();
  replanCBTimer_.stop();
}

void PantherRos::pubObstacles(mt::Edges edges_obstacles)
{
  if (edges_obstacles.size() > 0)
  {
    pub_obstacles_.publish(edges2Marker(edges_obstacles, color(RED_NORMAL)));
  }

  return;
}

void PantherRos::trajCB(const panther_msgs::DynTraj& msg)
{
  if (msg.id == id_)
  {  // This is my own trajectory
    return;
  }

  Eigen::Vector3d w_pos(msg.pos.x, msg.pos.y, msg.pos.z);  // position in world frame
  double dist = (state_.pos - w_pos).norm();

  bool can_use_its_info = (dist <= 4 * par_.Ra);  // See explanation of 4*Ra in Panther::updateTrajObstacles

  //////

  if (par_.impose_FOV_in_trajCB)
  {
    Eigen::Vector3d c_pos = (par_.c_T_b) * (w_T_b_.inverse()) * w_pos;  // position of the obstacle in the camera frame
                                                                        // (i.e., depth optical frame)
    bool inFOV =                                                        // check if it's inside the field of view.
        c_pos.z() < par_.fov_depth &&                                   //////////////////////
        fabs(atan2(c_pos.x(), c_pos.z())) <
            ((par_.fov_x_deg * M_PI / 180.0) / 2.0) &&  ///// Note that fov_x_deg means x camera_depth_optical_frame
        fabs(atan2(c_pos.y(), c_pos.z())) <
            ((par_.fov_y_deg * M_PI / 180.0) / 2.0);  ///// Note that fov_y_deg means x camera_depth_optical_frame

    // inFOV_old_ = inFOV;
    // inFOV_time_old_ = ros::Time::now().toSec();

    if (inFOV == false)
    {
      // std::cout << "B_pos= " << B_pos.transpose() << " is NOT inFOV" << std::endl;
      // std::cout << "w_pos= " << w_pos.transpose() << " is NOT inFOV" << std::endl;
      // std::cout << "[inFOV] w_T_b_= " << w_T_b_.matrix() << std::endl;
      return;
    }
    // else
    // {
    //   std::cout << "B_pos= " << B_pos.transpose() << " is inFOV!!!" << std::endl;
    // }
  }
  /////

  // if (can_use_its_info == false)
  // {
  //   return;
  // } //TODO: Commented (4-Feb-2021)

  mt::dynTraj tmp;
  tmp.use_pwp_field = msg.use_pwp_field;

  if (msg.use_pwp_field)
  {
    tmp.pwp_mean = pwpMsg2Pwp(msg.pwp_mean);
    tmp.pwp_var = pwpMsg2Pwp(msg.pwp_var);
  }
  else
  {
    tmp.s_mean.push_back(msg.s_mean[0]);
    tmp.s_mean.push_back(msg.s_mean[1]);
    tmp.s_mean.push_back(msg.s_mean[2]);

    tmp.s_var.push_back(msg.s_var[0]);
    tmp.s_var.push_back(msg.s_var[1]);
    tmp.s_var.push_back(msg.s_var[2]);
  }

  tmp.bbox << msg.bbox[0], msg.bbox[1], msg.bbox[2];
  tmp.id = msg.id;
  tmp.is_agent = msg.is_agent;
  tmp.time_received = ros::Time::now().toSec();

  panther_ptr_->updateTrajObstacles(tmp);
}

// This trajectory contains all the future trajectory (current_pos --> A --> final_point_of_traj), because it's the
// composition of pwp
void PantherRos::publishOwnTraj(const mt::PieceWisePol& pwp)
{
  panther_msgs::DynTraj msg;
  msg.use_pwp_field = true;
  msg.pwp_mean = pwp2PwpMsg(pwp);

  mt::state tmp;
  tmp.setZero();
  msg.pwp_var = pwp2PwpMsg(createPwpFromStaticPosition(tmp));  // zero variance

  // msg.function = s;
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.bbox.push_back(2 * par_.drone_radius);
  msg.pos.x = state_.pos.x();
  msg.pos.y = state_.pos.y();
  msg.pos.z = state_.pos.z();
  msg.id = id_;

  msg.is_agent = true;
  pub_traj_.publish(msg);
}

void PantherRos::replanCB(const ros::TimerEvent& e)
{
  if (ros::ok() && published_initial_position_ == true)
  {
    mt::Edges edges_obstacles;
    std::vector<mt::state> X_safe;

    std::vector<Hyperplane3D> planes;
    mt::PieceWisePol pwp;
    mt::log log;

    bool replanned =
        panther_ptr_->replan(edges_obstacles, X_safe, planes, num_of_LPs_run_, num_of_QCQPs_run_, pwp, log);

    if (log.drone_status != DroneStatus::GOAL_REACHED)  // log.replanning_was_needed
    {
      pub_log_.publish(log2LogMsg(log));
    }

    if (par_.visual)
    {
      // Delete markers to publish stuff
      visual_tools_->deleteAllMarkers();
      visual_tools_->enableBatchPublishing();

      pubObstacles(edges_obstacles);
      pubTraj(X_safe);
      publishPlanes(planes);
    }

    if (replanned)
    {
      publishOwnTraj(pwp);
      pwp_last_ = pwp;
    }
    else
    {
      int time_ms = int(ros::Time::now().toSec() * 1000);

      if (timer_stop_.elapsedSoFarMs() > 500.0)  // publish every half a second. TODO set as param
      {
        publishOwnTraj(pwp_last_);  // This is needed because is drone DRONE1 stops, it needs to keep publishing his
                                    // last planned trajectory, so that other drones can avoid it (even if DRONE1 was
                                    // very far from the other drones with it last successfully planned a trajectory).
                                    // Note that these trajectories are time-indexed, and the last position is taken if
                                    // t>times.back(). See eval() function in the pwp struct
        timer_stop_.reset();
      }
    }

    // std::cout << "[Callback] Leaving replanCB" << std::endl;
  }
}

void PantherRos::publishPlanes(std::vector<Hyperplane3D>& planes)
{
  auto color = visual_tools_->getRandColor();

  int i = 0;
  for (auto plane_i : planes)
  {
    if ((i % par_.num_seg) == 0)  // planes for a new obstacle --> new color
    {
      color = visual_tools_->getRandColor();  // rviz_visual_tools::TRANSLUCENT_LIGHT;
    }
    Eigen::Isometry3d pose;
    pose.translation() = plane_i.p_;

    // Calculate the rotation matrix from the original normal z_0 = (0,0,1) to new normal n = (A,B,C)
    Eigen::Vector3d z_0 = Eigen::Vector3d::UnitZ();
    Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(z_0, plane_i.n_);
    pose.linear() = q.toRotationMatrix();

    double height = 0.001;  // very thin
    double x_width = 2;     // very thin
    double y_width = 2;     // very thin
    visual_tools_->publishCuboid(pose, x_width, y_width, height, color);
    i++;

    /*    double d_i = -plane_i.n_.dot(plane_i.p_);
        std::cout << bold << "Publishing plane, d_i= " << d_i << reset << std::endl;
        visual_tools_->publishABCDPlane(plane_i.n_.x(), plane_i.n_.y(), plane_i.n_.z(), d_i, rvt::MAGENTA, 2, 2);*/
  }
  visual_tools_->trigger();
}

void PantherRos::publishPoly(const vec_E<Polyhedron<3>>& poly)
{
  // std::cout << "Going to publish= " << (poly[0].hyperplanes())[0].n_ << std::endl;
  decomp_ros_msgs::PolyhedronArray poly_msg = DecompROS::polyhedron_array_to_ros(poly);
  poly_msg.header.frame_id = world_name_;

  poly_safe_pub_.publish(poly_msg);
}

void PantherRos::stateCB(const snapstack_msgs::State& msg)
{
  mt::state state_tmp;
  state_tmp.setPos(msg.pos.x, msg.pos.y, msg.pos.z);
  state_tmp.setVel(msg.vel.x, msg.vel.y, msg.vel.z);
  state_tmp.setAccel(0.0, 0.0, 0.0);
  state_tmp.setYaw(0.0);  // This field is not used

  // double roll, pitch, yaw;
  // quaternion2Euler(msg.quat, roll, pitch, yaw);
  // state_tmp.setYaw(yaw); TODO: use here (for yaw) the convention PANTHER is using (psi)

  state_ = state_tmp;

  // std::cout << bold << yellow << "PANTHER_ROS state= " << reset;
  // state_tmp.print();
  panther_ptr_->updateState(state_tmp);

  w_T_b_ = Eigen::Translation3d(msg.pos.x, msg.pos.y, msg.pos.z) *
           Eigen::Quaterniond(msg.quat.w, msg.quat.x, msg.quat.y, msg.quat.z);

  if (published_initial_position_ == false)
  {
    pwp_last_ = createPwpFromStaticPosition(state_);
    publishOwnTraj(pwp_last_);
    published_initial_position_ = true;
  }
  if (panther_ptr_->IsTranslating() == true && par_.visual)
  {
    pubActualTraj();
  }
}

void PantherRos::whoPlansCB(const panther_msgs::WhoPlans& msg)
{
  if (msg.value != msg.PANTHER)
  {  // PANTHER does nothing
    sub_state_.shutdown();
    sub_term_goal_.shutdown();
    pubCBTimer_.stop();
    replanCBTimer_.stop();
    panther_ptr_->resetInitialization();
    std::cout << on_blue << "**************PANTHER STOPPED" << reset << std::endl;
  }
  else
  {  // PANTHER is the one who plans now (this happens when the take-off is finished)
    sub_term_goal_ = nh1_.subscribe("term_goal", 1, &PantherRos::terminalGoalCB, this);  // TODO: duplicated from above
    sub_state_ = nh1_.subscribe("state", 1, &PantherRos::stateCB, this);                 // TODO: duplicated from above
    pubCBTimer_.start();
    replanCBTimer_.start();
    std::cout << on_blue << "**************PANTHER STARTED" << reset << std::endl;
  }
}

void PantherRos::pubCB(const ros::TimerEvent& e)
{
  mt::state next_goal;
  if (panther_ptr_->getNextGoal(next_goal))
  {
    snapstack_msgs::Goal goal;

    goal.p = eigen2rosvector(next_goal.pos);
    goal.v = eigen2rosvector(next_goal.vel);
    goal.a = eigen2rosvector((par_.use_ff) * next_goal.accel);
    goal.j = eigen2rosvector((par_.use_ff) * next_goal.jerk);
    goal.dpsi = next_goal.dyaw;
    goal.psi = next_goal.yaw;
    goal.header.stamp = ros::Time::now();
    goal.header.frame_id = world_name_;
    goal.power = true;  // allow the outer loop to send low-level autopilot commands

    pub_goal_.publish(goal);

    setpoint_.header.stamp = ros::Time::now();
    setpoint_.pose.position.x = goal.p.x;
    setpoint_.pose.position.y = goal.p.y;
    setpoint_.pose.position.z = goal.p.z;

    pub_setpoint_.publish(setpoint_);
  }

  publishFOV();
}

void PantherRos::clearMarkerArray(visualization_msgs::MarkerArray* tmp, ros::Publisher* publisher)
{
  if ((*tmp).markers.size() == 0)
  {
    return;
  }
  int id_begin = (*tmp).markers[0].id;

  for (int i = 0; i < (*tmp).markers.size(); i++)
  {
    visualization_msgs::Marker m;
    m.header.frame_id = "world";
    m.header.stamp = ros::Time::now();
    m.type = visualization_msgs::Marker::ARROW;
    m.action = visualization_msgs::Marker::DELETE;
    m.id = i + id_begin;
    m.scale.x = 0.15;
    m.scale.y = 0;
    m.scale.z = 0;
    (*tmp).markers[i] = m;
  }

  (*publisher).publish(*tmp);
  (*tmp).markers.clear();
}

void PantherRos::pubTraj(const std::vector<mt::state>& data)
{
  // Trajectory
  nav_msgs::Path traj;
  traj.poses.clear();
  traj.header.stamp = ros::Time::now();
  traj.header.frame_id = world_name_;

  geometry_msgs::PoseStamped temp_path;

  int increm = (int)std::max(data.size() / par_.res_plot_traj, 1.0);  // this is to speed up rviz

  for (int i = 0; i < data.size(); i = i + increm)
  {
    temp_path.pose.position.x = data[i].pos(0);
    temp_path.pose.position.y = data[i].pos(1);
    temp_path.pose.position.z = data[i].pos(2);
    temp_path.pose.orientation.w = 1;
    temp_path.pose.orientation.x = 0;
    temp_path.pose.orientation.y = 0;
    temp_path.pose.orientation.z = 0;
    traj.poses.push_back(temp_path);
  }

  pub_traj_safe_.publish(traj);
  clearMarkerArray(&traj_safe_colored_, &pub_traj_safe_colored_);

  double scale = 0.15;

  traj_safe_colored_ = trajectory2ColoredMarkerArray(data, par_.v_max.maxCoeff(), increm, name_drone_, scale,
                                                     par_.color_type, id_, par_.n_agents);
  pub_traj_safe_colored_.publish(traj_safe_colored_);
}

void PantherRos::pubActualTraj()
{
  static geometry_msgs::Point p_last = pointOrigin();

  mt::state current_state;
  panther_ptr_->getState(current_state);
  Eigen::Vector3d act_pos = current_state.pos;

  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::ADD;
  m.id = actual_trajID_;  // % 3000;  // Start the id again after ___ points published (if not RVIZ goes very slow)
  m.ns = "ActualTraj_" + name_drone_;
  actual_trajID_++;
  // m.color = getColorJet(current_state.vel.norm(), 0, par_.v_max.maxCoeff());  // color(RED_NORMAL);

  if (par_.color_type == "vel")
  {
    m.color = getColorJet(current_state.vel.norm(), 0, par_.v_max.maxCoeff());  // note that par_.v_max is per axis!
  }
  else
  {
    m.color = getColorJet(id_, 0, par_.n_agents);  // note that par_.v_max is per axis!
  }

  m.scale.x = 0.15;
  m.scale.y = 0.0001;
  m.scale.z = 0.0001;
  m.header.stamp = ros::Time::now();
  m.header.frame_id = world_name_;

  // pose is actually not used in the marker, but if not RVIZ complains about the quaternion
  m.pose.position = pointOrigin();
  m.pose.orientation.x = 0.0;
  m.pose.orientation.y = 0.0;
  m.pose.orientation.z = 0.0;
  m.pose.orientation.w = 1.0;

  geometry_msgs::Point p;
  p = eigen2point(act_pos);
  m.points.push_back(p_last);
  m.points.push_back(p);
  p_last = p;

  if (m.id == 0)
  {
    return;
  }

  pub_actual_traj_.publish(m);
}

void PantherRos::clearMarkerActualTraj()
{
  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::DELETEALL;
  m.id = 0;
  m.scale.x = 0.02;
  m.scale.y = 0.04;
  m.scale.z = 1;
  pub_actual_traj_.publish(m);
  actual_trajID_ = 0;
}

void PantherRos::clearMarkerColoredTraj()
{
  visualization_msgs::Marker m;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::DELETEALL;
  m.id = 0;
  m.scale.x = 1;
  m.scale.y = 1;
  m.scale.z = 1;
  pub_actual_traj_.publish(m);
}

void PantherRos::pubState(const mt::state& data, const ros::Publisher pub)
{
  geometry_msgs::PointStamped p;
  p.header.frame_id = world_name_;
  p.point = eigen2point(data.pos);
  pub.publish(p);
}

void PantherRos::terminalGoalCB(const geometry_msgs::PoseStamped& msg)
{
  mt::state G_term;
  double z;
  if (fabs(msg.pose.position.z) < 1e-5)  // This happens when you click in RVIZ (msg.z is 0.0)
  {
    z = 1.0;
  }
  else  // This happens when you publish by yourself the goal (should always be above the ground)
  {
    z = msg.pose.position.z;
  }
  G_term.setPos(msg.pose.position.x, msg.pose.position.y, z);
  panther_ptr_->setTerminalGoal(G_term);

  mt::state G;  // projected goal
  panther_ptr_->getG(G);

  pubState(G_term, pub_point_G_term_);
  pubState(G, pub_point_G_);

  clearMarkerActualTraj();
}

void PantherRos::constructFOVMarker()
{
  marker_fov_.header.frame_id = name_camera_depth_optical_frame_tf_;  // name_drone_;
  marker_fov_.header.stamp = ros::Time::now();
  marker_fov_.ns = "marker_fov";
  marker_fov_.id = 0;
  marker_fov_.frame_locked = true;
  marker_fov_.type = marker_fov_.LINE_LIST;
  marker_fov_.action = marker_fov_.ADD;
  marker_fov_.pose = identityGeometryMsgsPose();

  double delta_y = par_.fov_depth * fabs(tan((par_.fov_x_deg * M_PI / 180) / 2.0));
  double delta_z = par_.fov_depth * fabs(tan((par_.fov_y_deg * M_PI / 180) / 2.0));

  geometry_msgs::Point v0 = eigen2point(Eigen::Vector3d(0.0, 0.0, 0.0));
  geometry_msgs::Point v1 = eigen2point(Eigen::Vector3d(-delta_y, delta_z, par_.fov_depth));
  geometry_msgs::Point v2 = eigen2point(Eigen::Vector3d(delta_y, delta_z, par_.fov_depth));
  geometry_msgs::Point v3 = eigen2point(Eigen::Vector3d(delta_y, -delta_z, par_.fov_depth));
  geometry_msgs::Point v4 = eigen2point(Eigen::Vector3d(-delta_y, -delta_z, par_.fov_depth));

  // geometry_msgs::Point v1 = eigen2point(Eigen::Vector3d(par_.fov_depth, delta_y, -delta_z));
  // geometry_msgs::Point v2 = eigen2point(Eigen::Vector3d(par_.fov_depth, -delta_y, -delta_z));
  // geometry_msgs::Point v3 = eigen2point(Eigen::Vector3d(par_.fov_depth, -delta_y, delta_z));
  // geometry_msgs::Point v4 = eigen2point(Eigen::Vector3d(par_.fov_depth, delta_y, delta_z));

  marker_fov_.points.clear();

  // Line
  marker_fov_.points.push_back(v0);
  marker_fov_.points.push_back(v1);

  // Line
  marker_fov_.points.push_back(v0);
  marker_fov_.points.push_back(v2);

  // Line
  marker_fov_.points.push_back(v0);
  marker_fov_.points.push_back(v3);

  // Line
  marker_fov_.points.push_back(v0);
  marker_fov_.points.push_back(v4);

  // Line
  marker_fov_.points.push_back(v1);
  marker_fov_.points.push_back(v2);

  // Line
  marker_fov_.points.push_back(v2);
  marker_fov_.points.push_back(v3);

  // Line
  marker_fov_.points.push_back(v3);
  marker_fov_.points.push_back(v4);

  // Line
  marker_fov_.points.push_back(v4);
  marker_fov_.points.push_back(v1);

  marker_fov_.scale.x = 0.03;
  marker_fov_.scale.y = 0.00001;
  marker_fov_.scale.z = 0.00001;
  marker_fov_.color.a = 1.0;
  marker_fov_.color.r = 0.0;
  marker_fov_.color.g = 1.0;
  marker_fov_.color.b = 0.0;
}

void PantherRos::publishFOV()
{
  marker_fov_.header.stamp = ros::Time::now();
  pub_fov_.publish(marker_fov_);
  return;
}


#include <boost/test/unit_test.hpp>
#include "../../src/SpaceTime.hpp"
#include "../../src/solvers/csp/Types.hpp"
#include "../../src/solvers/csp/propagators/IsPath.hpp"
#include "../../src/solvers/csp/propagators/IsValidTransportEdge.cpp"
#include "../../src/solvers/csp/utils/Formatter.hpp"
#include <gecode/search.hh>
#include <gecode/minimodel.hh>

using namespace templ::solvers::csp;

class TestValidTransportEdge : public Gecode::Space
{
    uint32_t mNumberOfTimepoints;
    uint32_t mNumberOfFluents;

    uint32_t mNumberOfVertices;
    uint32_t mNumberOfTimelines;

    std::vector<int> mSupplyDemand;
    templ::solvers::csp::ListOfAdjacencyLists mTimelines;

public:
    TestValidTransportEdge(size_t numberOfTimepoints, size_t numberOfFluents, const std::vector<int>& supplyDemand)
        : Gecode::Space()
        , mNumberOfTimepoints(numberOfTimepoints)
        , mNumberOfFluents(numberOfFluents)
        , mNumberOfVertices(numberOfTimepoints*numberOfFluents)
        , mNumberOfTimelines(supplyDemand.size())
        , mSupplyDemand(supplyDemand)
    {
        for(size_t t = 0; t < mNumberOfTimelines; ++t)
        {
            unsigned int minCard = 0;
            unsigned int maxCard = 1;
            Gecode::SetVarArray timeline(*this, mNumberOfVertices, Gecode::IntSet::empty, Gecode::IntSet(0,mNumberOfVertices-1), minCard, maxCard);
            mTimelines.push_back(timeline);

            std::stringstream ss;
            ss << t;

            // get full path length
            templ::solvers::csp::propagators::isPath(*this, timeline, ss.str(), numberOfTimepoints, numberOfFluents, numberOfTimepoints-1);
        }

        for(size_t t = 0; t < mNumberOfVertices; ++t)
        {
            Gecode::SetVarArray multiEdge(*this, mSupplyDemand.size());
            for(size_t i = 0; i < mSupplyDemand.size(); ++i)
            {
                multiEdge[i] = mTimelines[i][t];
            }
            propagators::isValidTransportEdge(*this, multiEdge, supplyDemand, t/mNumberOfFluents, t%mNumberOfFluents, mNumberOfFluents);

            Gecode::SetAFC afc(*this, multiEdge, 0.99);
            afc.decay(*this, 0.95);
            branch(*this, multiEdge, Gecode::SET_VAR_AFC_MIN(afc), Gecode::SET_VAL_MIN_INC());
        }

        //for(size_t t = 0; t < mNumberOfTimelines; ++t)
        //{
        //    Gecode::SetAFC afc(*this, mTimelines[t], 0.99);
        //    afc.decay(*this, 0.95);
        //    branch(*this, mTimelines[t], Gecode::SET_VAR_AFC_MIN(afc), Gecode::SET_VAL_MIN_INC());
        //}

    }

    TestValidTransportEdge(TestValidTransportEdge& other)
        : Gecode::Space(other)
        , mNumberOfTimepoints(other.mNumberOfTimepoints)
        , mNumberOfFluents(other.mNumberOfFluents)
        , mNumberOfVertices(other.mNumberOfVertices)
        , mNumberOfTimelines(other.mNumberOfTimelines)
        , mSupplyDemand(other.mSupplyDemand)
    {
        for(size_t i = 0; i < other.mTimelines.size(); ++i)
        {
            AdjacencyList array;
            mTimelines.push_back(array);
            mTimelines[i].update(*this, other.mTimelines[i]);
        }
    }

    virtual Gecode::Space* copy()
    {
        return new TestValidTransportEdge(*this);
    }

    std::string toString() const
    {
        std::stringstream ss;
        ss << "Solution (" << mNumberOfTimelines << ")" << std::endl;
        for(size_t i = 0; i < mNumberOfTimelines; ++i)
        {
            ss << "Timeline (" << i << ")" << std::endl;
            ss << utils::Formatter::toString(mTimelines[i], mNumberOfFluents);
            ss << std::endl;
        }
        return ss.str();
    }
};

void run_test(uint32_t numberOfTimepoints, uint32_t numberOfFluents, std::vector<int> supplyDemand, const std::string& tag)
{
    TestValidTransportEdge* testSpace = new TestValidTransportEdge(numberOfTimepoints, numberOfFluents, supplyDemand);
    Gecode::BAB<TestValidTransportEdge> searchEngine(testSpace);
    TestValidTransportEdge* best = NULL;
    TestValidTransportEdge* current = NULL;
    BOOST_TEST_MESSAGE("Run search");
    while((current = searchEngine.next()))
    {
        delete best;
        best = current;

        try {
            BOOST_REQUIRE_MESSAGE(true, "Found solution for " << tag << " -- " << best->toString());
            break;
        } catch(const std::exception& e)
        {
            BOOST_REQUIRE_MESSAGE(false, "\n" << best->toString() << "\n" << tag << " Solution not valid: " << e.what());
        }
    }
    if(!current)
    {
        BOOST_REQUIRE_MESSAGE(false, "\n No solution found for " << tag );
    }
    if(best == NULL)
    {
        delete current;
    }

    std::cout << "Statistics: " << std::endl;
    std::cout << "    propagate " << searchEngine.statistics().propagate << std::endl;

    std::cout << "    fail " << searchEngine.statistics().fail << std::endl;
    std::cout << "    node " << searchEngine.statistics().node << std::endl;
    std::cout << "    depth" << searchEngine.statistics().depth << std::endl;
    std::cout << "    restart " << searchEngine.statistics().restart << std::endl;
    std::cout << "    nogoods " << searchEngine.statistics().nogood << std::endl;

}

BOOST_AUTO_TEST_SUITE(propagators_is_valid_transport_edge)

BOOST_AUTO_TEST_CASE(is_valid_timeline)
{
    std::vector<int> supplyDemand;
    for(int i = 0; i < 2; ++i)
    {
        supplyDemand.push_back(10);
        supplyDemand.push_back(-1);
        supplyDemand.push_back(-1);
    }

    run_test(5,5, supplyDemand, "5x5");
}

BOOST_AUTO_TEST_CASE(is_valid_timeline_25_x_25)
{
    std::vector<int> supplyDemand;
    for(int i = 0; i < 2; ++i)
    {
        supplyDemand.push_back(10);
        supplyDemand.push_back(-1);
        supplyDemand.push_back(-1);
    }

    run_test(25,25, supplyDemand, "5x5");
}

BOOST_AUTO_TEST_CASE(is_valid_timeline_50_x_50)
{
    std::vector<int> supplyDemand;
    for(int i = 0; i < 2; ++i)
    {
        supplyDemand.push_back(10);
        supplyDemand.push_back(-1);
        supplyDemand.push_back(-1);
    }

    run_test(25,25, supplyDemand, "5x5");
}

BOOST_AUTO_TEST_CASE(is_valid_timeline_50_x_50_x_5)
{
    std::vector<int> supplyDemand;
    for(int i = 0; i < 5; ++i)
    {
        supplyDemand.push_back(10);
        supplyDemand.push_back(-1);
        supplyDemand.push_back(-1);
    }

    run_test(25,25, supplyDemand, "5x5");
}

BOOST_AUTO_TEST_SUITE_END()


#include <string>
#include <boost/python/numpy.hpp>
#include <file/CELFileData.h>

namespace py = boost::python;
namespace np = boost::python::numpy;

namespace {
    np::ndarray read_cel (std::string const &path) {
        affxcel::CCELFileData cel;
        cel.SetFileName(path.c_str());
        cel.Read();
        int cols = cel.GetCols();
        int rows = cel.GetRows();
        np::ndarray array = np::zeros(py::make_tuple(rows, cols), np::dtype::get_builtin<float>());
        float *ptr = (float *)array.get_data();
        affxcel::CELFileEntryType e;
        for (int i = 0; i < rows; ++i) {
            for (int j = 0; j < cols; ++j) {
                cel.GetEntry(j, i, e);
                *ptr = e.Intensity;
                ++ptr;
            }
        }
        return array;
    }
}

BOOST_PYTHON_MODULE(cLatte) {
    np::initialize();
    def("read_cel", ::read_cel);
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file steiner_strategy.hpp
 * @brief
 * @author Maciej Andrejczuk
 * @version 1.0
 * @date 2013-08-01
 */
#ifndef PAAL_STEINER_STRATEGY_HPP
#define PAAL_STEINER_STRATEGY_HPP

#include "paal/data_structures/bimap.hpp"
#include "paal/data_structures/subset_iterator.hpp"
#include "paal/iterative_rounding/steiner_tree/steiner_components.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/algorithm_ext/erase.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm/unique.hpp>
#include <boost/random/discrete_distribution.hpp>

#include <random>
#include <unordered_set>
#include <vector>

namespace paal {
namespace ir {

/**
 * Generates all the components possible.
 * It iterates over all subsets of terminals with no more than K elements.
 */
class steiner_tree_all_generator {
private:
    template<typename Metric>
    using MTraits = typename data_structures::metric_traits<Metric>;

public:
    /// Constructor.
    steiner_tree_all_generator(int K = 4) : m_component_max_size(K) {}

    /// Generates all possible components.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename MTraits<Metric>::VertexType,
                typename MTraits<Metric>::DistanceType>& components) {

        using Vertex = typename MTraits<Metric>::VertexType;
        using Dist = typename MTraits<Metric>::DistanceType;
        std::vector<Vertex> current_terminals;
        gen_all_components<Vertex, Dist>(components, 0, terminals.size(),
            current_terminals, cost_map, terminals, steiner_vertices);
    }
private:
    template<typename Vertex, typename Dist, typename Metric, typename Terminals>
    void gen_all_components(steiner_components<Vertex, Dist>& components,
            int first_avail, int last, std::vector<Vertex>& curr,
            const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices) {

        if (curr.size() > 1) {
            steiner_component<Vertex, Dist> c(cost_map, curr, steiner_vertices);
            components.add(std::move(c));
        }
        if ((int) curr.size() >= m_component_max_size)
            return;
        for (int i = first_avail; i < last; ++i) {
            curr.push_back(terminals[i]);
            gen_all_components(components, i + 1, last, curr, cost_map,
                terminals, steiner_vertices);
            curr.pop_back();
        }
    }

    int m_component_max_size;
};

namespace detail {

template<typename Vertex>
struct vertex_filter {
    bool operator()(Vertex v) const { return vertices.find(v) == vertices.end(); }
    std::unordered_set<Vertex> vertices;
};

}// detail

/**
 * Generates all the components possible based on the underlying graph.
 * It iterates over all subsets of terminals with no more than K elements.
 */
template<typename Graph, typename Vertex, typename Terminals>
class steiner_tree_graph_all_generator {
private:
    template<typename Metric>
    using MTraits = typename data_structures::metric_traits<Metric>;

public:
    /// Constructor.
    steiner_tree_graph_all_generator(const Graph& graph,
        const Terminals& terminals, int K = 4) : m_component_max_size(K),
            m_index(terminals),
            m_terminals_graph(m_index.size()) {
        initialize_terminals_graph(graph, terminals);
    }

    /// Generates all possible components.
    template<typename Metric>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename MTraits<Metric>::VertexType,
                typename MTraits<Metric>::DistanceType>& components) {

        using Dist = typename MTraits<Metric>::DistanceType;
        merge_vertices<Dist>(cost_map);
        std::vector<Vertex> current_terminals;
        gen_all_components<Dist>(components, 0, terminals.size(),
            current_terminals, cost_map, terminals, steiner_vertices);
    }

private:
    using AuxGraph = boost::adjacency_list<boost::setS, boost::vecS, boost::undirectedS>;
    using VertexIndex = data_structures::bimap<Vertex>;

    int m_component_max_size;
    VertexIndex m_index;
    AuxGraph m_terminals_graph;

    void initialize_terminals_graph(const Graph& graph, const Terminals& terminals) {
        detail::vertex_filter<Vertex> filter;
        filter.vertices.insert(terminals.begin(), terminals.end());
        auto index = get(boost::vertex_index, graph);
        std::vector<int> components(num_vertices(graph));
        for (auto u : terminals) {
            for (auto v : terminals) {
                if (u == v) continue;
                filter.vertices.erase(u);
                filter.vertices.erase(v);
                boost::filtered_graph<Graph, boost::keep_all, decltype(filter)>
                    fg(graph, boost::keep_all{}, filter);
                boost::connected_components(fg, &components[0]);
                if (components[index[u]] == components[index[v]]) {
                    add_edge(get_idx(u), get_idx(v), m_terminals_graph);
                }
                filter.vertices.insert(u);
                filter.vertices.insert(v);
            }
        }
    }

    template<typename Dist, typename Metric>
    void merge_vertices(const Metric& cost_map) {
        auto range = vertices(m_terminals_graph);
        for (auto v_pair : data_structures::make_subsets_iterator_range<2>(
                range.first, range.second)) {
            auto i = std::get<0>(v_pair);
            auto j = std::get<1>(v_pair);
            if (cost_map(get_val(i), get_val(j)) == Dist{}) {
                merge_vertices(i, j);
                merge_vertices(j, i);
            }
        }
    }

    void merge_vertices(Vertex trg, Vertex src) {
        for (auto v : boost::as_array(adjacent_vertices(src,
                m_terminals_graph))) {
            if (trg != static_cast<Vertex>(v)) {
                add_edge(trg, v, m_terminals_graph);
            }
        }
    }

    auto get_idx(Vertex v) const -> decltype(m_index.get_idx(v)) {
        return m_index.get_idx(v);
    }

    auto get_val(int idx) const -> decltype(m_index.get_val(idx)) {
        return m_index.get_val(idx);
    }

    bool is_graph_component(const std::vector<Vertex>& comp) const {
        for (auto term_pair : data_structures::make_subsets_iterator_range<2>(
                comp.begin(), comp.end())) {
            if (!edge(
                    get_idx(std::get<0>(term_pair)),
                    get_idx(std::get<1>(term_pair)),
                    m_terminals_graph).second) {
                return false;
            }
        }
        return true;
    }

    template<typename Dist, typename Metric>
    void gen_all_components(steiner_components<Vertex, Dist>& components,
            int first_avail, int last, std::vector<Vertex>& curr,
            const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices) {

        // TODO implement a subset_iterator with K passed as a parameter
        // not as a template parameter (also in the gen_all_components method
        // in steiner_tree_all_generator)
        if (curr.size() > 1) {
            if (!is_graph_component(curr))
                return;
            steiner_component<Vertex, Dist> c(cost_map, curr, steiner_vertices);
            components.add(std::move(c));
        }
        if ((int) curr.size() >= m_component_max_size)
            return;
        for (int i = first_avail; i < last; ++i) {
            curr.push_back(terminals[i]);
            gen_all_components(components, i + 1, last, curr, cost_map,
                terminals, steiner_vertices);
            curr.pop_back();
        }
    }
};

/**
 * Makes a graph_all_generator object.
 */
template<typename Vertex, typename Graph, typename Terminals>
steiner_tree_graph_all_generator<Graph, Vertex, Terminals>
make_steiner_tree_graph_all_generator(const Graph& graph,
        const Terminals& terminals, int K = 4) {
    return steiner_tree_graph_all_generator<Graph, Vertex, Terminals>(
        graph, terminals, K);
}

/**
 * Generates specified number of components by selecting random elements.
 */
class steiner_tree_random_generator {
public:
    /// Constructor.
    steiner_tree_random_generator(int N = 100, int K = 3) :
            m_iterations(N), m_component_max_size(K) {
    }

    /// Generates a specified number of components by selecting random elements.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals & terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                typename data_structures::metric_traits<Metric>::VertexType,
                typename data_structures::metric_traits<Metric>::DistanceType>& components) {

        using Vertex = typename data_structures::metric_traits<Metric>::VertexType;
        using Dist = typename data_structures::metric_traits<Metric>::DistanceType;
        if (terminals.size() < 2) {
            return;
        }
        for (int i = 0; i < m_iterations; ++i) {
            std::set<Vertex> curr;
            while ((int)curr.size() < m_component_max_size) {
                if (curr.size() > 1) {
                    int c =
                        (int)rand() %
                        m_component_max_size; // TODO: Is this fair probability?
                    if (c == 0) {
                        break;
                    }
                }
                int r = (int)rand() % terminals.size();
                curr.insert(terminals[r]);
            }
            std::vector<Vertex> elements(curr.begin(), curr.end());
            steiner_component<Vertex, Dist> c(cost_map, elements, steiner_vertices);
            components.add(std::move(c));
        }
        // TODO some terminals may not be in any component
    }

  private:
    int m_iterations;
    int m_component_max_size;
};

/**
 * Generates specified number of components by randomly selecting elements
 * with probability dependent on distance from vertices already selected.
 */
class steiner_tree_smart_generator {
    std::default_random_engine m_rng;
public:
    /// Constructor.
    steiner_tree_smart_generator(int N = 100, int K = 3, std::default_random_engine rng = std::default_random_engine{}) :
            m_iterations(N), m_component_max_size(K) {
    }

    /// Generates components.
    template<typename Metric, typename Terminals>
    void gen_components(const Metric& cost_map, const Terminals& terminals,
            const Terminals& steiner_vertices,
            steiner_components<
                    typename data_structures::metric_traits<Metric>::VertexType,
                    typename data_structures::metric_traits<Metric>::DistanceType>& components) {

        using Vertex = typename data_structures::metric_traits<Metric>::VertexType;
        using Dist = typename data_structures::metric_traits<Metric>::DistanceType;
        std::vector<Vertex> elements;
        std::vector<double> prob;
        for (Vertex start : terminals) {
            for (int i = 0; i < m_iterations; ++i) {
                elements.clear();
                elements.push_back(start);
                int limit = 2 + rand() % (m_component_max_size - 1);
                while ((int)elements.size() < limit) {
                    prob.resize(terminals.size());
                    for (int k = 0; k < (int)prob.size(); ++k) {
                        for (auto e : elements) {
                            if (e == terminals[k]) {
                                prob[k] = 0;
                                break;
                            }
                            int cost = cost_map(e, terminals[k]);
                            assert(cost > 0);
                            assign_max(prob[k], 1. / cost);
                        }
                    }
                    auto selected = boost::random::discrete_distribution<std::size_t>(prob)(m_rng);
                    if (selected == prob.size()) break;
                    elements.push_back(terminals[selected]);
                }
                boost::erase(elements, boost::unique<boost::return_found_end>(boost::sort(elements)));

                steiner_component<Vertex, Dist> c(cost_map, elements, steiner_vertices);
                components.add(std::move(c));
            }
        }
    }

  private:
    int m_iterations;
    int m_component_max_size;
};

} // ir
} // paal

#endif // PAAL_STEINER_STRATEGY_HPP


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// InnerExplicit.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file InnerExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades inner products of project_namespace.

#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

#include <Eigen/Core>

#include "project_namespace/Mvec.hpp"
#include "project_namespace/Inner.hpp"
#include "project_namespace/Constants.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace {
    template<typename T> class Mvec;

    project_explicit_inner_functions
    project_explicit_inner_pointer_functions

}/// End of Namespace

#endif // project_inclusion_guard


#include <cstdio>
#include <iostream>
#include <armadillo>
#include "../timer.h"

int main() {

    const size_t dim_r = 10;
    const size_t dim_c = 3;
    const size_t n_elem = dim_r * dim_c;

    const uint64 n_iter = 1e8;

    arma::sword cv[n_elem];
    for (size_t i = 0; i < n_elem; i++)
        cv[i] = i + 1;

    arma::imat m(cv, dim_r, dim_c);
    m = m.t();
    // m.print("m");
    m.save("arma_sword_mat.dat", arma::arma_ascii);

    uint64 start;
    uint64 stop;
    uint64 elapsed;
    uint64 per;

    arma::ivec v3;
    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        v3 = vectorise(m);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    // v3.print("v");
    std::cout << "v3 (vectorise, declare outside): " << elapsed << std::endl;
    v3.save("arma_sword_vec.dat", arma::arma_ascii);

    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        arma::ivec v1 = vectorise(m);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    std::cout << "v1 (vectorise, declare inside): " << elapsed << std::endl;
    // bad scope
    // v1.print("v1");

    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        const arma::ivec v2 = vectorise(m);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    std::cout << "v2 (vectorise, declare inside const): " << elapsed << std::endl;

    arma::ivec v4;
    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        v4 = arma::ivec(m.memptr(), n_elem, false, true);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    // v4.print("v");
    std::cout << "v4: " << elapsed << std::endl;

    arma::ivec v5;
    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        v5 = arma::ivec(m.memptr(), n_elem, false, false);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    // v5.print("v");
    std::cout << "v5: " << elapsed << std::endl;

    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        const arma::ivec v6(m.memptr(), n_elem, false, false);
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    // bad scope
    // v6.print("v");
    std::cout << "v6: " << elapsed << std::endl;

    arma::ivec v7(n_elem);
    start = GetTimeMs64();
    for (uint64 i = 0; i < n_iter; i++) {
        memcpy(v7.memptr(), m.memptr(), n_elem * sizeof(double));
    }
    stop = GetTimeMs64();
    elapsed = stop - start;
    per = elapsed / n_iter;
    // v7.print("v");
    std::cout << "v7 (memcpy): " << elapsed << std::endl;

    return 0;
}


#include <mesh_array.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(MESH_ARRAY);

BOOST_AUTO_TEST_CASE(triangle_ciculator)
{
  typedef mesh_array::T3Mesh                        mesh_type;
  typedef mesh_array::VertexRing< mesh_type >       vertex_ring_type;
  typedef mesh_array::TriangleCirculator<mesh_type> circulator_type;

  // Empty mesh case
  {
    mesh_type M;

    vertex_ring_type ring_info( M );

    circulator_type C(ring_info, 0);

    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
  }

  // Non empty mesh case
  {
    mesh_type M;

    M.set_capacity(9,7);

    /**

     0   1   2
     *---*---*
     |\ / \ /
     | *3--*4  *5
     |/ \ / \
     *---*---*
     6   7   8
     */

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_triangle( 0, 6, 3);
    M.push_triangle( 0, 3, 1);
    M.push_triangle( 1, 3, 4);
    M.push_triangle( 1, 4, 2);
    M.push_triangle( 3, 6, 7);
    M.push_triangle( 3, 7, 4);
    M.push_triangle( 4, 7, 8);

    vertex_ring_type ring_info( M );

    // Vertex with 2 triangles
    circulator_type C(ring_info, 0);
    BOOST_CHECK_EQUAL( C(), true );
    BOOST_CHECK_EQUAL( C->idx(), 0);
    ++C;
    BOOST_CHECK_EQUAL( C(), true );
    BOOST_CHECK_EQUAL( C->idx(), 1);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);

    // Vertex with 0 triangles
    C =  circulator_type(ring_info, 5);
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());

    // Vertex with 1 triangle
    C =  circulator_type(ring_info, 2);
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 3);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);

    // Vertex with 4 triangles
    C =  circulator_type(ring_info, 4);
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 2);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 3);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 5);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 6);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
  }
}

BOOST_AUTO_TEST_CASE(tetrahedron_ciculator)
{
  typedef mesh_array::T4Mesh                           mesh_type;
  typedef mesh_array::VertexRing< mesh_type >          vertex_ring_type;
  typedef mesh_array::TetrahedronCirculator<mesh_type> circulator_type;

  // Empty mesh case
  {
    mesh_type M;

    vertex_ring_type ring_info( M );

    circulator_type C(ring_info, 0);

    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
  }

  // Non empty mesh case
  {
    mesh_type M;

    M.set_capacity(10,7);

    /**

     0   1   2
     *---*---*
     |\ / \ /
     | *3--*4  *5
     |/ \ / \
     *---*---*
     6   7   8
     */

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_tetrahedron( 0, 6, 3, 9);
    M.push_tetrahedron( 0, 3, 1, 9);
    M.push_tetrahedron( 1, 3, 4, 9);
    M.push_tetrahedron( 1, 4, 2, 9);
    M.push_tetrahedron( 3, 6, 7, 9);
    M.push_tetrahedron( 3, 7, 4, 9);
    M.push_tetrahedron( 4, 7, 8, 9);

    vertex_ring_type ring_info( M );

    // Vertex with 2 triangles
    circulator_type C(ring_info, 0);
    BOOST_CHECK_EQUAL( C(), true );
    BOOST_CHECK_EQUAL( C->idx(), 0);
    ++C;
    BOOST_CHECK_EQUAL( C(), true );
    BOOST_CHECK_EQUAL( C->idx(), 1);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);

    // Vertex with 0 triangles
    C =  circulator_type(ring_info, 5);
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());
    ++C;
    BOOST_CHECK_EQUAL( C() , false);
    BOOST_CHECK_EQUAL( C->idx(), mesh_array::UNASSIGNED());

    // Vertex with 1 triangle
    C =  circulator_type(ring_info, 2);
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 3);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);

    // Vertex with 4 triangles
    C =  circulator_type(ring_info, 4);
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 2);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 3);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 5);
    ++C;
    BOOST_CHECK_EQUAL( C() , true);
    BOOST_CHECK_EQUAL( C->idx(), 6);
    ++C;
    BOOST_CHECK_EQUAL( C() , false);

  }
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * Copyright (c) 2017-present, Facebook, Inc. and its affiliates.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */
#include <chrono>

#include <boost/any.hpp>
#include <boost/program_options.hpp>
#include <gtest/gtest.h>

#include "logdevice/common/commandline_util_chrono.h"

namespace facebook { namespace logdevice {

typedef chrono_interval_t<std::chrono::milliseconds> MsInterval;

template <class DurationType>
static DurationType parse(const std::string& str) {
  boost::any any;
  validate(any, std::vector<std::string>{str}, (DurationType*)nullptr, 0);
  return boost::any_cast<DurationType>(any);
}

// "External constructor" for intervals. Needed because EXPECT_EQ macro does
// not mix with universal initialization syntax.
template <typename D1, typename D2>
static inline MsInterval makeMsInterval(D1 lo, D2 hi) noexcept {
  return MsInterval{lo, hi};
}

// Equality operator for use by EXPECT_EQ
template <typename D1, typename D2>
static inline bool operator==(const chrono_interval_t<D1>& left,
                              const chrono_interval_t<D2>& right) noexcept {
  return (left.lo == right.lo) && (left.hi == right.hi);
}

// Equality operator for use by EXPECT_EQ
template <typename D1, typename D2>
static inline bool operator==(const chrono_expbackoff_t<D1>& left,
                              const chrono_expbackoff_t<D2>& right) noexcept {
  return (left.initial_delay == right.initial_delay) &&
      (left.max_delay == right.max_delay) &&
      (left.multiplier == right.multiplier);
}

TEST(CommandLineUtilTest, ValidDurations) {
  using namespace std::chrono;

  // Test all the basic types
  EXPECT_EQ(nanoseconds(1), parse<nanoseconds>("1ns"));
  EXPECT_EQ(microseconds(2), parse<microseconds>("2us"));
  EXPECT_EQ(milliseconds(3), parse<milliseconds>("3ms"));
  EXPECT_EQ(seconds(4), parse<seconds>("4s"));
  EXPECT_EQ(minutes(5), parse<minutes>("5min"));
  EXPECT_EQ(hours(6), parse<seconds>("6h"));

  // Conversions
  EXPECT_EQ(milliseconds(1000), parse<milliseconds>("1s"));
  EXPECT_EQ(seconds(1), parse<seconds>("1000ms"));

  // Days don't have a type
  EXPECT_EQ(hours(24), parse<hours>("1day"));
  EXPECT_EQ(hours(48), parse<hours>("2days"));

  // Fractionals
  EXPECT_EQ(milliseconds(100), parse<milliseconds>("0.1s"));
  EXPECT_EQ(milliseconds(1500), parse<milliseconds>("1.5s"));
  EXPECT_EQ(microseconds(100), parse<microseconds>("0.1ms"));
  EXPECT_EQ(hours(36), parse<hours>("1.5days"));

  // Spaces are fine
  EXPECT_EQ(milliseconds(1), parse<milliseconds>("1  ms"));
  EXPECT_EQ(hours(48), parse<hours>("2 days"));

  // Zero should parse without a suffix
  EXPECT_EQ(seconds(0), parse<seconds>("0"));
}

TEST(CommandLineUtilTest, InvalidDurations) {
  using namespace std::chrono;
  using namespace boost::program_options;
  ASSERT_THROW(parse<seconds>(""), validation_error);
  ASSERT_THROW(parse<seconds>(" "), validation_error);
  ASSERT_THROW(parse<seconds>("a"), validation_error);
  ASSERT_THROW(parse<seconds>("a "), validation_error);
  ASSERT_THROW(parse<seconds>("1"), validation_error);
  ASSERT_THROW(parse<seconds>("0.1"), validation_error);
  ASSERT_THROW(parse<seconds>("0.1a"), validation_error);
  ASSERT_THROW(parse<seconds>("ms"), validation_error);
  ASSERT_THROW(parse<seconds>(" ms"), validation_error);
  ASSERT_THROW(parse<seconds>("- ms"), validation_error);
  ASSERT_THROW(parse<seconds>("1 msabcdefghijklmnopqrstuv"), validation_error);

  // This is the tricky case where "100 ms" gets truncated to zero seconds.
  ASSERT_THROW(parse<seconds>("100 ms"), error);
}

TEST(CommandLineUtilTest, Intervals) {
  using namespace std::chrono;
  using namespace boost::program_options;

  // Basic
  EXPECT_EQ(makeMsInterval(milliseconds(100), milliseconds(200)),
            parse<MsInterval>("100ms..200ms"));
  EXPECT_EQ(makeMsInterval(hours(1), hours(72)), parse<MsInterval>("1h..3d"));

  // Fractionals
  EXPECT_EQ(makeMsInterval(milliseconds(100), milliseconds(1500)),
            parse<MsInterval>("0.1s..1.5s"));

  // Spaces are fine
  EXPECT_EQ(makeMsInterval(milliseconds(1), hours(48)),
            parse<MsInterval>("1  ms..2 days"));

  // Invalid
  ASSERT_THROW(parse<MsInterval>(""), validation_error);
  ASSERT_THROW(parse<MsInterval>(" "), validation_error);
  ASSERT_THROW(parse<MsInterval>(".."), validation_error);
  ASSERT_THROW(parse<MsInterval>("1ms.."), validation_error);
  ASSERT_THROW(parse<MsInterval>("..1ms"), validation_error);
  ASSERT_THROW(parse<MsInterval>("1ms..x"), validation_error);
  ASSERT_THROW(parse<MsInterval>("x..1ms"), validation_error);
  ASSERT_THROW(parse<MsInterval>("1ms..100ns"), error);
}

TEST(CommandLineUtilTest, ExpBackoffs) {
  using namespace std::chrono;
  using namespace boost::program_options;
  typedef chrono_expbackoff_t<milliseconds> ExpBackoff;

  // Basic
  EXPECT_EQ(ExpBackoff(milliseconds(100), milliseconds(200)),
            parse<ExpBackoff>("100ms..200ms"));
  EXPECT_EQ(ExpBackoff(hours(1), hours(72)), parse<ExpBackoff>("1h..3d"));
  EXPECT_EQ(ExpBackoff(milliseconds(100), milliseconds(200), 3),
            parse<ExpBackoff>("100ms..200ms-3x"));

  // Spaces are fine
  EXPECT_EQ(ExpBackoff(milliseconds(1), hours(48)),
            parse<ExpBackoff>("1  ms..2 days"));
  EXPECT_EQ(ExpBackoff(milliseconds(1), hours(48), 3),
            parse<ExpBackoff>("1  ms..2 days-3x"));

  // Invalid
  ASSERT_THROW(parse<ExpBackoff>(""), validation_error);
  ASSERT_THROW(parse<ExpBackoff>(" "), validation_error);
  ASSERT_THROW(parse<ExpBackoff>(".."), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms.."), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("..1ms"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..x"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("x..1ms"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("x..1ms"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("-"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("..-"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..-3x"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("..1ms-3x"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms-"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms-3"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms-3y"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms-3xy"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms-ax"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..2ms--3x"), validation_error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..100ns"), error);
  ASSERT_THROW(parse<ExpBackoff>("1ms..100ns-3x"), error);
}

}} // namespace facebook::logdevice


#include <boost/test/unit_test.hpp>

#include "math/point.h" // header to test

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_point)

BOOST_AUTO_TEST_CASE(point_ctor) {
  math::point p;
  BOOST_CHECK(p[0] == 0.0);
  BOOST_CHECK(p[1] == 0.0);
  BOOST_CHECK(p[2] == 0.0);
  BOOST_CHECK(p == p);

  p = math::point({1.0, 2.0, 3.0});
  BOOST_CHECK(p[0] == 1.0);
  BOOST_CHECK(p[1] == 2.0);
  BOOST_CHECK(p[2] == 3.0);
  BOOST_CHECK(p == p);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by lukas on 15.09.19.
//

#ifndef PROGRAM_DEV_DAT_BOOST_INTEGRATION_HPP
#define PROGRAM_DEV_DAT_BOOST_INTEGRATION_HPP

#include <boost/numeric/odeint.hpp>

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/external/thrust/thrust.hpp>

#include "dev_dat.hpp"

namespace boost { namespace numeric { namespace odeint {

            template<>
            struct is_resizeable< DevDatC >
            {
                typedef boost::true_type type;
                static const bool value = type::value;
            };

            template<>
            struct same_size_impl< DevDatC, DevDatC >
            { // define how to check size
                __host__ __device__
                static bool same_size( const DevDatC &v1,
                                       const DevDatC &v2 )
                {
                    return (v1.size() == v2.size()) && (v1.dim_size() == v2.dim_size());
                }
            };

            template<>
            struct resize_impl< DevDatC, DevDatC >
            { // define how to resize
                __host__ __device__
                static void resize( DevDatC &v1,
                                    const DevDatC &v2 )
                {
                    v1.resize( v2.size() );
                    v1.set_dim(v2.dim_size());
                    v1.set_N(v2.size() / v2.dim_size());
                    v1.initialize_dimension_iterators();
                }
            };

        } } }

// Dispatchers

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

// Specializations for the DevDat
namespace boost {
    namespace numeric {
        namespace odeint {
            template<typename Vec, typename VecIterator, typename ConstVecIterator>
            struct algebra_dispatcher< DevDat<Vec, VecIterator, ConstVecIterator > >
        {
            typedef thrust_algebra algebra_type;
        };

    } // namespace odeint
} // namespace numeric
} // namespace boost

#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

// Support for DevDat
namespace boost {
    namespace numeric {
        namespace odeint {
            template<typename Vec, typename VecIterator, typename ConstVecIterator>
            struct operations_dispatcher< DevDat<Vec, VecIterator, ConstVecIterator> >
        {
            typedef thrust_operations operations_type;
        };
    } // namespace odeint
} // namespace numeric
} // namespace boost

#endif //PROGRAM_DEV_DAT_BOOST_INTEGRATION_HPP