#include "ros/ros.h"
#include "ros/master.h"
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "ackermann_msgs/AckermannDriveStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <string>
#include <sstream>
#include "nhttc_interface.h"
#include <boost/algorithm/string.hpp>

/**
 * nhttc_ros class.
 *
 * This class provides a ros interface to the nhttc backend
 */
class nhttc_ros
{
public:
  ros::Subscriber sub_other_pose[16],sub_other_control[16];
  ros::Subscriber sub_goal,sub_wp;
  ros::Publisher pub_cmd, viz_pub, time_pub;

  std::string self_name;
  std::string other_name;
  char index;
  std::ostringstream s;

  Eigen::Vector2f goal;
  int own_index;
  int solver_time;
  int num_agents_max;
  bool simulation;
  bool goal_received;
  float cutoff_dist;
  float steer_limit;
  float wheelbase;
  float turning_radius;
  float carrot_goal_ratio;
  float max_ttc;
  SGDOptParams global_params;
  std::vector<Agent> agents; //all agents
  int count;
  float speed_lim = 0.4f;
  float cur_time_stamp;
  float safety_radius;
  int time_index;
  bool obey_time;
  bool allow_reverse;
  bool adaptive_lookahead;

  std::vector<TTCObstacle*> obstacles = BuildObstacleList(agents);

  std::vector<Eigen::Vector2f> waypoints;
  std::vector<float> time_stamps;
  std::vector<std::string> name_list;
  int current_wp_index, max_index;

  ros::master::V_TopicInfo master_topics;

  ros::Time begin;
  /**
   * setting up agents
   *
   * This function initializes the nhttc agents with a preset starting pose and goal
   *
   * @param takes the index value corresponding to that agent
   */
  void agent_setup(int i)
  {
    Eigen::Vector2f goal(0.0,0.0);
    Eigen::VectorXf pos = Eigen::VectorXf::Zero(3);
    agents.emplace_back(GetAgentParts(6, pos, true, goal), global_params);
  }

  /**
   * Quaternion to euler angle converter
   *
   * Converts quaternion representation to euler angle representation. rpy = [roll, pitch, yaw]. rpy is in radians, following the ENU reference frame
   * @params euler angle array (float), pose message (geometry_msgs::PoseStamped)
   */
  void rpy_from_quat(float rpy[3],const geometry_msgs::PoseStamped::ConstPtr& msg)
  {
    float q[4];
    q[0] = msg->pose.orientation.x;
    q[1] = msg->pose.orientation.y;
    q[2] = msg->pose.orientation.z;
    q[3] = msg->pose.orientation.w;
    rpy[2] = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]),1 - 2 * (q[1] * q[1] + q[2] * q[2]));
    rpy[0] = asinf(2.0f * (q[0] * q[2] - q[3] * q[1]));
    rpy[1] = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3]));
  }

  /**
   * Pose callback
   *
   * This function listens to the pose-publishers of all the agents
   *
   * @param takes the pose msg (geometry_msgs::PoseStamped) and the index value corresponding to the agent.
   */
  void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg, int i)
  {
    float rpy[3];
    Eigen::VectorXf x_o = Eigen::VectorXf::Zero(3);
    rpy_from_quat(rpy,msg);
    x_o[0] = msg->pose.position.x;
    x_o[1] = msg->pose.position.y;
    x_o[2] = rpy[2];
    agents[i].SetEgo(x_o);
  }

  /**
   * Control callback
   *
   * This function listens to the control-publishers of all the agents. This is required by the nhttc as it takes into account the current state and actions of the other agents
   *
   * @param takes the control msg (ackermann_msgs::AckermannDriveStamped) and the index value corresponding to the agent.
   */
  void ControlCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& msg, int i)
  {
    Eigen::VectorXf controls = Eigen::VectorXf::Zero(2);
    controls[0] = msg->drive.speed;
    controls[1] = msg->drive.steering_angle;
    if(i!=own_index)
    {
      agents[i].SetControls(controls);
    }
  }

  /**
   * Waypoint callback
   *
   * This function listens to the waypoint messages (geometry_msgs::PoseArray) published by the global planner.
   *
   * @param takes the waypoint message.
   */
  void WPCallBack(const geometry_msgs::PoseArray::ConstPtr& msg)
  {
    Eigen::Vector2f wp,wp_last;
    int num = msg->poses.size();
    waypoints.clear(); // reset
    float time_stamp=0;
    time_stamps.clear();
    current_wp_index = 0;
    wp[0] = msg->poses[1].position.x - msg->poses[0].position.x;
    wp[1] = msg->poses[1].position.y - msg->poses[0].position.y;
    float time_step = wp.norm()/speed_lim;
    for(int i =0;i<num;i++)
    {
      wp[0] = msg->poses[i].position.x;
      wp[1] = msg->poses[i].position.y;
      time_stamps.push_back(time_stamp);
      time_stamp += (msg->poses[i].position.z*1000)*time_step;
      waypoints.push_back(wp);
    }
    goal_received = true;
    goal = waypoints[current_wp_index];
    agents[own_index].UpdateGoal(goal); // set the goal
    max_index = num;
    cur_time_stamp = time_stamps[1];
    time_index = 1; //reset
    begin = ros::Time::now();// + ros::Duration(0.02); // add 0.02 seconds corresponding to the 50 hz update rate.
  }

  /**
   * Single-goal callback
   *
   * Used for a single-goal point mission.
   *
   * @param goal point message (geometry_msgs::PoseStamped)
   */
  void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
  {
    goal[0] = msg->pose.position.x;
    goal[1] = msg->pose.position.y;
    agents[own_index].UpdateGoal(goal);
    goal_received = true;
  }

  /**
   * Send throttle/steering commands
   *
   * sends commands to the vesc/mux/input/navigation topic
   *
   * @param speed (float, m/s) steering angle (float, radius)
   */
  void send_commands(float speed, float steer)
  {
    ackermann_msgs::AckermannDriveStamped output_msg;
    output_msg.header.stamp = ros::Time::now();
    output_msg.header.frame_id = "";
    output_msg.drive.steering_angle = steer;
    output_msg.drive.speed = speed;
    pub_cmd.publish(output_msg);
  }
  /**
   * Publish position of current goal-point for visualization
   *
   * Publishes the current nhttc-goal to the /car_/cur_goal topic. Used for visualizing where the car is currently trying to go.
   */
  void viz_publish()
  {
    geometry_msgs::PoseStamped output_msg;
    output_msg.header.stamp = ros::Time::now();
    output_msg.header.frame_id = "/map";
    output_msg.pose.position.x = agents[own_index].goal[0];
    output_msg.pose.position.y = agents[own_index].goal[1];
    viz_pub.publish(output_msg);
  }

  /**
   * Publish position of current time-point for visualization
   *
   * Publishes the current time-point to the /car_/time_goal topic. Used for visualizing the point with which the car is trying to
   * synchronize it's timing.
   */
  void time_viz_publish()
  {
    geometry_msgs::PoseStamped output_msg;
    output_msg.header.stamp = ros::Time::now();
    output_msg.header.frame_id = "/map";
    output_msg.pose.position.x = waypoints[time_index][0];
    output_msg.pose.position.y = waypoints[time_index][1];
    time_pub.publish(output_msg);
  }

  /**
   * Check for new agents
   *
   * updates the agent array by finding published topics with topic type PoseStamped and unique car_name.
   * @params: ros::NodeHandle
   * @returns: None
   */
  void check_new_agents(ros::NodeHandle &nh)
  {
    ros::master::getTopics(master_topics);

    for (ros::master::V_TopicInfo::iterator it = master_topics.begin() ; it != master_topics.end(); it++)
    {
      const ros::master::TopicInfo& info = *it;
      std::vector<std::string> strs;
      if(info.datatype == "geometry_msgs/PoseStamped")
      {
        boost::split(strs, info.name, boost::is_any_of("/"));
        if(!strs.empty())
        {
          if(strs[2]=="car_pose" or strs[2]=="mocap_pose")
          {
            bool common = false;
            for(int i=0;i<=name_list.size() and name_list.size()!=0 ;i++)
            {
              if(strs[1]==name_list[i])
              {
                common = true;
              }
            }
            if(not common)
            {
              ROS_INFO("car found:%s",strs[1].c_str());
              name_list.push_back(strs[1]);
              count++;
              if(strs[1] == self_name)
              {
                own_index = count;
              }
              agent_setup(count);

              sub_other_pose[count] = nh.subscribe<geometry_msgs::PoseStamped>(info.name,10,boost::bind(&nhttc_ros::PoseCallback,this,_1,count));
              s.str("");
              s<<"/";
              s<<strs[1];
              s<<"/mux/ackermann_cmd_mux/input/navigation";
              sub_other_control[count] = nh.subscribe<ackermann_msgs::AckermannDriveStamped>((s.str()).c_str(),10,boost::bind(&nhttc_ros::ControlCallback,this,_1,count));
            }
          }
        }
      }
    }
  }

  /**
   * nhttc ros constructor
   *
   * Sets up the publishers, subscribers and the whole shebang
   */
  nhttc_ros(ros::NodeHandle &nh)
  {
    own_index = -1;
    goal_received = false; // start with the assumption that the car has no goal
    current_wp_index = 0; // set current waypoint index to 0
    max_index = 0; //set max_waypoint index to 0
    // get all the params
    nh.getParam("car_name",self_name);
    if(not nh.getParam("/solver_time",solver_time))
    {
      solver_time = 10; // 10 ms solver time for each agent.
    }
    if(not nh.getParam("sim",simulation))
    {
      simulation = true; // run in simulation by default.
    }
    if(not nh.getParam("max_agents",num_agents_max))
    {
      num_agents_max = 8; // default maximum number of agents
    }
    if(not nh.getParam("/carrot_goal_ratio",carrot_goal_ratio))
    {
      carrot_goal_ratio = 1.0f; //default distance to the ever-changing goal
    }
    if(not nh.getParam("/max_ttc",max_ttc))
    {
      max_ttc = 6.0f; // default ttc
    }
    if(not nh.getParam("/obey_time", obey_time))
    {
      obey_time = false;// false by default
    }
    if(not nh.getParam("/allow_reverse", allow_reverse))
    {
      allow_reverse = true;// true by default (default behavior is to not have any constraints on the nav engine)
    }
    if(not nh.getParam("/adaptive_lookahead",adaptive_lookahead))
    {
      adaptive_lookahead = false;
    }
    if(not nh.getParam("/safety_radius", safety_radius))
    {
      safety_radius = 0.1f;
    }

    ConstructGlobalParams(&global_params);
    count = -1; //number of agents. start from -1.
    // sleep rate. Wait for all the agents to be set up before you start counting.
    /*
     * The following code-block listens to all the published topics and filters out those which end with /car_pose or /mocap_pose.
     * When it finds the corresponding topic, it creates the subscribers/publishers for that agent and increases the count for the number of agents.
     */
    ros::Rate r(1);
    for(int i=0;i<5;i++)
    {
      check_new_agents(nh);
      r.sleep();
    } // We do this once at the beginning in order to find the car belonging to our own nav controller
    // set up all the publishers/subscribers
    sub_wp = nh.subscribe("/"+self_name+"/waypoints",10,&nhttc_ros::WPCallBack,this);
    pub_cmd = nh.advertise<ackermann_msgs::AckermannDriveStamped>("/"+ self_name +"/mux/ackermann_cmd_mux/input/navigation",10);
    viz_pub = nh.advertise<geometry_msgs::PoseStamped>("/"+self_name+"/cur_goal",10);
    time_pub = nh.advertise<geometry_msgs::PoseStamped>("/"+self_name+"/time_goal",10);
    ROS_INFO("node started"); // tell the world that the node has initialized.
  }

  /**
   * Parameter setting for the ego-agent.
   *
   * Sets up the tuning parameters for the ego-agent. These parameters are taken from the launch file. The tuning parameters include
   * 1) The carrot-goal ratio: (lookahead distance)/(turning radius of the car)
   * 2) max_ttc: maximum time-to-collision
   */
  void setup()
  {
    steer_limit = 0.1*M_PI; // max steering angle ~18 degrees. :(. I wanted to drift with the MuSHR car.
    wheelbase = agents[own_index].prob->params.wheelbase;
    agents[own_index].prob->params.safety_radius = safety_radius;
    agents[own_index].prob->params.steer_limit = steer_limit;
    agents[own_index].prob->params.vel_limit = speed_lim;
    agents[own_index].prob->params.u_lb = allow_reverse ? Eigen::Vector2f(-speed_lim, -steer_limit) : Eigen::Vector2f(0, -steer_limit);
    agents[own_index].prob->params.u_ub = Eigen::Vector2f(speed_lim,steer_limit);
    agents[own_index].prob->params.max_ttc = max_ttc;
    turning_radius = wheelbase/tanf(fabs(steer_limit));
    fabs(steer_limit) == 0 ? cutoff_dist = 1.0 : cutoff_dist = carrot_goal_ratio*turning_radius;
    cutoff_dist += agents[own_index].prob->params.radius;
    ROS_INFO("carrot_goal_ratio: %f",carrot_goal_ratio);
    ROS_INFO("max_ttc: %f", max_ttc);
    ROS_INFO("solver_time: %d", solver_time);
    ROS_INFO("obey_time:%d", int(obey_time));
    ROS_INFO("allow_reverse: %d", int(allow_reverse));
    ROS_INFO("safety_radius: %f", safety_radius);
    ROS_INFO("adaptive_lookahead, %d", int(adaptive_lookahead));
  }

  /**
  * do_skip_wp
  *
  * checks if the current waypoint should be skipped or not based on whether it sits within the turning circle
  */
  bool do_skip_waypoint(Eigen::Vector2f wp_vec, float th)
  {
    if(!adaptive_lookahead)
    {
      return false;
    }
    Eigen::Vector2f turning_center_right,turning_center_left;
    float turning_radius = wheelbase/tanf(fabs(steer_limit));
    // X is forward, Y is left
    turning_center_left = turning_radius*Eigen::Vector2f(cosf(M_PI*0.5 + th), sinf(M_PI*0.5 - th)); // rotate the turning center into world frame
    turning_center_right = turning_radius*Eigen::Vector2f(cosf(M_PI*0.5 - th), sinf(-M_PI*0.5 + th));
    // find the distance between the current waypoint and the turning centers in the car's body frame.
    turning_center_left -= wp_vec;
    turning_center_right -= wp_vec;
    if(turning_center_right.norm() < turning_radius - 0.1125 or turning_center_left.norm() < turning_radius - 0.1125)
    {
      return true;
    }
    return false;
  }

  /**
   * local planner
   *
   * Calls the nhttc solver and sends the output of the solver to the ego-agent. Note that the solver solves only for one agent but it does need
   * to know what the other agents are doing.
   */
  void plan()
  {
    // create obstacle list.
    if(agents.size()>0)
    {
      obstacles = BuildObstacleList(agents);
      agents[own_index].SetPlanTime(solver_time); //20 ms planning window TODO: see if this only needs to be done once
      agents[own_index].SetObstacles(obstacles, size_t(own_index)); // set the obstacles

      Eigen::VectorXf controls = Eigen::VectorXf::Zero(2); //controls are 0,0 by default.
      Eigen::VectorXf agent_state = agents[own_index].prob->params.x_0; //get agent's current state
      //if we have a goal
      if(goal_received)
      {
        // Find distance to current nhttc waypoint
        Eigen::Vector2f wp_vec = (agents[own_index].goal - agent_state.head(2)); // vector joining agent to waypoint
        Eigen::Vector2f head_vec = Eigen::Vector2f(cosf(agent_state[2]),sinf(agent_state[2])); // heading vector
        float dist = wp_vec.norm(); //distance from goal wp taking into account the aspect angle. If the point is perpendicular to my direction of motion, I have probably passed it.
        // now search for the nearest waypoint thats still ahead of me.
        // note, this depends on previously calculated heading vector. This evaluation works similarly to the above calc.
        Eigen::Vector2f tp_vec = (waypoints[time_index] - agent_state.head(2)); // tp: time point
        float multiplier = tp_vec.dot(head_vec)>0? 1.0f : -1.0f;
        float time_point_dist = multiplier*tp_vec.norm();
        if(time_point_dist < agents[own_index].prob->params.safety_radius)
        {
          time_index++;
          time_viz_publish();
          // re-evaluate the time_point_distance
          tp_vec = (waypoints[time_index] - agent_state.head(2));
          multiplier = tp_vec.dot(head_vec)>0? 1.0f : -1.0f;
          time_point_dist = multiplier*tp_vec.norm();
        }

        if(obey_time)
        {
          float current_time = float((ros::Time::now() - begin).toSec()); //the current time relative to the time at which the plans were published.
          cur_time_stamp = time_stamps[time_index];  // time stamp by which the car must arrive at the time-waypoint. The time waypoint is simply the next closest waypoint in the plan.
          float delta_time = cur_time_stamp - current_time; //time left to reach the time-waypoint
          float virtual_dist = delta_time*speed_lim; // distance from the time-waypoint at which the car should be if it had to travel at the default speed
          float dist_error = time_point_dist - virtual_dist; // error between the expected distance-to-go and the actual distance-to-go
          // I can detect a stall from the dist_error*10.
          float delta_speed = std::min(std::max(dist_error*10,-speed_lim),speed_lim*2); // a Proportional controller for changing the speed with some min-max limits
          agents[own_index].prob->params.vel_limit = speed_lim + delta_speed; //set the new speed limit
          agents[own_index].prob->params.u_lb = allow_reverse ? Eigen::Vector2f(-(speed_lim + delta_speed), -steer_limit) : Eigen::Vector2f(0, -steer_limit); // set the upper and lower bound corresponding to this limit
          agents[own_index].prob->params.u_ub = Eigen::Vector2f(speed_lim + delta_speed,steer_limit);
        }

        if(do_skip_waypoint(wp_vec,agent_state[2]) and current_wp_index < max_index - 1) //second condition is to ensure it doesn't finess the last waypoint
        {
          dist = cutoff_dist - 0.1;  // set the distance to be just below cutoff. Safe as long as dist is used for decision making only
        }
        if(dist > cutoff_dist) // cutoff distance is the tolerance to goal. When goal-distance is less than cutoff, the goal is assumed to be reached
        {
          controls = agents[own_index].UpdateControls();
        }
        if(dist < cutoff_dist)
        {
          if(current_wp_index < max_index-1)
          {
            agents[own_index].goal = waypoints[++current_wp_index];
            viz_publish(); // publish new goal point
          }
          if(dist < wheelbase and current_wp_index >= max_index-1) //condition for having reached the final waypoint. the cutoff distance for the final waypoint is less than the standard cutoff. This can later be extended to "must-pass" waypoints.
          {
            controls[0] = 0;
            controls[1] = 0;
            goal_received = false;
          }
          else
          {
            controls = agents[own_index].UpdateControls(); // in case it is the final waypoint, keep going until dist-to-go is less than wheelbase
          }
        }
      }
      float speed = controls[0]; //speed in m/s
      float steering_angle = controls[1]; //steering angle in radians. +ve is left. -ve is right
      send_commands(speed,steering_angle); //just sending out anything for now;
      return;
    }
  }
};

