#include "ai_base.hpp"

#include <boost/lexical_cast.hpp>

#include <cmath>
#include <fstream>
#include <iostream>
#include <random>
#include <vector>
#include <algorithm>

class my_ai
  : public aiwc::ai_base
{
  static constexpr double PI = 3.1415926535;

public:
  my_ai(std::string server_ip, std::size_t port, std::string realm, std::string key, std::string datapath)
    : aiwc::ai_base(std::move(server_ip), port, std::move(realm), std::move(key), std::move(datapath))
    , robot_wheels{}
  {
    std::cout << "I am ready." << std::endl;
  }

private:
  // this function is called at the beginning of a game
  void init()
  {
    // from here, you have access to game-specific constant information such as field dimensions
    // check example 'general_check-variables_cpp' to see what information are available

    // you can initialize some customvariables here
  }

  // copy coordinates from frames to different variables just for convenience
  auto get_coord(const aiwc::frame& f)
  {
    //Get data in the frame
    decltype(cur_posture) cur;
    decltype(cur_posture_op) cur_op;
      std::array<double, 2> pos_ball = { (*f.opt_coordinates).ball.x, (*f.opt_coordinates).ball.y };

    for(std::size_t id = 0; id < 5; ++id) {
      cur[id][X]  = (*f.opt_coordinates).robots[MYTEAM][id].x;
      cur[id][Y]  = (*f.opt_coordinates).robots[MYTEAM][id].y;
      cur[id][TH] = (*f.opt_coordinates).robots[MYTEAM][id].th;
      cur[id][ACTIVE] = (*f.opt_coordinates).robots[MYTEAM][id].active;
      cur[id][TOUCH] = (*f.opt_coordinates).robots[MYTEAM][id].touch;

      cur_op[id][X]  = (*f.opt_coordinates).robots[OPPONENT][id].x;
      cur_op[id][Y]  = (*f.opt_coordinates).robots[OPPONENT][id].y;
      cur_op[id][TH] = (*f.opt_coordinates).robots[OPPONENT][id].th;
      cur_op[id][ACTIVE] = (*f.opt_coordinates).robots[MYTEAM][id].active;
      cur_op[id][TOUCH] = (*f.opt_coordinates).robots[MYTEAM][id].touch;
    }

    return std::make_tuple(cur, cur_op, pos_ball);
  }

  // this function is called at each timestep. in 'f', current step's information are stored
  // check example 'general_check-variables_cpp' to see what information are available here
  // you should implement an AI soccer algorithm that sets robot wheel velocities for each timestep here
  void update(const aiwc::frame& f)
  {
	  frames.push_back(f);
    if(f.reset_reason == aiwc::GAME_START) {
      previous_frame = f;
      std::tie(cur_posture, cur_posture_op, cur_ball) = get_coord(f);
      return;
    }
    else if(f.reset_reason == aiwc::EPISODE_END) {
      // EPISODE_END is sent instead of GAME_END when 'repeat' option is set to 'true'
      // to mark the end of episode
      // you can reinitialize the parameters, count the number of episodes done, etc. here

      // this example does not do anything at episode end
    }
    else if(f.reset_reason == aiwc::HALFTIME) {
      // halftime is met - from next frame, received_frame.half_passed will be set to True
      // although the simulation switches sides,
      // coordinates and images given to your AI soccer algorithm will stay the same
      // that your team is red and located on left side whether it is 1st half or 2nd half

      // this example does not do anything at halftime
    }
    else if(f.reset_reason == aiwc::GAME_END) {
      return;
    }

    // update current robot and ball postures
    std::tie(cur_posture, cur_posture_op, cur_ball) = get_coord(f);
    find_closest_robot();

    // array for holding wheel speeds
    std::array<double, 10> ws;

    // act differently based on the current game state
    switch(f.game_state) {
		//////////////////////////////////////////////////////////////////////
      case aiwc::STATE_DEFAULT:
        {
          // robots functions in STATE_DEFAULT
          // goalkeeper simply executes goalkeeper algorithm on its own
          goalkeeper(0);

          // defenders and forwards can pass ball to each other if necessary
          std::array<size_t, 4> list = {1, 2, 3, 4};
          passing_play(list);
        }
        break;
		//////////////////////////////////////////////////////////////////////
      case aiwc::STATE_KICKOFF:
        {
          // if the ball belongs to my team, initiate kickoff
          if (f.ball_ownership)
            set_target_position(4, 0, 0, 1.4, 3.0, 0.4, false);
        }
        break;
		//////////////////////////////////////////////////////////////////////
      case aiwc::STATE_GOALKICK:
        {
          // if the ball belongs to my team,
          // drive the goalkeeper to kick the ball
          if (f.ball_ownership)
            set_wheel_velocity(0, info.max_linear_velocity[0], info.max_linear_velocity[0], true);
        }
        break;
		//////////////////////////////////////////////////////////////////////
      case aiwc::STATE_CORNERKICK:
        {
          // just play as simple as possible
          goalkeeper(0);
          defender(1);
          defender(2);
          forward(3);
          forward(4);
        }
        break;
		//////////////////////////////////////////////////////////////////////
      case aiwc::STATE_PENALTYKICK:
        {
          // if the ball belongs to my team,
          // drive the forward to kick the ball
          if (f.ball_ownership)
            set_wheel_velocity(4, info.max_linear_velocity[4], info.max_linear_velocity[4], true);
        }
        break;
      default:
        break;
    }

    // hand over current frame as previous frame to next step
    prev_sender = sender;
    prev_receiver = receiver;
    prev_ball = cur_ball;
    prev_posture = cur_posture;
    prev_posture_op = cur_posture_op;
    previous_frame = f;

    // collect the robot wheel speeds
    for(std::size_t id = 0; id < 5; ++id) {
      ws[2*id    ] = robot_wheels[id][0]; // left
      ws[2*id + 1] = robot_wheels[id][1]; // right
    }

    // send wheel speed data to the simulator
    set_wheel(ws); // this function is defined at ai_base.cpp(examples/common)
  }

  // this function is called when the game is over
  // you can save some data here if necessary
  void finish()
  {
    // you have less than 30 seconds before it's killed.
    std::ofstream ofs(datapath + "/result.txt");

    // write something
    ofs << "time, a.score, b.score, ";
    ofs << "ball.x, ball.y" << std::endl;

    // close the file
    ofs.close();
  }

  // All functions below are used for rulebased robot controls used in update()
  // convert degree to radian
  double d2r(double deg) {
    return deg * PI / 180;
  }

  // convert radian to degree
  double r2d(double rad) {
    return rad * 180 / PI;
  }

  // measure the distance between two coordinates (x1, y1) and (x2, y2)
  double dist(double x1, double x2, double y1, double y2)
  {
    const auto dx = x1 - x2;
    const auto dy = y1 - y2;
    return std::sqrt(dx * dx + dy * dy);
  }

  // convert radian in (-inf, inf) range to (-PI, PI) range
  double trim_radian(double rad)
  {
    double adj_rad = rad;
    while (adj_rad > PI)
      adj_rad -= 2 * PI;
    while (adj_rad < -PI)
      adj_rad += 2 * PI;
    return adj_rad;
  }

  // set the left and right wheel velocities of robot with id 'id'
  // 'max_velocity' scales the velocities up to the point where at least one of wheel is operating at max velocity
  void set_wheel_velocity(std::size_t id, double l, double r, bool max_velocity)
  {
    double multiplier = 1;

    // wheel velocities need to be scaled so that none of wheels exceed the maximum velocity available
    // otherwise, the velocity above the limit will be set to the max velocity by the simulation program
    // if that happens, the velocity ratio between left and right wheels will be changed that the robot may not execute
    // turning actions correctly.
    if (std::abs(l) > info.max_linear_velocity[id] || std::abs(r) > info.max_linear_velocity[id] || max_velocity) {
      if (std::abs(l) > std::abs(r)) {
        multiplier = info.max_linear_velocity[id] / std::abs(l);
      }
      else {
        multiplier = info.max_linear_velocity[id] / std::abs(r);
      }

      l *= multiplier;
      r *= multiplier;
    }

    robot_wheels[id] = {l, r};
  }

  // let the robot with id 'id' move to a target position (x, y)
  // the trajectory to reach the target position is determined by several different parameters
  void set_target_position(std::size_t id, double x, double y, double scale, double mult_lin, double mult_ang, bool max_velocity)
  {
    const double damping = 0.35;
    double ka = 0;
    int sign = 1;

    // calculate how far the target position is from the robot
    const double dx = x - cur_posture[id][X];
    const double dy = y - cur_posture[id][Y];
    const double d_e = std::sqrt(dx * dx + dy * dy);

    // calculate how much the direction is off
    const double desired_th = (dx == 0 && dy == 0) ? (PI / 2) : std::atan2(dy, dx);
    double d_th = desired_th - cur_posture[id][TH];
    while(d_th > PI) d_th -= 2 * PI;
    while(d_th < -PI) d_th += 2 * PI;

    // based on how far the target position is, set a parameter that
    // decides how much importance should be put into changing directions
    // farther the target is, less need to change directions fastly
    if(d_e > 1) {        ka = 17/90; }
    else if(d_e > 0.5) { ka = 19/90; }
    else if(d_e > 0.3) { ka = 21/90; }
    else if(d_e > 0.2) { ka = 23/90; }
    else               { ka = 25/90; }

    // if the target position is at rear of the robot, drive backward instead
    if(d_th > d2r(95)) {
      d_th -= PI;
      sign = -1;
    }
    else if(d_th < d2r(-95)) {
      d_th += PI;
      sign = -1;
    }

    // if the direction is off by more than 85 degrees,
    // make a turn first instead of start moving toward the target
    if(std::abs(d_th) > d2r(85)) {
      set_wheel_velocity(id, -mult_ang * d_th, mult_ang * d_th, false);
    }
    // otherwise
    else {
      // scale the angular velocity further down if the direction is off by less than 40 degrees
      if(d_e < 5.0 && std::abs(d_th) < d2r(40)) {
	      ka = 0.1;
      }
      ka *= 4;

      // set the wheel velocity
      // 'sign' determines the direction [forward, backward]
      // 'scale' scales the overall velocity at which the robot is driving
      // 'mult_lin' scales the linear velocity at which the robot is driving
      // larger distance 'd_e' scales the base linear velocity higher
      // 'damping' slows the linear velocity down
      // 'mult_ang' and 'ka' scales the angular velocity at which the robot is driving
      // larger angular difference 'd_th' scales the base angular velocity higher
      // if 'max_velocity' is true, the overall velocity is scaled to the point
      // where at least one wheel is operating at maximum velocity
      set_wheel_velocity(id,
	       sign * scale * (mult_lin * (1 / (1 + std::exp(-3 * d_e)) - damping) - mult_ang * ka * d_th),
	       sign * scale * (mult_lin * (1 / (1 + std::exp(-3 * d_e)) - damping) + mult_ang * ka * d_th), max_velocity);
    }
  }

  // predict where the ball will be located after 'steps' steps
  std::array<double, 2> predict_ball_location(int steps)
  {
    double dx = cur_ball[X] - prev_ball[X];
    double dy = cur_ball[Y] - prev_ball[Y];
    return {cur_ball[X] + steps * dx, cur_ball[Y] + steps * dy};
  }

  // check if a certain position is inside the penalty area of 'team'
  bool in_penalty_area(std::array<double, 2> obj, std::size_t team)
  {
    if (std::abs(obj[Y]) > info.penalty_area[Y] / 2)
      return false;

    if (team == MYTEAM)
      return (obj[X] < -info.field[X] / 2 + info.penalty_area[X]);
    else
      return (obj[X] > info.field[X] / 2 - info.penalty_area[X]);
  }

  // check if the robot with id 'id' has a chance to shoot
  bool shoot_chance(size_t id)
  {
    double dx = cur_ball[X] - cur_posture[id][X];
    double dy = cur_ball[Y] - cur_posture[id][Y];

    // if the ball is located further on left than the robot, it will be hard to shoot
  	if (dx < 0) {
  		return false;
  	}

    // if the robot->ball direction aligns with opponent's goal, the robot can shoot
    double y = (info.field[X] / 2 - cur_ball[X]) * dy / dx + cur_posture[id][Y];
  	if (abs(y) < info.goal[Y] / 2) {
  		return true;
  	}
  	else {
  		return false;
  	}
  }

  // find a defender and a forward closest to the ball
  void find_closest_robot(void)
  {
    // find the closest defender
    int min_idx = 0;
    double min_dist = 9999.99;
    double def_min_dist = 9999.99;

    for (int i = 1; i < 3; i++) {
      auto measured_dist = dist(cur_ball[X], cur_posture[i][X], cur_ball[Y], cur_posture[i][Y]);
      if (measured_dist < min_dist) {
        min_dist = measured_dist;
        min_idx = i;
        def_idx = min_idx;
        def_min_dist = min_dist;
      }
    }

    // find the closest forward
    min_idx = 0;
    min_dist = 9999.99;
    double fwd_min_dist = 9999.99;

    for (int i = 3; i < 5; i++) {
      auto measured_dist = dist(cur_ball[X], cur_posture[i][X], cur_ball[Y], cur_posture[i][Y]);
      if (measured_dist < min_dist) {
        min_dist = measured_dist;
        min_idx = i;
        fwd_idx = min_idx;
        fwd_min_dist = min_dist;
      }
    }

    // record the robot closer to the ball between the two too
    if (def_min_dist < fwd_min_dist){
        closest_idx = def_idx;
    }
    else{
        closest_idx = fwd_idx;
    }
  }

  // turn to face 'desired_th' direction
  void angle(std::size_t id, double desired_th)
  {
    double mult_ang = 0.4;
    double sign = 1;

    double d_th = desired_th - cur_posture[id][TH];
    d_th = trim_radian(d_th);

    // the robot instead puts the direction rear if the angle difference is large
    if (d_th > d2r(95)) {
        d_th -= PI;
        sign = -1;
    }
    else if (d_th < d2r(-95)) {
        d_th += PI;
        sign = -1;
    }
    set_wheel_velocity(id, -mult_ang * d_th, mult_ang * d_th, false);
  }

  // returns the angle toward a specific position from current robot posture
  double direction_angle(std::size_t id, double x, double y)
  {
    double dx = x - cur_posture[id][X];
    double dy = y - cur_posture[id][Y];

    if ((dx == 0) && (dy == 0))
      return (PI / 2);
    else
      return std::atan2(dy, dx);
  }

  // let the robot face toward specific direction
  void face_specific_position(std::size_t id, double x, double y)
  {
    double dx = x - cur_posture[id][X];
    double dy = y - cur_posture[id][Y];

    double desired_th;
    if ((dx == 0) && (dy == 0))
      desired_th = PI / 2;
    else
      desired_th = std::atan2(dy, dx);

    angle(id, desired_th);
  }

  // check if the ball is coming toward the robot
  bool ball_coming_toward_robot(std::size_t id)
  {
    double x_dir = std::abs(cur_posture[id][X] - prev_ball[X]) > std::abs(cur_posture[id][X] - cur_ball[X]);
    double y_dir = std::abs(cur_posture[id][Y] - prev_ball[Y]) > std::abs(cur_posture[id][Y] - cur_ball[Y]);

    // ball is coming closer
    if ((x_dir) && (y_dir))
      return true;
    else
      return false;
  }

  // a basic goalkeeper rulebased algorithm
  void goalkeeper(std::size_t id)
  {
    // default desired position
    double x = (-info.field[X] / 2) + (info.robot_size[id] / 2) + 0.05;
    double y = std::max(std::min(cur_ball[Y], (info.goal[Y] / 2 - info.robot_size[id] / 2)), -info.goal[Y] / 2 + info.robot_size[id] / 2);
    std::array<double, 2> a = {cur_posture[id][X], cur_posture[id][Y]};

    // if the robot is inside the goal, try to get out
    if (cur_posture[id][X] < -info.field[X] / 2){
      if (cur_posture[id][Y] < 0)
        set_target_position(id, x, cur_posture[id][Y] + 0.2, 1.4, 5.0, 0.4, false);
      else
        set_target_position(id, x, cur_posture[id][Y] - 0.2, 1.4, 5.0, 0.4, false);
    }
    // if the goalkeeper is outside the penalty area
    else if (!in_penalty_area(a, MYTEAM)){
      // return to the desired position
      set_target_position(id, x, y, 1.4, 5.0, 0.4, true);
    }
    // if the goalkeeper is inside the penalty area
    else{
      // if the ball is inside the penalty area
      if (in_penalty_area(cur_ball, MYTEAM)){
        // if the ball is behind the goalkeeper
        if (cur_ball[X] < cur_posture[id][X]){
          // if the ball is not blocking the goalkeeper's path
          if (std::abs(cur_ball[Y] - cur_posture[id][Y]) > 2 * info.robot_size[id]){
            // try to get ahead of the ball
            set_target_position(id, cur_ball[X] - info.robot_size[id], cur_posture[id][Y], 1.4, 5.0, 0.4, false);
          }
          else{
            // just give up and try not to make a suicidal goal
            angle(id, PI / 2);
          }
        }
        // if the ball is ahead of the goalkeeper
        else{
          double desired_th = direction_angle(id, cur_ball[X], cur_ball[Y]);
          double rad_diff = trim_radian(desired_th - cur_posture[id][TH]);
          // if the robot direction is too away from the ball direction
          if (rad_diff > PI / 3){
            // give up kicking the ball and block the goalpost
            set_target_position(id, x, y, 1.4, 5.0, 0.4, false);
          }
          else{
            // try to kick the ball away from the goal
            set_target_position(id, cur_ball[X], cur_ball[Y], 1.4, 3.0, 0.8, true);
          }
        }
      }
      // if the ball is not in the penalty area
      else{
        // if the ball is within alert range and y position is not too different
        if ((cur_ball[X] < -info.field[X] / 2 + 1.5 * info.penalty_area[X])
        && (std::abs(cur_ball[Y]) < 1.5 * info.penalty_area[Y] / 2)
        && (std::abs(cur_ball[Y] - cur_posture[id][Y]) < 0.2))
          face_specific_position(id, cur_ball[X], cur_ball[Y]);
        // otherwise
        else
          set_target_position(id, x, y, 1.4, 5.0, 0.4, true);
      }
    }
  }

  // a basic defender rulebased algorithm
  void defender(std::size_t id)
  {
    // if the robot is inside the goal, try to get out
    if (cur_posture[id][X] < -info.field[X] / 2){
      if (cur_posture[id][Y] < 0)
        set_target_position(id, -0.7 * info.field[X] / 2, cur_posture[id][Y] + 0.2, 1.4, 3.5, 0.6, false);
      else
        set_target_position(id, -0.7 * info.field[X] / 2, cur_posture[id][Y] - 0.2, 1.4, 3.5, 0.6, false);
      return;
    }
    // the defender may try to shoot if condition meets
    if ((id == def_idx) && (shoot_chance(id)) && (cur_ball[X] < 0.3 * info.field[X] / 2)){
      set_target_position(id, cur_ball[X], cur_ball[Y], 1.4, 5.0, 0.4, true);
      return;
    }
    // if this defender is closer to the ball than the other defender
    if (id == def_idx){
      // ball is on our side
      if (cur_ball[X] < 0){
        // if the robot can push the ball toward opponent's side, do it
        if (cur_posture[id][X] < cur_ball[X] - info.ball_radius)
          set_target_position(id, cur_ball[X], cur_ball[Y], 1.4, 5.0, 0.4, true);
        else{
          // otherwise go behind the ball
          if (std::abs(cur_ball[Y] - cur_posture[id][Y]) > 0.3)
            set_target_position(id, std::max(cur_ball[X] - 0.5, -info.field[X] / 2 + info.robot_size[id] / 2), cur_ball[Y], 1.4, 3.5, 0.6, false);
          else
            set_target_position(id, std::max(cur_ball[X] - 0.5, -info.field[X] / 2 + info.robot_size[id] / 2), cur_posture[id][Y], 1.4, 3.5, 0.6, false);
        }
      }
      else
        set_target_position(id, -0.7 * info.field[X] / 2, cur_ball[Y], 1.4, 3.5, 0.4, false);
    }
    // if this defender is not closer to the ball than the other defender
    else{
      // ball is on our side
      if (cur_ball[X] < 0){
        // ball is on our left
        if (cur_ball[Y] > info.goal[Y] / 2 + 0.15)
          set_target_position(id, std::max(cur_ball[X] - 0.5, -info.field[X] / 2 + info.robot_size[id] / 2 + 0.1), info.goal[Y] / 2 + 0.15, 1.4, 3.5, 0.4, false);
        // ball is on our right
        else if (cur_ball[Y] < -info.goal[Y] / 2 - 0.15)
          set_target_position(id, std::max(cur_ball[X] - 0.5, -info.field[X] / 2 + info.robot_size[id] / 2 + 0.1), -info.goal[Y] / 2 - 0.15, 1.4, 3.5, 0.4, false);
        // ball is in center
        else
          set_target_position(id, std::max(cur_ball[X] - 0.5, -info.field[X] / 2 + info.robot_size[id] / 2 + 0.1), cur_ball[Y], 1.4, 3.5, 0.4, false);
      }
      else{
        // ball is on right side
        if (cur_ball[Y] < 0)
          set_target_position(id, -0.7 * info.field[X] / 2, std::min(cur_ball[Y] + 0.5, info.field[Y] / 2 - info.robot_size[id] / 2), 1.4, 3.5, 0.4, false);
        // ball is on left side
        else
          set_target_position(id, -0.7 * info.field[X] / 2, std::max(cur_ball[Y] - 0.5, -info.field[Y] / 2 + info.robot_size[id] / 2), 1.4, 3.5, 0.4, false);
      }
    }
  }

  // a basic forward rulebased algorithm
  void forward(std::size_t id)
  {
    // if the robot is blocking the ball's path toward opponent side
    if ((cur_ball[X] > -0.3 * info.field[X] / 2) && (cur_ball[X] < 0.3 * info.field[X] / 2) && (cur_posture[id][X] > cur_ball[X] + 0.1) && (std::abs(cur_posture[id][Y] - cur_ball[Y]) < 0.3)){
      if (cur_ball[Y] < 0)
        set_target_position(id, cur_posture[id][X] - 0.25, cur_ball[Y] + 0.75, 1.4, 3.0, 0.8, false);
      else
        set_target_position(id, cur_posture[id][X] - 0.25, cur_ball[Y] - 0.75, 1.4, 3.0, 0.8, false);
      return;
    }

    // if the robot can shoot from current position
    if ((id == fwd_idx) && (shoot_chance(id))){
      std::array<double, 2> pred_ball = predict_ball_location(2);
      set_target_position(id, pred_ball[X], pred_ball[Y], 1.4, 5.0, 0.4, true);
      return;
    }

    // if the ball is coming toward the robot, seek for shoot chance
    if ((id == fwd_idx) && (ball_coming_toward_robot(id))){
      double dx = cur_ball[X] - prev_ball[X];
      double dy = cur_ball[Y] - prev_ball[Y];
      double pred_x = (cur_posture[id][Y] - cur_ball[Y]) * dx / dy + cur_ball[X];
      double steps = (cur_posture[id][Y] - cur_ball[Y]) / dy;

      // if the ball will be located in front of the robot
      if (pred_x > cur_posture[id][X]){
        double pred_dist = pred_x - cur_posture[id][X];
        // if the predicted ball location is close enough
        if ((pred_dist > 0.1) && (pred_dist < 0.3) && (steps < 10)){
          // find the direction towards the opponent goal and look toward it
          double goal_angle = direction_angle(id, info.field[X] / 2, 0);
          angle(id, goal_angle);
          return;
        }
      }
    }

    // if this forward is closer to the ball than the other forward
    if (id == fwd_idx){
      if (cur_ball[X] > -0.3 * info.field[X] / 2){
        // if the robot can push the ball toward opponent's side, do it
        if (cur_posture[id][X] < cur_ball[X] - info.ball_radius)
          set_target_position(id, cur_ball[X], cur_ball[Y], 1.4, 5.0, 0.4, true);
        else{
          // otherwise go behind th ball
          if (std::abs(cur_ball[Y] - cur_posture[id][Y]) > 0.3)
            set_target_position(id, cur_ball[X] - 0.2, cur_ball[Y], 1.4, 3.5, 0.6, false);
          else
            set_target_position(id, cur_ball[X] - 0.2, cur_posture[id][Y], 1.4, 3.5, 0.6, false);
        }
      }
      else
        set_target_position(id, -0.1 * info.field[X] / 2, cur_ball[Y], 1.4, 3.5, 0.4, false);
    }
    // if this forward is closer to the ball than the other forward
    else{
      if (cur_ball[X] > -0.3 * info.field[X] / 2){
        // ball is on our right
        if (cur_ball[Y] < 0)
          set_target_position(id, cur_ball[X] - 0.25, info.goal[Y] / 2, 1.4, 3.5, 0.4, false);
        // ball is on our left
        else
          set_target_position(id, cur_ball[X] - 0.25, -info.goal[Y] / 2, 1.4, 3.5, 0.4, false);
      }
      else{
        // ball is on right side
        if (cur_ball[Y] < 0)
          set_target_position(id, -0.1 * info.field[X] / 2, std::min(-cur_ball[Y] - 0.5, info.field[Y] / 2 - info.robot_size[id] / 2), 1.4, 3.5, 0.4, false);
        // ball is on left side
        else
          set_target_position(id, -0.1 * info.field[X] / 2, std::max(-cur_ball[Y] + 0.5, -info.field[Y] / 2 + info.robot_size[id] / 2), 1.4, 3.5, 0.4, false);
      }
    }
  }

  // check if sender/receiver pair should be reset
  bool check_reset_condition(void)
  {
	  // if the time is over, setting is reset
	  if ((end_count > 0) && (end_count - cur_count < 0))
		  return true;

  	// if there is no sender and receiver is not in shoot chance, setting is cleared
	  if (sender > 5 && receiver < 5 && !shoot_chance(receiver))
		  return true;

    return false;
  }

  // check if a sender can be selected
  bool check_sender_condition(void)
  {
    // if any of my robot is near the ball, a sender is available
    for(int i = 1; i < 5; i++) {
      double distance = dist(cur_posture[i][X], cur_ball[X], cur_posture[i][Y], cur_ball[Y]);
    	if ((distance < 0.5) && cur_posture[i][ACTIVE]) {
    		return true;
    	}
    }
    return false;
  }

  // check if a receiver should be selected
  bool check_receiver_condition(void)
  {
	  // if a sender exists and receiver does not exist yet, a receiver can be set
	  if ((sender < 5) && (receiver > 5))
		  return true;

    return false;
  }

  // select a sender
  size_t set_sender(std::array<size_t, 4> _player_list)
  {
    std::array<double, 4> distance_list;

    // leave non-candidates out
    for (int i = 0; i < 4; i++) {
      if (_player_list[i] > 5) {
        distance_list[i] = 999;
        continue;
      }

      std::array<double, 2> predict_ball = predict_ball_location(3);
      double ball_distance = dist(predict_ball[X], cur_posture[_player_list[i]][X], predict_ball[Y], cur_posture[_player_list[i]][Y]);
      distance_list[i] = ball_distance;
    }

    double min = 999;
    size_t index;
    for(int i = 0; i < 4; i++) {
      if (distance_list[i] < min) {
        min = distance_list[i];
        index = _player_list[i];
      }
    }

    // if the distance between ball and sender is predicted to be less than 1, choose the closest robot as the sender
    if (min < 1.0)
      return index;

    // otherwise, there is no sender
    return 99;
  }

  // select a receiver
  size_t set_receiver(std::array<size_t, 4> _player_list)
  {
    std::array<double, 4> receiver_op_dist_list;

	  // the robot farthest from opponents will be the receiver
    for(int r = 0; r < 4; r++) {
      std::array<double, 5> temp_receiver_op_dist_list;

	    // if a robot is not active, set distance to a high value
      if (_player_list[r] > 5) {
        temp_receiver_op_dist_list[r] = 999;
        continue;
      }

      // the sender is not a receiver candidate
      if(r + 1 == sender) {
      	receiver_op_dist_list[r] = 999;
      	continue;
      }

	    // get the distance between the robot and opponents
      for (int op = 1; op < 5; op++) { // four non-goalkeeper robots
        double op_distance = dist(cur_posture[r][X], cur_posture_op[op][X], cur_posture[r][Y], cur_posture[op][Y]);
        temp_receiver_op_dist_list[op - 1] = op_distance;
      }

      double min = 999;
      for(int i = 0; i < 5; i++) {
        if (temp_receiver_op_dist_list[i] < min)
          min = temp_receiver_op_dist_list[i];
      }
      receiver_op_dist_list[r] = min; // save the shortest distance between this robot and one of opponents
    }

    std::array<double, 4> receiver_ball_list;

    for(int r = 0; r < 4; r++) {
      // if the minimum distance between player and opponent's player is less than 0.5, this robot cannot be receiver
      if ((receiver_op_dist_list[r] < 0.5) || receiver_op_dist_list[r] == 999) {
        receiver_ball_list[r] = 999;
        continue;
      }
      size_t index = _player_list[r];
      double receiver_ball_distance = dist(cur_ball[X], cur_posture[index][X], cur_ball[Y], cur_posture[index][Y]);
      receiver_ball_list[r] = receiver_ball_distance;
    }

    double min = 999;
    size_t min_index = 99;
    for(int i = 0; i < 4; i++) {
      if (receiver_ball_list[i] < min) {
        min = receiver_ball_list[i];
        min_index = _player_list[i];
      }
    }

    return min_index;
  }

  // let robot with id 'id' execute an action directed by 'mode'
  void actions(size_t id, int mode, std::array<double, 2> target_pts = {-1,-1}, std::array<double, 4> params = {-1,-1,-1,-1}, bool refine = false)
  {
    std::array<double, 4> _params;
    std::array<double, 2> _target_pts;
    if (id > 5)
      return;

    // if the player state is set to 'stop', force the mode to be 'stop'
    if (player_state[id] == STOP)
      mode = STOP;

    // actions are set differently based on the modes
    switch(mode) {
    case NONE:
      {
        // reset the robot's status
        if (sender == id) {
          sender = 99;
          touch[0] = false;
          touch[1] = false;
          touch[2] = false;
          touch[3] = false;
          touch[4] = false;
        }
        if (receiver == id)
          receiver = 99;
        player_state[id] = NONE;
      }
      break;
    case FOLLOW:
      {
        // let the robot follow the ball
        if (target_pts[0] == -1)
          _target_pts = predict_ball_location(3);
        else
          _target_pts = target_pts;
        if (params[0] == -1){
          _params[0] = 1.0;
          _params[1] = 3.0;
          _params[2] = 0.6;
          _params[3] = false;
        }
        if (refine)
          _params = set_pos_parameters(id, _target_pts, params);
        set_target_position(id, _target_pts[X], _target_pts[Y], _params[0], _params[1], _params[2], _params[3]);
        player_state[id] = FOLLOW;
      }
      break;
    case DRIBBLE:
      {
        // let the robot follow the ball but at a faster speed
        if (target_pts[0] == -1)
          _target_pts = cur_ball;
        else
          _target_pts = target_pts;
        if (params[0] == -1){
          _params[0] = 1.4;
          _params[1] = 5.0;
          _params[2] = 0.8;
          _params[3] = false;
        }
        if (refine)
          _params = set_pos_parameters(id, _target_pts, params);
        set_target_position(id, _target_pts[X], _target_pts[Y], _params[0], _params[1], _params[2], _params[3]);
        player_state[id] = DRIBBLE;
      }
      break;
    case KICK:
      {
        // kick the ball
        if (target_pts[0] == -1)
          _target_pts = cur_ball;
        else
          _target_pts = target_pts;
        if (params[0] == -1){
          _params[0] = 1.4;
          _params[1] = 5.0;
          _params[2] = 0.8;
          _params[3] = true;
        }
        if ((end_count == 0) && !(touch[id]))
          end_count = cur_count + 10;
        player_state[id] = KICK;
        if (touch[id])
          player_state[id] = STOP;
        else
          touch[id] = cur_posture[id][TOUCH];
        if (player_state[id] == STOP){
          _params[0] = 0;
          _params[1] = 0;
          _params[2] = 0;
          _params[3] = false;
        }
        set_target_position(id, _target_pts[X], _target_pts[Y], _params[0], _params[1], _params[2], _params[3]);
      }
      break;
    case STOP:
      {
        // stop while counter is on
        if (params[0] == -1){
          _params[0] = 0;
          _params[1] = 0;
          _params[2] = false;
        }
        set_wheel_velocity(id, _params[0], _params[1], _params[2]);
        if (end_count == 0)
          end_count = cur_count + 5;
        player_state[id] = STOP;
        if (end_count - 1 == cur_count)
          player_state[id] = NONE;
      }
      break;
    case BACKWARD:
      {
        // retreat from the current position
        if (target_pts[0] == -1){
          _target_pts[0] = cur_posture[id][X] + 0.2;
          _target_pts[1] = cur_posture[id][Y];
        }
        else
          _target_pts = target_pts;
        if (params[0] == -1){
          _params[0] = 1.4;
          _params[1] = 5.0;
          _params[2] = 0.8;
          _params[3] = false;
        }
        if (refine)
          _params = set_pos_parameters(id, _target_pts, params);
        set_target_position(id, _target_pts[X], _target_pts[Y], _params[0], _params[1], _params[2], _params[3]);
        player_state[id] = BACKWARD;
      }
      break;
    case POSITION:
      {
        // go toward target position
        set_target_position(id, target_pts[X], target_pts[Y], _params[0], _params[1], _params[2], _params[3]);
      }
      break;
    default:
      break;
    }
  }

  void pass_ball(void)
  {
    if ((prev_sender == receiver && receiver < 5) || (prev_receiver == sender && sender < 5)) {
      double sender_ball_dist = dist(cur_posture[sender][X], cur_ball[X], cur_posture[sender][Y], cur_ball[Y]);
      if (sender_ball_dist < 1.5) {
        sender = prev_sender;
        receiver = prev_receiver;
      }
    }
    receive_ball();
    send_ball();
  }

  void send_ball(void)
  {
    if (sender > 5)
      return;

    std::array<double, 2> null2 = {-1, -1};
    std::array<double, 4> null4 = {-1, -1, -1, -1};

    double goal_dist = dist(info.field[X] / 2, cur_posture[sender][X], 0, cur_posture[sender][Y]);

    // if the sender has a shoot chance, it tries to shoot
    if (shoot_chance(sender)){
      if (goal_dist > 0.3 * info.field[X] / 2){
        actions(receiver, DRIBBLE, null2, null4, true);
        return;
      }
      else{
        actions(sender, KICK);
        return;
      }
    }

    // if the receiver exists, get the distance between the sender and the receiver
    double sender_receiver_dist = 999;
    if (receiver < 5)
      double sender_receiver_dist = dist(cur_posture[sender][X], cur_posture[receiver][X], cur_posture[sender][Y], cur_posture[receiver][Y]);

    // if the sender is close to the receiver, the sender kicks the ball
    if (sender_receiver_dist != 999)
      if ((sender_receiver_dist < 0.3) && !cur_posture[receiver][TOUCH])
        actions(sender, KICK);
        return;

    bool ift = is_facing_target(sender, cur_ball[X], cur_ball[Y]);
    double theta_diff = is_facing_target_direction(sender, cur_ball[X], cur_ball[Y]);
    // after the sender kicks, it stops
    if (!ift){
      if (theta_diff > PI * 3/4){
        actions(sender, NONE);
        return;
      }
      else{
        actions(sender, FOLLOW, null2, null4, true);
        return;
      }
    }

    // if the ball is in front of the sender and sender is moving backward
    if (cur_posture[sender][X] < -0.8 * info.field[X] / 2){
      if ((cur_posture[sender][X] - prev_posture[sender][X]) < 0)
        actions(sender, BACKWARD);
    }
    actions(sender, DRIBBLE, null2, null4, true);
    return;
  }

  void receive_ball(void)
  {
    // if receiver does not exist, do nothing
    if (receiver > 5)
      return;

    std::array<double, 2> null2 = {-1, -1};
    std::array<double, 4> null4 = {-1, -1, -1, -1};

    double goal_dist = dist(info.field[X] / 2, cur_posture[receiver][X], 0, cur_posture[receiver][Y]);
    // if sender is in shoot chance, receiver does nothing(reset)
    if (shoot_chance(sender)) {
      actions(receiver,NONE);
      return;
    }
    // if receiver is in shoot chance, kick or dribble forwards to the goal
    if (shoot_chance(receiver)) {
      if (goal_dist > 0.3 * info.field[X] / 2) {
        actions(receiver, DRIBBLE, null2, null4, true);
        return;
      }
      else {
        actions(receiver, KICK);
        return;
      }
    }
    // if sender exists
    if (sender < 5) {
      bool s2risFace = is_facing_target(sender, cur_posture[receiver][X], cur_posture[receiver][Y], 4);
      bool r2sisFace = is_facing_target(receiver, cur_posture[sender][X], cur_posture[sender][Y], 4);
      // if sender and receiver directs each other
      if (s2risFace && r2sisFace) {
        if ((cur_posture[receiver][TH] > 0) || (cur_posture[receiver][TH] < -3)) {
          actions(receiver, FOLLOW, {prev_posture[receiver][X], prev_posture[receiver][Y]-0.5 * info.field[Y]});
          return;
        }
        actions(receiver, FOLLOW, {prev_posture[receiver][X], prev_posture[receiver][Y] + 0.5 * info.field[Y]});
        return;
      }
    }
    std::array<double, 2> r_point = cur_ball;
    // if sender exists, get the expected point to meet receiver
    if (sender < 5)
      r_point = receive_position();
    double receiver_ball_dist = dist(cur_ball[X], cur_posture[receiver][X], cur_ball[Y], cur_posture[receiver][Y]);
    // if ball is close to receiver
    if (receiver_ball_dist > 0.3 * info.field[X] / 2) {
      actions(receiver, FOLLOW, {r_point[X], r_point[Y]}, null4, true);
      return;
    }
    bool r2bisFace = is_facing_target(receiver, cur_ball[X], cur_ball[Y], 4);
    if (!r2bisFace){
      actions(receiver, FOLLOW, null2, null4, true);
      return;
    }
    // if receiver is moving to our goal area
    if (cur_posture[receiver][X] < -0.8 * info.field[X] / 2) {
      if ((cur_posture[receiver][X] - prev_posture[receiver][X])< 0)
        actions(receiver, BACKWARD);
    }
    actions(receiver, DRIBBLE);
    return;
  }

  std::array<double, 4> set_pos_parameters(size_t id, std::array<double, 2> target_pts, std::array<double, 4> params, double mult = 1.2)
  {
    std::array<double, 4> _params;
    double prev_dist = dist(prev_posture[id][X], target_pts[X], prev_posture[id][Y], target_pts[Y]);
    double cur_dist = dist(cur_posture[id][X], target_pts[X], cur_posture[id][Y], target_pts[Y]);
    if (cur_dist > prev_dist - 0.02){
      _params[0] = params[0] * mult;
      _params[1] = params[1] * mult;
      _params[2] = params[2] * mult;
      _params[3] = params[3];
    }
    return _params;

  }

  bool is_facing_target(size_t index, double x, double y, double div = 4)
  {
    double dx = x - cur_posture[index][X];
    double dy = y - cur_posture[index][Y];
    double ds = std::sqrt(dx * dx + dy * dy);
    double desired_th;
    if (ds == 0)
      desired_th = 0;
    else
      desired_th = std::acos(dx / ds);

    double theta = cur_posture[index][X];
    if (desired_th < 0)
      desired_th += PI * 2;
    if (theta < 0)
      theta += PI * 2;
    double diff_theta = std::abs(desired_th - theta);
    if (diff_theta > PI)
      diff_theta = std::min(diff_theta, PI * 2 - diff_theta);
    if ((diff_theta < PI / div) || (diff_theta > PI * (1 - 1 / div)))
      return true;
    return false;
  }

  double is_facing_target_direction(size_t index, double x, double y, double div = 4)
  {
    double dx = x - cur_posture[index][X];
    double dy = y - cur_posture[index][Y];
    double ds = std::sqrt(dx * dx + dy * dy);
    double desired_th;
    if (ds == 0)
      desired_th = cur_posture[index][X];
    else
      desired_th = std::acos(dx / ds);

    double theta = cur_posture[index][X];
    if (desired_th < 0)
      desired_th += PI * 2;
    if (theta < 0)
      theta += PI * 2;
    double diff_theta = std::abs(desired_th - theta);
    if (diff_theta > PI)
      diff_theta = std::min(diff_theta, PI * 2 - diff_theta);
    return diff_theta;
  }

  std::array<double, 2> receive_position(void)
  {
    int step = 5;
    double ball_receiver_dist = dist(cur_ball[X], cur_posture[receiver][X], cur_ball[Y], cur_posture[receiver][Y]);
    double prev_ball_receiver_dist = dist(prev_ball[X], prev_posture[receiver][X], prev_ball[Y], prev_posture[receiver][Y]);
    double diff_dist = prev_ball_receiver_dist - ball_receiver_dist;
    if (diff_dist > 0)
      step = ball_receiver_dist / diff_dist;
    step = std::min(step, 15);
    std::array<double, 2> predict_pass_point = predict_ball_location(step);
    double ball_goal_dist = dist(cur_ball[X], info.field[X] / 2, cur_ball[Y], 0);
    double prev_ball_goal_dist = dist(prev_ball[X], info.field[X] / 2, prev_ball[Y], 0);
    if (ball_goal_dist > prev_ball_goal_dist)
      predict_pass_point[X] = predict_pass_point[X] - 0.15;
    return predict_pass_point;

  }

  void default_rulebased(std::array<size_t, 4> player_list)
  {
    for(int p = 0; p < 4; p++){
      // ff player is sender, receiver or out, do nothing
      if (player_list[p] > 5){
        continue;
      }
      // ff this robot is stuck at field sides, move forward the center
      if (pow(prev_posture[player_list[p]][X] - cur_posture[player_list[p]][X], 2) + pow(prev_posture[player_list[p]][Y] - cur_posture[player_list[p]][Y], 2) < 5e-6 && std::abs(cur_posture[player_list[p]][Y] - info.field[Y] / 2) < info.robot_size[player_list[p]] / 2 + 1e-2){
        if (cur_posture[player_list[p]][Y] > 0){
          set_target_position(p, 0, 0, 1.4, 3.5, 0.4, false);
          continue;
        }
      }
      // default rulebased
      if (player_list[p] == 0){
        goalkeeper(0);
        continue;
      }
      if (player_list[p] == 1){
        defender(1);
        continue;
      }
      if (player_list[p] == 2){
        defender(2);
        continue;
      }
      if (player_list[p] == 3){
        forward(3);
        continue;
      }
      if (player_list[p] == 4){
        forward(4);
        continue;
      }
    }
  }

  std::array<size_t, 4> find_active_player(std::array<size_t, 4> ids)
  {
    std::array<size_t, 4> _ids;
    for (int i = 0; i < 4; i++){
	    if (cur_posture[ids[i]][ACTIVE]) {
  			_ids[i] = ids[i];
  	  }
    	else {
    		_ids[i] = 99;
    	}
    }
    return _ids;
  }

  void passing_play(std::array<size_t, 4> player_list)
  {
    // select only alive player among forwards and defenders
    std::array<size_t, 4> _player_list = find_active_player(player_list);
    cur_count = floor(time * 20 + 0.5);

    if (end_count == cur_count)
      end_count = 0;

    if (check_reset_condition()){
      sender = 99;
      receiver = 99;
      sender_touch = false;
    }
    // check if sender candidate exists
    if (check_sender_condition()){
      sender = set_sender(_player_list);
    }
    // check if receiver candidate exists
    if (check_receiver_condition()){
      receiver = set_receiver(_player_list);
    }
    if (sender < 5 && receiver < 5){
      pass_ball();
      for (int i = 0; i < 4; i++){
        // if player is sender
        if (sender == i + 1){
          _player_list[i] = 99;
        }
        // if player is receiver
        if (receiver == i + 1){
          _player_list[i] = 99;
        }
      }
    }
    default_rulebased(_player_list);
    return;
  }

private: // member variable
  aiwc::frame previous_frame;

  std::array<std::array<double, 5>, 5> cur_posture;	//X,Y,THETA,ACTIVE,TOUCH
  std::array<std::array<double, 5>, 5> cur_posture_op;
  std::array<std::array<double, 5>, 5> prev_posture;
  std::array<std::array<double, 5>, 5> prev_posture_op;
  std::array<double, 2> prev_ball;
  std::array<double, 2> cur_ball;
  double time;

  std::array<std::array<double, 2>, 5> robot_wheels;

  std::array<bool, 5> touch;
  int cur_count = 0;
  int end_count = 0;
  size_t prev_sender;
  size_t prev_receiver;
  size_t sender;
  bool sender_touch;
  size_t receiver;
  size_t def_idx;
  size_t fwd_idx;
  size_t closest_idx;
  std::array<bool, 5> player_state;
  enum { NONE, FOLLOW, DRIBBLE, KICK, STOP, BACKWARD, POSITION};

  std::vector<aiwc::frame> frames;
};

