//------------------------------------------------------------------------------
// @file   : path_planning.cpp
// @brief  : robot's path planning using voronoi map
// @author : Yoonseok Pyo
// @version: Ver 1.0.2 (since 2015.08.10)
// @date   : 2015.09.03
//------------------------------------------------------------------------------
// include for ROS
#include <ros/ros.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

#include <tms_msg_db/TmsdbGetData.h>
#include <tms_msg_db/TmsdbStamped.h>
#include <tms_msg_db/Tmsdb.h>

#include <tms_msg_ss/tracking_points.h>

#include <tms_msg_rp/rps_voronoi_path_planning.h>
#include <tms_msg_rp/rps_map_full.h>
#include <tms_msg_rp/rps_route.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

// include for std
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <sstream>
#include <iostream>
#include <vector>

// #define rad2deg(x)	((x)*(180.0)/M_PI)
// #define deg2rad(x)	((x)*M_PI/180.0)

namespace tms_rp
{
using namespace std;
using Eigen::Vector3d;
using Eigen::Matrix3d;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix3d Matrix3;

class CollisionMapData
{
public:
  bool object_;
  double dist_from_obj_;
  bool collision_;
  bool voronoi_;
  int thinning_flg_;
  double dist_from_voronoi_;
  bool path_;
  double dist_from_path_;
  double dist_from_goal_;
  bool person_view_;
  double table_height_;
};

class TmsRpPathPlanning
{
private:
  // ROS NodeHandle
  ros::NodeHandle nh;
  ros::NodeHandle nh_priv;
  // ROS Topic Subscriber
  ros::Subscriber dynamic_map_sub_;
  ros::ServiceServer service_voronoi_path_;
  ros::Publisher robot_path_pub_, path_marker_pub, mesh_marker_pub;
  // ROS Parameters:
  bool is_debug;
  const double kSmartPal5CollisionThreshold_;
  const double kSmartPal4CollisionThreshold_;
  const double kKobukiCollisionThreshold_;
  const double kKKPCollisionThreshold_;
  const double kMimamorukunCollisionThreshold_;
  const double kRobotControlWagonDist_;
  const double kSmoothVoronoiPathThreshold_;
  const double kPushWagonPathThreshold_;
  double x_llimit_, x_ulimit_, y_llimit_, y_ulimit_, cell_size_;

public:
  vector< vector< CollisionMapData > > dynamic_map_;
  TmsRpPathPlanning()
    : kSmartPal5CollisionThreshold_(0.4)
    , kSmartPal4CollisionThreshold_(0.4)
    , kKobukiCollisionThreshold_(0.2)
    , kKKPCollisionThreshold_(0.4)
    , kMimamorukunCollisionThreshold_(0.6)
    , kRobotControlWagonDist_(0.4)
    , kSmoothVoronoiPathThreshold_(0.1)
    , kPushWagonPathThreshold_(0.2)
    , nh_priv("~")
    , is_debug(false)
  {
    // Init parameter
    nh_priv.param("is_debug", is_debug, is_debug);
    ROS_ASSERT(initTmsRpPathPlanning());
    // Subscriber for tms_db_data topic
    dynamic_map_sub_ = nh.subscribe("/rps_dynamic_map", 10, &TmsRpPathPlanning::getDynamicMap, this);
    service_voronoi_path_ =
        nh.advertiseService("/rps_voronoi_path_planning", &TmsRpPathPlanning::voronoiPathPlanner, this);
    robot_path_pub_ = nh.advertise< tms_msg_rp::rps_route >("/rps_robot_path", 1);
    path_marker_pub = nh.advertise< visualization_msgs::MarkerArray >("robot_path_marker", 1);
    mesh_marker_pub = nh.advertise< visualization_msgs::MarkerArray >("shadow_path_marker", 1);
  }

  ~TmsRpPathPlanning()
  {
    ROS_ASSERT(shutdownTmsRpPathPlanning());
  }

private:
  bool initTmsRpPathPlanning()
  {
    ROS_INFO("tms_rp_path_planning : Init OK!\n");
    return true;
  }

  bool shutdownTmsRpPathPlanning()
  {
    return true;
  }

  void getDynamicMap(const tms_msg_rp::rps_map_full::ConstPtr& original_map)
  {
    dynamic_map_.clear();

    x_llimit_ = original_map->x_llimit;
    x_ulimit_ = original_map->x_ulimit;
    y_llimit_ = original_map->y_llimit;
    y_ulimit_ = original_map->y_ulimit;
    cell_size_ = original_map->cell_size;

    vector< CollisionMapData > temp_map_line;
    CollisionMapData temp_map_d;

    for (unsigned int x = 0; x < original_map->rps_map_x.size(); x++)
    {
      temp_map_line.clear();
      for (unsigned int y = 0; y < original_map->rps_map_x[x].rps_map_y.size(); y++)
      {
        temp_map_d.object_ = original_map->rps_map_x[x].rps_map_y[y].object;
        temp_map_d.collision_ = original_map->rps_map_x[x].rps_map_y[y].object;
        temp_map_d.voronoi_ = original_map->rps_map_x[x].rps_map_y[y].voronoi;
        temp_map_d.dist_from_obj_ = original_map->rps_map_x[x].rps_map_y[y].dist_from_obj_f;

        temp_map_line.push_back(temp_map_d);
      }
      dynamic_map_.push_back(temp_map_line);
    }
  }

  bool voronoiPathPlanner(tms_msg_rp::rps_voronoi_path_planning::Request& req,
                          tms_msg_rp::rps_voronoi_path_planning::Response& res)
  {
    res.success = 0;
    res.VoronoiPath.clear();
    res.voronoi_path.rps_route.clear();
    ROS_INFO("Start Voronoi Path Plan...");
    //~ clock_t t_start,t_end;
    //~ t_start = clock();

    double collision_threshold = getRobotCollisionThreshold(req.robot_id);

    vector< double > start, goal;
    start.resize(3);
    goal.resize(3);

    start[0] = req.start_pos.x;
    start[1] = req.start_pos.y;
    start[2] = req.start_pos.th;

    goal[0] = req.goal_pos.x;
    goal[1] = req.goal_pos.y;
    goal[2] = req.goal_pos.th;

    ROS_INFO("Init map ...");
    vector< vector< CollisionMapData > > using_map;
    vector< CollisionMapData > temp_map_line;

    while (1)
    {
      if (!dynamic_map_.empty())
      {
        using_map.clear();
        for (int i = 0; i < dynamic_map_.size(); i++)
        {
          temp_map_line.clear();
          copy(dynamic_map_.at(i).begin(), dynamic_map_.at(i).end(), back_inserter(temp_map_line));
          using_map.push_back(temp_map_line);
        }
        break;
      }
    }

    ROS_INFO("Calculate voronoi_path...");

    res.success = setCollisionArea(using_map, collision_threshold, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = setVoronoiLine(using_map, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = calcDistFromVoronoi(using_map, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = connectToVoronoi(using_map, start, res.message);
    if (!res.success)
    {
      ROS_ERROR("Error : Start Point");
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = calcDistFromVoronoi(using_map, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = connectToVoronoi(using_map, goal, res.message);
    if (!res.success)
    {
      ROS_ERROR("Error : Goal Point");
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = calcDistFromVoronoi(using_map, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    res.success = calcDistFromGoal(using_map, goal, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    vector< vector< double > > voronoi_path, smooth_path, comp_path;

    res.success = calcVoronoiPath(using_map, start, goal, voronoi_path, res.message);
    if (!res.success)
    {
      ROS_ERROR((res.message).c_str());
      return false;
    }

    smoothVoronoiPath(using_map, start, goal, voronoi_path, smooth_path, kSmoothVoronoiPathThreshold_);
    //~ compVoronoiPath(smooth_path, comp_path);

    //~ t_end = clock();
    //~ printf("calc_time:%.2f(sec)\n",(double)(t_end-t_start)/CLOCKS_PER_SEC);

    tms_msg_rp::rps_position temp_pos;
    tms_msg_rp::rps_route rps_robot_path;
    rps_robot_path.robot_id = req.robot_id;
    rps_robot_path.rps_route.clear();

    for (unsigned int i = 0; i < smooth_path.size(); i++)
    {
      temp_pos.x = smooth_path[i][0];
      temp_pos.y = smooth_path[i][1];
      temp_pos.th = smooth_path[i][2];

      res.VoronoiPath.push_back(temp_pos);
      res.voronoi_path.rps_route.push_back(temp_pos);

      temp_pos.th = smooth_path[i][2];
      rps_robot_path.rps_route.push_back(temp_pos);
    }

    //
    uint32_t shape = visualization_msgs::Marker::ARROW;
    visualization_msgs::Marker marker;
    visualization_msgs::MarkerArray markers;
    marker.header.frame_id = "world_link";
    marker.header.stamp = ros::Time::now();
    marker.ns = "robot_path";

    marker.type = shape;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 0.03;
    marker.scale.y = 0.08;
    marker.scale.z = 0.1;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 1.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0;
    //

    visualization_msgs::Marker delete_olds;
    delete_olds.action = (uint8_t)3;  // なぜか"visualization_msgs::Marker::DELETEALL"が見つからないので直接定数代入
    markers.markers.push_back(delete_olds);
    for (unsigned int i = 1; i < smooth_path.size() - 1; i++)
    {
      marker.id = i;
      marker.points.resize(2);
      marker.points[0].x = smooth_path[i][0];
      marker.points[0].y = smooth_path[i][1];
      marker.points[0].z = 0.03;
      marker.points[1].x = smooth_path[i + 1][0];
      marker.points[1].y = smooth_path[i + 1][1];
      marker.points[1].z = 0.03;
      markers.markers.push_back(marker);
    }
    //
    //
    visualization_msgs::Marker mesh;
    visualization_msgs::MarkerArray meshs;
    mesh.header.frame_id = "world_link";
    mesh.header.stamp = ros::Time::now();
    mesh.ns = "robot_shadow";
    mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
    mesh.action = visualization_msgs::Marker::ADD;
    mesh.scale.x = mesh.scale.y = mesh.scale.z = 1.0;
    mesh.mesh_resource = "package://smartpal5_description/meshes/blender/smartpal5.dae";
    mesh.color.r = 0.6;
    mesh.color.g = 0.6;
    mesh.color.b = 0.6;
    mesh.color.a = 1.0;

    tf2::Quaternion q;

    meshs.markers.push_back(delete_olds);
    for (unsigned int i = 1; i < smooth_path.size(); i++)
    {
      mesh.id = i;
      mesh.pose.position.x = smooth_path[i][0];
      mesh.pose.position.y = smooth_path[i][1];
      mesh.pose.position.z = 0.03;
      q.setRPY(0, 0, smooth_path[i][2]);
      mesh.pose.orientation.x = q.x();
      mesh.pose.orientation.y = q.y();
      mesh.pose.orientation.z = q.z();
      mesh.pose.orientation.w = q.w();
      meshs.markers.push_back(mesh);
    }

    path_marker_pub.publish(markers);
    mesh_marker_pub.publish(meshs);

    ROS_INFO("...Voronoi Path Plan Success");

    int k = 0;
    while (k < 100)
    {
      robot_path_pub_.publish(rps_robot_path);
      k++;
    }
    ROS_INFO("path publish");

    //~ int a;
    //~ scanf("%d", &a);

    res.message = "Voronoi Path Plan Success";
    res.success = 1;
    return true;
  }

  double getRobotCollisionThreshold(int robot_id)
  {
    double collision_threshold = 0.0;

    switch (robot_id)
    {
      case 2001:  // SmartPal4
        collision_threshold = kSmartPal4CollisionThreshold_;
        break;
      case 2002:  // SmartPal5_1
        collision_threshold = kSmartPal5CollisionThreshold_;
        break;
      case 2003:  // SmartPal5_2
        collision_threshold = kSmartPal5CollisionThreshold_;
        break;
      case 2004:  // turtlebot2
        collision_threshold = kKobukiCollisionThreshold_;
        break;
      case 2005:  // kobuki
        collision_threshold = kKobukiCollisionThreshold_;
        break;
      case 2006:  // KKP
        collision_threshold = kKKPCollisionThreshold_;
        break;
      case 2007:
        collision_threshold = kMimamorukunCollisionThreshold_;
        break;
      default:
        collision_threshold = 0.1;
        break;
    }

    return collision_threshold;
  }

  bool setCollisionArea(vector< vector< CollisionMapData > >& map, double threshold, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    for (unsigned int x = 0; x < map.size(); x++)
    {
      for (unsigned int y = 0; y < map[x].size(); y++)
      {
        if (map[x][y].dist_from_obj_ - threshold < 1.0e-5)
          map[x][y].collision_ = true;
        else
          map[x][y].collision_ = false;
      }
    }

    return true;
  }

  bool setVoronoiLine(vector< vector< CollisionMapData > >& map, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    for (unsigned int x = 0; x < map.size(); x++)
    {
      for (unsigned int y = 0; y < map[x].size(); y++)
      {
        map[x][y].voronoi_ = false;
        if (!map[x][y].collision_)
          map[x][y].thinning_flg_ = 1;
        else
          map[x][y].thinning_flg_ = 0;
      }
    }

    bool flg = true;

    while (flg)
    {
      flg = false;
      for (int k = 0; k < 4; k++)
      {
        for (int x = 1; x < map.size() - 1; x++)
        {
          for (int y = 1; y < map[x].size() - 1; y++)
          {
            int i, j;

            switch (k)
            {
              case 0:
                i = x + 1;
                j = y;
                break;
              case 1:
                i = x;
                j = y - 1;
                break;
              case 2:
                i = x - 1;
                j = y;
                break;
              case 3:
                i = x;
                j = y + 1;
                break;
            }

            if ((map[x][y].thinning_flg_ == 1) && (map[i][j].thinning_flg_ == 0))
            {
              if (((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 1) && (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 1) && (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x][y + 1].thinning_flg_ == 1)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 1) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y].thinning_flg_ == 0) && (map[x - 1][y + 1].thinning_flg_ == 1) &&
                   (map[x][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 1) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x - 1][y].thinning_flg_ == 0)) ||
                  ((map[x][y - 1].thinning_flg_ == 0) && (map[x + 1][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y].thinning_flg_ == 0)) ||
                  ((map[x + 1][y].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 1)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x + 1][y].thinning_flg_ == 1) && (map[x - 1][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y - 1].thinning_flg_ == 0) &&
                   (map[x - 1][y].thinning_flg_ == 1) && (map[x + 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 1) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 1) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 1) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x][y - 1].thinning_flg_ == 0) &&
                   (map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x - 1][y - 1].thinning_flg_ == 0) && (map[x - 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)) ||
                  ((map[x + 1][y - 1].thinning_flg_ == 0) && (map[x + 1][y].thinning_flg_ == 0) &&
                   (map[x - 1][y + 1].thinning_flg_ == 0) && (map[x][y + 1].thinning_flg_ == 0) &&
                   (map[x + 1][y + 1].thinning_flg_ == 0)))
                map[x][y].thinning_flg_ = 1;
              else
              {
                flg = true;
                map[x][y].thinning_flg_ = 3;
              }
            }
          }
        }
        for (int x = 1; x < map.size() - 1; x++)
        {
          for (int y = 1; y < map[x].size() - 1; y++)
          {
            if (map[x][y].thinning_flg_ == 3)
              map[x][y].thinning_flg_ = 0;
          }
        }
      }
    }