/**
 * main function
 *
 * Sets up the nhttc object and initializes everything.
 */
int main(int argc, char** argv)
{
  ros::init(argc,argv,"nhttc_local_planner");
  ros::NodeHandle nh("~");
  nhttc_ros local_planner(nh);
  ros::Rate r(40);
  bool init = false; //flag for node initialization (indicates if the car's car_pose topic has been found)

  while(ros::ok)
  {
    ros::spinOnce();
    // local_planner.check_new_agents(nh);
    if(local_planner.own_index != -1 and !init) //if car had not been initialized before and has now found the car_pose topic
    {
      init = true; // set init to true
      local_planner.setup(); // set up the agent
    }
    if(init) //if init
    {
      local_planner.plan();
    }
    r.sleep();
  }
  return 0;
}


#define BOOST_TEST_MODULE pcraster model_engine areamap
#include <boost/test/unit_test.hpp>
#include "geo_rasterspace.h"
#include "PCRasterXSD.h"
#include "calc_areamap.h"

BOOST_AUTO_TEST_CASE(testInit)
{
  using namespace calc;

  geo::RasterSpace in(3,10,5);

  AreaMap amIn(in);

  std::auto_ptr<pcrxml::CheckContext> cc(amIn.createXMLContext ());

  AreaMap amOut1(amIn.rasterSpace());

  // not possible anymore pcrxml::AreaMap != pcrxml::AreaMapScript
  // AreaMap amOut2(cc->areaMap().get());

  BOOST_CHECK(in == amOut1.rasterSpace());
  // BOOST_CHECK(in == amOut2.rasterSpace());
}


  //==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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_PREDICATES_FUNCTIONS_SCALAR_ISINSIDE_HPP_INCLUDED
#define NT2_PREDICATES_FUNCTIONS_SCALAR_ISINSIDE_HPP_INCLUDED