int main(int argc, char *argv[])
{
  if(argc < 6) {
    std::cerr << "Usage " << argv[0] << " server_ip port realm key datapath" << std::endl;
    return -1;
  }

  const auto& server_ip = std::string(argv[1]);
  const auto& port      = boost::lexical_cast<std::size_t>(argv[2]);
  const auto& realm     = std::string(argv[3]);
  const auto& key       = std::string(argv[4]);
  const auto& datapath  = std::string(argv[5]);

  my_ai ai(server_ip, port, realm, key, datapath);

  ai.run();

  return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef LINALG_PATCHDOMAINDECOMPOSITIONPPRECONDITIONER_HH
#define LINALG_PATCHDOMAINDECOMPOSITIONPPRECONDITIONER_HH

#include <vector>

#include <boost/timer/timer.hpp>

#include <dune/common/dynvector.hh>
#include "dune/istl/preconditioner.hh"

#include "linalg/dynamicMatrix.hh"
#include "linalg/symmetricOperators.hh"
#include "fem/supports.hh"
#include "utilities/threading.hh"
#include "utilities/timing.hh"

namespace Kaskade
{
  /**
   * \ingroup iterative
   * \brief An additive overlapping domain decomposition type preconditioner for higher order finite elements applied to elliptic equations.
   *
   * The subdomains are the stars around grid vertices. The ansatz functions with a support contained in a subdomain
   * are treated as a block, and the corresponding submatrix is inverted using dense linear algebra. The subdomain
   * corrections are added up.
   *
   * \tparam Space a finite element space (FEFunctionSpace)
   * \tparam m the number of components in the equation
   */
  template <class Space, int m>
  class PatchDomainDecompositionPreconditioner: public SymmetricPreconditioner<typename Space::template Element<m>::type::StorageType,
                                                                               typename Space::template Element<m>::type::StorageType>
  {
    using Scalar = typename Space::Scalar;
    using Entry = Dune::FieldMatrix<Scalar,m,m>;
    using VEntry = Dune::FieldVector<Scalar,m>;
    using PEntry = float;
  public:
    using domain_type = typename Space::template Element<m>::type::StorageType;
    using range_type = domain_type;

    template <class Matrix>
    PatchDomainDecompositionPreconditioner(Space const& space, Matrix const& A)
    : patchMatrices(space.gridView().size(space.gridView().dimension)),
      patchDofs(patchMatrices.size())
    {
      assert(A.N()>0);
      assert(A.N()==A.M());
      Timings& timer = Timings::instance();
      timer.start("make Schwarz preconditioner");

      auto patches = computePatches(space.gridView());
      assert(patches.size()==patchMatrices.size());

      // We can create more tasks than CPU cores available - these will just wait in the thread pool queue.
      // Having more tasks can be beneficial if the loads are unbalanced (e.g. in unstructured grids
      // or when in a multi-user environement different processes occupy some cores).
      // On a single-CPU machine, however, a larger number of tasks can be slower, probably because
      // more threads than cores compete for cache and memory bandwith.
      int const taskFactor = 1;

      parallelFor([&patches,this,&A,&space](size_t j, size_t s)
      {
        DynamicMatrix<Entry> aloc;
        for (size_t i=uniformWeightRangeStart(j,s,patches.size()); i<uniformWeightRangeStart(j+1,s,patches.size()); ++i)
        {
          this->patchDofs[i] = algebraicPatch(space,patches[i]);
          auto const& idx = this->patchDofs[i];
          int n = idx.size();
          auto& aloc = this->patchMatrices[i];
          aloc.resize(n*m,n*m);
//          aloc = 0;
          for (int i=0; i<n*m;i++)                     // workaround for a bug in
            for (int j=0; j<n*m;j++) aloc[i][j]=0;     // Macintosh clang++
          for (int k=0; k<n; ++k)         // extract the submatrix of current patch
          {
            auto Ak = A[idx[k]];
            for (int l=0; l<n; ++l)       // TODO: scanning through the row, ticking off the desired entries, is
            {                             //       probably more efficient (idx is sorted anyway)
              auto const& akl = Ak[idx[l]];
              for (int r=0; r<m; ++r)
                for (int s=0; s<m; ++s)
                  aloc[k*m+r][l*m+s] = akl[r][s];
            }
          }
//          line below temporarely outcommented, because it depends on further changes which
//          cannot yet be checked in (L.W.)
//          invertSpd(aloc); // Jacobi style: invert the local block
        }
      },taskFactor*NumaThreadPool::instance().cpus());

      timer.stop("make Schwarz preconditioner");
    }

    virtual Scalar applyDp(domain_type& x, range_type const& y)
    {
      apply(x,y);
      return x*y;
    }

    virtual void apply(domain_type& x, range_type const& y)
    {
      // TODO: consider parallelization, e.g. maintain groups of
      //       patches where the patches in each group are algebraically
      //       disjoint and can be processed in parallel. Of course, this
      //       requires a higher setup cost.
      std::vector<PEntry> xi, yi;
      for (size_t i=0; i<patchMatrices.size(); ++i)
      {
        auto const& idx = patchDofs[i];
        xi.resize(m*patchMatrices[i].N());
        yi.resize(m*patchMatrices[i].N());
        for (int j=0; j<idx.size(); ++j)   // extract the local residual values
          for (int k=0; k<m; ++k)
            yi[j*m+k] = y[idx[j]][k];
        patchMatrices[i].mv(yi,xi);        // solve the local system (using BLAS ssymv does not improve performance)
        for (int j=0; j<idx.size(); ++j)   // copy the solution values back, adding up
          for (int k=0; k<m; ++k)
            x[idx[j]][k] += xi[j*m+k];     // all the corrections
      }
    }

    virtual bool requiresInitializedInput() const
    {
      return true;
    }

  private:
    std::vector<DynamicMatrix<PEntry>> patchMatrices;
    std::vector<std::vector<size_t>>   patchDofs;
  };
}

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_FUNCTIONAL_HASH_HPP
#define BOOST_COMPUTE_FUNCTIONAL_HASH_HPP

#include <boost/compute/function.hpp>
#include <boost/compute/types/fundamental.hpp>

namespace boost {
namespace compute {
namespace detail {

template<class Key>
std::string make_hash_function_name()
{
    return std::string("boost_hash_") + type_name<Key>();
}

template<class Key>
inline std::string make_hash_function_source()
{
  std::stringstream source;
  source << "inline ulong " << make_hash_function_name<Key>()
         << "(const " << type_name<Key>() << " x)\n"
         << "{\n"
         // note we reinterpret the argument as a 32-bit uint and
         // then promote it to a 64-bit ulong for the result type
         << "    ulong a = as_uint(x);\n"
         << "    a = (a ^ 61) ^ (a >> 16);\n"
         << "    a = a + (a << 3);\n"
         << "    a = a ^ (a >> 4);\n"
         << "    a = a * 0x27d4eb2d;\n"
         << "    a = a ^ (a >> 15);\n"
         << "    return a;\n"
         << "}\n";
    return source.str();
}

template<class Key>
struct hash_impl
{
    typedef Key argument_type;
    typedef ulong_ result_type;

    hash_impl()
        : m_function("")
    {
        m_function = make_function_from_source<result_type(argument_type)>(
            make_hash_function_name<argument_type>(),
            make_hash_function_source<argument_type>()
        );
    }

    template<class Arg>
    invoked_function<result_type, boost::tuple<Arg> >
    operator()(const Arg &arg) const
    {
        return m_function(arg);
    }

    function<result_type(argument_type)> m_function;
};

} // end detail namespace

/// The hash function returns a hash value for the input value.
///
/// The return type is \c ulong_ (the OpenCL unsigned long type).
template<class Key> struct hash;

/// \internal_
template<> struct hash<int_> : detail::hash_impl<int_> { };

/// \internal_
template<> struct hash<uint_> : detail::hash_impl<uint_> { };

/// \internal_
template<> struct hash<float_> : detail::hash_impl<float_> { };

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_FUNCTIONAL_HASH_HPP


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE bse_test

// Standard includes
#include <fstream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/bseoperator_btda.h"
#include "votca/xtp/matrixfreeoperator.h"

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(bse_test_operatorbtda)

class HermitianBlockOperator : public MatrixFreeOperator {
 public:
  HermitianBlockOperator() = default;

  void attach_matrix(const Eigen::MatrixXd &mat);
  Eigen::MatrixXd matmul(const Eigen::MatrixXd &input) const {
    return mat_ * input;
  }

  Eigen::VectorXd diagonal() const { return mat_.diagonal(); }

 private:
  Eigen::MatrixXd mat_;
};

void HermitianBlockOperator::attach_matrix(const Eigen::MatrixXd &mat) {
  mat_ = mat;
}

BOOST_AUTO_TEST_CASE(bse_test_operatorbtda) {

  votca::Index size = 7;

  Eigen::MatrixXd r1 = Eigen::MatrixXd::Random(size, size);
  Eigen::MatrixXd symm1 = r1 + r1.transpose();

  Eigen::MatrixXd r2 = Eigen::MatrixXd::Random(size, size);
  Eigen::MatrixXd symm2 = r2 + r2.transpose();

  HermitianBlockOperator Rop;
  Rop.set_size(size);
  Rop.attach_matrix(symm1);

  HermitianBlockOperator Cop;
  Cop.set_size(size);
  Cop.attach_matrix(symm2);

  // create Hamiltonian operator
  HamiltonianOperator<HermitianBlockOperator, HermitianBlockOperator> Hop(Rop,
                                                                          Cop);

  Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(2 * size, 2 * size);

  Eigen::MatrixXd H = Hop * identity;

  Eigen::MatrixXd ref = Eigen::MatrixXd::Zero(2 * size, 2 * size);
  ref.topLeftCorner(size, size) = symm1;
  ref.topRightCorner(size, size) = symm2;
  ref.bottomLeftCorner(size, size) = -symm2;
  ref.bottomRightCorner(size, size) = -symm1;

  bool check = ref.isApprox(H, 1E-9);
  BOOST_CHECK_EQUAL(check, true);
  if (!check) {
    std::cout << "ref" << std::endl;
    std::cout << ref << std::endl;
    std::cout << "result" << std::endl;
    std::cout << H << std::endl;
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/graph/edge_connectivity.hpp>


/**********************************************************************
  Cluster - Implementation of an atomic cluster

  Copyright (C) 2010-2011 by David C. Lonie

  This source code is released under the New BSD License, (the "License").

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
 ***********************************************************************/

#include <gapc/structures/cluster.h>

#include <globalsearch/macros.h>

#include <avogadro/neighborlist.h>

#include <Eigen/Array>

#include <algorithm>
#include <deque>
#include <vector>

using namespace Avogadro;
using namespace std;

namespace GAPC {

Cluster::Cluster(QObject* parent) : Structure(parent)
{
}

Cluster::~Cluster()
{
}

void Cluster::constructRandomCluster(
  const QHash<unsigned int, unsigned int>& comp, float minIAD, float maxIAD)
{
  INIT_RANDOM_GENERATOR();
  // Get atomic numbers
  QList<unsigned int> atomicnums = comp.keys();
  unsigned int totalSpecies = atomicnums.size();

  // Get total number of atoms
  unsigned int totalAtoms = 0;
  for (int i = 0; i < totalSpecies; i++) {
    totalAtoms += comp.value(atomicnums.at(i));
  }

  // Use this queue to determine the order to add the atoms. Value
  // is atomic number
  deque<unsigned int> q;

  // Populate queue
  // - Fill queue
  for (int i = 0; i < totalSpecies; i++) {
    unsigned int atomicnum = atomicnums.at(i);
    for (int j = 0; j < comp[atomicnum]; j++) {
      q.push_back(atomicnum);
    }
  }
  // - Randomize
  random_shuffle(q.begin(), q.end());

  // Populate cluster
  clear();
  Eigen::Vector3d tmpvec;
  QList<Atom*> neighbors;
  QList<double> distances;
  while (!q.empty()) {
    // Center the molecule at the origin
    centerAtoms();
    // Upper limit for new position distance
    double max = radius() + maxIAD;
    // temp coordinates
    double x, y, z;
    // Set first atom to origin
    if (numAtoms() == 0) {
      x = y = z = 0.0;
    }
    // Set second atom randomly between minIAD and maxIAD from origin
    else if (numAtoms() == 1) {
      tmpvec.setRandom();
      tmpvec.normalize();
      tmpvec = tmpvec * (maxIAD - minIAD) * RANDDOUBLE() +
               (Eigen::Vector3d::Ones() * minIAD);
      x = tmpvec.x();
      y = tmpvec.y();
      z = tmpvec.z();
    }
    // Randomly generate other atoms, ensuring that they lie between
    // minIAD and maxIAD from at least two other atoms and no more
    // than minIAD from any atom
    else {
      forever
      {
        // Randomly generate coordinates
        tmpvec.setRandom();
        tmpvec.normalize();
        tmpvec = tmpvec * max * RANDDOUBLE();
        x = tmpvec.x();
        y = tmpvec.y();
        z = tmpvec.z();
        neighbors = getNeighbors(x, y, z, maxIAD, &distances);

        Q_ASSERT(neighbors.size() == distances.size());

        // If not neighbors, continue
        if (neighbors.size() < 2) {
          continue;
        }

        unsigned int found = 0;
        bool invalid = false;
        for (QList<double>::const_iterator dit = distances.constBegin(),
                                           dit_end = distances.constEnd();
             dit != dit_end; ++dit) {
          // check that the distance is greater than minIAD from the new point
          if (*dit < minIAD) {
            invalid = true;
            break;
          }
          ++found;
        }

        // If the conditions aren't met, generate a new set of coordinates
        if (invalid || found < 2) {
          continue;
        }
        // Otherwise add the atom.
        break;
      }
    }
    Atom* atm = addAtom();
    Eigen::Vector3d pos(x, y, z);
    atm->setPos(pos);
    atm->setAtomicNumber(q.front());
    q.pop_front();
  }

  resetEnergy();
  resetEnthalpy();
  emit moleculeChanged();
}

void Cluster::centerAtoms()
{
  translate(-center());
}

QHash<QString, QVariant> Cluster::getFingerprint() const
{
  QHash<QString, QVariant> fp = Structure::getFingerprint();
  fp.insert("IADFreq", QVariant(m_histogramFreq));
  fp.insert("IADDist", QVariant(m_histogramDist));
  return fp;
}

Eigen::Vector3f ev3dToev3f(const Eigen::Vector3d* v)
{
  return Eigen::Vector3f(v->x(), v->y(), v->z());
}

inline void QListUniqueAppend(QList<Atom*>* perm, const QList<Atom*>* tmp)
{
  for (int i = 0; i < tmp->size(); i++) {
    if (!perm->contains(tmp->at(i))) {
      perm->append(tmp->at(i));
    }
  }
}

bool Cluster::checkForExplosion(double rcut) const
{
  NeighborList nl(const_cast<Cluster*>(this), rcut);
  QList<Atom *> found, tmp;

  // The found list is treated as unique list, using the
  // QListUniqueAppend function above. We'll start at the farthest
  // atom from the molecule's center, then iteratively add all
  // neighbors within rcut. This is repeated until we have added all
  // atoms that we can using this method of traversal. Then we
  // compare the size of the found tracker to numAtoms() to see if
  // all atoms were found using this method.

  const Atom* start = farthestAtom();
  Eigen::Vector3f fpos = ev3dToev3f(start->pos());
  tmp = nl.nbrs(&fpos);
  QListUniqueAppend(&found, &tmp);

  for (int i = 0; i < found.size(); i++) {
    Eigen::Vector3f fpos = ev3dToev3f(found.at(i)->pos());
    tmp = nl.nbrs(&fpos);
    QListUniqueAppend(&found, &tmp);
  }

  if (found.size() != numAtoms()) {
    return false;
  }
  return true;
}

void Cluster::expand(double factor)
{
  // Center atom
  centerAtoms();

  // Perform expansion on all atoms
  double rho, phi, theta, x, y, z;
  Atom* atom;
  const Eigen::Vector3d* pos;
  Eigen::Vector3d npos;
  for (int i = 0; i < numAtoms(); i++) {
    // Convert cartestian coords to spherical coords
    atom = atoms().at(i);
    pos = atom->pos();
    x = pos->x();
    y = pos->y();
    z = pos->z();
    rho = sqrt(x * x + y * y + z * z);
    phi = acos(z / rho);
    theta = asin(y / sqrt(x * x + y * y));
    if (x < 0)
      theta = M_PI - theta;
    // Expand
    rho *= factor;
    // Back to cartesian
    npos.x() = rho * sin(phi) * cos(theta);
    npos.y() = rho * sin(phi) * sin(theta);
    npos.z() = rho * cos(phi);
    atom->setPos(&npos);
  }
  emit update();
}

} // end namespace GAPC


//==============================================================================
//         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 BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_HI_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SIMD_COMMON_HI_HPP_INCLUDED
#include <boost/simd/bitwise/functions/hi.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/constants/digits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::hi_, tag::cpu_, (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<result_type>::type int_type;
      BOOST_STATIC_CONSTANT(int_type, shift = sizeof(int_type)*4);
      BOOST_STATIC_CONSTANT(int_type, pattern = int_type(int_type(-1)<<shift));
      return shri((boost::simd::integral_constant<result_type,pattern>()&a0),shift);
    }
  };
} } }