    for (int x = 0; x < map.size(); x++)
    {
      for (int y = 0; y < map[x].size(); y++)
      {
        if (map[x][y].thinning_flg_ == 1)
        {
          map[x][y].voronoi_ = 1;
          map[x][y].dist_from_voronoi_ = 0.0;
        }
        else
          map[x][y].voronoi_ = 0;
      }
    }
    return true;
  }

  bool calcDistFromVoronoi(vector< vector< CollisionMapData > >& map, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    for (int x = 0; x < map.size(); x++)
    {
      for (int y = 0; y < map[x].size(); y++)
      {
        if (map[x][y].voronoi_ && (!map[x][y].collision_))
          map[x][y].dist_from_voronoi_ = 0.0;
        else
          map[x][y].dist_from_voronoi_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      }
    }

    for (int x = 1; x < map.size() - 1; x++)
    {
      for (int y = 1; y < map[x].size() - 1; y++)
      {
        if (!map[x][y].collision_)
        {
          map[x][y].dist_from_voronoi_ = min(min(min(map[x - 1][y - 1].dist_from_voronoi_ + sqrt(2.0) * cell_size_,
                                                     map[x][y - 1].dist_from_voronoi_ + 1.0 * cell_size_),
                                                 min(map[x - 1][y + 1].dist_from_voronoi_ + sqrt(2.0) * cell_size_,
                                                     map[x - 1][y].dist_from_voronoi_ + 1.0 * cell_size_)),
                                             map[x][y].dist_from_voronoi_);
        }
      }
    }

    for (int x = map.size() - 2; x > 0; x--)
    {
      for (int y = map[x].size() - 2; y > 0; y--)
      {
        if (!map[x][y].collision_)
        {
          map[x][y].dist_from_voronoi_ = min(min(min(map[x + 1][y].dist_from_voronoi_ + 1.0 * cell_size_,
                                                     map[x + 1][y - 1].dist_from_voronoi_ + sqrt(2.0) * cell_size_),
                                                 min(map[x][y + 1].dist_from_voronoi_ + 1.0 * cell_size_,
                                                     map[x + 1][y + 1].dist_from_voronoi_ + sqrt(2.0) * cell_size_)),
                                             map[x][y].dist_from_voronoi_);
        }
      }
    }

    return true;
  }

  bool connectToVoronoi(vector< vector< CollisionMapData > >& map, vector< double > connect_point, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    if ((connect_point[0] < x_llimit_) || (connect_point[0] > x_ulimit_) || (connect_point[1] < y_llimit_) ||
        (connect_point[1] > y_ulimit_))
    {
      message = "Error : Connect point is out of range";
      cout << message << endl;
      cout << "	x:" << connect_point[0] << "	y:" << connect_point[1] << "	limit_x:" << x_llimit_ << " ~ " << x_ulimit_
           << "	limit_y:" << y_llimit_ << " ~ " << y_ulimit_ << endl;
      return false;
    }

    int target_x, target_y, temp_x, temp_y, dx = 0, dy = 0;
    target_x = (int)round((connect_point[0] - x_llimit_) / cell_size_);
    target_y = (int)round((connect_point[1] - y_llimit_) / cell_size_);

    temp_x = target_x;
    temp_y = target_y;

    double temp_dist = map[temp_x][temp_y].dist_from_voronoi_;

    if (map[temp_x][temp_y].collision_)
    {
      message = "Error : Connect point is collision";
      cout << message << endl;
      cout << "	x:" << temp_x << "	y:" << temp_y << endl;
      return false;
    }

    if (temp_dist >= 0 && temp_dist <= 0.01)
    {
      return true;
    }

    while (temp_dist < 0 || temp_dist > 0.01)
    {
      if (map[temp_x - 1][temp_y].dist_from_voronoi_ < temp_dist)
      {
        dx = -1;
        dy = 0;
        temp_dist = map[temp_x + dx][temp_y].dist_from_voronoi_;
      }

      if (map[temp_x][temp_y - 1].dist_from_voronoi_ < temp_dist)
      {
        dx = 0;
        dy = -1;
        temp_dist = map[temp_x][temp_y + dy].dist_from_voronoi_;
      }

      if (map[temp_x + 1][temp_y].dist_from_voronoi_ < temp_dist)
      {
        dx = 1;
        dy = 0;
        temp_dist = map[temp_x + dx][temp_y].dist_from_voronoi_;
      }

      if (map[temp_x][temp_y + 1].dist_from_voronoi_ < temp_dist)
      {
        dx = 0;
        dy = 1;
        temp_dist = map[temp_x][temp_y + dy].dist_from_voronoi_;
      }

      if (map[temp_x - 1][temp_y - 1].dist_from_voronoi_ < temp_dist)
      {
        dx = -1;
        dy = -1;
        temp_dist = map[temp_x + dx][temp_y + dy].dist_from_voronoi_;
      }

      if (map[temp_x - 1][temp_y + 1].dist_from_voronoi_ < temp_dist)
      {
        dx = -1;
        dy = 1;
        temp_dist = map[temp_x + dx][temp_y + dy].dist_from_voronoi_;
      }

      if (map[temp_x + 1][temp_y - 1].dist_from_voronoi_ < temp_dist)
      {
        dx = 1;
        dy = -1;
        temp_dist = map[temp_x + dx][temp_y + dy].dist_from_voronoi_;
      }

      if (map[temp_x + 1][temp_y + 1].dist_from_voronoi_ < temp_dist)
      {
        dx = 1;
        dy = 1;
        temp_dist = map[temp_x + dx][temp_y + dy].dist_from_voronoi_;
      }

      temp_x += dx;
      temp_y += dy;
    }

    vector< double > vec;
    vec.resize(2);
    vec[0] = temp_x - target_x;
    vec[1] = temp_y - target_y;
    double vec_norm = sqrt((vec[0] * vec[0]) + (vec[1] * vec[1]));
    vec[0] /= vec_norm;
    vec[1] /= vec_norm;

    double temp_norm = 1.0;
    while (temp_norm < vec_norm)
    {
      temp_x = (int)round(target_x + temp_norm * vec[0]);
      temp_y = (int)round(target_y + temp_norm * vec[1]);
      map[temp_x][temp_y].voronoi_ = true;
      temp_norm += 1.0;
    }

    temp_x = (int)round(target_x + vec_norm * vec[0]);
    temp_y = (int)round(target_y + vec_norm * vec[1]);
    map[temp_x][temp_y].voronoi_ = true;

    map[target_x][target_y].voronoi_ = true;

    return true;
  }

  bool calcDistFromGoal(vector< vector< CollisionMapData > >& map, vector< double > goal_point, string& message)
  {
    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
      return false;
    }

    if ((goal_point[0] < x_llimit_) || (goal_point[0] > x_ulimit_) || (goal_point[1] < y_llimit_) ||
        (goal_point[1] > y_ulimit_))
    {
      message = "Error : Goal is out of range";
      cout << message << endl;
      return false;
    }

    int temp_x, temp_y;
    temp_x = (int)round((goal_point[0] - x_llimit_) / cell_size_);
    temp_y = (int)round((goal_point[1] - y_llimit_) / cell_size_);

    for (int x = 0; x < map.size(); x++)
    {
      for (int y = 0; y < map[x].size(); y++)
      {
        map[x][y].dist_from_goal_ = (x_ulimit_ - x_llimit_) * (y_ulimit_ - y_llimit_);
      }
    }

    map[temp_x][temp_y].dist_from_goal_ = 0.0;

    bool change_flg = true;
    double temp = 0.0;

    while (change_flg)
    {
      change_flg = false;

      for (unsigned int i = 1; i < map.size() - 1; i++)
      {
        for (unsigned int j = 1; j < map[i].size() - 1; j++)
        {
          if (map[i][j].voronoi_)
          {
            temp = map[i][j].dist_from_goal_;
            map[i][j].dist_from_goal_ =
                min(min(min(map[i - 1][j - 1].dist_from_goal_ + sqrt(2.0) * cell_size_,
                            map[i - 1][j].dist_from_goal_ + 1.0 * cell_size_),
                        map[i - 1][j + 1].dist_from_goal_ + sqrt(2.0) * cell_size_),
                    min(map[i][j - 1].dist_from_goal_ + 1.0 * cell_size_, map[i][j].dist_from_goal_));
            if (change_flg == false)
            {
              if (map[i][j].dist_from_goal_ != temp)
                change_flg = true;
            }
          }
        }
      }

      for (int i = map.size() - 2; i > 0; i--)
      {
        for (int j = map[i].size() - 2; j > 0; j--)
        {
          if (map[i][j].voronoi_)
          {
            temp = map[i][j].dist_from_goal_;
            map[i][j].dist_from_goal_ =
                min(min(map[i][j].dist_from_goal_, map[i][j + 1].dist_from_goal_ + 1.0 * cell_size_),
                    min(map[i + 1][j - 1].dist_from_goal_ + sqrt(2.0) * cell_size_,
                        min(map[i + 1][j].dist_from_goal_ + 1.0 * cell_size_,
                            map[i + 1][j + 1].dist_from_goal_ + sqrt(2.0) * cell_size_)));
            if (change_flg == false)
            {
              if (map[i][j].dist_from_goal_ != temp)
                change_flg = true;
            }
          }
        }
      }
    }

    return true;
  }

  bool calcVoronoiPath(vector< vector< CollisionMapData > >& map, vector< double > start, vector< double > goal,
                       vector< vector< double > >& out_path, string& message)
  {
    out_path.clear();

    if (map.empty())
    {
      message = "Error : Map is empty";
      cout << message << endl;
    }

    if ((start[0] < x_llimit_) || (start[0] > x_ulimit_) || (start[1] < y_llimit_) || (start[1] > y_ulimit_))
    {
      message = "Error : Start point is out of range";
      cout << message << endl;
      cout << "	x:" << start[0] << "	y:" << start[1] << "	limit_x:" << x_llimit_ << " ~ " << x_ulimit_
           << "	limit_y:" << y_llimit_ << " ~ " << y_ulimit_ << endl;
      return false;
    }

    if ((goal[0] < x_llimit_) || (goal[0] > x_ulimit_) || (goal[1] < y_llimit_) || (goal[1] > y_ulimit_))
    {
      message = "Error : Goal point is out of range";
      cout << "	x:" << goal[0] << "	y:" << goal[1] << "	limit_x:" << x_llimit_ << " ~ " << x_ulimit_
           << "	limit_y:" << y_llimit_ << " ~ " << y_ulimit_ << endl;
      cout << message << endl;
      return false;
    }

    int i_start_x, i_start_y, i_goal_x, i_goal_y, i_temp_x, i_temp_y, dx = 0, dy = 0;
    i_start_x = (int)round((start[0] - x_llimit_) / cell_size_);
    i_start_y = (int)round((start[1] - y_llimit_) / cell_size_);
    i_goal_x = (int)round((goal[0] - x_llimit_) / cell_size_);
    i_goal_y = (int)round((goal[1] - y_llimit_) / cell_size_);

    if (map[i_start_x][i_start_y].collision_)
    {
      message = "Error : Start is collision";
      cout << message << endl;
      cout << "	x:" << i_start_x << "	y:" << i_start_y << endl;
      return false;
    }

    if (map[i_goal_x][i_goal_y].collision_)
    {
      message = "Error : Goal is collision";
      cout << message << endl;
      cout << "	x:" << i_goal_x << "	y:" << i_goal_y << endl;
      return false;
    }

    map[i_start_x][i_start_y].path_ = true;
    map[i_goal_x][i_goal_y].path_ = true;

    //~ out_path.push_back(start);

    i_temp_x = i_start_x;
    i_temp_y = i_start_y;
    double temp_dist = map[i_temp_x][i_temp_y].dist_from_goal_;
    vector< double > temp_pos;
    temp_pos.resize(2);
    if (temp_dist >= 0 && temp_dist <= 0.01)
    {
      return true;
    }

    while (1)
    {
      if (temp_dist > map[i_temp_x - 1][i_temp_y].dist_from_goal_)
      {
        dx = -1;
        dy = 0;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x][i_temp_y - 1].dist_from_goal_)
      {
        dx = 0;
        dy = -1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x + 1][i_temp_y].dist_from_goal_)
      {
        dx = 1;
        dy = 0;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x][i_temp_y + 1].dist_from_goal_)
      {
        dx = 0;
        dy = 1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x - 1][i_temp_y - 1].dist_from_goal_)
      {
        dx = -1;
        dy = -1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x - 1][i_temp_y + 1].dist_from_goal_)
      {
        dx = -1;
        dy = 1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x + 1][i_temp_y - 1].dist_from_goal_)
      {
        dx = 1;
        dy = -1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      if (temp_dist > map[i_temp_x + 1][i_temp_y + 1].dist_from_goal_)
      {
        dx = 1;
        dy = 1;
        temp_dist = map[i_temp_x + dx][i_temp_y + dy].dist_from_goal_;
      }

      i_temp_x += dx;
      i_temp_y += dy;
      map[i_temp_x][i_temp_y].path_ = true;

      if (temp_dist >= 0 && temp_dist <= 0.01)
      {
        break;
      }

      temp_pos[0] = (i_temp_x * cell_size_) + x_llimit_;
      temp_pos[1] = (i_temp_y * cell_size_) + y_llimit_;
      out_path.push_back(temp_pos);
    }

    //~ out_path.push_back(goal);

    //~ for(int i=0;i<out_path.size();i++){
    //~ cout<<out_path[i][0]<<"	"<<out_path[i][1]<<endl;
    //~ }
    //~ cout<<endl;
    return true;
  }

  bool smoothVoronoiPath(vector< vector< CollisionMapData > >& map, vector< double > start, vector< double > goal,
                         vector< vector< double > > in_path, vector< vector< double > >& out_path, double threshold)
  {
    out_path.clear();
    vector< double > temp_pos;
    temp_pos.resize(3);

    if (in_path.size() < 2)
    {
      out_path.push_back(start);
      out_path.push_back(goal);
      return true;
    }

    out_path.push_back(start);
    temp_pos = start;

    int i_temp_x, i_temp_y, i_prev_x, i_prev_y, i_next_x, i_next_y;
    double temp_th = 0.0;
    i_prev_x = (int)round((start[0] - x_llimit_) / cell_size_);
    i_prev_y = (int)round((start[1] - y_llimit_) / cell_size_);

    vector< double > vec;
    vec.resize(2);
    double vec_norm, temp_norm;
    bool smoothing = true;

    for (unsigned int i = 1; i < in_path.size(); i++)
    {
      smoothing = true;
      i_next_x = (int)round((in_path[i][0] - x_llimit_) / cell_size_);
      i_next_y = (int)round((in_path[i][1] - y_llimit_) / cell_size_);

      vec[0] = i_next_x - i_prev_x;
      vec[1] = i_next_y - i_prev_y;
      vec_norm = sqrt((vec[0] * vec[0]) + (vec[1] * vec[1]));
      vec[0] /= vec_norm;
      vec[1] /= vec_norm;

      temp_norm = 1.0;
      while (temp_norm < vec_norm)
      {
        i_temp_x = (int)round(i_prev_x + temp_norm * vec[0]);
        i_temp_y = (int)round(i_prev_y + temp_norm * vec[1]);
        if (map[i_temp_x][i_temp_y].dist_from_voronoi_ > threshold)
        {
          smoothing = false;
          break;
        }
        temp_norm += 1.0;
      }

      if (!smoothing)
      {
        temp_pos[2] = atan2(i_next_y - i_prev_y, i_next_x - i_prev_x);
        out_path.push_back(temp_pos);
        temp_pos[0] = (i_next_x * cell_size_) + x_llimit_;
        temp_pos[1] = (i_next_y * cell_size_) + y_llimit_;
        out_path.push_back(temp_pos);
        i_prev_x = i_next_x;
        i_prev_y = i_next_y;
      }
    }

    temp_pos[2] =
        atan2(goal[1] - ((i_prev_y * cell_size_) + y_llimit_), goal[0] - ((i_prev_x * cell_size_) + x_llimit_));
    out_path.push_back(temp_pos);
    temp_pos[0] = goal[0];
    temp_pos[1] = goal[1];
    out_path.push_back(temp_pos);

    out_path.push_back(goal);
    //~ for(int i=0;i<out_path.size();i++){
    //~ cout<<out_path[i][0]<<"	"<<out_path[i][1]<<"	"<<rad2deg(out_path[i][2])<<endl;
    //~ }

    return true;
  }
};
}  // namespace tms_rp