#include <nt2/predicates/functions/isinside.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/size.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/is_less.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <nt2/include/functions/is_greater_equal.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( isinside_, tag::cpu_
                            , (A0)(A1)
                            , (fusion_sequence_<A0>)
                              (fusion_sequence_<A1>)
                            )
  {
    typedef typename boost::fusion::result_of::at_c<A0,0>::type   at_t;
    typedef typename meta::strip<at_t>::type                      base_t;
    typedef typename boost::simd::meta::as_logical<base_t>::type  result_type;

    BOOST_FORCEINLINE
    result_type operator()(const A0& a0,const A1& a1) const
    {
      typedef typename boost::fusion::result_of::size<A0>::type sz_t;
      return eval(a0,a1,boost::mpl::int_<sz_t::value-1>());
    }

    template<class N> BOOST_FORCEINLINE result_type
    eval(const A0& a0,const A1& a1, N const&) const
    {
      return l_and( eval(a0,a1, boost::mpl::int_<N::value-1>())
                  , lt( boost::fusion::at_c<N::value>(a0)
                      , splat<base_t>(boost::fusion::at_c<N::value>(a1))
                      )
                  );
    }

    BOOST_FORCEINLINE result_type
    eval(const A0& a0,const A1& a1,boost::mpl::int_<0> const&) const
    {
      return lt ( boost::fusion::at_c<0>(a0)
                , splat<base_t>(boost::fusion::at_c<0>(a1))
                );
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( isinside_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (fusion_sequence_<A0>)
                              (fusion_sequence_<A1>)
                              (fusion_sequence_<A2>)
                            )
  {
    typedef typename boost::fusion::result_of::at_c<A0,0>::type   at_t;
    typedef typename meta::strip<at_t>::type                      base_t;
    typedef typename boost::simd::meta::as_logical<base_t>::type  result_type;

    BOOST_FORCEINLINE
    result_type operator()(const A0& a0,const A1& a1,const A2& a2) const
    {
      typedef typename boost::fusion::result_of::size<A0>::type sz_t;
      return eval(a0,a1,a2,boost::mpl::int_<sz_t::value-1>());
    }

    template<class N> BOOST_FORCEINLINE result_type
    eval(const A0& a0,const A1& a1,const A2& a2, N const&) const
    {
      return l_and( eval(a0,a1,a2, boost::mpl::int_<N::value-1>())
                  , eval_at ( boost::fusion::at_c<N::value>(a0)
                            , boost::fusion::at_c<N::value>(a1)
                            , boost::fusion::at_c<N::value>(a2)
                            )
                  );
    }

    BOOST_FORCEINLINE result_type
    eval(const A0& a0,const A1& a1,const A2& a2, boost::mpl::int_<0> const&) const
    {
      return eval_at( boost::fusion::at_c<0>(a0)
                    , boost::fusion::at_c<0>(a1)
                    , boost::fusion::at_c<0>(a2)
                    );
    }

    template<class T0,class T1, class T2> BOOST_FORCEINLINE
    result_type eval_at(const T0& a0,const T1& a1,const T2& a2) const
    {
      return l_and( ge( a0, splat<base_t>(a2))
                  , lt( a0, splat<base_t>(a2+a1))
                  );
    }
  };
} }

#endif


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#include "itkTractsToRgbaImageFilter.h"

// VTK
#include <vtkPolyLine.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>

// misc
#include <math.h>
#include <boost/progress.hpp>
#include <mitkDiffusionFunctionCollection.h>

namespace itk{

template< class OutputImageType >
TractsToRgbaImageFilter< OutputImageType >::TractsToRgbaImageFilter()
  : m_UpsamplingFactor(1)
  , m_UseImageGeometry(false)
  , m_InputImage(nullptr)
{

}

template< class OutputImageType >
TractsToRgbaImageFilter< OutputImageType >::~TractsToRgbaImageFilter()
{
}

template< class OutputImageType >
itk::Point<float, 3> TractsToRgbaImageFilter< OutputImageType >::GetItkPoint(double point[3])
{
  itk::Point<float, 3> itkPoint;
  itkPoint[0] = point[0];
  itkPoint[1] = point[1];
  itkPoint[2] = point[2];
  return itkPoint;
}

template< class OutputImageType >
void TractsToRgbaImageFilter< OutputImageType >::GenerateData()
{
  if(&typeid(OutPixelType) != &typeid(itk::RGBAPixel<unsigned char>))
    return;

  // generate upsampled image
  mitk::BaseGeometry::Pointer geometry = m_FiberBundle->GetGeometry();
  typename OutputImageType::Pointer outImage = this->GetOutput();

  // calculate new image parameters
  itk::Vector<double,3> newSpacing;
  mitk::Point3D newOrigin;
  itk::Matrix<double, 3, 3> newDirection;
  ImageRegion<3> upsampledRegion;
  if (m_UseImageGeometry && !m_InputImage.IsNull())
  {
    newSpacing = m_InputImage->GetSpacing()/m_UpsamplingFactor;
    upsampledRegion = m_InputImage->GetLargestPossibleRegion();
    newOrigin = m_InputImage->GetOrigin();
    typename OutputImageType::RegionType::SizeType size = upsampledRegion.GetSize();
    size[0] *= m_UpsamplingFactor;
    size[1] *= m_UpsamplingFactor;
    size[2] *= m_UpsamplingFactor;
    upsampledRegion.SetSize(size);
    newDirection = m_InputImage->GetDirection();
  }
  else
  {
    newSpacing = geometry->GetSpacing()/m_UpsamplingFactor;
    newOrigin = geometry->GetOrigin();
    mitk::Geometry3D::BoundsArrayType bounds = geometry->GetBounds();
    newOrigin[0] += bounds.GetElement(0);
    newOrigin[1] += bounds.GetElement(2);
    newOrigin[2] += bounds.GetElement(4);

    for (int i=0; i<3; i++)
      for (int j=0; j<3; j++)
        newDirection[j][i] = geometry->GetMatrixColumn(i)[j];
    upsampledRegion.SetSize(0, geometry->GetExtent(0)*m_UpsamplingFactor);
    upsampledRegion.SetSize(1, geometry->GetExtent(1)*m_UpsamplingFactor);
    upsampledRegion.SetSize(2, geometry->GetExtent(2)*m_UpsamplingFactor);
  }
  typename OutputImageType::RegionType::SizeType upsampledSize = upsampledRegion.GetSize();

  // apply new image parameters
  outImage->SetSpacing( newSpacing );
  outImage->SetOrigin( newOrigin );
  outImage->SetDirection( newDirection );
  outImage->SetRegions( upsampledRegion );
  outImage->Allocate();
  outImage->FillBuffer(0.0);

  itk::Image< itk::RGBAPixel<double>, 3 >::Pointer double_out = itk::Image< itk::RGBAPixel<double>, 3 >::New();
  double_out->SetSpacing( newSpacing );
  double_out->SetOrigin( newOrigin );
  double_out->SetDirection( newDirection );
  double_out->SetRegions( upsampledRegion );
  double_out->Allocate();
  double_out->FillBuffer(0.0);

  vtkSmartPointer<vtkPolyData> fiberPolyData = m_FiberBundle->GetFiberPolyData();
  int numFibers = m_FiberBundle->GetNumFibers();

  float scale = 100 * pow((float)m_UpsamplingFactor,3);
  boost::progress_display disp(numFibers);
  for( int i=0; i<numFibers; ++i )
  {
    ++disp;
    vtkCell* cell = fiberPolyData->GetCell(i);
    int numPoints = cell->GetNumberOfPoints();
    vtkPoints* points = cell->GetPoints();

    for( int j=0; j<numPoints-1; ++j)
    {
      Point<float, 3> startVertex = GetItkPoint(points->GetPoint(j));
      Index<3> startIndex;
      ContinuousIndex<float, 3> startIndexCont;
      double_out->TransformPhysicalPointToIndex(startVertex, startIndex);
      double_out->TransformPhysicalPointToContinuousIndex(startVertex, startIndexCont);

      Point<float, 3> endVertex = GetItkPoint(points->GetPoint(j + 1));
      Index<3> endIndex;
      ContinuousIndex<float, 3> endIndexCont;
      double_out->TransformPhysicalPointToIndex(endVertex, endIndex);
      double_out->TransformPhysicalPointToContinuousIndex(endVertex, endIndexCont);

      Vector<float, 3> dir;
      dir[0] = fabs(endVertex[0]-startVertex[0]);
      dir[1] = fabs(endVertex[1]-startVertex[1]);
      dir[2] = fabs(endVertex[2]-startVertex[2]);
      dir.Normalize();

      std::vector< std::pair< itk::Index<3>, double > > segments = mitk::imv::IntersectImage(newSpacing, startIndex, endIndex, startIndexCont, endIndexCont);
      for (std::pair< itk::Index<3>, double > segment : segments)
      {
        if (!double_out->GetLargestPossibleRegion().IsInside(segment.first))
          continue;

        itk::RGBAPixel<double> pix = double_out->GetPixel(segment.first);
        pix[0] += dir[0] * scale;
        pix[1] += dir[1] * scale;
        pix[2] += dir[2] * scale;
        pix[3] += segment.second * scale;

        double_out->SetPixel(segment.first, pix);
      }
    }

    float maxRgb = 0.000000001;
    float maxInt = 0.000000001;
    int w = upsampledSize[0];
    int h = upsampledSize[1];
    int d = upsampledSize[2];
    int numPix = w*h*d*4;

    double* buffer = (double*)double_out->GetBufferPointer();
    // calc maxima
    for(int i=0; i<numPix; i++)
    {
      if((i-3)%4 != 0)
      {
        if(buffer[i] > maxRgb)
          maxRgb = buffer[i];
      }
      else
      {
        if(buffer[i] > maxInt)
          maxInt = buffer[i];
      }
    }

    // write output, normalized uchar 0..255
    unsigned char* outImageBufferPointer = (unsigned char*)outImage->GetBufferPointer();
    for(int i=0; i<numPix; i++)
    {
      if((i-3)%4 != 0)
        outImageBufferPointer[i] = (unsigned char) (255.0 * buffer[i] / maxRgb);
      else
        outImageBufferPointer[i] = (unsigned char) (255.0 * buffer[i] / maxInt);
    }
  }
}
}


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to 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_MATH_COMPLEX_DETAILS_INCLUDED
#define BOOST_MATH_COMPLEX_DETAILS_INCLUDED
//
// This header contains all the support code that is common to the
// inverse trig complex functions, it also contains all the includes
// that we need to implement all these functions.
//

#include <cmath>
#include <complex>
#include <limits>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/constants/constants.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T>
inline T mult_minus_one(const T& t)
{
   return (boost::math::isnan)(t) ? t : (boost::math::changesign)(t);
}

template <class T>
inline std::complex<T> mult_i(const std::complex<T>& t)
{
   return std::complex<T>(mult_minus_one(t.imag()), t.real());
}

template <class T>
inline std::complex<T> mult_minus_i(const std::complex<T>& t)
{
   return std::complex<T>(t.imag(), mult_minus_one(t.real()));
}

template <class T>
inline T safe_max(T t)
{
   return std::sqrt((std::numeric_limits<T>::max)()) / t;
}
inline long double safe_max(long double t)
{
   // long double sqrt often returns infinity due to
   // insufficient internal precision:
   return std::sqrt((std::numeric_limits<double>::max)()) / t;
}

template <class T>
inline T safe_min(T t)
{
   return std::sqrt((std::numeric_limits<T>::min)()) * t;
}
inline long double safe_min(long double t)
{
   // long double sqrt often returns zero due to
   // insufficient internal precision:
   return std::sqrt((std::numeric_limits<double>::min)()) * t;
}

} } } // namespaces

#endif // BOOST_MATH_COMPLEX_DETAILS_INCLUDED


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_INTERFACE_GRAPH_HPP
#define GSPARSE_INTERFACE_GRAPH_HPP

#include <cstddef>    // size_t
#include <memory>   //shared_ptr
#include <Eigen/Dense> // Eigen Matrix
#include "../Config.hpp" // Library configuration