#endif


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Bbox_2.h>
#include <QtCore/qglobal.h>
#include <CGAL/Three/Three.h>
#include <CGAL/Qt/manipulatedCameraFrame.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/Fuzzy_sphere.h>
#include <CGAL/linear_least_squares_fitting_3.h>

#include "Messages_interface.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"

#include <CGAL/Three/Scene_interface.h>
#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Three.h>
#include "ui_Point_set_selection_widget.h"
#include <Plugins/PCA/Scene_edit_box_item.h>
#include "Point_set_3.h"

#include <QAction>
#include <QMainWindow>
#include <QApplication>

#include <QEvent>
#include <QKeyEvent>
#include <QMouseEvent>
#include <QMessageBox>

#include <QMultipleInputDialog.h>
#include <QDoubleSpinBox>

#include <map>
#include <fstream>
#include <boost/make_shared.hpp>
#include "Selection_visualizer.h"

//#undef  CGAL_LINKED_WITH_TBB
#ifdef CGAL_LINKED_WITH_TBB
#include <tbb/parallel_for.h>
#include <mutex>
#include <tbb/blocked_range.h>
#include <tbb/scalable_allocator.h>
#endif // CGAL_LINKED_WITH_TBB

// Class for visualizing selection
// provides mouse selection functionality
using namespace CGAL::Three;