int main(int argc, char** argv)
{
  // Init ROS node
  ros::init(argc, argv, "tms_rp_path_planning");
  tms_rp::TmsRpPathPlanning pp;
  ros::spin();
  return 0;
}
// EOF


// Copyright (c) 2016, ETH Zurich
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
// may be used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

// T. Sattler, M. Havlena, K. Schindler, M. Pollefeys.
// Large-Scale Location Recognition And The Geometric Burstiness Problem.
// CVPR 2016.
// Author: Torsten Sattler (sattlert@inf.ethz.ch)

#include <algorithm>
#include <limits>
#include <cmath>
#include <iostream>
#include <fstream>
#include <random>
#include <stdint.h>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>

namespace geometric_burstiness_hesaff {
struct HesAffRootSIFT {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // Specifies the parameters of an ellipse such that a point (u,v) on the
  // ellipse satisfies a(u-x)^2 + 2b(u-x)(v-y) + c(v-y)^2 = 1.
  float x;
  float y;
  float a;
  float b;
  float c;

  Eigen::Matrix<uint8_t, 128, 1> descriptor;
};

typedef std::vector<HesAffRootSIFT,
                    Eigen::aligned_allocator<HesAffRootSIFT> > HesAffRootSIFTVec;

// Loads a .sift file generated with the binary from Michal Perdoch (see
// https://github.com/perdoch/hesaff) from a text file. Converts the SIFT
// descriptors to RootSIFT descriptors while loading.
bool LoadHesAffFile(const std::string& filename,
                    HesAffRootSIFTVec* sifts) {
  sifts->clear();

  std::ifstream ifs(filename.c_str(), std::ios::in);
  if (!ifs.is_open()) {
    std::cerr << " ERROR: Cannot read features from " << filename << std::endl;
    return false;
  }

  int desc_dim = 0;
  ifs >> desc_dim;
  if (desc_dim != 128) {
    std::cerr << " ERROR: Expecting 128 descriptor entries instead of "
              << desc_dim << std::endl;
    return false;
  }

  int num_features = 0;
  ifs >> num_features;
  if (num_features < 0) {
    std::cerr << " ERROR: Expecting positive number of features " << std::endl;
    std::cout << filename << std::endl;
    ifs.close();
    return false;
  }
  if (num_features == 0) {
    ifs.close();
    return true;
  }

  sifts->resize(num_features);
  HesAffRootSIFTVec& s_vec = *sifts;

  for (int i = 0; i < num_features; ++i) {
    ifs >> s_vec[i].x >> s_vec[i].y >> s_vec[i].a >> s_vec[i].b >> s_vec[i].c;

    Eigen::Matrix<float, 128, 1> desc;
    for (int j = 0; j < 128; ++j) {
      int d = 0;
      ifs >> d;
      desc[j] = static_cast<float>(d);
    }

    // Conversion to RootSIFT.
    desc.normalize();
    float L1norm = 0.0f;
    for (int j = 0; j < 128; ++j) L1norm += desc[j];

    desc /= L1norm;
    for (int j = 0; j < 128; ++j) desc[j] = std::sqrt(desc[j]);

    // Stores the descriptor.
    for (int j = 0; j < 128; ++j) {
      float d = 512.0f * desc[j] + 0.5f;
      if (d > 255.5f) {
        std::cout << " Large descriptor element: " << d << std::endl;
        d = 255.0f;
      }
      s_vec[i].descriptor[j] = static_cast<uint8_t>(d);
    }
  }

  ifs.close();

  return true;
}

// Writes a set of RootSIFT features to a binary file. The file format is
// similar to the one of HesAff:
// number_of_features (uint32_t)
// for each feature : x y a b c (all float) 128 descriptor entries (uint8_t)
bool SaveBinaryHesAffFile(const std::string& filename,
                          const HesAffRootSIFTVec& sifts) {
  std::ofstream ofs(filename.c_str(), std::ios::out | std::ios::binary);
  if (!ofs.is_open()) {
    std::cerr << " ERROR: Cannot write features to " << filename << std::endl;
    return false;
  }

  uint32_t num_features = static_cast<uint32_t>(sifts.size());
  ofs.write((const char*) &num_features, sizeof(uint32_t));

  for (uint32_t i = 0; i < num_features; ++i) {
    float x = sifts[i].x;
    float y = sifts[i].y;
    float a = sifts[i].a;
    float b = sifts[i].b;
    float c = sifts[i].c;

    ofs.write((const char*) &x, sizeof(float));
    ofs.write((const char*) &y, sizeof(float));
    ofs.write((const char*) &a, sizeof(float));
    ofs.write((const char*) &b, sizeof(float));
    ofs.write((const char*) &c, sizeof(float));

    for (int j = 0; j < 128; ++j) {
      uint8_t d = static_cast<uint8_t>(sifts[i].descriptor[j]);
      ofs.write((const char*) &d, sizeof(uint8_t));
    }
  }
  ofs.close();

  return true;
}

}  // namespace geometric_burstiness_hesaff