namespace gSparse
{
    //!  An interface class for Graph object.
    /*!
        This class defines an interface for gSparse's Graph primitives.
    */
	class IGraph
	{
	public:
		//!  Pure virtual function to get Graph Adjacency Matrix
		virtual const gSparse::SparsePrecisionMatrix & GetAdjacentMatrix() const = 0;
        //!  Pure virtual function to get Graph Incident Matrix
		virtual const gSparse::SparsePrecisionMatrix & GetIncidentMatrix() const = 0;
        //!  Pure virtual function to get Graph Degree Matrix
		virtual const gSparse::SparsePrecisionMatrix & GetDegreeMatrix() const = 0;
        //!  Pure virtual function to get Graph Laplacian Matrix
		virtual const gSparse::SparsePrecisionMatrix & GetLaplacianMatrix() const = 0;
        //!  Pure virtual function to get Graph Weight Matrix
		virtual const gSparse::SparsePrecisionMatrix & GetWeightMatrix() const = 0;
        //!  Pure virtual function to get Graph Edge List Matrix
		virtual const gSparse::EdgeMatrix & GetEdgeList() const = 0;
        //!  Pure virtual function to get Graph Weight List Matrix
		virtual const gSparse::PrecisionRowMatrix & GetWeightList() const = 0;
        //!  Pure virtual function to get number of edges in the graph
		virtual std::size_t GetEdgeCount() const = 0;
        //!  Pure virtual function to get number of nodes in the graph
		virtual std::size_t GetNodeCount() const = 0;
		/* Default Destructor*/
		virtual ~IGraph() = default;
	protected:
        //!  Protected default constructor.
		IGraph() = default;
	};
	typedef std::shared_ptr<IGraph> Graph;
}
#endif


// $Id$
/*
 * This is an extensive modification by Greg Landrum of
 * pieces from several files in the vflib-2.0 distribution
 *
 * The initial version of the modifications was completed
 *   in April 2009.
 *
 * the original author of the vflib files is:
 *    Author: P. Foggia
 *  http://amalfi.dis.unina.it/graph/db/vflib-2.0/doc/vflib.html
 *
 */
#include <boost/graph/adjacency_list.hpp>
#include <vector>
#include <algorithm>
#include <cstring>

#ifndef __BGL_VF2_SUB_STATE_H__
#define __BGL_VF2_SUB_STATE_H__
//#define RDK_VF2_PRUNING
#define RDK_ADJ_ITER typename Graph::adjacency_iterator

namespace boost{
  namespace detail {
    typedef unsigned short node_id;
    const node_id NULL_NODE=0xFFFF;
    struct NodeInfo {
      node_id id;
      node_id in;
      node_id out;
    };

    template <class Graph>
    struct Pair {
      node_id n1, n2;
      bool hasiter;
      RDK_ADJ_ITER nbrbeg, nbrend;

      Pair() : n1(NULL_NODE), n2(NULL_NODE), hasiter(false) {
      }
    };

    /**
     * The ordering by in/out degree
     */
    static bool nodeInfoComp1(const NodeInfo &a, const NodeInfo &b) {
      if(a.out < b.out) return true;
      if(a.out > b.out) return false;
      if(a.in < b.in) return true;
      if(a.in > b.in) return false;
      return false;
    }

    /**
     * The ordering by frequency/valence.
     * The frequency is in the out field, the valence in `in'.
     */
    static int nodeInfoComp2(const NodeInfo &a, const NodeInfo &b) {
      if (!a.in && b.in ) return 1;
      if (a.in && !b.in) return -1;
      if (a.out < b.out) return -1;
      if (a.out > b.out) return 1;
      if( a.in < b.in ) return -1;
      if (a.in > b.in) return 1;
      return 0;
    }

    template <class Graph,class VertexDescr,class EdgeDescr>
    VertexDescr getOtherIdx(const Graph &g,const EdgeDescr &edge,const VertexDescr &vertex) {
      VertexDescr tmp=boost::source(edge,g);
      if(tmp==vertex){
        tmp=boost::target(edge,g);
      }
      return tmp;
    }

    /*----------------------------------------------------
     * Sorts the nodes of a graphs, returning a
     * heap-allocated vector (using new) with the node ids
     * in the proper orders.
     * The sorting criterion takes into account:
     *    1 - The number of nodes with the same in/out
     *        degree.
     *    2 - The valence of the nodes.
     * The nodes at the beginning of the vector are
     * the most singular, from which the matching should
     * start.
     *--------------------------------------------------*/
    template <class Graph>
    node_id* SortNodesByFrequency(const Graph *g) {
      std::vector<NodeInfo> vect;
      vect.reserve(boost::num_vertices(*g));
      typename Graph::vertex_iterator bNode,eNode;
      boost::tie(bNode,eNode) = boost::vertices(*g);
      while(bNode!=eNode){
        NodeInfo t;
        t.id=vect.size();
        t.in=boost::out_degree(*bNode,*g);// <- assuming undirected graph
        t.out=boost::out_degree(*bNode,*g);
        vect.push_back(t);
        ++bNode;
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp1);

      unsigned int run=1;
      for(unsigned int i=0; i<vect.size(); i+=run){
        for(run=1; i+run<vect.size() &&
              vect[i+run].in==vect[i].in &&
              vect[i+run].out==vect[i].out;
            ++run)
          ;
        for(unsigned int j=0; j<run; ++j) {
          vect[i+j].in += vect[i+j].out;
          vect[i+j].out=run;
        }
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp2);

      node_id *nodes=new node_id[vect.size()];
      for(unsigned int i=0; i<vect.size(); ++i){
        nodes[i]=vect[i].id;
      }

      return nodes;
    }

    /*----------------------------------------------------------
     * class VF2SubState
     * A representation of the SSS current state
     ---------------------------------------------------------*/
    template <class Graph,class VertexCompatible,class EdgeCompatible,class MatchChecking >
    class VF2SubState
    {
    private:
      Graph *g1, *g2;
      VertexCompatible &vc;
      EdgeCompatible &ec;
      MatchChecking &mc;
      unsigned int n1, n2;

      unsigned int core_len;
      unsigned int t1_len;
      unsigned int t2_len; // Core nodes are also counted by these...
      node_id *core_1;
      node_id *core_2;
      node_id *term_1;
      node_id *term_2;

      node_id *order;

      long *share_count;
      int *vs_compared;

    public:
      VF2SubState(Graph *ag1, Graph *ag2,
                  VertexCompatible &avc,
                  EdgeCompatible &aec,
                  MatchChecking &amc,
                  bool sortNodes=false) : g1(ag1), g2(ag2), vc(avc), ec(aec), mc(amc),
                                          n1(num_vertices(*ag1)),n2(num_vertices(*ag2)) {
        if (sortNodes){
          order = SortNodesByFrequency(ag1);
        } else {
          order = NULL;
        }

        core_len=0;
        t1_len=0;
        t2_len=0;

        core_1=new node_id[n1];
        core_2=new node_id[n2];
        term_1=new node_id[n1];
        term_2=new node_id[n2];
        share_count = new long;

        for(unsigned int i=0; i<n1; i++){
          core_1[i]=NULL_NODE;
          term_1[i]=0;
        }
        for(unsigned int i=0; i<n2; i++){
          core_2[i]=NULL_NODE;
          term_2[i]=0;
        }
        vs_compared=0;
        //vs_compared = new int[n1*n2];
        //memset((void *)vs_compared,0,n1*n2*sizeof(int));

        //es_compared = new std::map<unsigned int,bool>();
        *share_count = 1;
      };

      VF2SubState(const VF2SubState &state) :
        g1(state.g1), g2(state.g2), vc(state.vc), ec(state.ec), mc(state.mc),
        n1(state.n1),n2(state.n2), order(state.order),vs_compared(state.vs_compared)
        //es_compared(state.es_compared)
      {

        core_len=state.core_len;
        t1_len=state.t1_len;
        t2_len=state.t2_len;

        core_1=state.core_1;
        core_2=state.core_2;
        term_1=state.term_1;
        term_2=state.term_2;
        share_count=state.share_count;

        ++(*share_count);
      };

      ~VF2SubState(){
        if (-- *share_count == 0) {
          delete [] core_1;
          delete [] core_2;
          delete [] term_1;
          delete [] term_2;
          delete share_count;
          delete [] order;
          //delete [] vs_compared;
          //delete es_compared;
        }
      };

      bool IsGoal() { return core_len==n1 ; };
      bool MatchChecks(const node_id c1[],const node_id c2[]){
        return mc(c1,c2);
      };
      bool IsDead() { return n1>n2  ||
          t1_len>t2_len;
      };
      unsigned int CoreLen() { return core_len; }
      Graph *GetGraph1() { return g1; }
      Graph *GetGraph2() { return g2; }

      bool NextPair(Pair<Graph> &pair){
        if (pair.n1==NULL_NODE)
          pair.n1=0;
        if (pair.n2==NULL_NODE)
          pair.n2=0;
        else
          pair.n2++;

#if 0
    std::cerr<<" **** np: "<< prev_n1<<","<<prev_n2<<std::endl;
    std::cerr<<"in_1 ";
    for(unsigned int i=0;i<n1;++i){
      std::cerr<<"("<<in_1[i]<<","<<out_1[i]<<"), ";
    }
    std::cerr<<std::endl;
    std::cerr<<"in_2 ";
    for(unsigned int i=0;i<n2;++i){
      std::cerr<<"("<<in_2[i]<<","<<out_2[i]<<"), ";
    }
    std::cerr<<std::endl;
#endif
        if (t1_len>core_len && t2_len>core_len) {
          while (pair.n1<n1 &&
                 (core_1[pair.n1]!=NULL_NODE || term_1[pair.n1]==0) ) {
            pair.n1++;
            pair.n2=0;
          }

          /* Initialize VF2 Plus neighbor iterator.
           * The next query node (pair.n1) has been selected from the terminal
           * set and is therefore adjacent to an already mapped atom (in
           * core_1). Rather than select pair.n2 from all atoms (0...n2) we can
           * select it from the neighbors of this mapped atom (0...deg(nbor))
           * since it must also be adajcent to this mapped atom!
           */
          if (!pair.hasiter) {
            RDK_ADJ_ITER n1iter_beg, n1iter_end;
            boost::tie(n1iter_beg, n1iter_end)
            = boost::adjacent_vertices(pair.n1,*g1);

            while (n1iter_beg!=n1iter_end && core_1[*n1iter_beg]==NULL_NODE)
              ++n1iter_beg;

            assert(n1iter_beg!=n1iter_end);

            boost::tie(pair.nbrbeg, pair.nbrend)
              = boost::adjacent_vertices(core_1[*n1iter_beg],*g2);
            pair.hasiter = true;
          }
        }
        else if (pair.n1==0 && order!=NULL) {
          // Optimisation: if the order vector is laid out in a DFS/BFS then this
          // loop can be replaced with:
          //   pair.n1=order[core_len];
          // :)
          unsigned int i=0;
          while (i<n1 && core_1[pair.n1=order[i]] != NULL_NODE)
            i++;
          if (i==n1)
            pair.n1=n1;
        }
        else {
          while (pair.n1<n1 && core_1[pair.n1]!=NULL_NODE ){
            pair.n1++;
            pair.n2=0;
          }
        }

        /* VF2 Plus iterator available? */
        if (pair.hasiter) {
          while (pair.nbrbeg < pair.nbrend && core_2[*pair.nbrbeg]!=NULL_NODE) {
            ++pair.nbrbeg;
          }

          if (pair.nbrbeg < pair.nbrend) {
              pair.n2 = *pair.nbrbeg;
              ++pair.nbrbeg;
          }
          else {
              pair.n2 = n2;
          }
        }
        else if (t1_len>core_len && t2_len>core_len) {
          while (pair.n2<n2 &&
                 (core_2[pair.n2]!=NULL_NODE || term_2[pair.n2]==0) ) {
            pair.n2++;
          }
        }
        else {
          while (pair.n2<n2 && core_2[pair.n2]!=NULL_NODE ){
            pair.n2++;
          }
        }
        return pair.n1 < n1 && pair.n2 < n2;
      };
      bool IsFeasiblePair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_1[node1] == NULL_NODE);
        assert(core_2[node2] == NULL_NODE);