Viewer_interface* getActiveViewer()
{
  Q_FOREACH(CGAL::QGLViewer* v, CGAL::QGLViewer::QGLViewerPool())
  {
    if(v->hasFocus())
    {
      return static_cast<Viewer_interface*>(v);
    }
  }
  return Three::mainViewer();
}
class Q_DECL_EXPORT Scene_point_set_selection_visualizer
{

 private:
  typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
  typedef K::Point_2 Point_2;
  typedef K::Point_3 Point_3;
  typedef CGAL::Polygon_2<K> Polygon_2;
  typedef std::vector<Point_2> Polyline_2;
  typedef std::vector<Polyline_2> Polylines;
  typedef Scene_item::Bbox Bbox;

  bool rectangle;
  std::vector<Point_2> contour_2d;
  Polylines* polyline;
  Scene_item::Bbox point_set_bbox;
  CGAL::Bbox_2 domain_rectangle;
  Polygon_2 domain_freeform;

public:

  Scene_point_set_selection_visualizer(bool rectangle, const Bbox& point_set_bbox)
    : rectangle (rectangle), point_set_bbox (point_set_bbox)
  {
    polyline = new Polylines(0);
    polyline->push_back(Polyline_2());
  }
  ~Scene_point_set_selection_visualizer() {
  }