int main(int argc, char **argv) {

  if (argc < 3) {
    std::cout << "________________________________________________________________________________________________" << std::endl;
    std::cout << " -                                                                                - " << std::endl;
    std::cout << " -    Loads SIFTs from a text file, converts them to RootSIFT and saves them to a binary file - " << std::endl;
    std::cout << " -                       written 2016 by Torsten Sattler (sattlert@inf.ethz.ch)               - " << std::endl;
    std::cout << " -    copyright ETH Zurich, 2016                                                              - " << std::endl;
    std::cout << " -                                                                                - " << std::endl;
    std::cout << " - usage: hesaff_sift_to_binary_root_sift list_in list_out                                    - " << std::endl;
    std::cout << " - Parameters:                                                                                - " << std::endl;
    std::cout << " -  list_sifts                                                                                - " << std::endl;
    std::cout << " -     A text file containing the filenames of all descriptor files.                          - " << std::endl;
    std::cout << " -                                                                                - " << std::endl;
    std::cout << " -  list_out                                                                                - " << std::endl;
    std::cout << " -     A text file containing the list of filenames of the binary files to be created.        - " << std::endl;
    std::cout << " -                                                                                - " << std::endl;
    std::cout << "________________________________________________________________________________________________" << std::endl;
    return -1;
  }

  std::vector<std::string> input_names;
  {
    std::ifstream ifs(argv[1], std::ios::in);
    if (!ifs.is_open()) {
      std::cerr << "ERROR: Cannot read from " << argv[1] << std::endl;
      return -1;
    }

    while (!ifs.eof()) {
      std::string s = "";
      ifs >> s;
      if (!s.empty()) {
        if (!input_names.empty()) {
          if (input_names.back().compare(s) == 0) continue;
        }
        input_names.push_back(s);
      }
    }
    ifs.close();
  }
  int num_files = static_cast<int>(input_names.size());

  std::vector<std::string> output_names;
  {
    std::ifstream ifs(argv[2], std::ios::in);
    if (!ifs.is_open()) {
      std::cerr << "ERROR: Cannot read from " << argv[2] << std::endl;
      return -1;
    }

    for (int i = 0; i < num_files; ++i) {
      std::string s = "";
      ifs >> s;
      output_names.push_back(s);
    }
    ifs.close();
  }
  std::cout << " Processing " << num_files << " files " << std::endl;

  for (int i = 0; i < num_files; ++i) {
    geometric_burstiness_hesaff::HesAffRootSIFTVec sifts;
    if (geometric_burstiness_hesaff::LoadHesAffFile(input_names[i], &sifts)) {
      geometric_burstiness_hesaff::SaveBinaryHesAffFile(output_names[i], sifts);
    }

    if (i % 100 == 0) std::cout << "  \r " << i << std::flush;
  }
  std::cout << " ... done" << std::endl;

  return 0;
}


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "includes/model_part.h"
#include "python/pointer_vector_set_python_interface.h"
#include "custom_python/add_utilities_to_python.h"
#include "custom_utilities/patch.h"
#include "custom_utilities/nurbs/bsplines_patch_utility.h"
#include "custom_utilities/multipatch_utility.h"
#include "custom_utilities/multipatch_refinement_utility.h"
#include "custom_utilities/bending_strip_utility.h"
#include "custom_utilities/trim/isogeometric_intersection_utility.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

////////////////////////////////////////

template<int TDim>
typename Patch<TDim>::Pointer MultiPatchUtility_CreatePatchPointer(MultiPatchUtility& rDummy, const std::size_t& Id, typename FESpace<TDim>::Pointer pFESpace)
{
    return rDummy.template CreatePatchPointer<TDim>(Id, pFESpace);
}

template<int TDim>
void MultiPatchUtility_MakeInterface(MultiPatchUtility& rDummy, typename Patch<TDim>::Pointer pPatch1, const BoundarySide& side1,
    typename Patch<TDim>::Pointer pPatch2, const BoundarySide& side2)
{
    rDummy.MakeInterface<TDim>(pPatch1, side1, pPatch2, side2);
}

std::size_t MultiPatchUtility_GetLastNodeId(MultiPatchUtility& rDummy, ModelPart& r_model_part)
{
    return rDummy.GetLastNodeId(r_model_part);
}

std::size_t MultiPatchUtility_GetLastElementId(MultiPatchUtility& rDummy, ModelPart& r_model_part)
{
    return rDummy.GetLastElementId(r_model_part);
}

std::size_t MultiPatchUtility_GetLastConditionId(MultiPatchUtility& rDummy, ModelPart& r_model_part)
{
    return rDummy.GetLastConditionId(r_model_part);
}

Condition::Pointer MultiPatchUtility_CreateConditionFromElement(MultiPatchUtility& rDummy,
        const std::string& sample_condition_name, std::size_t lastConditionId,
        Element::Pointer pElement, Properties::Pointer pProperties)
{
    return rDummy.CreateConditionFromElement(sample_condition_name, lastConditionId, pElement, pProperties);
}

void MultiPatchUtility_ListModelPart(MultiPatchUtility& rDummy, ModelPart& r_model_part)
{
    rDummy.ListModelPart(r_model_part);
}

template<typename TVariableType>
std::size_t MultiPatchUtility_GetEquationId(MultiPatchUtility& rDummy, ModelPart::NodeType& rNode, const TVariableType& rVariable)
{
    return rDummy.GetEquationId(rNode, rVariable);
}

std::size_t MultiPatchUtility_BoundaryFlag(MultiPatchUtility& rDummy, const BoundarySide& side)
{
    return BOUNDARY_FLAG(side);
}

std::size_t MultiPatchUtility_BoundaryFlag2D(MultiPatchUtility& rDummy, const BoundarySide2D& side)
{
    return BOUNDARY_FLAG(side);
}

std::size_t MultiPatchUtility_BoundaryFlag3D(MultiPatchUtility& rDummy, const BoundarySide3D& side)
{
    return BOUNDARY_FLAG(side);
}

template<class TClassType>
void MultiPatchUtility_PrintAddress(MultiPatchUtility& rDummy, typename TClassType::Pointer pInstance)
{
    rDummy.PrintAddress<TClassType>(std::cout, pInstance);
}

//////////////////////////////////////////////////

template<int TDim>
void MultiPatchRefinementUtility_InsertKnots(MultiPatchRefinementUtility& rDummy,
       typename Patch<TDim>::Pointer& pPatch,
       boost::python::list ins_knots)
{
    std::vector<std::vector<double> > ins_knots_array(TDim);
    std::size_t dim = 0;

    typedef boost::python::stl_input_iterator<boost::python::list> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& ins_knots_x,
                std::make_pair(iterator_value_type(ins_knots), // begin
                iterator_value_type() ) ) // end
    {
        std::vector<double> knots;

        typedef boost::python::stl_input_iterator<double> iterator_value_type2;
        BOOST_FOREACH(const iterator_value_type2::value_type& knot,
                    std::make_pair(iterator_value_type2(ins_knots_x), // begin
                    iterator_value_type2() ) ) // end
        {
            knots.push_back(knot);
        }

        ins_knots_array[dim++] = knots;
        if (dim == TDim)
            break;
    }

    if (dim != TDim)
        KRATOS_THROW_ERROR(std::logic_error, "insufficient dimension", "")

    rDummy.InsertKnots<TDim>(pPatch, ins_knots_array);
}

template<int TDim>
boost::python::dict MultiPatchRefinementUtility_InsertKnots2(MultiPatchRefinementUtility& rDummy,
       typename Patch<TDim>::Pointer& pPatch,
       boost::python::list ins_knots)
{
    std::vector<std::vector<double> > ins_knots_array(TDim);
    std::size_t dim = 0;

    typedef boost::python::stl_input_iterator<boost::python::list> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& ins_knots_x,
                std::make_pair(iterator_value_type(ins_knots), // begin
                iterator_value_type() ) ) // end
    {
        std::vector<double> knots;

        typedef boost::python::stl_input_iterator<double> iterator_value_type2;
        BOOST_FOREACH(const iterator_value_type2::value_type& knot,
                    std::make_pair(iterator_value_type2(ins_knots_x), // begin
                    iterator_value_type2() ) ) // end
        {
            knots.push_back(knot);
        }

        ins_knots_array[dim++] = knots;
        if (dim == TDim)
            break;
    }

    if (dim != TDim)
        KRATOS_THROW_ERROR(std::logic_error, "insufficient dimension", "")

    std::map<std::size_t, Matrix> trans_mats;
    rDummy.InsertKnots<TDim>(pPatch, ins_knots_array, trans_mats);
    // KRATOS_WATCH(trans_mats.size())

    boost::python::dict res;
    for (std::map<std::size_t, Matrix>::iterator it = trans_mats.begin(); it != trans_mats.end(); ++it)
        res[it->first] = it->second;

    return res;
}