        //std::cerr<<"  ifp:"<<node1<<"-"<<node2<<" "<<vs_compared->size()<<std::endl;
        // int &isCompat=vs_compared[node1*n2+node2];
        // if(isCompat==0){
        //   isCompat=vc(node1,node2)?1:-1;
        // }
        // if( isCompat<0 ){
        //   //std::cerr<<"  short1"<<std::endl;
        //   return false;
        // }

        // O(1) check for adjacency list
        if (boost::out_degree(node1,*g1)>boost::out_degree(node2,*g2))
          return false;
        if(!vc(node1,node2)) return false;

        unsigned int other1, other2;
#ifdef RDK_VF2_PRUNING
        unsigned int term1 = 0, term2 = 0;
        unsigned int new1 = 0, new2 = 0;
#endif

        // Check the out edges of node1
        typename Graph::out_edge_iterator bNbrs,eNbrs;
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          other1=getOtherIdx(*g1,*bNbrs,node1);
          if (core_1[other1] != NULL_NODE) {
            other2 = core_1[other1];
            typename Graph::edge_descriptor oEdge;
            bool found;
            boost::tie(oEdge,found) = boost::edge(node2,other2,*g2);
            if(!found || !ec(*bNbrs,oEdge) ){
              //std::cerr<<"  short2"<<std::endl;
              return false;
            }
          }
#ifdef RDK_VF2_PRUNING
          else {
            if (term_1[other1]) ++term1;
            if (!term_1[other1]) ++new1;
          }
#endif
          ++bNbrs;
        }

#ifdef RDK_VF2_PRUNING
        // Check the out edges of node2
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          other2=getOtherIdx(*g2,*bNbrs,node2);
          if (core_2[other2] != NULL_NODE) {
            // do nothing
          } else {
            if (term_2[other2]) ++term2;
            if (!term_2[other2]) ++new2;
          }
          ++bNbrs;
        }
        //std::cerr<<(termin1 <= termin2 && termout1 <= termout2 && (termin1+termout1+new1)<=(termin2+termout2+new2))<<std::endl;

        // n.b. term1+new1 == boost::out_degree(node1) and
        //      term2+new2 == boost::out_degree(node2)
        return term1 <= term2 && (term1+new1)<=(term2+new2);
#else
        return true;
#endif
      };
      void AddPair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_len < n1);
        assert(core_len < n2);

        ++core_len;
        if (!term_1[node1]) {
          term_1[node1] = core_len;
          ++t1_len;
        }

        if (!term_2[node2]) {
          term_2[node2] = core_len;
          ++t2_len;
        }

        core_1[node1] = node2;
        core_2[node2] = node1;

        typename Graph::out_edge_iterator bNbrs,eNbrs;
        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g1,*bNbrs,node1);
          if (!term_1[other]) {
            term_1[other] = core_len;
            ++t1_len;
          }
          ++bNbrs;
        }

        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
          if (!term_2[other]) {
            term_2[other] = core_len;
            ++t2_len;
          }
          ++bNbrs;
        }
      };
      void GetCoreSet(node_id c1[], node_id c2[]){
        unsigned int i, j;
        for (i = 0, j = 0; i < n1; ++i){
          if (core_1[i] != NULL_NODE) {
            c1[j] = i;
            c2[j] = core_1[i];
            ++j;
          }
        }

      };
      VF2SubState *Clone(){
        return new VF2SubState(*this);
      };
      void BackTrack(node_id node1, node_id node2){
        if (term_1[node1] == core_len) {
          term_1[node1] = 0;
          --t1_len;
        }

        typename Graph::out_edge_iterator bNbrs,eNbrs;
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g1,*bNbrs,node1);
          if (term_1[other] == core_len) {
            term_1[other] = 0;
            --t1_len;
          }
          ++bNbrs;
        }

        if (term_2[node2] == core_len) {
          term_2[node2] = 0;
          --t2_len;
        }

        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
          if (term_2[other] == core_len) {
            term_2[other] = 0;
            --t2_len;
          }
          ++bNbrs;
        }

        core_1[node1] = NULL_NODE;
        core_2[node2] = NULL_NODE;
        --core_len;
      };
      bool Match(node_id c1[], node_id c2[])
      {
        if (IsGoal()) {
          GetCoreSet(c1, c2);
          if(MatchChecks(c1,c2))
            return true;
        }

        if (IsDead())
          return false;

        Pair<Graph> pair;
        while (NextPair(pair)) {
          if (IsFeasiblePair(pair.n1, pair.n2)){
            AddPair(pair.n1, pair.n2);
            if (Match(c1, c2)) // recurse
              return true;
            BackTrack(pair.n1, pair.n2);
          }
        }
        return false;
      }

      template <class DoubleBackInsertionSequence>
      bool MatchAll(node_id c1[], node_id c2[],
                    DoubleBackInsertionSequence &res, unsigned int lim=0)
      {
        if (IsGoal()) {
          GetCoreSet(c1, c2);
          if(MatchChecks(c1,c2)) {
            typename DoubleBackInsertionSequence::value_type newSeq;
            for(unsigned int i=0;i<core_len;++i){
              newSeq.push_back(std::pair<int,int>(c1[i],c2[i]));
            }
            res.push_back(newSeq);
            return lim && res.size() >= lim;
          }
        }

        if (IsDead())
          return false;

        Pair<Graph> pair;
        while (NextPair(pair)) {
          if (IsFeasiblePair(pair.n1, pair.n2)){
            AddPair(pair.n1, pair.n2);
            if (MatchAll(c1, c2, res, lim)) // recurse
              return true;
            BackTrack(pair.n1, pair.n2);
          }
        }
        return false;
      }
    };

    /*-------------------------------------------------------------
     * static bool match(pn, c1, c2, s)
     * Finds a matching between two graphs, if it exists, starting
     * from state s.
     * Returns true a match has been found.
     * *pn is assigned the numbero of matched nodes, and
     * c1 and c2 will contain the ids of the corresponding nodes
     * in the two graphs.
     ------------------------------------------------------------*/
    template <class SubState>
    bool match(int *pn, node_id c1[], node_id c2[], SubState &s)
    {
      if (s.Match(c1, c2)) {
        // not needed, pn = num query atoms (n1)...
        *pn=s.CoreLen();
        return true;
      }
      return false;
    }

    /*-------------------------------------------------------------
     * static bool match(c1, c2, vis, usr_data, pcount)
     * Visits all the matchings between two graphs,  starting
     * from state s.
     * Returns true if the caller must stop the visit.
     * Stops when max_results is reached, set max_results to 0 to
     * keep going until there are no more matches
     *
     ------------------------------------------------------------*/
    template <class SubState,class DoubleBackInsertionSequence>
    bool match(node_id c1[], node_id c2[], SubState &s, DoubleBackInsertionSequence &res,
               unsigned int max_results) {
      s.MatchAll(c1, c2, res, max_results);
      return !res.empty();
    }
  }; //end of namespace detail

  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class BackInsertionSequence   // contains std::pair<vertex_descriptor,vertex_descriptor>
              >
  bool vf2(const Graph &g1,const Graph &g2,
           VertexLabeling& vertex_labeling,
           EdgeLabeling& edge_labeling,
           MatchChecking& match_checking,
           BackInsertionSequence& F){
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];
    int n=0;

    F.clear();
    F.resize(0);
    if(match(&n,ni1,ni2,s0)){
      for(unsigned int i=0;i<num_vertices(g1);i++){
        F.push_back(std::pair<int,int>(ni1[i],ni2[i]));
      }
    }
    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class DoubleBackInsertionSequence   // contains a back insertion sequence
              >
  bool vf2_all(const Graph& g1, const Graph& g2,
               VertexLabeling& vertex_labeling,
               EdgeLabeling& edge_labeling,
               MatchChecking& match_checking,
               DoubleBackInsertionSequence& F,
               unsigned int max_results=1000) {
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];

    F.clear();
    F.resize(0);

    match(ni1,ni2,s0,F,max_results);

    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
} // end of namespace boost
#endif

#undef RDK_VF2_PRUNING
#undef RDK_ADJ_ITER


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include "poplibs_support/TestDevice.hpp"

#include <boost/multi_array.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <boost/program_options.hpp>

#include <poplar/Engine.hpp>
#include <poplar/Graph.hpp>

#include "poputil/exceptions.hpp"
#include <poplibs_support/Algorithm.hpp>
#include <poplibs_test/GeneralMatrixMultiply.hpp>
#include <poplibs_test/SparseMatrix.hpp>
#include <poplibs_test/Util.hpp>
#include <poputil/TileMapping.hpp>

#include "poplin/codelets.hpp"
#include "popops/codelets.hpp"

#include "popsparse/MatMul.hpp"
#include "popsparse/SparsePartitioner.hpp"
#include "popsparse/codelets.hpp"

#include <fstream>

// Tolerances used when data is not ignored
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1

using namespace poplar;
using namespace poplar::program;

using namespace poplibs_test::util;
using namespace poplibs_support;

using namespace popsparse;
using namespace popsparse::dynamic;
using namespace poputil;

// Tolerances used when data is not ignored
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1