  void render(QImage& image) const {

    CGAL::Three::Viewer_interface* viewer = getActiveViewer();

    QPen pen;
    pen.setColor(QColor(Qt::green));
    pen.setWidth(5);
    QImage temp(image);

    QPainter *painter = new QPainter(&temp);
    painter->setPen(pen);
    for(std::size_t i=0; i<polyline->size(); ++i)
    {
      Polyline_2 poly = (*polyline)[i];
      if(!poly.empty())
        for(std::size_t j=0; j<poly.size()-1; ++j)
        {
          painter->drawLine(poly[j].x(), poly[j].y(), poly[j+1].x(), poly[j+1].y());
        }
    }
    painter->end();
    delete painter;
    viewer->set2DSelectionMode(true);
    viewer->setStaticImage(temp);
    viewer->update();
  }

  Polyline_2& poly() const
  { return polyline->front(); }

  bool update_polyline () const
  {
    if (contour_2d.size() < 2 ||
        (!(poly().empty()) && contour_2d.back () == poly().back()))
      return false;

    if (rectangle)
      {
	poly().clear();

        poly().push_back ( Point_2 (domain_rectangle.xmin(),
                                domain_rectangle.ymin()));
        poly().push_back ( Point_2 (domain_rectangle.xmax(),
                                domain_rectangle.ymin()));
        poly().push_back ( Point_2 (domain_rectangle.xmax(),
                                domain_rectangle.ymax()));
        poly().push_back ( Point_2 (domain_rectangle.xmin(),
                                domain_rectangle.ymax()));
        poly().push_back ( Point_2 (domain_rectangle.xmin(),
                                                domain_rectangle.ymin()));

      }
    else
      {
        if (!(poly().empty()) && contour_2d.back () == poly().back())
	  return false;

	poly().clear();

	for (unsigned int i = 0; i < contour_2d.size (); ++ i)
          poly().push_back (contour_2d[i]);
      }
    return true;
  }