template<int TDim>
void MultiPatchRefinementUtility_DegreeElevate(MultiPatchRefinementUtility& rDummy,
       typename Patch<TDim>::Pointer& pPatch,
       boost::python::list order_increment)
{
    std::vector<std::size_t> order_incr_array(TDim);
    std::size_t dim = 0;

    typedef boost::python::stl_input_iterator<int> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& t,
               std::make_pair(iterator_value_type(order_increment), // begin
               iterator_value_type() ) ) // end
    {
       order_incr_array[dim++] = static_cast<std::size_t>(t);
       if (dim == TDim)
            break;
    }

    if (dim != TDim)
        KRATOS_THROW_ERROR(std::logic_error, "insufficient dimension", "")

    rDummy.DegreeElevate<TDim>(pPatch, order_incr_array);
}

//////////////////////////////////////////////////

template<int TDim>
typename Patch<TDim>::Pointer BSplinesPatchUtility_CreateConnectedPatch(BSplinesPatchUtility& dummy,
        typename Patch<TDim-1>::Pointer pPatch1, typename Patch<TDim-1>::Pointer pPatch2)
{
    return BSplinesPatchUtility::CreateConnectedPatch<TDim>(pPatch1, pPatch2);
}

template<int TDim>
typename Patch<TDim>::Pointer BSplinesPatchUtility_CreateConnectedPatchFromList(BSplinesPatchUtility& dummy,
        boost::python::list patch_list, const int& order)
{
    std::vector<typename Patch<TDim-1>::Pointer> pPatches;

    typedef boost::python::stl_input_iterator<typename Patch<TDim-1>::Pointer> iterator_value_type;
    BOOST_FOREACH(const typename iterator_value_type::value_type& pPatch,
                std::make_pair(iterator_value_type(patch_list), // begin
                iterator_value_type() ) ) // end
    {
        pPatches.push_back(pPatch);
    }
    KRATOS_WATCH(pPatches.size())

    return BSplinesPatchUtility::CreateConnectedPatch<TDim>(pPatches, order);
}

boost::python::list BSplinesPatchUtility_CreatePatchFromGeo(BSplinesPatchUtility& dummy,
        const std::string& filename)
{
    int Dim = BSplinesPatchUtility::GetDimensionOfGeo(filename);
    boost::python::list patches;
    if (Dim == 2)
        patches.append(BSplinesPatchUtility::CreatePatchFromGeo<2>(filename));
    else if (Dim == 3)
        patches.append(BSplinesPatchUtility::CreatePatchFromGeo<3>(filename));
    else
        KRATOS_THROW_ERROR(std::logic_error, "The dimension of the patch is invalid", "")
    return patches;
}

void BSplinesPatchUtility_MakeInterface2D(BSplinesPatchUtility& rDummy,
    typename Patch<2>::Pointer pPatch1, const int& iside1,
    typename Patch<2>::Pointer pPatch2, const int& iside2,
    const BoundaryDirection& direction)
{
    BoundarySide side1 = static_cast<BoundarySide>(iside1);
    BoundarySide side2 = static_cast<BoundarySide>(iside2);
    rDummy.MakeInterface2D(pPatch1, side1, pPatch2, side2, direction);
}

void BSplinesPatchUtility_MakeInterface3D(BSplinesPatchUtility& rDummy,
    typename Patch<3>::Pointer pPatch1, const int& iside1,
    typename Patch<3>::Pointer pPatch2, const int& iside2,
    const bool& uv_or_vu,
    const BoundaryDirection& direction1, const BoundaryDirection& direction2)
{
    BoundarySide side1 = static_cast<BoundarySide>(iside1);
    BoundarySide side2 = static_cast<BoundarySide>(iside2);
    rDummy.MakeInterface3D(pPatch1, side1, pPatch2, side2, uv_or_vu, direction1, direction2);
}

template<int TDim>
void BSplinesPatchUtility_Reverse(BSplinesPatchUtility& rDummy,
    typename Patch<TDim>::Pointer pPatch, const std::size_t& idir)
{
    rDummy.Reverse<TDim>(pPatch, idir);
}

//////////////////////////////////////////////////

template<int TDim>
typename Patch<TDim>::Pointer BendingStripUtility_CreateBendingStripNURBSPatch1(
        BendingStripUtility& rDummy,
        const std::size_t& Id,
        typename Patch<TDim>::Pointer pPatch1, const BoundarySide& side1,
        typename Patch<TDim>::Pointer pPatch2, const BoundarySide& side2,
        const int& Order)
{
    return rDummy.CreateBendingStripNURBSPatch<TDim>(Id, pPatch1, side1, pPatch2, side2, Order);
}

template<int TDim>
typename Patch<TDim>::Pointer BendingStripUtility_CreateBendingStripNURBSPatch2(
        BendingStripUtility& rDummy,
        const std::size_t& Id,
        typename Patch<TDim>::Pointer pPatch1, const BoundarySide& side1,
        typename Patch<TDim>::Pointer pPatch2, const BoundarySide& side2,
        boost::python::list order_list)
{
    std::vector<int> Orders(TDim);

    std::size_t dim = 0;

    typedef boost::python::stl_input_iterator<int> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& t,
                std::make_pair(iterator_value_type(order_list), // begin
                iterator_value_type() ) ) // end
    {
        Orders[dim++] = static_cast<int>(t);
        if (dim == TDim)
            break;
    }

    return rDummy.CreateBendingStripNURBSPatch<TDim>(Id, pPatch1, side1, pPatch2, side2, Orders);
}

//////////////////////////////////////////////////

boost::python::list IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Two_Curves(IsogeometricIntersectionUtility& rDummy,
    const double& starting_point_1,
    const double& starting_point_2,
    Patch<1>::Pointer pPatch1,
    Patch<1>::Pointer pPatch2,
    const int& max_iters,
    const double& TOL,
    const int& option_space)
{
    double intersection_point_1, intersection_point_2;

    int stat = rDummy.ComputeIntersectionByNewtonRaphson(starting_point_1, starting_point_2,
        intersection_point_1, intersection_point_2,
        pPatch1, pPatch2, max_iters, TOL, option_space);

    boost::python::list point;
    point.append(intersection_point_1);
    point.append(intersection_point_2);

    boost::python::list output;
    output.append(stat);
    output.append(point);
    return output;
}

boost::python::list IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Curve_Plane(IsogeometricIntersectionUtility& rDummy,
    const double& starting_point,
    Patch<1>::Pointer pPatch,
    const double& A, const double& B, const double& C, const double& D,
    const int& max_iters,
    const double& TOL)
{
    // std::cout << "invoking " << __FUNCTION__ << std::endl;

    double intersection_point = starting_point;

    int stat = rDummy.ComputeIntersectionByNewtonRaphson(intersection_point, pPatch, A, B, C, D, max_iters, TOL);

    boost::python::list point;
    point.append(intersection_point);

    boost::python::list output;
    output.append(stat);
    output.append(point);
    return output;
}

boost::python::list IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Patch2_Plane(IsogeometricIntersectionUtility& rDummy,
    boost::python::list list_starting_points,
    Patch<2>::Pointer pPatch,
    const double& A, const double& B, const double& C, const double& D,
    const int& max_iters,
    const double& TOL)
{
    // std::cout << "invoking " << __FUNCTION__ << std::endl;

    std::vector<double> starting_points;
    typedef boost::python::stl_input_iterator<double> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& v,
               std::make_pair(iterator_value_type(list_starting_points), // begin
               iterator_value_type() ) ) // end
    {
        starting_points.push_back(v);
    }

    std::vector<std::vector<double> > intersection_points;

    std::vector<int> stat = rDummy.ComputeIntersectionByNewtonRaphson(starting_points, intersection_points,
        pPatch, A, B, C, D, max_iters, TOL);

    boost::python::list list_points;
    for (std::size_t i = 0; i < intersection_points.size(); ++i)
    {
        boost::python::list point;
        point.append(intersection_points[i][0]);
        point.append(intersection_points[i][1]);
        list_points.append(point);
    }

    boost::python::list list_stat;
    for (std::size_t i = 0; i < stat.size(); ++i)
        list_stat.append(stat[i]);

    boost::python::list output;
    output.append(list_stat);
    output.append(list_points);
    return output;
}

boost::python::list IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Patch3_Plane(IsogeometricIntersectionUtility& rDummy,
    boost::python::list list_starting_points,
    Patch<3>::Pointer pPatch,
    const double& A, const double& B, const double& C, const double& D,
    const int& max_iters,
    const double& TOL)
{
    // std::cout << "invoking " << __FUNCTION__ << std::endl;

    std::vector<double> starting_points;
    typedef boost::python::stl_input_iterator<double> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& v,
               std::make_pair(iterator_value_type(list_starting_points), // begin
               iterator_value_type() ) ) // end
    {
        starting_points.push_back(v);
    }

    std::vector<std::vector<double> > intersection_points;

    std::vector<int> stat = rDummy.ComputeIntersectionByNewtonRaphson(starting_points, intersection_points,
        pPatch, A, B, C, D, max_iters, TOL);

    boost::python::list list_points;
    for (std::size_t i = 0; i < intersection_points.size(); ++i)
    {
        boost::python::list point;
        point.append(intersection_points[i][0]);
        point.append(intersection_points[i][1]);
        point.append(intersection_points[i][2]);
        list_points.append(point);
    }

    boost::python::list list_stat;
    for (std::size_t i = 0; i < stat.size(); ++i)
        list_stat.append(stat[i]);

    boost::python::list output;
    output.append(list_stat);
    output.append(list_points);
    return output;
}

boost::python::list IsogeometricIntersectionUtility_ComputeIntersectionByBisection_Patch3_Plane(IsogeometricIntersectionUtility& rDummy,
    Patch<3>::Pointer pPatch,
    const double& A, const double& B, const double& C, const double& D,
    const int& max_iters,
    const double& TOL)
{
    // std::cout << "invoking " << __FUNCTION__ << std::endl;

    std::vector<array_1d<double, 3> > intersection_points;

    std::vector<int> stat = rDummy.ComputeIntersectionByBisection(intersection_points, pPatch, A, B, C, D, max_iters, TOL);

    boost::python::list list_points;
    for (std::size_t i = 0; i < intersection_points.size(); ++i)
        list_points.append(intersection_points[i]);

    boost::python::list list_stat;
    for (std::size_t i = 0; i < stat.size(); ++i)
        list_stat.append(stat[i]);

    boost::python::list output;
    output.append(list_stat);
    output.append(list_points);
    return output;
}