int main(int argc, char **argv) try {
  namespace po = boost::program_options;

  DeviceType deviceType = DeviceType::IpuModel2;
  constexpr unsigned numIPUs = 1;
  boost::optional<unsigned> tilesPerIPU;
  std::string profileJsonPath;
  unsigned groups = 1, m, k, n;
  double sparsityFactor;
  Type dataType = HALF;
  Type partialsType = FLOAT;
  ShapeOption<std::size_t> weightedAreaBegin;
  ShapeOption<std::size_t> weightedAreaEnd;
  ShapeOption<std::size_t> blockSize;

  weightedAreaBegin.val = weightedAreaEnd.val = {0, 0};
  double weightedAreaWeighting = 1.0;
  bool transposeLHS = false;
  bool transposeRHS = false;
  std::string matmulOptionsString;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("compile-only", "Stop after compilation; don't run the program")
    ("device-type",
     po::value<DeviceType>(&deviceType)->default_value(deviceType),
     deviceTypeHelp)
    ("profile", "Enable profiling and print profiling report")
    ("profile-json", po::value<std::string>(&profileJsonPath)->default_value(profileJsonPath),
     "Path to a file into which the profiling report will be output in json format")
    ("ignore-data", "Don't validate results")
    ("data-type", po::value(&dataType)->default_value(dataType), "Data type of operands")
    ("groups", po::value(&groups)->default_value(groups), "Number of groups")
    ("m", po::value(&m)->required(), "Rows in left-hand operand")
    ("k", po::value(&k)->required(), "Columns in left-hand operand/Rows in right-hand operand")
    ("n", po::value(&n)->required(), "Columns in right-hand operand")
    ("sparsity-factor", po::value(&sparsityFactor)->required(),
     "Proportion of elements of left-hand operand which are non-zero")
    ("block-size",
     po::value<ShapeOption<std::size_t>>(&blockSize)->default_value(1),
     "Block size as rows and columns (only square blocks are supported)")
    ("transpose-lhs", po::value(&transposeLHS),
     "Transpose the left-hand operand of the matmul such that the matmul "
     "becomes {k, m} * {m, n} = {k, n}")
    ("transpose-rhs", po::value(&transposeRHS),
     "Transpose the right-hand operand of the matmul")
    ("weighted-area-begin",
     po::value<ShapeOption<std::size_t>>(&weightedAreaBegin)->default_value(weightedAreaBegin),
     "Starting indices of an area of the sparse operand with a different "
     "level of sparsity to the rest")
    ("weighted-area-end",
     po::value<ShapeOption<std::size_t>>(&weightedAreaEnd)->default_value(weightedAreaEnd),
     "Ending indices of an area of the sparse operand with a different "
     "level of sparsity to the rest")
    ("weighted-area-weighting",
     po::value<double>(&weightedAreaWeighting)->default_value(weightedAreaWeighting),
     "Weighting for probability that a sparse element resides within the "
     "specified area")
    ("tiles-per-ipu", po::value(&tilesPerIPU), "Number of tiles per IPU")
    ("matmul-options", po::value<std::string>(&matmulOptionsString),
     "Options to use for the matrix multiplication, specified as a JSON "
     "string, e.g. {\"key\":\"value\"}")
  ;
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  bool profile = vm.count("profile");
  bool profilingEnabled = profile || !profileJsonPath.empty();
  bool ignoreData = vm.count("ignore-data");

  const std::size_t blockRows = blockSize[0];
  const std::size_t blockCols =
      blockSize.val.size() == 1 ? blockRows : blockSize[1];
  const auto blockArea = blockRows * blockCols;

  if (m % blockRows) {
    throw poputil::poplibs_error("Input size must be an integer multiple of "
                                 "rows in a block");
  }

  if (k % blockCols) {
    throw poputil::poplibs_error("output size must be an integer multiple of "
                                 "columns in a block");
  }

  // align weighted area to a block size grid
  weightedAreaBegin.val[0] = roundDown(weightedAreaBegin.val[0], blockRows);
  weightedAreaBegin.val[1] = roundDown(weightedAreaBegin.val[1], blockCols);
  weightedAreaEnd.val[0] = roundDown(weightedAreaEnd.val[0], blockRows);
  weightedAreaEnd.val[1] = roundDown(weightedAreaEnd.val[1], blockCols);

  PlanningCache cache;

  OptionFlags matmulOptions;
  // User options specified via --matmul-options override defaults
  if (!matmulOptionsString.empty()) {
    poplar::readJSON(matmulOptionsString, matmulOptions);
  }

  auto device = tilesPerIPU
                    ? createTestDevice(deviceType, numIPUs, *tilesPerIPU, true)
                    : createTestDeviceFullSize(deviceType, numIPUs, true);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);
  poplin::addCodelets(graph);
  popsparse::addCodelets(graph);
  Sequence uploadProg, prog, downloadProg;

  const auto sparsityType =
      blockArea == 1 ? SparsityType::Element : SparsityType::Block;
  SparsityParams sparsityParams(sparsityType, SparsityStructure::Unstructured,
                                {blockRows, blockCols});
  const auto params = MatMulParams::createWithNzRatio(
      std::move(sparsityParams), sparsityFactor, groups, m, k, n);
  // No support for groups yet
  assert(groups == 1);
  const SparseTensor lhs = createSparseDenseMatMulLHS(
      graph, dataType, params, "lhs", matmulOptions, &cache);

  Tensor rhs;
  if (transposeLHS) {
    std::vector<std::size_t> rhsShape = {groups, m, n};
    if (transposeRHS) {
      std::swap(rhsShape.at(1), rhsShape.at(2));
    }
    // If the left-hand operand is to be transposed, the right-hand
    // operand becomes of shape {m, n}. We don't have a way to allocate this
    // correctly externally so just allocate linearly in this case.
    rhs = graph.addVariable(dataType, rhsShape, "rhs");
    poputil::mapTensorLinearly(graph, rhs);
  } else {
    rhs = createSparseDenseMatMulRHS(graph, dataType, params, "rhs",
                                     matmulOptions, &cache);
    if (transposeRHS) {
      rhs = rhs.dimRoll(1, 2);
    }
  }

  const Tensor out =
      sparseDenseMatMul(graph, lhs, rhs, prog, transposeLHS, transposeRHS,
                        "multiply", matmulOptions, &cache);

  std::vector<std::pair<std::string, char *>> tmap;
  auto rawMetaInfo =
      allocateHostMemoryForTensor(lhs.getMetaInfoTensor(), "lhs.meta", graph,
                                  uploadProg, downloadProg, tmap);
  auto rawNzInfo =
      allocateHostMemoryForTensor(lhs.getNzValuesTensor(), "lhs.values", graph,
                                  uploadProg, downloadProg, tmap);
  auto rawRHS = allocateHostMemoryForTensor(rhs, "rhs", graph, uploadProg,
                                            downloadProg, tmap);
  auto rawOut = allocateHostMemoryForTensor(out, "out", graph, uploadProg,
                                            downloadProg, tmap);

  Sequence controlProg(std::move(uploadProg), std::move(prog));
  if (!ignoreData) {
    controlProg.add(downloadProg);
  }

  using EType = float;
  Partitioner<EType> partitioner(params, dataType, target, matmulOptions,
                                 &cache);

  OptionFlags engineOptions;
  if (profilingEnabled) {
    engineOptions.set("debug.instrument", "true");
  }
  Engine engine(graph, std::move(controlProg), engineOptions);

  if (vm.count("compile-only"))
    return 0;

  attachStreams(engine, tmap);

  std::mt19937 randomEngine;

  const bool floatingPointCouldRepresentMaxAccum = [&] {
    const auto maxVal = maxContiguousInteger(dataType);

    double weightedThreshold, remainingThreshold;
    std::tie(weightedThreshold, remainingThreshold) =
        poplibs_test::sparse::calculateWeightedVsRemainingSparsityFactor(
            {params.getM() / blockRows, params.getK() / blockCols},
            sparsityFactor,
            {weightedAreaBegin[0] / blockRows,
             weightedAreaBegin[1] / blockCols},
            {weightedAreaEnd[0] / blockRows, weightedAreaEnd[1] / blockCols},
            weightedAreaWeighting);
    const auto numWeightedK = (weightedAreaEnd[1] - weightedAreaBegin[1]);
    const auto numWeightedM = (weightedAreaEnd[0] - weightedAreaBegin[0]);
    std::size_t maxK = numWeightedK * weightedThreshold +
                       (params.getK() - numWeightedK) * remainingThreshold;
    std::size_t maxM = numWeightedM * weightedThreshold +
                       (params.getM() - numWeightedM) * remainingThreshold;
    maxM = roundDown(maxM, blockRows);
    maxK = roundDown(maxK, blockCols);
    const auto getOpsPerOutputElementEstimate =
        [&](const bool lhsTransposed) -> int {
      const auto numAccumulations = lhsTransposed ? maxM : maxK;
      return numAccumulations;
    };
    // We use a modifier to account for the unlikeliness of picking all positive
    // or negative 1s which would actually get us to the max precisely
    // represented integer.
    constexpr int modifier = 10;
    // We use another modifier to account for the chance that sparsity is not
    // perfectly evenly spread in this instant.
    constexpr double wiggleRoom = 1.3;
    if (wiggleRoom * getOpsPerOutputElementEstimate(transposeLHS) >
        maxVal * modifier) {
      return false;
    }
    return true;
  }();
  bool useBipolarDistribution = floatingPointCouldRepresentMaxAccum;
  std::array<std::size_t, 2> blockDims = {blockRows, blockCols};
  CSRMatrix<EType> csrMatrix(blockDims);
  std::tie(csrMatrix.nzValues, csrMatrix.columnIndices, csrMatrix.rowIndices) =
      poplibs_test::sparse::buildCSRMatrix<EType, std::size_t>(
          randomEngine, {m, k}, {blockRows, blockCols}, sparsityFactor,
          weightedAreaBegin, weightedAreaEnd, weightedAreaWeighting,
          useBipolarDistribution);
  boost::multi_array<double, 2> hostRHS(boost::extents[rhs.dim(1)][rhs.dim(2)]);
  if (useBipolarDistribution) {
    writeRandomBinaryValues(target, dataType, hostRHS, -1.0, 1.0, randomEngine);
  } else {
    writeRandomValues(target, dataType, hostRHS, -3.0, 3.0, randomEngine);
  }
  boost::multi_array<double, 2> hostOut(boost::extents[out.dim(1)][out.dim(2)]);

  const auto buckets = partitioner.createSparsityDataImpl(csrMatrix);
  copy(target, hostRHS, dataType, rawRHS.get());
  copy(target, buckets.metaInfo, UNSIGNED_SHORT, rawMetaInfo.get());
  copy(target, buckets.nzValues, dataType, rawNzInfo.get());

  device.bind([&](const Device &d) { engine.loadAndRun(d); });

  bool matchesModel = true;
  if (!ignoreData) {
    const double relTolerance = dataType == HALF ? HALF_REL_TOL : FLOAT_REL_TOL;
    copy(target, out.elementType(), rawOut.get(), hostOut);
    boost::multi_array<double, 2> hostDenseLHS(boost::extents[m][k]);
    boost::multi_array<double, 2> modelOut(
        boost::extents[out.dim(1)][out.dim(2)]);
    hostDenseLHS = poplibs_test::sparse::csrToDenseMatrix(
        csrMatrix.nzValues.data(), csrMatrix.columnIndices.data(),
        csrMatrix.rowIndices.data(), csrMatrix.nzValues.size(), m, k, blockRows,
        blockCols);
    poplibs_test::gemm::generalMatrixMultiply(hostDenseLHS, hostRHS, modelOut,
                                              transposeLHS, transposeRHS);
    matchesModel &= checkIsClose("out", hostOut, modelOut, relTolerance);
  }

  if (profile) {
    engine.printProfileSummary(std::cout, {{"showExecutionSteps", "true"}});
  }
  if (!profileJsonPath.empty()) {
    std::ofstream os(profileJsonPath, std::ios_base::out);
    const auto &pr = engine.getProfile();
    poplar::serializeToJSON(os, pr);
  }

  if (!matchesModel) {
    std::cerr << "Validation failed\n";
    return 1;
  }

  return 0;
} catch (const poplar::graph_memory_allocation_error &e) {
  if (e.graphProfile.type() == ProfileValue::Type::MAP) {
    poplar::printGraphSummary(std::cerr, e.graphProfile,
                              {{"showVarStorage", "true"}});
  }
  throw;
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_WORLD_MAP_MAP_INTERFACE_HPP_
#define MODULES_WORLD_MAP_MAP_INTERFACE_HPP_

#include <vector>
#include <string>
#include <utility>
#include <boost/geometry/index/rtree.hpp>

#include "modules/geometry/geometry.hpp"
#include "modules/world/map/map_interface.hpp"
#include "modules/world/map/roadgraph.hpp"
#include "modules/world/map/driving_corridor.hpp"

namespace modules {
namespace world {
namespace map {

using modules::world::opendrive::LanePtr;
using modules::world::opendrive::LaneId;
using modules::world::opendrive::OpenDriveMapPtr;
using modules::geometry::Point2d;
using modules::geometry::Line;

using rtree_lane_model = boost::geometry::model::segment<Point2d>;
using rtree_lane_id = LanePtr;
using rtree_lane_value = std::pair<rtree_lane_model, rtree_lane_id>;
using rtree_lane = boost::geometry::index::rtree<rtree_lane_value,
                   boost::geometry::index::linear<16, 4> >;
using PathBoundaries = std::vector<std::pair<LanePtr, LanePtr>>;

class MapInterface {
 public:
  bool interface_from_opendrive(const OpenDriveMapPtr& open_drive_map);

  void ConcatenateLines(const std::vector<LanePtr>& lanes,
                        Line& line_of_corridor,
                        std::vector< std::pair<int, LaneId> >& lane_ids);

  /*
  * Finds the ID's of the nearest lanes to point
  * Note that the point doesn't necessarily lie within the lane of the closest point
  * or inside the lane of any of them.
  */
  bool FindNearestLanes(const Point2d& point,
                         const unsigned& num_lanes,
                         std::vector<opendrive::LanePtr>& lanes,
                         bool type_driving_only = true) const;

  LanePtr FindLane(const Point2d& point) const;

  bool HasCorrectDrivingDirection(const Point2d& point, const float orientation) const;

  bool LineSegmentInsideCorridor(const DrivingCorridorPtr corridor, const Point2d& p1, const Point2d& p2) const;

  bool IsInLane(const Point2d& point, LaneId id) const;

  //std::pair< std::vector<LanePtr>, std::vector<LanePtr> > ComputeLaneBoundariesHorizon(
  //                                const LaneId& startid, const LaneId& goalid) const;

  DrivingCorridor ComputeDrivingCorridorFromStartToGoal(const LaneId& startid, const LaneId& goalid);

  DrivingCorridor ComputeDrivingCorridorForRange(std::vector<LaneId> lane_ids);

  bool ComputeAllDrivingCorridors();

  std::vector<PathBoundaries> ComputeAllPathBoundaries(const std::vector<LaneId>& lane_ids) const;

  std::pair<LanePtr, bool> get_inner_neighbor(const LaneId lane_id) const;
  std::pair<LanePtr, bool> get_outer_neighbor(const LaneId lane_id) const;
  std::vector<LaneId> get_successor_lanes(const LaneId lane_id) const;

  std::vector<DrivingCorridorPtr> GetAdjacentDrivingCorridorsSameDirection(const DrivingCorridorPtr corridor, const Pose& pose);
  std::vector<DrivingCorridorPtr> GetSplittingDrivingCorridors(const DrivingCorridorPtr corridor, const Pose& pose);

  virtual std::pair<Point2d, Point2d> BoundingBox() const { return bounding_box_;}

  bool set_open_drive_map(OpenDriveMapPtr map) {
    open_drive_map_ = map;
    interface_from_opendrive(open_drive_map_);
    return true;
  }

  bool set_roadgraph(RoadgraphPtr roadgraph) {
    roadgraph_ = roadgraph;
    return true;
  }

  LanePtr get_lane(const LaneId& id) const {
    return roadgraph_->get_laneptr(id);
  }

  std::vector<DrivingCorridorPtr> get_all_corridors() const { return all_corridors_; }

  //! Functions
  OpenDriveMapPtr get_open_drive_map() { return open_drive_map_; }
  RoadgraphPtr get_roadgraph() { return roadgraph_; }

 private:
  OpenDriveMapPtr open_drive_map_;
  RoadgraphPtr roadgraph_;
  std::vector<DrivingCorridorPtr> all_corridors_;
  rtree_lane rtree_lane_;
  std::pair<Point2d, Point2d> bounding_box_;

  static bool is_lane_type(rtree_lane_value const &m) {return (m.second->get_lane_type() == LaneType::DRIVING); }

};

using MapInterfacePtr = std::shared_ptr<MapInterface>;

}  // namespace map
}  // namespace world
}  // namespace modules

#endif  // MODULES_WORLD_MAP_MAP_INTERFACE_HPP_


#include <util/CuckooFilter.h>
#include <util/Int2String.h>

#include <boost/test/unit_test.hpp>
#include <boost/timer.hpp>
#include <boost/thread.hpp>

#include <string>
#include <iostream>

BOOST_AUTO_TEST_CASE(CuckooFilter_test)
{
    const size_t bits_per_item = 12;
    const size_t total_items   = 1000000;

    // simple one:
    typedef izenelib::util::CuckooFilter<size_t, bits_per_item> CuckooFilterType;
    CuckooFilterType filter(total_items);
    // insert items
    size_t num_inserted = 0;
    for (size_t i = 0; i < total_items; i++, num_inserted++) {
        if (filter.Add(i) != CuckooFilterType::Ok) {
            break;
        }
    }

    // check existing items:
    // every item should be there
    for (size_t i = 0; i < num_inserted; i++) {
        BOOST_CHECK(filter.Contain(i) == CuckooFilterType::Ok);
    }

    // checking non-existing items
    // there are false positives
    size_t total_queries = 0;
    size_t false_queries = 0;
    for (size_t i = total_items; i < 2 * total_items; i++) {
        if (filter.Contain(i) == CuckooFilterType::Ok) {
            false_queries++;
        }
        total_queries++;
    }

}


#include <boost/test/unit_test.hpp>
#include <boost/dynamic_bitset.hpp>
#include "variant/variant_reader.h"
#include "variant/variant.h"

using namespace std;
using namespace gamgee;
using namespace boost;

constexpr auto GQ_THRESH = 35;

BOOST_AUTO_TEST_CASE( select_if_individual_fields ) {
  const auto actual_gq_selects = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("111")),  //Beware that in setting dynamic_bitset that way
    dynamic_bitset<>(string("110")),  //the bit order is inversed:  Here it is actually 011
    dynamic_bitset<>(string("100")),
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("011"))};
  const auto actual_pl_selects = vector<dynamic_bitset<>>{
    dynamic_bitset<>(string("010")),
    dynamic_bitset<>(string("000")),
    dynamic_bitset<>(string("001")),
    dynamic_bitset<>(string("111")),
    dynamic_bitset<>(string("100"))};
  for (const auto& filename : {"testdata/test_variants_02.vcf"}) {
    const auto reader = SingleVariantReader{filename};
    auto record_idx = 0u;
    for (const auto& record : SingleVariantReader{filename}) {
      const auto g_quals = record.integer_individual_field("GQ");
      const auto p_likes = record.integer_individual_field("PL");
      const auto high_gq = [](const IndividualFieldValue<int32_t>& x) { return x[0] >= GQ_THRESH; };
      const auto hom_ref = [](const IndividualFieldValue<int32_t>& x) { return x[0] == 0; };
      const auto comput_gq_select = Variant::select_if(g_quals.begin(), g_quals.end(), high_gq);
      const auto comput_pl_select = Variant::select_if(p_likes.begin(), p_likes.end(), hom_ref);
      BOOST_CHECK_EQUAL(comput_gq_select.size(), 3u);
      BOOST_CHECK_EQUAL(comput_pl_select.size(), 3u);
      const auto actual_gq_select = actual_gq_selects[record_idx];
      const auto actual_pl_select = actual_pl_selects[record_idx];
      BOOST_CHECK(comput_pl_select == actual_pl_select);
      ++record_idx;
    }
    BOOST_CHECK_EQUAL(record_idx, 5u);
  }
}