  void sample_mouse_path(QImage& image)
  {
    CGAL::QGLViewer* viewer = getActiveViewer();
    const QPoint& p = viewer->mapFromGlobal(QCursor::pos());

    if (rectangle && contour_2d.size () == 2)
      {
	contour_2d[1] = Point_2 (p.x (), p.y ());
	domain_rectangle = CGAL::bbox_2 (contour_2d.begin (), contour_2d.end ());
      }
    else
      contour_2d.push_back (Point_2 (p.x (), p.y ()));

    if (update_polyline ())
      {

        render(image);
      }
  }

  void apply_path()
  {
    update_polyline ();
    domain_rectangle = CGAL::bbox_2 (contour_2d.begin (), contour_2d.end ());
    if (!rectangle)
      domain_freeform = Polygon_2 (contour_2d.begin (), contour_2d.end ());
  }

  bool is_selected (CGAL::qglviewer::Vec& p)
  {
    if (domain_rectangle.xmin () < p.x &&
	p.x < domain_rectangle.xmax () &&
	domain_rectangle.ymin () < p.y &&
	p.y < domain_rectangle.ymax ())
      {
	if (rectangle)
	  return true;
/*
 * domain_freeform.has_on_bounded_side() requires the polygon to be simple, which is never the case.
 * However, it works very well even if the polygon is not simple, so we use this instead to avoid
 * the cgal_assertion on is_simple().*/

        if (CGAL::bounded_side_2(domain_freeform.container().begin(),
                                 domain_freeform.container().end(),
                                 Point_2(p.x, p.y),
                                 domain_freeform.traits_member())  == CGAL::ON_BOUNDED_SIDE)
          return true;
      }
    return false;
  }
};
///////////////////////////////////////////////////////////////////////////////////////////////////

class Selection_test {

  Point_set* point_set;
  bool* selected;
  CGAL::qglviewer::Camera* camera;
  const CGAL::qglviewer::Vec offset;
  Scene_edit_box_item* edit_box;
  Selection_visualizer* visualizer;
  QVector4D* clipbox;
  const Ui::PointSetSelection& ui_widget;

public:

  Selection_test(Point_set* point_set,
                 bool* selected,
                 CGAL::qglviewer::Camera* camera,
                 const CGAL::qglviewer::Vec offset,
                 Scene_edit_box_item* edit_box,
                 Selection_visualizer* visualizer,
                 QVector4D* clipbox,
                 const Ui::PointSetSelection& ui_widget)
    : point_set (point_set)
    , selected (selected)
    , camera (camera)
    , offset (offset)
    , edit_box (edit_box)
    , visualizer (visualizer)
    , clipbox (clipbox)
    , ui_widget (ui_widget)
  {
  }

#ifdef CGAL_LINKED_WITH_TBB
  void operator()(const tbb::blocked_range<std::size_t>& r) const
  {
    for( std::size_t i = r.begin(); i != r.end(); ++i)
      apply (i);
  }
#endif // CGAL_LINKED_WITH_TBB

  void apply (std::size_t i) const
  {
    Point_set::Index idx = *(point_set->begin() + i);
    const Kernel::Point_3& p = point_set->point (idx);

    // Points outside clipbox are not affected
    if (!is_inside_clipbox (p))
    {
      selected[idx] = point_set->is_selected (point_set->begin() + i);
      return;
    }

    CGAL::qglviewer::Vec vp (p.x (), p.y (), p.z ());
    bool now_selected = false;
    if(!ui_widget.box->isChecked())
    {
      CGAL::qglviewer::Vec vsp = camera->projectedCoordinatesOf (vp + offset);
      now_selected = visualizer->is_selected (vsp);
    }
    else if(edit_box)
    {
      if(p.x() >= edit_box->point(0,0) - offset.x &&
         p.y() >= edit_box->point(0,1) - offset.y &&
         p.z() <= edit_box->point(0,2) - offset.z &&
         p.x() <= edit_box->point(6,0) - offset.x &&
         p.y() <= edit_box->point(6,1) - offset.y &&
         p.z() >= edit_box->point(6,2) - offset.z
        )
      {
        now_selected = true;
      }
    }

    if (ui_widget.new_selection->isChecked())
      selected[idx] = now_selected;
    else
    {
      bool already_selected = point_set->is_selected (point_set->begin() + i);
      if (ui_widget.union_selection->isChecked())
        selected[idx] = (already_selected || now_selected);
      else if (ui_widget.intersection->isChecked())
        selected[idx] = (already_selected && now_selected);
      else if (ui_widget.diff->isChecked())
        selected[idx] = (already_selected && !now_selected);
    }
  }

  bool is_inside_clipbox (const Kernel::Point_3& p) const
  {
    if(!static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->isClipping())
      return true;

    double x = p.x()+offset.x, y = p.y()+offset.y, z = p.z()+offset.z;

    return !(clipbox[0][0]*x+clipbox[0][1]*y+clipbox[0][2]*z+clipbox[0][3] >0 ||
             clipbox[1][0]*x+clipbox[1][1]*y+clipbox[1][2]*z+clipbox[1][3]>0 ||
             clipbox[2][0]*x+clipbox[2][1]*y+clipbox[2][2]*z+clipbox[2][3]>0 ||
             clipbox[3][0]*x+clipbox[3][1]*y+clipbox[3][2]*z+clipbox[3][3]>0 ||
             clipbox[4][0]*x+clipbox[4][1]*y+clipbox[4][2]*z+clipbox[4][3]>0 ||
             clipbox[5][0]*x+clipbox[5][1]*y+clipbox[5][2]*z+clipbox[5][3]>0);
  }

};

class Neighborhood
{
  typedef Kernel Geom_traits;