boost::python::list IsogeometricIntersectionUtility_ComputeIntersection_Curve_Surface(IsogeometricIntersectionUtility& rDummy,
    const double& starting_point_1,
    const double& starting_point_2_1,
    const double& starting_point_2_2,
    Patch<1>::Pointer pPatch1,
    Patch<2>::Pointer pPatch2,
    const int& max_iters,
    const double& TOL)
{
    // std::cout << "invoking " << __FUNCTION__ << std::endl;

    double intersection_point_1;
    std::vector<double> starting_point_2(2), intersection_point_2(2);

    starting_point_2[0] = starting_point_2_1;
    starting_point_2[1] = starting_point_2_2;

    int stat = rDummy.ComputeIntersectionByNewtonRaphson(starting_point_1, starting_point_2,
        intersection_point_1, intersection_point_2,
        pPatch1, pPatch2, max_iters, TOL);

    boost::python::list point;
    point.append(intersection_point_2[0]);
    point.append(intersection_point_2[1]);

    boost::python::list output;
    output.append(stat);
    output.append(intersection_point_1);
    output.append(point);
    return output;
}

template<int TDim>
boost::python::list IsogeometricIntersectionUtility_CheckIntersection(IsogeometricIntersectionUtility& rDummy,
    typename Patch<TDim>::Pointer pPatch,
    const double& A, const double& B, const double& C, const double& D)
{
    std::pair<int, std::vector<int> > result = rDummy.CheckIntersection<TDim, 0>(pPatch, A, B, C, D);
    boost::python::list output;
    output.append(result.first);
    boost::python::list tmp;
    for (std::size_t i = 0; i < result.second.size(); ++i)
        tmp.append(result.second[i]);
    output.append(tmp);
    return output;
}

//////////////////////////////////////////////////
//////////////////////////////////////////////////

void IsogeometricApplication_AddFrontendUtilitiesToPython()
{

    class_<MultiPatchUtility, MultiPatchUtility::Pointer, boost::noncopyable>
    ("MultiPatchUtility", init<>())
    .def("CreatePatchPointer", &MultiPatchUtility_CreatePatchPointer<1>)
    .def("CreatePatchPointer", &MultiPatchUtility_CreatePatchPointer<2>)
    .def("CreatePatchPointer", &MultiPatchUtility_CreatePatchPointer<3>)
    .def("MakeInterface", &MultiPatchUtility_MakeInterface<1>)
    .def("MakeInterface", &MultiPatchUtility_MakeInterface<2>)
    .def("MakeInterface", &MultiPatchUtility_MakeInterface<3>)
    .def("GetLastNodeId", &MultiPatchUtility_GetLastNodeId)
    .def("GetLastElementId", &MultiPatchUtility_GetLastElementId)
    .def("GetLastConditionId", &MultiPatchUtility_GetLastConditionId)
    .def("CreateConditionFromElement", &MultiPatchUtility_CreateConditionFromElement)
    .def("ListModelPart", &MultiPatchUtility_ListModelPart)
    .def("GetEquationId", &MultiPatchUtility_GetEquationId<Variable<double> >)
    .def("GetEquationId", &MultiPatchUtility_GetEquationId<Kratos::VariableComponent<Kratos::VectorComponentAdaptor<Kratos::array_1d<double, 3> > > >)
    .def("BoundaryFlag", &MultiPatchUtility_BoundaryFlag)
    .def("BoundaryFlag", &MultiPatchUtility_BoundaryFlag2D)
    .def("BoundaryFlag", &MultiPatchUtility_BoundaryFlag3D)
    .def("PrintAddress", &MultiPatchUtility_PrintAddress<Patch<1> >)
    .def("PrintAddress", &MultiPatchUtility_PrintAddress<Patch<2> >)
    .def("PrintAddress", &MultiPatchUtility_PrintAddress<Patch<3> >)
    ;

    class_<MultiPatchRefinementUtility, MultiPatchRefinementUtility::Pointer, boost::noncopyable>
    ("MultiPatchRefinementUtility", init<>())
    .def("InsertKnots", MultiPatchRefinementUtility_InsertKnots<1>)
    .def("InsertKnots", MultiPatchRefinementUtility_InsertKnots<2>)
    .def("InsertKnots", MultiPatchRefinementUtility_InsertKnots<3>)
    .def("InsertKnots2", MultiPatchRefinementUtility_InsertKnots2<1>) // deprecated
    .def("InsertKnots2", MultiPatchRefinementUtility_InsertKnots2<2>) // deprecated
    .def("InsertKnots2", MultiPatchRefinementUtility_InsertKnots2<3>) // deprecated
    .def("InsertKnotsGetTrans", MultiPatchRefinementUtility_InsertKnots2<1>)
    .def("InsertKnotsGetTrans", MultiPatchRefinementUtility_InsertKnots2<2>)
    .def("InsertKnotsGetTrans", MultiPatchRefinementUtility_InsertKnots2<3>)
    .def("DegreeElevate", MultiPatchRefinementUtility_DegreeElevate<1>)
    .def("DegreeElevate", MultiPatchRefinementUtility_DegreeElevate<2>)
    .def("DegreeElevate", MultiPatchRefinementUtility_DegreeElevate<3>)
    ;

    class_<BSplinesPatchUtility, BSplinesPatchUtility::Pointer, boost::noncopyable>
    ("BSplinesPatchUtility", init<>())
    .def("CreateConnectedPatch", &BSplinesPatchUtility_CreateConnectedPatch<2>)
    .def("CreateConnectedPatch", &BSplinesPatchUtility_CreateConnectedPatch<3>)
    .def("CreateConnectedPatchFromList2D", &BSplinesPatchUtility_CreateConnectedPatchFromList<2>)
    .def("CreateConnectedPatchFromList3D", &BSplinesPatchUtility_CreateConnectedPatchFromList<3>)
    .def("CreatePatchFromGeo", &BSplinesPatchUtility_CreatePatchFromGeo)
    .def("MakeInterface", &BSplinesPatchUtility_MakeInterface2D)
    .def("MakeInterface", &BSplinesPatchUtility_MakeInterface3D)
    .def("Reverse", &BSplinesPatchUtility_Reverse<1>)
    .def("Reverse", &BSplinesPatchUtility_Reverse<2>)
    .def("Reverse", &BSplinesPatchUtility_Reverse<3>)
    ;

    class_<BendingStripUtility, BendingStripUtility::Pointer, boost::noncopyable>
    ("BendingStripUtility", init<>())
    .def("CreateBendingStripNURBSPatch", &BendingStripUtility_CreateBendingStripNURBSPatch1<2>)
    .def("CreateBendingStripNURBSPatch", &BendingStripUtility_CreateBendingStripNURBSPatch1<3>)
    .def("CreateBendingStripNURBSPatch", &BendingStripUtility_CreateBendingStripNURBSPatch2<2>)
    .def("CreateBendingStripNURBSPatch", &BendingStripUtility_CreateBendingStripNURBSPatch2<3>)
    ;

    class_<IsogeometricIntersectionUtility, IsogeometricIntersectionUtility::Pointer, boost::noncopyable>
    ("IsogeometricIntersectionUtility", init<>())
    .def("ComputeIntersectionByNewtonRaphson", &IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Two_Curves)
    .def("ComputeIntersectionByNewtonRaphson", &IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Curve_Plane)
    .def("ComputeIntersectionByNewtonRaphson", &IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Patch2_Plane)
    .def("ComputeIntersectionByNewtonRaphson", &IsogeometricIntersectionUtility_ComputeIntersectionByNewtonRaphson_Patch3_Plane)
    .def("ComputeIntersectionByBisection", &IsogeometricIntersectionUtility_ComputeIntersectionByBisection_Patch3_Plane)
    .def("CheckIntersection", &IsogeometricIntersectionUtility_CheckIntersection<1>)
    .def("CheckIntersection", &IsogeometricIntersectionUtility_CheckIntersection<2>)
    .def("CheckIntersection", &IsogeometricIntersectionUtility_CheckIntersection<3>)
    ;

}

}  // namespace Python.

} // Namespace Kratos


/*!
 * @file
 * demo for data-flow.
 *
 * Shows iterative data-flow pipeline, a diamond like pipeline with splitter
 * followed by a joiner and other expressions such as addFlow, flow<>()...
 * ![figures](../doc/dataflow.png)
 *
 * Results are dumped on stdout.
 * */
#include <iostream>
#include <stdexcept>
#include <tuple>
#include <vector>

#include <boost/mpi.hpp>

#include <ezl.hpp>
#include <ezl/algorithms/io.hpp>
#include <ezl/algorithms/filters.hpp>

// returns a map-flow that can be placed in a pipeline later
auto sqr() {
  return ezl::flow<char, int>()
      .map<2>([](int i) { return i * i; }).colsTransform()
      .build();
}

void demoFlow() {
  using std::vector;
  using std::tuple;
  using std::make_tuple;

  vector<tuple<char, vector<int>>> buf;
  buf.emplace_back(make_tuple('a', vector<int>{2}));
  buf.emplace_back(make_tuple('b', vector<int>{3, 4, 5}));

  auto pivot = sqr();

  ezl::flow(pivot).run(); // doesn't do anything as there isn't a rise yet

  // a circular pipeline, also showing a pipeline run inside a map.
  auto ld = ezl::rise(ezl::fromMem(buf).split())
    .map<2>([](const vector<int>& v) {
      return ezl::rise(ezl::fromMem(v)).runResult();
    }).colsTransform()
    .addFlow(pivot)  // adds the flow and continues adding to it
      .filter<2>(ezl::gt(100)).dump()
      .oneUp()  // moves to adding to pivot again
    .filter<2>(ezl::lt(100))
    .addFlow(pivot)
    .run();

  // when recieving result as ref and returning a ref don't forget to put
  // auto& as explicit return type of your lambda (I just saw the compile
  // error for that)
  auto joiner = ezl::flow<int, double>().reduce<1>(
      [](int key, double val, tuple<vector<double>> &ret) -> auto& {
      std::get<0>(ret).emplace_back(val);
        return ret;
      }, tuple<vector<double>>{}).ordered() // w/o ordered output rets reversed
      .build();

  // a pipeline with a splitter followed by a joiner
  auto source = ezl::fromMem({4, 2, 1, 3, 5}).split();
  auto flow1 = ezl::rise(source)
                  .branchFlow(  // adds flow as a branch
                    ezl::flow<int>()
                      .map([](int x) { return double(x)/2.; })
                        .branchFlow(joiner)
                      .build()
                  )
                .map([](int x) { return double(x*2); })
                .addFlow(joiner)
                .filter([](int, vector<double> halfnDouble) {
                   return true;
                 }).dump("", "number, (half, double)")
                .run();

  // running again
  source = source.buffer({6 ,9 ,8 ,7});

  ezl::flow(flow1).run();
}

int main(int argc, char *argv[]) {
  boost::mpi::environment env(argc, argv, false);
  try {
    demoFlow();
  } catch (const std::exception& ex) {
    std::cerr<<"error: "<<ex.what()<<'\n';
    env.abort(1);
  } catch (...) {
    std::cerr<<"unknown exception\n";
    env.abort(2);
  }
  return 0;
}


#ifndef HYPSYS1D_CFL_CONDITION_HPP
#define HYPSYS1D_CFL_CONDITION_HPP

#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>

#include <ancse/includes.hpp>
#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/dg_handler.hpp>

/// Interface for computing CFL restricted timesteps.
class CFLCondition {
  public:
    virtual ~CFLCondition() = default;

    /// Compute the largest time-step satisfying the CFL condition.
    virtual double operator()(const Eigen::MatrixXd &u) const = 0;
};