BOOST_AUTO_TEST_CASE( select_if_shared_field ) {
  const auto truth_an_counts = std::vector<uint32_t>{1,1,1,1,1,1,1};
  const auto truth_af_counts = std::vector<uint32_t>{0,0,0,0,1,0,1};
  auto truth_index = 0u;
  for (const auto& record : SingleVariantReader{"testdata/test_variants.vcf"}) {
    const auto an = record.integer_shared_field("AN");
    const auto r1 = Variant::select_if(an.begin(), an.end(), [](const auto& v) { return v == 6; });
    BOOST_CHECK_EQUAL(r1.count(), truth_an_counts[truth_index]);
    const auto af = record.float_shared_field("AF");
    const auto r2 = Variant::select_if(af.begin(), af.end(), [](const auto& v) { return v < 0.5; });
    BOOST_CHECK_EQUAL(r2.count(), truth_af_counts[truth_index++]);
  }
}


#include <set>
#include <sstream>
#include <iostream>
#include <algorithm>
using std::cout;
using std::endl;
#include <iomanip>
#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/particles/ParticleOperators.h>
#include <spatialops/structured/FieldHelper.h>
#include <spatialops/structured/MatVecFields.h>
#include <spatialops/util/TimeLogger.h>
#include <test/TestHelper.h>
#include <stdio.h>
#include <spatialops/structured/FieldComparisons.h>

#include <spatialops/structured/ExternalAllocators.h>

#include <cfloat>
#include <stdio.h>
#include <fstream>
#include <stdlib.h>

using namespace SpatialOps;
using namespace SpatialOps::Particle;
using std::cout;
using std::endl;

#include <boost/program_options.hpp>
namespace po = boost::program_options;