  typedef CGAL::Search_traits_3<Geom_traits> SearchTraits_3;
  typedef CGAL::Search_traits_adapter <Point_set::Index, Point_set::Point_map, SearchTraits_3> Search_traits;
  typedef CGAL::Orthogonal_k_neighbor_search<Search_traits> Neighbor_search;
  typedef Neighbor_search::Tree Tree;
  typedef Neighbor_search::Distance Distance;
  typedef CGAL::Fuzzy_sphere<Search_traits> Sphere;

  Scene_points_with_normal_item* points_item;
  boost::shared_ptr<Tree> tree;

public:

  Neighborhood () : points_item (NULL)
  {
  }

  ~Neighborhood ()
  {

  }

  Neighborhood& point_set (Scene_points_with_normal_item* points_item)
  {
    if (this->points_item != points_item)
    {
      this->points_item = points_item;

      tree = boost::make_shared<Tree> (points_item->point_set()->begin(),
                                       points_item->point_set()->end(),
                                       Tree::Splitter(),
                                       Search_traits (points_item->point_set()->point_map()));

    }
    return *this;
  }

  void expand()
  {
    if (points_item->point_set()->nb_selected_points() == 0)
      return;

    Distance tr_dist(points_item->point_set()->point_map());

    std::vector<bool> selected_bitmap (points_item->point_set()->size(), false);

    for (Point_set::iterator it = points_item->point_set()->first_selected();
         it != points_item->point_set()->end(); ++ it)
    {
      Neighbor_search search(*tree, points_item->point_set()->point(*it), 6, 0, true, tr_dist);
      for (Neighbor_search::iterator nit = search.begin(); nit != search.end(); ++ nit)
        selected_bitmap[nit->first] = true;
    }

    points_item->point_set()->set_first_selected
      (std::partition (points_item->point_set()->begin(), points_item->point_set()->end(),
                       [&] (const Point_set::Index& idx) -> bool
                       { return !selected_bitmap[idx]; }));

    points_item->invalidateOpenGLBuffers();
    points_item->itemChanged();
  }

  void reduce()
  {
    if (points_item->point_set()->nb_selected_points() == 0)
      return;

    points_item->invertSelection();
    expand();
    points_item->invertSelection();

    points_item->invalidateOpenGLBuffers();
    points_item->itemChanged();
  }

  void grow_region (const Kernel::Point_3& query, double epsilon, double cluster_epsilon,
                    unsigned int normal_threshold)
  {
    double cos_threshold = std::cos (CGAL_PI * normal_threshold / 180.);

    Distance tr_dist(points_item->point_set()->point_map());

    std::set<Point_set::Index> index_container;
    std::vector<Point_set::Index> index_container_former_ring;
    std::set<Point_set::Index> index_container_current_ring;

    int conti = 0; 	//for accelerate least_square fitting

    std::vector<Kernel::Point_3> init_points;
    init_points.reserve (6);

    Neighbor_search search(*tree, query, 6, 0, true, tr_dist);
    for (Neighbor_search::iterator nit = search.begin(); nit != search.end(); ++ nit)
    {
      index_container.insert (nit->first);
      index_container_former_ring.push_back (nit->first);
      init_points.push_back (points_item->point_set()->point (nit->first));
    }

    Kernel::Plane_3 optimal_plane;
    CGAL::linear_least_squares_fitting_3(init_points.begin(),
                                         init_points.end(),
                                         optimal_plane,
                                         CGAL::Dimension_tag<0>());

    Kernel::Vector_3 plane_normal = optimal_plane.orthogonal_vector();
    plane_normal = plane_normal / std::sqrt(plane_normal * plane_normal);

    std::vector<Point_set::Index> neighbors;

    bool propagation = false;
    do
    {
      propagation = false;

      for (std::vector<Point_set::Index>::iterator icfrit
             = index_container_former_ring.begin();
           icfrit != index_container_former_ring.end(); ++ icfrit)
      {
        Point_set::Index point_index = *icfrit;

        neighbors.clear();
        Sphere fs (points_item->point_set()->point(point_index),
                   cluster_epsilon, 0, tree->traits());
        tree->search (std::back_inserter (neighbors), fs);

        for (std::size_t nb = 0; nb < neighbors.size(); ++ nb)
        {
          Point_set::Index neighbor_index = neighbors[nb];

          if (index_container.find(neighbor_index) != index_container.end())
            continue;

          const Kernel::Point_3& neighbor = points_item->point_set()->point(neighbor_index);
          double distance = CGAL::squared_distance(neighbor, optimal_plane);

          if (distance > epsilon * epsilon)
            continue;

          if (points_item->point_set()->has_normal_map())
          {
            Kernel::Vector_3 normal = points_item->point_set()->normal (neighbor_index);
            normal = normal / std::sqrt (normal * normal);
            if (std::fabs (normal * plane_normal) < cos_threshold)
              continue;
          }

          index_container.insert (neighbor_index);
          propagation = true;
          index_container_current_ring.insert(neighbor_index);
        }
      }

      //update containers
      index_container_former_ring.clear();
      index_container_former_ring.reserve (index_container_current_ring.size());

      for (std::set<Point_set::Index>::iterator lit = index_container_current_ring.begin();
           lit != index_container_current_ring.end(); ++lit)
        index_container_former_ring.push_back(*lit);

      index_container_current_ring.clear();

      conti++;
      if (index_container.size() < 5)
        continue;

      if ((conti < 10) || (conti<50 && conti % 10 == 0) || (conti>50 && conti % 500 == 0))
      {
        std::list<Kernel::Point_3> listp;
        for (std::set<Point_set::Index>::iterator icit = index_container.begin();
             icit != index_container.end(); ++ icit)
          listp.push_back(points_item->point_set()->point (*icit));

        Kernel::Plane_3 reajusted_plane;
        CGAL::linear_least_squares_fitting_3(listp.begin(),
                                             listp.end(),
                                             reajusted_plane,
                                             CGAL::Dimension_tag<0>());
        optimal_plane = reajusted_plane;
        plane_normal = optimal_plane.orthogonal_vector();
        plane_normal = plane_normal / std::sqrt(plane_normal * plane_normal);
      }
    }
    while (propagation);

    std::vector<Point_set::Index> unselected, selected;

    for(Point_set::iterator it = points_item->point_set()->begin ();
	it != points_item->point_set()->end(); ++ it)
      if (index_container.find (*it) != index_container.end())
        selected.push_back (*it);
      else
        unselected.push_back (*it);

    for (std::size_t i = 0; i < unselected.size(); ++ i)
      *(points_item->point_set()->begin() + i) = unselected[i];
    for (std::size_t i = 0; i < selected.size(); ++ i)
      *(points_item->point_set()->begin() + (unselected.size() + i)) = selected[i];

    if (selected.empty ())
      {
	points_item->point_set()->unselect_all();
      }
    else
      {
	points_item->point_set()->set_first_selected
	  (points_item->point_set()->begin() + unselected.size());
      }
    points_item->invalidateOpenGLBuffers();
    points_item->itemChanged();
  }

};

class Polyhedron_demo_point_set_selection_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
    Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
    Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
public:
  bool applicable(QAction*) const {
      return qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
  }
  void print_message(QString message) { CGAL::Three::Three::information(message); }
  QList<QAction*> actions() const { return QList<QAction*>() << actionPointSetSelection; }

  void init(QMainWindow* mainWindow, CGAL::Three::Scene_interface* scene_interface, Messages_interface* m) {
    mw = mainWindow;
    scene = scene_interface;
    messages = m;
    rg_epsilon = -1;
    rg_cluster_epsilon = -1;
    rg_normal_threshold = 20;
    actionPointSetSelection = new QAction(tr("Selection"), mw);
    actionPointSetSelection->setObjectName("actionPointSetSelection");
    connect(actionPointSetSelection, SIGNAL(triggered()), this, SLOT(selection_action()));

    dock_widget = new QDockWidget("Point Set Selection", mw);
    dock_widget->setVisible(false);

    ui_widget.setupUi(dock_widget);
    addDockWidget(dock_widget);

    connect(ui_widget.region, SIGNAL(toggled(bool)), this, SLOT(set_region_parameters(bool)));

    // Fill actions of menu
    ui_widget.menu->setMenu (new QMenu("Point Set Selection Menu", ui_widget.menu));
    QAction* select_all = ui_widget.menu->menu()->addAction ("Select all points");
    connect(select_all,  SIGNAL(triggered()), this, SLOT(on_Select_all_button_clicked()));
    QAction* clear = ui_widget.menu->menu()->addAction ("Clear selection");
    connect(clear,  SIGNAL(triggered()), this, SLOT(on_Clear_button_clicked()));
    QAction* invert = ui_widget.menu->menu()->addAction ("Invert selection");
    connect(invert,  SIGNAL(triggered()), this, SLOT(on_Invert_selection_button_clicked()));
    QAction* erase = ui_widget.menu->menu()->addAction ("Erase selected points");
    connect(erase,  SIGNAL(triggered()), this, SLOT(on_Erase_selected_points_button_clicked()));
    QAction* create = ui_widget.menu->menu()->addAction ("Create point set item from selected points");
    connect(create, SIGNAL(triggered()), this, SLOT(on_Create_point_set_item_button_clicked()));

    ui_widget.menu->menu()->addSection ("Box selection");
    add_box = ui_widget.menu->menu()->addAction ("Apply");
    connect(add_box, SIGNAL(triggered()), this, SLOT(select_points()));
    add_box->setEnabled(false);

    connect(ui_widget.box, SIGNAL(toggled(bool)),
            this, SLOT(on_box_toggled(bool)));
    connect(ui_widget.helpButton, &QAbstractButton::clicked,
    [this](){
      QMessageBox::information(dock_widget, QString("Help"),
                               QString("SHIFT + Left Click : selection\n"
                                       "CONTROL + Left Click : print coordinates of point under cursor."));
    }
  );

    visualizer = NULL;
    edit_box = NULL;
    shift_pressing = false;
    ctrl_pressing = false;

    Q_FOREACH(CGAL::QGLViewer* viewer, CGAL::QGLViewer::QGLViewerPool())
    {
      viewer->installEventFilter(this);
    }
    connect(mw, SIGNAL(newViewerCreated(QObject*)),
            this, SLOT(connectNewViewer(QObject*)));
    mainWindow->installEventFilter(this);

  }
  virtual void closure()
  {
    dock_widget->hide();
  }