/// make CFL condition for FVM
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   double cfl_number);

/// make CFL condition for DG
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   const DGHandler &dg_handler,
                   double cfl_number);

#endif // HYPSYS1D_CFL_CONDITION_HPP


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    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.

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <iostream>
#include "Utils/random_utils.hpp"
#include <vector>
#include "Utils/json_utils.hpp"
#include "abstract_hilbert.hpp"

#ifndef NETKET_CUSTOM_HILBERT_HPP
#define NETKET_CUSTOM_HILBERT_HPP

namespace netket {

/**
  User-Define Hilbert space
*/

class CustomHilbert : public AbstractHilbert {
  std::vector<double> local_;

  int nstates_;

  int size_;

 public:
  explicit CustomHilbert(const json &pars) {
    if (FieldExists(pars["Hilbert"], "QuantumNumbers")) {
      if (!pars["Hilbert"]["QuantumNumbers"].is_array()) {
        throw InvalidInputError("QuantumNumbers is not an array");
      }
      local_ = pars["Hilbert"]["QuantumNumbers"].get<std::vector<double>>();
    } else {
      throw InvalidInputError("QuantumNumbers are not defined");
    }

    if (FieldExists(pars["Hilbert"], "Size")) {
      size_ = pars["Hilbert"]["Size"];
      if (size_ <= 0) {
        throw InvalidInputError("Hilbert Size parameter must be positive");
      }
    } else {
      throw InvalidInputError("Hilbert space extent is not defined");
    }

    nstates_ = local_.size();
  }

  bool IsDiscrete() const override { return true; }

  int LocalSize() const override { return nstates_; }

  int Size() const override { return size_; }

  std::vector<double> LocalStates() const override { return local_; }

  void RandomVals(Eigen::VectorXd &state,
                  netket::default_random_engine &rgen) const override {
    std::uniform_int_distribution<int> distribution(0, nstates_ - 1);

    assert(state.size() == size_);

    // unconstrained random
    for (int i = 0; i < state.size(); i++) {
      state(i) = local_[distribution(rgen)];
    }
  }

  void UpdateConf(Eigen::VectorXd &v, const std::vector<int> &tochange,
                  const std::vector<double> &newconf) const override {
    assert(v.size() == size_);

    int i = 0;
    for (auto sf : tochange) {
      v(sf) = newconf[i];
      i++;
    }
  }
};

}  // namespace netket
#endif


#include <iostream>
#include <fstream>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <sys/time.h>

#include <vector>
#include <string>
#include <algorithm>
#include <numeric>
#include <queue>

#include <math.h>
#include <matrix.h>
#include <mex.h>

boost::random::mt19937 gen;

using namespace std;

int collisions (int &dimx_xp, deque<int> &xist_gene, deque<int> &tsix_gene,
        int &sum_xp, int &sum_tp, boost::random::uniform_int_distribution<> &dist)
{
    int q;
    for (q=0; q<dimx_xp; q++)
            {
            if ((xist_gene[q] + tsix_gene[q])>1) {
                if (dist(gen)==0) {
                    xist_gene[q]=0;
                    sum_xp--;
                } else {
                    tsix_gene[q]=0;
                    sum_tp--;
                }
            }
        }
}

int elongation (int &dimx_xp, deque<int> &xist_gene, deque<int> &tsix_gene, int &sum_xp, int &sum_tp,
        bool &xpol_ini, bool &tpol_ini, int &xist_rna, int &tsix_rna,
        vector<int> &all_xist, int &temp_sum_xr, int &temp_sum_tr, int &temp_sum_xp, int &temp_sum_tp,
        boost::random::uniform_int_distribution<> &dist, int &step, double &p8)
{
    xist_gene.push_front(xpol_ini);
    sum_xp = sum_xp - xist_gene.back() + xist_gene.front();
    xist_rna = xist_rna + (int)xist_gene.back();
    xist_gene.pop_back();
    if (p8>0) {
        collisions (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp, dist);
    }
    tsix_gene.push_back(tpol_ini);
    sum_tp = sum_tp - tsix_gene.front() + tsix_gene.back();
    tsix_rna = tsix_rna + (int)tsix_gene.front();
    tsix_gene.pop_front();
    if (p8>0) {
        collisions (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp, dist);
    }
    all_xist[step] = xist_rna;

    temp_sum_xr = temp_sum_xr + xist_rna;
    temp_sum_tr = temp_sum_tr + tsix_rna;
    temp_sum_xp = temp_sum_xp + sum_xp;
    temp_sum_tp = temp_sum_tp + sum_tp;
}