int main( int iarg, char* carg[] )
{
    int nparticles, minx,miny,minz, maxx,maxy,maxz, pd;

    double h = 1.0f;

    int seed;

    bool sort = false, bf = false;

    po::options_description desc("Supported Options");
    desc.add_options()
    ( "help", "print help message\n" )
    ( "np",   po::value<int>(&nparticles)->default_value(1000), "number of particles" )
    ( "gridminx" , po::value<int>(&minx)->default_value(0), "lower bound for particle x coordinate" )
    ( "gridminy" , po::value<int>(&miny)->default_value(0), "lower bound for particle y coordinate" )
    ( "gridminz" , po::value<int>(&minz)->default_value(0), "lower bound for particle z coordinate" )
    ( "gridmaxx" , po::value<int>(&maxx)->default_value(50), "upper bound for particle x coordinate" )
    ( "gridmaxy" , po::value<int>(&maxy)->default_value(50), "upper bound for particle y coordinate" )
    ( "gridmaxz" , po::value<int>(&maxz)->default_value(50), "upper bound for particle z coordinate" )
    ( "seed" , po::value<int>(&seed)->default_value(10), "seed for particle position generation" )
    ( "h" , po::value<double>(&h)->default_value(1.0f), "smoothing radius for each particle" )
    ( "sort", "sort data?" )
    ( "bf" , "brute force?" )
    ( "pd" , po::value<int>(&pd)->default_value(-1), "particle density per cell, if specified overrides num particles" );

    po::variables_map args;
    po::store( po::parse_command_line(iarg,carg,desc), args );
    po::notify(args);

    if( args.count("sort") ) sort = true;
    if( args.count("bf") ) bf = true;

    if( args.count("help") ){
        cout << desc << endl
        << "Examples:" << endl
        << " range_search_grid --gridminx 5 --sort" << endl
        << " range_search_grid  --seed 1 --bf" << endl
        << endl;
        return -1;
    }

    IntVec gridMin (-1, -1, -1);
    IntVec gridMax (10, 10, 1);

    int fluid_particles(500);
    double height(5); double width(1.5);    // Dam Dimensions
    double dist(sqrt(height*width/fluid_particles));  // Distance (horizontally and vertically) between particles
    int xparticles(width/dist); int yparticles(height/dist); // number of particles in one row of dam
    int domain_height(8); int domain_width(8);
    h=2.5*dist;

    // initializing Dam particle positions*:
    std::vector<DoubleVec> positions;
    std::vector<IntVec> maskSet;

    double ppx,ppy,ppz;

    int i=0;
    for (int dx = 0; dx <= xparticles; dx++) // arranged such that it loops from bot left to top right (going from bottom to top)
        for (int dy = 0; dy <= yparticles; dy++)
            //      for (int z = 0; z < zparticles; z++)
        {
            ppx = dist*dx;
            ppy = dist*dy;
            ppz = 0;

            positions.push_back(DoubleVec(ppx,ppy,ppz));
            positions.push_back(DoubleVec(domain_width - ppx,ppy,ppz));

            i++;
        }
    fluid_particles=i*2; // redefine number of particles
    i=i+1;
    // initializing Boundary Dam positions*:
    int d=0;
    int xbp ((domain_width+dist/1.5)/(dist));

    for (int dx = 0; dx <= xbp ; dx++) //bottom section
        for (int dy = 0; dy <= 1; dy++)
            //      for (int z = 0; z < zparticles; z++)
        {

            ppx = -dist/3+dist*(dx)+dist/2*(dy);
            ppy = -dist*(dy)-dist;
            ppz = 0;
            positions.push_back(DoubleVec(ppx,ppy,ppz));
            i++;
            d++;
        }

    int ybp (((domain_height+4.0*dist)/(dist)));

    for (int j = 0; j <= 1 ; j++)
        for (int dx = 0; dx <= 1 ; dx++)
            for (int dy = 0; dy <= ybp; dy++)
                //      for (int z = 0; z < zparticles; z++)
            {
                if (j==0){                                       // left wall
                    ppx = -dist*dx -dist;
                    ppy = -2*dist+dist*dy+dist/2*dx;
                    ppz = 0;
                    positions.push_back(DoubleVec(ppx,ppy,ppz));
                    i++;
                }
                else{
                    ppx = domain_width + dist*dx + dist;
                    ppy = -2*dist+dist*dy+dist/2*dx;
                    ppz = 0;
                    positions.push_back(DoubleVec(ppx,ppy,ppz));
                    i++;
                }
                d++;
            }

    int boundary_particles=d; // redefine number of boundary particles
    nparticles=fluid_particles+boundary_particles; // total number of particles

    const GhostData pg(0);
    const BoundaryCellInfo pbc = BoundaryCellInfo::build<SpatialOps::Particle::ParticleField>();
    const MemoryWindow pmwRS( get_window_with_ghost( IntVec(nparticles,0,0), pg, pbc) );

    SpatialOps::Particle::ParticleField mapped_val_field( pmwRS, pbc, pg, NULL );

    FieldVector<ParticleField> properties (5, pmwRS, pbc, pg);
    FieldVector<ParticleField> out (4, pmwRS, pbc, pg);
    FieldVector<ParticleField> pCoord (3, pmwRS, pbc, pg);

    for (int i= 0; i < nparticles; i++)
    {

        pCoord.at(0)[i]    = positions[i][0];
        pCoord.at(1)[i]    = positions[i][1];
        pCoord.at(2)[i]    = positions[i][2];

        if (i>=fluid_particles){
            maskSet.push_back( IntVec(i, 0, 0) );
        }
    }

    SpatialMask<ParticleField> maskx( pCoord.at(0), maskSet );
    SpatialMask<ParticleField> masky( pCoord.at(1), maskSet );
    SpatialMask<ParticleField> maskz( pCoord.at(2), maskSet );

    // this loop initializes the vector of properties of size 4 with random values
    for (size_t i = 0; i < properties.elements(); i++)
    {
        if (i==0)
            properties.at(0) <<= 1.0;   // density is 1000
        else
            properties.at(i) <<= 0.0;
        // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   set xvelcity to be 1
    }

    // initializes the output properties in the vector of fields of size 2 with 0
    for (size_t i = 0; i < out.elements(); i++)
    {
        out.at(i) <<= 0.0;
    }

    //   SpatialOps::SpatialMask<SpatialOps::Particle::ParticleField> mask( pCoord.at(0), maskSet );

    short int location = CPU_INDEX;

# ifdef ENABLE_CUDA
    pCoord.set_device_as_active(GPU_INDEX);
    properties.set_device_as_active(GPU_INDEX);
    out.set_device_as_active(GPU_INDEX);
    maskx.add_consumer(GPU_INDEX);
    masky.add_consumer(GPU_INDEX);
    maskz.add_consumer(GPU_INDEX);
    location = GPU_INDEX;
# endif

    /*
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     out(0)=density
     out(1)=xvelocity
     out(2)=yvelocity
     out(3)=zvelocity

     properties(0)=density
     properties(1)=xvelocity
     properties(2)=yvelocity
     properties(3)=zvelocity
     properties(4)=pressure

     rij= pow( pow(local(pCoord.at(0))-pCoord.at(0),2) + pow(local(pCoord.at(1))-pCoord.at(1),2),0.5)
     xvelocity_ij * xij = (local(properties.at(1)) - properties.at(1))*(local(pCoord.at(0))-pCoord.at(0))

     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

     */

    double rho0(1.0);
    double mass_p=(height*width*rho0/fluid_particles); // mass of each particle

    double pi(3.14);
    double c0 =10.0*sqrt(2*1.81*height); // reference speed of sound usually defined as 10*max(velocity)
    int gamma=7; // used in tait equation of state.
    double beta =rho0*c0*c0/gamma; // constant for tait equation
    double gravity(-1.81);

    double sim_time(0.0);
    double end_time(3.2);
    double delta_time(.0001); int iterations(0);
    double detla_t_io=.03; double output_time=0;
    h=1.25*dist;

    int file_count = 1;

    // for 2D
//    h=h/2;
    while (sim_time<=end_time){

        RangeSearch rangeSearch(pmwRS, pg, pbc, gridMin, gridMax, nparticles, h, pCoord.at(0),pCoord.at(1),!bf, sort, location);

        // Density eval

        out.at(0) <<= nebo_mapped_reduction (mass_p*
                                             ((local(properties.at(1)) - properties.at(1))*(local(pCoord.at(0))-pCoord.at(0)) // (xvelocity_ij * x_ij + ...
                                              + (local(properties.at(2)) - properties.at(2))*(local(pCoord.at(1))-pCoord.at(1))) // yvelocity_ij * y_ij) * ...
                                             *
                                             (cond( (mapped_value() > pow(10,-12)), // to avoid NaNs
                                                   cond( (mapped_value()/h <= 2),
                                                        ( 7/(4*pi*pow(h,3))) * // alpha *
                                                        (-5*mapped_value()/h    * // -5*r.ij/h *
                                                         pow(    1   -   .5  *  mapped_value()/h     ,3)) // (1- 0.5*r.ij/h)^3 /
                                                        /mapped_value() // rij
                                                        )
                                                   (0)) // else (0)
                                              (0)) // else 0                   dW/dr
                                             , rangeSearch.GetCurrentState(location));

        // xvelocity eval

        out.at(1) <<= nebo_mapped_reduction (mass_p *

                                             (local(properties.at(4))/pow(local(properties.at(0)),2) //    ( pressure_i / (rho_i) ^ 2 + ...
                                              + properties.at(4)/pow(properties.at(0),2))          //      pressure_j / (rho_j) ^ 2 )
                                             *
                                             (cond( mapped_value() > pow(10,-12), // to avoid NaNs

                                                   cond( (mapped_value()/h) <= 2,

                                                        (local(pCoord.at(0))-pCoord.at(0))  // x_ij  * only value that changes for velocity calc
                                                        *( 7/(4*pi*pow(h,3))) * //alpha *
                                                        (-5*mapped_value()/h    * //-5*r.ij/h *
                                                         pow(    1   -   (.5  *   mapped_value())/h     ,3)) // (1- (0.5*r.ij/h)^3 /
                                                        /mapped_value() //rij
                                                        )
                                                   (0)) //else (0)
                                              (0)) // else 0                   dW/dr                                  )
                                             , rangeSearch.GetCurrentState(location));

        // yvelocity eval

        out.at(2) <<= nebo_mapped_reduction (mass_p *
                                             (local(properties.at(4))/pow(local(properties.at(0)),2) //    ( pressure_i / (rho_i) ^ 2 + ...
                                              + properties.at(4)/pow(properties.at(0),2))          //      pressure_j / (rho_j) ^ 2 )
                                             * (   // dW/dr:

                                                cond( mapped_value() > pow(10,-12), // to avoid NaNs
                                                     (
                                                      cond( (mapped_value()/h <= 2), // if rij/h<=2

                                                           (
                                                            (local(pCoord.at(1))-pCoord.at(1))  // y_ij
                                                            *7/(4*pi*pow(h,3)))   * //alpha

                                                           (-5*mapped_value()/h    * //-5*r.ij/h

                                                            pow(    1   -   (.5  *   mapped_value())/h     ,3)) // (1- (0.5*r.ij/h)^3

                                                           /mapped_value() //rij

                                                           )

                                                      (0) // else 0
                                                      ))
                                                (0)
                                                )
                                             , rangeSearch.GetCurrentState(location));

        // Density update

        properties.at(0) <<=  properties.at(0) + delta_time * out.at(0);

        //Boundary particles' density is >=1000

        properties.at(0) <<= cond( maskx && properties.at(0)<rho0, rho0 )
                                                                   (properties.at(0) );

        //Pressure Eval

        properties.at(4) <<= beta*(pow(properties.at(0)/rho0 ,7.0 )-1.0); // beta * ((rho_i/rho0 )^7-1) (Tait Equation of State)

        // Pressure >0

        properties.at(4) <<= cond( properties.at(4)<0, 0 )
                                    ( properties.at(4) );

        // Velocity update

        properties.at(1) <<=  properties.at(1) + delta_time * (-out.at(1));
        properties.at(2) <<=  properties.at(2) + delta_time * (-out.at(2)+gravity);

        // boundary particles are stationary

        properties.at(1) <<= cond(maskx,0)
                                        (properties.at(1));

        properties.at(2) <<= cond(masky,0)
                                        (properties.at(2));

        // Position update

        pCoord.at(0) <<= pCoord.at(0) + properties.at(1)*delta_time;
        pCoord.at(1) <<= pCoord.at(1) + properties.at(2)*delta_time;

        sim_time=sim_time+delta_time;
        iterations+=1;

#ifndef NDEBUG

//
        if (output_time<= sim_time)
        {
//
            std::string filename = "out";
            std::string ext = ".dat";

            std::ostringstream oss;
            oss << filename << file_count++ << ext;
            std::ofstream outputFile(oss.str().c_str());
//            std::ofstream outputFile(filename.c_str());
            outputFile.precision(3);
            outputFile.width(20);
            outputFile.fill(0);

            outputFile.clear();

    #ifdef ENABLE_CUDA
            pCoord.set_device_as_active(CPU_INDEX);
    #endif

            for (i=0; i<nparticles; i++){
                outputFile<< std::fixed << pCoord.at(0)[i]<< ' ' << pCoord.at(1)[i] << ' ' << pCoord.at(2)[i] << endl ;
            }

    #ifdef ENABLE_CUDA
            pCoord.set_device_as_active(GPU_INDEX);
    #endif
            output_time=output_time+detla_t_io;
            outputFile<< endl;
            //system("/opt/local/bin/gnuplot -e \'plot \"out.txt\"  u 1:2\' ");

            outputFile.close();

        }

#endif
    } //   --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

    return 0;
};