protected:

  bool eventFilter(QObject *, QEvent *event) {
    static QImage background;
    if (dock_widget->isHidden() || !(dock_widget->isActiveWindow()) || ui_widget.box->isChecked())
      return false;

    Scene_points_with_normal_item* point_set_item
      = qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if(!point_set_item) {
      return false;
    }

    if(event->type() == QEvent::KeyPress || event->type() == QEvent::KeyRelease)
    {
      QKeyEvent *keyEvent = static_cast<QKeyEvent*>(event);
      Qt::KeyboardModifiers modifiers = keyEvent->modifiers();

      shift_pressing = modifiers.testFlag(Qt::ShiftModifier);
      ctrl_pressing = modifiers.testFlag(Qt::ControlModifier);
      Viewer_interface* viewer = getActiveViewer();
      background = viewer->grabFramebuffer();
    }

    // mouse events
    if(shift_pressing && event->type() == QEvent::MouseButtonPress)
      {
      background = static_cast<CGAL::Three::Viewer_interface*>(*CGAL::QGLViewer::QGLViewerPool().begin())->grabFramebuffer();
	QMouseEvent *mouseEvent = static_cast<QMouseEvent*>(event);
        Viewer_interface* viewer = getActiveViewer();
        background = viewer->grabFramebuffer();
	// Start selection
	if (mouseEvent->button() == Qt::LeftButton)
        {
          // Region growing
          if (ui_widget.region->isChecked())
          {
            QApplication::setOverrideCursor(Qt::WaitCursor);
            bool found = false;
            QPoint pixel(mouseEvent->pos().x(),
                         viewer->camera()->screenHeight() - 1 - mouseEvent->pos().y());

            CGAL::qglviewer::Vec point = viewer->camera()->pointUnderPixel(mouseEvent->pos(),
                                                                     found);
            if(!found)
            {
              QApplication::restoreOverrideCursor();
              return false;
            }
            const CGAL::qglviewer::Vec offset = Three::mainViewer()->offset();
            point = point - offset;

            neighborhood.point_set (point_set_item).grow_region
              (Kernel::Point_3 (point.x, point.y, point.z),
               rg_epsilon, rg_cluster_epsilon, rg_normal_threshold);

            QApplication::restoreOverrideCursor();
            return true;
          }
          // Start standard selection
          else if (!visualizer)
          {
	    QApplication::setOverrideCursor(Qt::CrossCursor);
            CGAL::QGLViewer* viewer = getActiveViewer();
            if (viewer->camera()->frame()->isSpinning())
              viewer->camera()->frame()->stopSpinning();

            visualizer = new Selection_visualizer(ui_widget.rectangle->isChecked(),
                                                                  point_set_item->bbox());

            visualizer->sample_mouse_path(background);
            return true;
          }
        }
	// Cancel selection
	else if (mouseEvent->button() == Qt::RightButton && visualizer)
	  {
	    visualizer = NULL;
	    QApplication::restoreOverrideCursor();
            return true;
	  }
      }
      // Expand/reduce selection
    else if (shift_pressing && event->type() == QEvent::Wheel)
      {
        QApplication::setOverrideCursor(Qt::WaitCursor);
        QWheelEvent *mouseEvent = static_cast<QWheelEvent*>(event);
        int steps = mouseEvent->delta() / 120;
        if (steps > 0)
          neighborhood.point_set (point_set_item).expand();
        else
          neighborhood.point_set (point_set_item).reduce();
        QApplication::restoreOverrideCursor();
      }
    // End selection
    else if (event->type() == QEvent::MouseButtonRelease && visualizer)
      {
	visualizer->apply_path();
	select_points();
	visualizer = NULL;
	QApplication::restoreOverrideCursor();
        getActiveViewer()->set2DSelectionMode(false);
	return true;
      }
    // Update selection
    else if (event->type() == QEvent::MouseMove && visualizer)
      {
        visualizer->sample_mouse_path(background);
	return true;
      }
    //Position request
    else if(ctrl_pressing && event->type() == QEvent::MouseButtonPress)
    {
      QMouseEvent* m_e = static_cast<QMouseEvent*>(event);
      Scene_points_with_normal_item* item =
          qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
      if(!item)
        return false;
      CGAL::QGLViewer* viewer = getActiveViewer();
      bool found = false;
      QPoint pixel(m_e->pos().x(),
             viewer->camera()->screenHeight()-1-m_e->pos().y());
      CGAL::qglviewer::Vec point = viewer->camera()->pointUnderPixel(m_e->pos(),
                                                               found);
      if(!found)
        return false;
      typedef Kernel::Point_3 Point;
      typedef CGAL::Search_traits_3<Kernel> TreeTraits;
      typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> Neighbor_search;
      typedef Neighbor_search::Tree Tree;
      Tree tree(item->point_set()->points().begin(), item->point_set()->points().end());

      const CGAL::qglviewer::Vec offset = Three::mainViewer()->offset();
      point = point - offset;

      Neighbor_search search(tree, Point(point.x, point.y, point.z), 1);
      Point res = search.begin()->first;
      CGAL::Three::Three::information(QString("Selected point : (%1, %2, %3)").arg(res.x()).arg(res.y()).arg(res.z()));
    }
    return false;
  }

protected Q_SLOTS:

  void connectNewViewer(QObject* o)
  {
    if(edit_box)
      o->installEventFilter(edit_box);
    o->installEventFilter(this);

  }

  void select_points()
  {
    Scene_points_with_normal_item* point_set_item = getSelectedItem<Scene_points_with_normal_item>();
    if(!point_set_item)
      {
	print_message("Error: no point set selected!");
	return;
      }

    Point_set* points = point_set_item->point_set();

    if (ui_widget.new_selection->isChecked())
      points->unselect_all();

    const CGAL::qglviewer::Vec offset = Three::mainViewer()->offset();
    CGAL::QGLViewer* viewer = getActiveViewer();

    CGAL::qglviewer::Camera* camera = viewer->camera();

    bool* selected_bitmap = new bool[points->size()]; // std::vector<bool> is not thread safe

    Selection_test selection_test (points, selected_bitmap,
                                   camera, offset, edit_box, visualizer,
                                   static_cast<CGAL::Three::Viewer_interface*>(viewer)->clipBox(),
                                   ui_widget);
#ifdef CGAL_LINKED_WITH_TBB
    tbb::parallel_for(tbb::blocked_range<size_t>(0, points->size()),
                      selection_test);
#else
    for (std::size_t i = 0; i < points->size(); ++ i)
      selection_test.apply(i);
#endif

    points->set_first_selected
      (std::partition (points->begin(), points->end(),
                       [&] (const Point_set::Index& idx) -> bool
                       { return !selected_bitmap[idx]; }));

    point_set_item->invalidateOpenGLBuffers();
    point_set_item->itemChanged();
  }

  void set_region_parameters(bool toggled)
  {
    if (!toggled)
      return;

    QMultipleInputDialog dialog ("Region Selection Parameters", mw);
    QDoubleSpinBox* epsilon = dialog.add<QDoubleSpinBox> ("Epsilon: ");
    epsilon->setRange (0.00001, 1000000.);
    epsilon->setDecimals (5);
    if (rg_epsilon < 0.)
      rg_epsilon = (std::max)(0.00001, 0.005 * scene->len_diagonal());
    epsilon->setValue (rg_epsilon);

    QDoubleSpinBox* cluster_epsilon = dialog.add<QDoubleSpinBox> ("Cluster epsilon: ");
    cluster_epsilon->setRange (0.00001, 1000000.);
    cluster_epsilon->setDecimals (5);
    if (rg_cluster_epsilon < 0.)
      rg_cluster_epsilon = (std::max)(0.00001, 0.03 * scene->len_diagonal());
    cluster_epsilon->setValue (rg_cluster_epsilon);

    QSpinBox* normal_threshold = dialog.add<QSpinBox> ("Normal threshold: ");
    normal_threshold->setRange (0, 90);
    normal_threshold->setSuffix (QString("°"));
    normal_threshold->setValue (rg_normal_threshold);

    if (dialog.exec())
    {
      rg_epsilon = epsilon->value();
      rg_cluster_epsilon = cluster_epsilon->value();
      rg_normal_threshold = normal_threshold->value();
    }
  }

public Q_SLOTS:
  void selection_action() {
    dock_widget->show();
    dock_widget->raise();
  }

  // Select all
  void on_Select_all_button_clicked() {
    Scene_points_with_normal_item* point_set_item = getSelectedItem<Scene_points_with_normal_item>();
    if(!point_set_item)
      {
	print_message("Error: no point set selected!");
	return;
      }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    point_set_item->selectAll();
    QApplication::restoreOverrideCursor();
  }

  // Clear selection
  void on_Clear_button_clicked() {
    Scene_points_with_normal_item* point_set_item
      = qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if(!point_set_item) {
      print_message("Error: no point set selected!");
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    point_set_item->resetSelection();
    QApplication::restoreOverrideCursor();
  }

  void on_Erase_selected_points_button_clicked() {
    Scene_points_with_normal_item* point_set_item
      = qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if(!point_set_item) {
      print_message("Error: no point set selected!");
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    point_set_item->deleteSelection();
    QApplication::restoreOverrideCursor();
  }

  void on_Invert_selection_button_clicked() {
    Scene_points_with_normal_item* point_set_item
      = qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if(!point_set_item) {
      print_message("Error: no point set selected!");
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    point_set_item->invertSelection();
    QApplication::restoreOverrideCursor();
  }

  void on_Create_point_set_item_button_clicked() {
    Scene_points_with_normal_item* point_set_item
      = qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex()));
    if(!point_set_item) {
      print_message("Error: no point set selected!");
      return;
    }
    if(point_set_item->isSelectionEmpty ()) {
      print_message("Error: there is no selected point in point set item!");
      return;
    }
    QApplication::setOverrideCursor(Qt::WaitCursor);
    Scene_points_with_normal_item* new_item = new Scene_points_with_normal_item;
    new_item->point_set()->copy_properties (*(point_set_item->point_set()));
    new_item->point_set()->check_colors();

    new_item->setName(QString("%1 (selected points)").arg(point_set_item->name()));

    new_item->setColor(point_set_item->color());
    new_item->setRenderingMode(point_set_item->renderingMode());
    new_item->setVisible(point_set_item->visible());

    std::cerr << point_set_item->point_set()->info() << std::endl;
    std::cerr << new_item->point_set()->info() << std::endl;

    typedef Point_set_3<Kernel> Point_set;
    for(Point_set::iterator it = point_set_item->point_set()->first_selected ();
        it != point_set_item->point_set()->end(); ++ it)
    {
      new_item->point_set()->insert (*(point_set_item->point_set()), *it);
    }
    new_item->resetSelection();
    new_item->invalidateOpenGLBuffers();

    scene->addItem(new_item);
    QApplication::restoreOverrideCursor();
 }

  void on_box_toggled (bool toggle)
  {
    if(toggle)
    {
      getActiveViewer()->set2DSelectionMode(false);
      edit_box = new Scene_edit_box_item(scene);
      edit_box->setRenderingMode(Wireframe);
      edit_box->setName("Selection Box");
      connect(edit_box, &Scene_edit_box_item::aboutToBeDestroyed,
              this, &Polyhedron_demo_point_set_selection_plugin::reset_editbox);
      scene->addItem(edit_box);
      Q_FOREACH(CGAL::QGLViewer* v, CGAL::QGLViewer::QGLViewerPool()){
        v->installEventFilter(edit_box);
      }
      connect(mw, SIGNAL(newViewerCreated(QObject*)),
              this, SLOT(connectNewViewer(QObject*)));
      add_box->setEnabled(true);
    }
    else
    {
      if(edit_box)
        scene->erase(scene->item_id(edit_box));
      edit_box = NULL;
      add_box->setEnabled(false);
    }

  }

  void reset_editbox()
  {
    edit_box = NULL;
  }

private:
  Messages_interface* messages;
  QAction* actionPointSetSelection;

  QDockWidget* dock_widget;
  QAction* add_box;

  Ui::PointSetSelection ui_widget;
  Selection_visualizer* visualizer;
  Neighborhood neighborhood;
  bool shift_pressing;
  bool ctrl_pressing;
  Scene_edit_box_item* edit_box;

  // Region growing selection
  double rg_epsilon;
  double rg_cluster_epsilon;
  unsigned int rg_normal_threshold;
}; // end Polyhedron_demo_point_set_selection_plugin

//Q_EXPORT_PLUGIN2(Polyhedron_demo_point_set_selection_plugin, Polyhedron_demo_point_set_selection_plugin)

#include "Point_set_selection_plugin.moc"


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pio2_1.hpp>
#include <boost/simd/constant/pio2_2.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/minus.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD (rem_pio2_straight_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          , bd::generic_ < bd::integer_<A1>  >
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE void operator() ( A0 const& x, A1 & n, A0 & xr) BOOST_NOEXCEPT
    {
      n = bs::rem_pio2_straight(x, xr);
    }
  };

  BOOST_DISPATCH_OVERLOAD (rem_pio2_straight_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_ < bd::floating_<A0> >
                          )
  {
    using int_t = bd::as_integer_t<A0>;
    using result_t = std::pair<int_t, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const BOOST_NOEXCEPT
    {
      A0 second;
      int_t const first = rem_pio2_straight(a0,second);
      return {first, second};
    }
  };

  BOOST_DISPATCH_OVERLOAD (rem_pio2_straight_
                          , (typename A0)
                          , bd::cpu_
                          , bd::generic_< bd::floating_<A0> >
                          , bd::generic_< bd::floating_<A0> >
                          )
  {
    using int_t = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE int_t operator() ( A0 const& x, A0 & xr) const BOOST_NOEXCEPT
    {
      xr = x-Pio2_1<A0>();
      xr = xr-Pio2_2<A0>();
      xr = xr-Pio2_3<A0>();
//       xr -= Pio2_2<A0>(); TODO
//       xr -= Pio2_3<A0>();
      return One<int_t>();
    }
  };
} } }

#endif