void mexFunction(int nhls, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

    //initiate random number generator
    struct timeval t1;
    gettimeofday(&t1, NULL);
    gen.seed(static_cast<unsigned int>(t1.tv_usec * t1.tv_sec));

	//declare variables // pointers that point at input and output variables
	mxArray *xist_pol, *tsix_pol, *xist_pol2, *tsix_pol2, *par, *const_par;
    mxArray *time_out, *xist_pol_out, *tsix_pol_out, *xist_rna_out, *tsix_rna_out,
            *xist_pol_out2, *tsix_pol_out2, *xist_rna_out2, *tsix_rna_out2, *test, *stable_switch_on_out;
	const mwSize *dims;
	double *xp, *tp, *xr, *tr, *xpr, *xp2, *tp2, *xr2, *tr2, *xpr2, *p, *cp,
            t, sil_threshold, t_diff, t_before;
    double *xpo, *tpo, *xro, *tro, *xpro, *xpo2, *tpo2, *xro2, *tro2, *xpro2,
            *time, v = 1.0/1440.0, out_step, *to, *stable_sw_on,
            k_adv_sil, k2_red_X1, k2_red_X2, k1_ind_X1, k1_ind_X2, tp_dox;
	int dimx_xp, dimx_tp, xist_rna, tsix_rna, xist_prom, xist_rna2, tsix_rna2, xist_prom2;
	int n_steps,n_steps_before, n_steps_dox, n_out_steps, n_small_steps, sec_chr, step, sel_rx;

	//associate inputs: first chromosome
    par = mxDuplicateArray(prhs[0]);
    const_par = mxDuplicateArray(prhs[1]);
	xist_pol = mxDuplicateArray(prhs[2]);
    tsix_pol = mxDuplicateArray(prhs[3]);
    xist_rna = mxGetScalar(prhs[4]);
    tsix_rna = mxGetScalar(prhs[5]);
    xist_prom = mxGetScalar(prhs[6]);

	//associate pointers
    xp = mxGetPr(xist_pol);
    tp = mxGetPr(tsix_pol);
    p = mxGetPr(par);
    cp = mxGetPr(const_par);
    t = cp[0];
    t_before =  cp[1];
    t_diff =  cp[2];
    sil_threshold = cp[3];
    out_step = cp[4];
    sec_chr = cp[5];
    k_adv_sil = cp[6];
    k2_red_X1 = cp[7];
    k2_red_X2 = cp[8];
    k1_ind_X1 = cp[9];
    k1_ind_X2 = cp[10];
    tp_dox = cp[11];

    //associate inputs: second chromosome
   if (sec_chr==1) {
        xist_pol2 = mxDuplicateArray(prhs[7]);
        tsix_pol2 = mxDuplicateArray(prhs[8]);
        xist_rna2 = mxGetScalar(prhs[9]);
        tsix_rna2 = mxGetScalar(prhs[10]);
        xist_prom2 = mxGetScalar(prhs[11]);

        xp2 = mxGetPr(xist_pol2);
        tp2 = mxGetPr(tsix_pol2);
   }

    n_out_steps = 1+ceil((t_diff+t_before)/out_step);
    n_small_steps = round(out_step/v);
    n_steps = (n_out_steps-1)*n_small_steps;
    n_steps_before = (ceil(t_before/out_step))*n_small_steps;
    n_steps_dox = (ceil(tp_dox/out_step))*n_small_steps;

	//figure out dimensions and associate output
	dims = mxGetDimensions(prhs[2]);
	dimx_xp = (int)dims[0];
    dims = mxGetDimensions(prhs[3]);
	dimx_tp = (int)dims[0];

    //output
    time_out = plhs[0] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_pol_out = plhs[1] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_pol_out = plhs[2] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_rna_out = plhs[3] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_rna_out = plhs[4] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xpo = mxGetPr(xist_pol_out);
    tpo= mxGetPr(tsix_pol_out);
    xro = mxGetPr(xist_rna_out);
    tro = mxGetPr(tsix_rna_out);
    time =   mxGetPr(time_out);

    xist_pol_out2 = plhs[5] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_pol_out2 = plhs[6] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    xist_rna_out2 = plhs[7] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    tsix_rna_out2 = plhs[8] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    test = plhs[9] = mxCreateDoubleMatrix(n_out_steps, 1, mxREAL);
    stable_switch_on_out = plhs[10] = mxCreateDoubleMatrix(1, 1, mxREAL);

    xpo2 = mxGetPr(xist_pol_out2);
    tpo2= mxGetPr(tsix_pol_out2);
    xro2 = mxGetPr(xist_rna_out2);
    tro2 = mxGetPr(tsix_rna_out2);
    to = mxGetPr(test);
    stable_sw_on = mxGetPr(stable_switch_on_out);
    // Initialize
    stable_sw_on[0] = t_diff+t_before;

    //write pol mxarray in deque and sum up polymerases
    deque<int> xist_gene, tsix_gene, xist_gene2, tsix_gene2;
    int q, sum_xp=0, sum_tp=0, sum_xp2 = 0, sum_tp2 = 0;

    for (q=0; q<dimx_xp; q++)
        {
            xist_gene.push_back (xp[q]);
            sum_xp = sum_xp + xp[q];
             if (sec_chr==1) {
                xist_gene2.push_back (xp2[q]);
                sum_xp2 = sum_xp2 + xp2[q];
             }
        }
    for (q=0; q<dimx_tp; q++)
        {
            tsix_gene.push_back (tp[q]);
            sum_tp = sum_tp + tp[q];
            if (sec_chr==1) {
                tsix_gene2.push_back (tp2[q]);
                 sum_tp2 = sum_tp2 + tp2[q];
            }
        }

    bool xpol_ini=0, tpol_ini=0, xpol_ini2=0, tpol_ini2=0;
    boost::random::uniform_int_distribution<> dist(0, 1);
    boost::random::uniform_real_distribution<> dist_real(0.0, 1.0);
    double t_next, r, sum_rx;
    vector<double> rx(16), cum_rx(16);
    vector<int> all_xist(n_steps), all_xist2(n_steps);

    int sil_xa = 0, sil_xa2 = 0, xa_sil_steps = round(p[11]/v),
            sil_tsix = 0, sil_tsix2 = 0,
	    sil_state_xa = 0, sil_state_xa2 = 0, sil_state_tsix = 0, sil_state_tsix2 = 0, stst = 0;
    double xa = 1, xa2 = 0;
    if (sec_chr==1) {xa2 = 1;};
    double temp_sum_xa = xa + xa2;
    int m = 0, b = 0, temp_sum_xr = 0, temp_sum_tr = 0, temp_sum_xp = 0, temp_sum_tp = 0,
            temp_sum_xr2 = 0, temp_sum_tr2 = 0, temp_sum_xp2 = 0, temp_sum_tp2 = 0;
    all_xist[0] = xist_rna;
    xro[b] = xist_rna;
    tro[b] = tsix_rna;
    xpo[b] = sum_xp;
    tpo[b] = sum_tp;
    to[b] = temp_sum_xa;
    if (sec_chr==1) {
        all_xist2[0] = xist_rna2;
        xro2[b] = xist_rna2;
        tro2[b] = tsix_rna2;
        xpo2[b] = sum_xp2;
        tpo2[b] = sum_tp2;
    }
    //while (act_time<t_max)
    for (step=0; step<n_steps; step++)
        {
			// Identify first timepoint at which stable state with 1 Xa and 1 Xi is reached
			if (xist_rna>=sil_threshold & xa ==0 & sil_tsix ==1 & xist_rna2<sil_threshold & xa2==1 & sil_tsix2==0 & stst==0) {stable_sw_on[0] = step*v; stst=1;}
			else { if (xist_rna2>=sil_threshold & xa2 ==0 & sil_tsix2 ==1 & xist_rna<sil_threshold & xa==1 & sil_tsix==0 & stst==0) {stable_sw_on[0] = step*v; stst=1;}
			}
			//Silencing
			//If Tsix silencing has reached final state, Tsix is silenced //p[12]=parameter that determines # of intermediate states until silencing of tsix occurs
			if (sil_state_tsix>=p[12]&xist_rna>=sil_threshold) {sil_tsix=1;}
			//If silencing has not reached the final state but Xist rna level is below threshold system falls back into initial tsix silencing state
			else {
			if (xist_rna<sil_threshold&sil_state_tsix<p[12]) {sil_state_tsix=0; sil_tsix=0;}
			}
			//If XA silencing has reached final state, XA is silenced //p[11]=parameter that determines # of intermediate states until silencing of xa occurs
			if (sil_state_xa>=p[11]&xist_rna>=sil_threshold) {xa=0;}
			//If silencing has not reached the final state but Xist rna level is below threshold system falls back into initial xa silencing state
			else {
			if (xist_rna<sil_threshold&sil_state_xa<p[11]) {sil_state_xa=0; xa=1;}
			}
			if (p[6]==0&tsix_gene.front()==1)
		        	{
		            	xist_prom = 2;
				}
		        if (sec_chr==1) {
		     	if (sil_state_tsix2>=p[12]&xist_rna2>=sil_threshold) {sil_tsix2=1;}
			else {
			if (xist_rna2<sil_threshold&sil_state_tsix2<p[12]) {sil_state_tsix2=0; sil_tsix2=0;}
			}
			if (sil_state_xa2>=p[11]&xist_rna2>=sil_threshold) {xa2=0;}
			else {
			if (xist_rna2<sil_threshold&sil_state_xa2<p[11]) {sil_state_xa2=0; xa2=1;}
			}
			if (p[6]==0&tsix_gene2.front()==1)
				{
		                	xist_prom2 = 2;
				}
		        }

			//Gillespie reactions
	        t_next = 0;

	        xpol_ini=0, tpol_ini=0, xpol_ini2=0, tpol_ini2=0;
	        while (t_next<v) {
				//Xist initiation
	            //before induction of diff k1 10 times reduced
	            if (step<n_steps_before & step<n_steps_dox) {rx[0] = p[0]*0*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
	            else {
					if (step<n_steps_dox) {rx[0] = p[0]*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
					// if Xist is induced with Dox (k1_ind_X1>0) the Xist initiation rate becomes independent of the XA concentration
					else {
						if (k1_ind_X1==0) {rx[0] = (p[0]*(step>n_steps_before))*(xa+xa2)*(xist_prom==1)*(tsix_gene.front()==0);}
						// If X1 is induced with dox, the Xist initiation rate becomes independent of xa+xa2
						else {rx[0] = (p[0]*(step>n_steps_before)+p[0]*k1_ind_X1)*(xist_prom==1)*(tsix_gene.front()==0);}
					}
				}
	            rx[1] = (p[1]-k2_red_X1*p[1])*(sil_tsix<1); //Tsix initiation
	            rx[2] = p[7]*(xist_prom==2)*(tsix_gene.front()==0); // turning on the Xist promoter
	            rx[3] = p[3]*xist_rna; //Xist degradation
	            rx[4] = p[4]*tsix_rna; //Tsix degradation

	            //second chr
	            //Xist initiation
	            if (step<n_steps_before & step<n_steps_dox) {rx[5] = (sec_chr==1)*p[0]*0*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
	            else {
					if (step<n_steps_dox) {rx[5] = (sec_chr==1)*p[0]*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
					else {
						if (k1_ind_X2==0) {rx[5] = (sec_chr==1)*(p[0]*(step>n_steps_before))*(xa+xa2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
						// If X2 is induced with dox, the Xist initiation rate becomes independent of xa+xa2
						else {rx[5] = (sec_chr==1)*(p[0]*(step>n_steps_before)+p[0]*k1_ind_X2)*(xist_prom2==1)*(tsix_gene2.front()==0);}
					}
				}
	            rx[6] = (sec_chr==1)*(p[1]-k2_red_X2*p[1])*(sil_tsix2<1);   //Tsix initiation
	            rx[7] = (sec_chr==1)*(p[7]*(xist_prom2==2))*(tsix_gene2.front()==0); // turning on the Xist promoter
	            rx[8] = (sec_chr==1)*p[3]*xist_rna2; //Xist degradation
	            rx[9] = (sec_chr==1)*p[4]*tsix_rna2; //Tsix degradation

		    //transition between silencing states as Gillespie reaction
		    ////chromosome 1: now k_adv_sil= rate for transition between silencing states
		    rx[10] = k_adv_sil*(xist_rna>=sil_threshold)*(sil_state_xa<p[11]|sil_state_tsix<p[12]); //XA and Tsix silencing advance one state
		    ////if system is in last silencing state of the xa but xist rna fall below the threshold the silencing state can go back to the initial state with rate k[13]
		    rx[11] = p[13]*(xist_rna<sil_threshold)*(sil_state_xa>=p[11]);
		    //if system is in last silencing state of tsix but xist rna fall below the threshold the silencing state can go back to the initial state with rate k[14]
		    rx[12] = p[14]*(xist_rna<sil_threshold)*(sil_state_tsix>=p[12]);
		    ////second chromosome
		    rx[13] = (sec_chr==1)*k_adv_sil*(xist_rna2>=sil_threshold)*(sil_state_xa2<p[11]|sil_state_tsix2<p[12]);
		    rx[14] = (sec_chr==1)*p[13]*(xist_rna2<sil_threshold)*(sil_state_xa2>=p[11]);
		    rx[15] = (sec_chr==1)*p[14]*(xist_rna2<sil_threshold)*(sil_state_tsix2>=p[12]);

            sum_rx = 0, sel_rx = 100;
            for (q=0; q<rx.size(); q++) {
                sum_rx = sum_rx + rx[q];
                cum_rx[q] = sum_rx;
            }

            if (sum_rx>0) {
                t_next = t_next + (-log(dist_real(gen)))/(sum_rx);
            } else {t_next=1;}

            if (t_next<v){
                // decide which reaction occurs
                r = dist_real(gen);
                for (q=0; q<rx.size(); q++) {
                    if (r<(cum_rx[q]/sum_rx)){
                        sel_rx = q;
                        break;
                    }
                }
                //execute reaction

                switch (sel_rx){
                    case 0:
                        xpol_ini = 1; break;
                    case 1:
                        tpol_ini = 1; break;
                    case 2:
                        xist_prom = 1; break;
                    case 3:
                        xist_rna--;
                         break;
                    case 4:
                        tsix_rna--; break;

                    //second chr

                   case 5:
                        xpol_ini2 = 1; break;
                    case 6:
                        tpol_ini2 = 1; break;
                    case 7:
                        xist_prom2 = 1; break;
                    case 8:
                        xist_rna2--;
                        break;
                    case 9:
                        tsix_rna2--; break;
					case 10:
						if (sil_state_xa<p[11]) {sil_state_xa++;}
						if (sil_state_tsix<p[12]) {sil_state_tsix++;}
						break;
					case 11:
						sil_state_xa=0; xa=1; break;
					case 12:
						sil_state_tsix=0; sil_tsix=0; break;
					case 13:
						if (sil_state_xa2<p[11]) {sil_state_xa2++;}
						if (sil_state_tsix2<p[12]) {sil_state_tsix2++;}
						break;
					case 14:
						sil_state_xa2=0; xa2=1; break;
					case 15:
						sil_state_tsix2=0; sil_tsix2=0; break;
					default:
                        break;
                }
            }
        }
        temp_sum_xa = temp_sum_xa + xa + xa2;
        //elongation and collisions
        elongation (dimx_xp, xist_gene, tsix_gene, sum_xp, sum_tp,
            xpol_ini, tpol_ini, xist_rna, tsix_rna,
            all_xist, temp_sum_xr, temp_sum_tr, temp_sum_xp, temp_sum_tp, dist, step, p[8]);

        //second chromosome
        if (sec_chr==1) {
            elongation (dimx_xp, xist_gene2, tsix_gene2, sum_xp2, sum_tp2,
            xpol_ini2, tpol_ini2, xist_rna2, tsix_rna2,
            all_xist2, temp_sum_xr2, temp_sum_tr2, temp_sum_xp2, temp_sum_tp2, dist, step, p[8]);

        }

        //write out smoothed variable
        m++;
        if (m==n_small_steps) {
            b++;
            xro[b] = temp_sum_xr/n_small_steps;
            tro[b] = temp_sum_tr/n_small_steps;
            xpo[b] = temp_sum_xp/n_small_steps;
            tpo[b] = temp_sum_tp/n_small_steps;
            to[b] = temp_sum_xa/n_small_steps;
            time[b] = step*v;
            temp_sum_xr = 0;
            temp_sum_tr = 0;
            temp_sum_xp = 0;
            temp_sum_tp = 0;
            temp_sum_xa = 0;
            if (sec_chr==1) {
                xro2[b] = temp_sum_xr2/n_small_steps;
                tro2[b] = temp_sum_tr2/n_small_steps;
                xpo2[b] = temp_sum_xp2/n_small_steps;
                tpo2[b] = temp_sum_tp2/n_small_steps;
                temp_sum_xr2 = 0;
                temp_sum_tr2 = 0;
                temp_sum_xp2 = 0;
                temp_sum_tp2 = 0;
            }
            m = 0;
        }

        }
    if (m>0) {
            b++;
            xro[b] = temp_sum_xr/m;
            tro[b] = temp_sum_tr/m;
            xpo[b] = temp_sum_xp/m;
            tpo[b] = temp_sum_tp/m;
            to[b] = temp_sum_xa/n_small_steps;
            time[b] = step*v;
            if (sec_chr==1) {
                xro2[b] = temp_sum_xr2/m;
                tro2[b] = temp_sum_tr2/m;
                xpo2[b] = temp_sum_xp2/m;
                tpo2[b] = temp_sum_tp2/m;
            }
    }
   	}


#include <boost/random/mersenne_twister.hpp>
#include <iostream>

int main(void)
{
    boost::mt19937 generator;
    uint64_t sum = 0;
    for (int i = 0; i < 5000; i++) {
        const uint32_t v = generator();
        std::cout << v << ((i % 5 == 4) ? "\n" : " ");
        sum += v;
    }
    std::cout << "Sum: " << sum << "\n";
    return 0;
}