#include <ros/ros.h>
#include <gazebo_msgs/SetLinkState.h>
#include <gazebo_msgs/LinkState.h>
#include <geometry_msgs/PoseStamped.h>
#include <boost/random.hpp>
#include <iostream>
#include <fstream>
#include <boost/filesystem.hpp>
#include <boost/math/constants/constants.hpp>
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>
#include <gazebo_msgs/ApplyBodyWrench.h>

#define USE_FIXED_SEED 1

namespace
{

using namespace std;
using namespace ros;

static const unsigned int FIXED_SEED = 0;
static const double pi = boost::math::constants::pi<double>();
static const double MASS = 0.896;
static const double DURATION = 0.02;

class RandomFallGenerator
{
private:

    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! Gazebo client
    ros::ServiceClient gazeboClient;

    //! Wrench client
    ros::ServiceClient wrenchClient;

    //! Create ground joint client
    ros::ServiceClient createGroundJointClient;

    //! Notifier
    ros::Publisher notifier;

    //! X position
    double x;

    //! Y position
    double y;

    //! X velocity
    double xDot;

    //! Y velocity
    double yDot;

    //! rng
    boost::mt19937 rng;
public:
    RandomFallGenerator() :
        pnh("~")
    {
        ros::service::waitForService("/gazebo/set_link_state");
        ros::service::waitForService("/create_ground_joint");
        ros::service::waitForService("/gazebo/apply_body_wrench");

        gazeboClient = nh.serviceClient<gazebo_msgs::SetLinkState>("/gazebo/set_link_state", true /* persistent */);
        createGroundJointClient = nh.serviceClient<std_srvs::Empty>("/create_ground_joint", true /* persistent */);
        wrenchClient = nh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench", true /* persistent */);
        notifier = nh.advertise<std_msgs::Header>("/human/fall", 1, true);

        unsigned int seed = 0;
#if(!USE_FIXED_SEED)
        seed = static_cast<unsigned int>(std::time(NULL));
#else
        const char* scenarioNumberStr = std::getenv("i");
        if(scenarioNumberStr != NULL)
        {
            unsigned int scenarioNumber = boost::lexical_cast<unsigned int>(scenarioNumberStr);
            seed = scenarioNumber + 1000;
        }
        else
        {
            ROS_INFO("No scenario number set. Using %u", FIXED_SEED);
            seed = FIXED_SEED;
        }
#endif
        rng.seed(seed);

        {
            // Generate x distance. 0.4 ensures the pole is in front of the base and the elbows
            boost::uniform_real<double> xRange(0.4, 0.6);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > getX(rng, xRange);
            x = getX();

            // Distance between the end effectors minus 15cm padding for the size of the links
            boost::uniform_real<double> yRange(-0.4, 0.4);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > getY(rng, yRange);
            y = getY();
            ROS_INFO("Generated x, y [%f, %f]", x, y);
        }

        {
            // Generate total velocity
            // maximum velocity of PR2 base is 1.0m/s
            boost::uniform_real<double> vRange(0, 1.0);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > genV(rng, vRange);
            double v = genV();
            ROS_INFO("Generated velocity [%f]", v);

            // Generate random direction
            boost::uniform_real<double> dRange(0.0, pi * 2.0);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > genD(rng, dRange);
            double d = genD();
            ROS_INFO("Generated direction [%f]", d);

            tf::Vector3 directionVector;

            // Set the vector length to the velocity
            directionVector.setX(v);

            // Rotate about the z axis
            directionVector = directionVector.rotate(tf::Vector3(0, 0, 1), tfScalar(d));

            xDot = directionVector.x();
            yDot = directionVector.y();

            // assert(fabs(directionVector.z()) < 0.001);
            ROS_INFO("Generated x_dot, y_dot [%f, %f]", xDot, yDot);
        }

        nh.setParam("base_x", x);
        nh.setParam("base_y", y);
        nh.setParam("base_z", 0.0);
    }

    void writeHeader(ofstream& outputCSV)
    {
        outputCSV << "Scenario Number, x, y, x_dot, y_dot" << endl;
    }

    void printVariables()
    {
        // Get the name of the folder to store the result in
        const char* resultsFolder = std::getenv("RESULTS_FOLDER");
        if(resultsFolder == NULL)
        {
            ROS_DEBUG_STREAM("Results folder not set. Using current directory.");
            resultsFolder = "";
        }

        const char* scenarioNumberStr = std::getenv("i");
        if(scenarioNumberStr == NULL)
        {
            ROS_DEBUG_STREAM("Scenario number not set. Using 0.");
            scenarioNumberStr = "0";
        }

        const string resultsFileName = boost::filesystem::current_path().string() + "/" + string(resultsFolder) + "/" + "velocities.csv";
        ROS_DEBUG_STREAM("Using results file: " << resultsFileName);
        bool exists = boost::filesystem::exists(resultsFileName);
        ofstream outputCSV;
        outputCSV.open(resultsFileName.c_str(), ios::out | ios::app);
        assert(outputCSV.is_open());

        if(!exists)
        {
            writeHeader(outputCSV);
        }

        outputCSV << scenarioNumberStr << ", " << x << ", " << y << ", " << xDot << ", " << yDot << endl;
        outputCSV.close();
        ROS_DEBUG_STREAM("Printing output file complete");
    }

    void apply()
    {
    	ROS_INFO("Setting position [%f, %f]", x, y);
        gazebo_msgs::SetLinkState state;
        state.request.link_state.link_name = "human::link";
        state.request.link_state.reference_frame = "world";
        state.request.link_state.pose.position.x = x;
        state.request.link_state.pose.position.y = y;
        state.request.link_state.pose.position.z = 0.8785;

        printVariables();

        ROS_INFO("Setting the base position in Gazebo");
        if (!gazeboClient.call(state))
        {
           ROS_ERROR("Failed to apply state");
           return;
        }

        std_srvs::Empty message = std_srvs::Empty();
        if (!createGroundJointClient.call(message)) {
            ROS_ERROR("Failed to create ground joint");
            return;
        }

        ROS_INFO("Setting velocity [%f, %f]", xDot, yDot);

        // Convert to acceleration
        const double forceX = 1.35 * MASS * xDot / DURATION ;
        const double forceY = 1.35 * MASS * yDot / DURATION;

        ROS_INFO("Applying forces [%f, %f]", forceX, forceY);

        gazebo_msgs::ApplyBodyWrench wrench;
        wrench.request.body_name = "human::link";
        wrench.request.wrench.force.x = forceX;
        wrench.request.wrench.force.y = forceY;
        wrench.request.wrench.torque.z = 0;
        wrench.request.duration = ros::Duration(DURATION);

        ROS_INFO("Starting the fall in Gazebo");
        if (!wrenchClient.call(wrench))
        {
           ROS_ERROR("Failed to apply wrench");
           return;
        }

        // Pause to allow the velocity to be applied
        // TODO: Reassess
        // ros::Duration(DURATION  + 0.0165 /* half frequency of IMU */).sleep();

        ROS_INFO("Notifying");
        std_msgs::Header header;
        header.stamp = ros::Time::now();
        notifier.publish(header);
        ROS_INFO("Setting position and velocity complete");
    }
};
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "random_fall_generator");
    RandomFallGenerator rfg;

    // Wait for the fall catcher to be ready
    ROS_INFO("Waiting for the catch_human_controller");
    ros::service::waitForService("/humanoid_catching/catch_human_controller");
    ROS_INFO("Catch human controller ready");

    // Allow all the sockets to connect
    ros::Duration(0.5).sleep();

    // Set the position/velocity and send the start fall message
    rfg.apply();

    // Wait until the trial is complete
    ros::Duration(10.0).sleep();
}


#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <interactive_markers/interactive_marker_server.h>
#include <interactive_markers/menu_handler.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/MarkerArray.h>
#include <cirkit_waypoint_manager_msgs/WaypointArray.h>

#include <math.h>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

#include <boost/tokenizer.hpp>
#include <boost/shared_array.hpp>
#include <boost/program_options.hpp>

using namespace visualization_msgs;

typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server;

class CirkitWaypointGenerator
{
public:
  CirkitWaypointGenerator():
    rate_(5)
  {
    ros::NodeHandle n("~");
    n.param("dist_th", dist_th_, 1.0); // distance threshold [m]
    n.param("yaw_th", yaw_th_, 45.0*3.1415/180.0); // yaw threshold [rad]
    odom_sub_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose",
                              1,
                              &CirkitWaypointGenerator::addWaypoint, this);
    clicked_sub_ = nh_.subscribe("clicked_point", 1, &CirkitWaypointGenerator::clickedPointCallback, this);
    reach_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/reach_threshold_markers", 1);
    waypoints_pub_ = nh_.advertise<cirkit_waypoint_manager_msgs::WaypointArray>("/waypoints", 1);
    waypoint_box_count_ = 0;
    server.reset( new interactive_markers::InteractiveMarkerServer("cube") );
  }

  void load(std::string waypoint_file)
  {
    const int rows_num = 9; // x, y, z, Qx, Qy, Qz, Qw, is_searching_area, reach_threshold
    boost::char_separator<char> sep("," ,"", boost::keep_empty_tokens);
    std::ifstream ifs(waypoint_file.c_str());
    std::string line;
    while(ifs.good()){
      getline(ifs, line);
      if(line.empty()){ break; }
      tokenizer tokens(line, sep);
      std::vector<double> data;
      tokenizer::iterator it = tokens.begin();
      for(; it != tokens.end() ; ++it){
        std::stringstream ss;
        double d;
        ss << *it;
        ss >> d;
        data.push_back(d);
      }
      if(data.size() != rows_num){
        ROS_ERROR("Row size is mismatch!!");
        return;
      }else{
        geometry_msgs::PoseWithCovariance new_pose;
        new_pose.pose.position.x = data[0];
        new_pose.pose.position.y = data[1];
        new_pose.pose.position.z = data[2];
        new_pose.pose.orientation.x = data[3];
        new_pose.pose.orientation.y = data[4];
        new_pose.pose.orientation.z = data[5];
        new_pose.pose.orientation.w = data[6];
        makeWaypointMarker(new_pose, (int)data[7], data[8]);
      }
    }
    ROS_INFO_STREAM(waypoint_box_count_ << "waypoints are loaded.");
  }

  double calculateDistance(geometry_msgs::PoseWithCovariance new_pose)
  {
    return sqrt(pow(new_pose.pose.position.x - last_pose_.pose.position.x, 2)
                + pow(new_pose.pose.position.y - last_pose_.pose.position.y, 2));
  }

  void getRPY(const geometry_msgs::Quaternion &q,
              double &roll,double &pitch,double &yaw){
    tf::Quaternion tfq(q.x, q.y, q.z, q.w);
    tf::Matrix3x3(tfq).getRPY(roll, pitch, yaw);
  }

  double calculateAngle(geometry_msgs::PoseWithCovariance new_pose)
  {
    double yaw, pitch, roll;
    getRPY(new_pose.pose.orientation, roll, pitch, yaw);
    double last_yaw, last_pitch, last_roll;
    getRPY(last_pose_.pose.orientation, last_roll, last_pitch, last_yaw);
    return fabs(yaw - last_yaw);
  }

  InteractiveMarkerControl& makeWaypointMarkerControl(InteractiveMarker &msg,
                                                      int is_searching_area)
  {
    InteractiveMarkerControl control;
    control.orientation.w = 1;
    control.orientation.x = 0;
    control.orientation.y = 1;
    control.orientation.z = 0;
    control.name = "rotate_z";
    control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    msg.controls.push_back(control);
    // control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
    // msg.controls.push_back(control);
    // control.independent_marker_orientation = true;
    control.orientation.w = 1;
    control.orientation.x = 0;
    control.orientation.y = 1;
    control.orientation.z = 0;
    control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
    msg.controls.push_back(control);

    Marker marker;
    marker.type = Marker::CUBE;
    marker.scale.x = msg.scale*0.5;
    marker.scale.y = msg.scale*0.5;
    marker.scale.z = msg.scale*0.5;
    marker.color.r = 0.05 + 1.0*(float)is_searching_area;
    marker.color.g = 0.80;
    marker.color.b = 0.02;
    marker.color.a = 1.0;

    control.markers.push_back(marker);
    control.always_visible = true;
    msg.controls.push_back(control);

    return msg.controls.back();

  }

  void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
  {
    std::ostringstream s;
    s << "Feedback from marker '" << feedback->marker_name << "' "
      << " / control '" << feedback->control_name << "'";

    switch ( feedback->event_type )
    {
      case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
        {
          reach_threshold_markers_.markers[std::stoi(feedback->marker_name)].pose
            = feedback->pose;
          reach_marker_pub_.publish(reach_threshold_markers_);
          waypoints_.waypoints[std::stoi(feedback->marker_name)].pose = feedback->pose;
          break;
        }
    }
    server->applyChanges();
  }

  void makeWaypointMarker(const geometry_msgs::PoseWithCovariance new_pose,
                          int is_searching_area, double reach_threshold)
  {
    InteractiveMarker int_marker;
    int_marker.header.frame_id = "map";
    int_marker.pose = new_pose.pose;
    int_marker.scale = 1;

    visualization_msgs::Marker reach_marker;
    reach_marker.header.frame_id = "map";
    reach_marker.header.stamp = ros::Time();
    reach_marker.id = waypoint_box_count_;
    reach_marker.type = visualization_msgs::Marker::CYLINDER;
    reach_marker.action = visualization_msgs::Marker::ADD;
    reach_marker.pose = new_pose.pose;
    reach_marker.scale.x = reach_threshold/2.0;
    reach_marker.scale.y = reach_threshold/2.0;
    reach_marker.scale.z = 0.1;
    reach_marker.color.a = 0.7;
    reach_marker.color.r = 0.0;
    reach_marker.color.g = 0.0;
    reach_marker.color.b = 1.0;
    reach_threshold_markers_.markers.push_back(reach_marker);
    reach_marker_pub_.publish(reach_threshold_markers_);

    std::stringstream s;
    s << waypoint_box_count_;
    int_marker.name = s.str();
    int_marker.description = s.str();

    makeWaypointMarkerControl(int_marker, is_searching_area);

    server->insert(int_marker);
    server->setCallback(int_marker.name, boost::bind(&CirkitWaypointGenerator::processFeedback, this, _1));
    server->applyChanges();

    cirkit_waypoint_manager_msgs::Waypoint waypoint;
    waypoint.number = waypoint_box_count_;
    waypoint.pose = new_pose.pose;
    waypoint.is_search_area = is_searching_area;
    waypoint.reach_tolerance = reach_threshold/2.0;
    waypoints_.waypoints.push_back(waypoint);

    waypoint_box_count_++;
  }

  void addWaypoint(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& amcl_pose)
  {
    double diff_dist = calculateDistance(amcl_pose->pose);
    double diff_yaw = calculateAngle(amcl_pose->pose);
    if(diff_dist > dist_th_ || diff_yaw > yaw_th_)
    {
      makeWaypointMarker(amcl_pose->pose, 0, 3.0);
      last_pose_ = amcl_pose->pose;
    }
  }

  void publishWaypointCallback(const ros::TimerEvent&)
  {
    reach_marker_pub_.publish(reach_threshold_markers_);
    waypoints_pub_.publish(waypoints_);
    server->applyChanges();
  }

  void clickedPointCallback(const geometry_msgs::PointStamped &point)
  {
    geometry_msgs::PoseWithCovariance pose;
    tf::pointTFToMsg(tf::Vector3( point.point.x, point.point.y, 0), pose.pose.position);
    tf::quaternionTFToMsg(tf::createQuaternionFromRPY(0, 0, 0), pose.pose.orientation);
    makeWaypointMarker(pose, 0, 3.0);
    server->applyChanges();
  }

  void tfSendTransformCallback(const ros::TimerEvent&)
  {
    tf::Transform t;
    ros::Time time = ros::Time::now();

    for (size_t i = 0; i < waypoints_.waypoints.size(); ++i) {
      std::stringstream s;
      s << waypoints_.waypoints[i].number;
      t.setOrigin(tf::Vector3(waypoints_.waypoints[i].pose.position.x,
                              waypoints_.waypoints[i].pose.position.y,
                              waypoints_.waypoints[i].pose.position.z));
      t.setRotation(tf::Quaternion(waypoints_.waypoints[i].pose.orientation.x,
                                   waypoints_.waypoints[i].pose.orientation.y,
                                   waypoints_.waypoints[i].pose.orientation.z,
                                   waypoints_.waypoints[i].pose.orientation.w));
      br_.sendTransform(tf::StampedTransform(t, time, "map", s.str()));
    }
  }

  void run()
  {
    ros::Timer frame_timer = nh_.createTimer(ros::Duration(0.1), boost::bind(&CirkitWaypointGenerator::publishWaypointCallback, this, _1));
    ros::Timer tf_frame_timer = nh_.createTimer(ros::Duration(0.1), boost::bind(&CirkitWaypointGenerator::tfSendTransformCallback, this, _1));
    while(ros::ok())
    {
      ros::spinOnce();
      rate_.sleep();
    }
  }
private:
  ros::NodeHandle nh_;
  ros::Rate rate_;
  ros::Subscriber odom_sub_;
  ros::Subscriber clicked_sub_;
  ros::Publisher reach_marker_pub_;
  ros::Publisher waypoints_pub_;
  geometry_msgs::PoseWithCovariance last_pose_;
  cirkit_waypoint_manager_msgs::WaypointArray waypoints_;
  double dist_th_;
  double yaw_th_;
  int waypoint_box_count_;
  std::vector<double> reach_thresholds_;
  visualization_msgs::MarkerArray reach_threshold_markers_;
  tf::TransformBroadcaster br_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "waypoint_generator");
  CirkitWaypointGenerator generator;

  boost::program_options::options_description desc("Options");
  desc.add_options()
    ("help", "Print help message")
    ("load", boost::program_options::value<std::string>(), "waypoint filename");

  boost::program_options::variables_map vm;
  try {
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
    boost::program_options::notify(vm);
    if( vm.count("help") ){
      std::cout << "This is waypoint generator node" << std::endl;
      std::cerr << desc << std::endl;
      return 0;
    }
    if (vm.count("load")) {
      generator.load(vm["load"].as<std::string>());
    }
  } catch (boost::program_options::error& e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return -1;
  }

  generator.run();
  return 0;
}


#include <boost/accumulators/statistics/tail_mean.hpp>


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2016  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "enumerate_linear_transformations.hpp"

#include <fstream>
#include <iostream>
#include <unordered_map>

#include <boost/dynamic_bitset.hpp>
#include <boost/format.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>

#include <igraph/igraph.h>

#include <core/utils/bitset_utils.hpp>
#include <core/utils/graph_utils.hpp>
#include <core/utils/range_utils.hpp>
#include <core/utils/string_utils.hpp>
#include <classical/utils/truth_table_utils.hpp>

#define L(x) { if ( verbose ) { std::cout << x << std::endl; } }

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

struct boolean_square_matrix
{
public:
  boolean_square_matrix( const boost::dynamic_bitset<>& matrix, unsigned dimension )
    : matrix( matrix ),
      dimension( dimension )
  {
  }

  boolean_square_matrix( const std::vector<boost::dynamic_bitset<>>& rows )
  {
    assert( !rows.empty() );

    dimension = rows.front().size();
    matrix.resize( dimension * dimension );

    unsigned index = 0u;

    for ( const auto& r : rows )
    {
      for ( auto c = 0u; c < dimension; ++c )
      {
        matrix[index++] = r[c];
      }
    }
  }

  static boolean_square_matrix identity( unsigned dimension )
  {
    std::vector<boost::dynamic_bitset<>> rows( dimension, boost::dynamic_bitset<>( dimension ) );
    for ( auto c = 0u; c < dimension; ++c )
    {
      rows[c].set( c );
    }
    return boolean_square_matrix( rows );
  }

  inline bool at( unsigned row, unsigned column ) const
  {
    return matrix.test( row * dimension + column );
  }

  inline void set( unsigned row, unsigned column, bool value )
  {
    matrix.set( row * dimension + column, value );
  }

  std::vector<boost::dynamic_bitset<>> get_row_vectors() const
  {
    std::vector<boost::dynamic_bitset<>> rows( dimension, boost::dynamic_bitset<>( dimension ) );

    for ( auto r = 0u; r < dimension; ++r )
    {
      for ( auto c = 0u; c < dimension; ++c )
      {
        rows[r][c] = at( r, c );
      }
    }

    return rows;
  }

  boolean_square_matrix get_submatrix_by_removing( unsigned row, unsigned column ) const
  {
    boost::dynamic_bitset<> subm( ( dimension - 1u ) * ( dimension - 1u ) );

    unsigned index = 0;
    for ( auto r = 0u; r < dimension; ++r )
    {
      if ( r == row ) continue;
      for ( auto c = 0u; c < dimension; ++c )
      {
        if ( c == column ) continue;
        subm[index++] = at( r, c );
      }
    }

    return boolean_square_matrix( subm, dimension - 1u );
  }

  bool determinant()
  {
    if ( dimension == 2u )
    {
      return ( at( 0, 0 ) && at( 1, 1 ) ) != ( at( 0, 1 ) && at( 1, 0 ) );
    }
    else
    {
      bool result = false;
      for ( auto c = 0u; c < dimension; ++c )
      {
        result = result != ( at( 0u, c ) * get_submatrix_by_removing( 0u, c ).determinant() );
      }
      return result;
    }
  }

  bool is_sorted()
  {
    const auto rows = get_row_vectors();

    auto v = 0u;

    for ( const auto& r : rows )
    {
      const auto rv = r.to_ulong();
      if ( rv < v ) return false;
      v = rv;
    }

    return true;
  }

  unsigned row_sum() const
  {
    boost::dynamic_bitset<> sum( dimension );
    for ( const auto& r : get_row_vectors() )
    {
      for ( auto c = 0u; c < dimension; ++c )
      {
        sum[c] = sum[c] != r[c];
      }
    }
    return sum.to_ulong();
  }

  boolean_square_matrix swap( unsigned row1, unsigned row2 ) const
  {
    boolean_square_matrix other = *this;

    for ( auto c = 0u; c < dimension; ++c )
    {
      other.set( row1, c, at( row2, c ) );
      other.set( row2, c, at( row1, c ) );
    }

    return other;
  }

  boolean_square_matrix add_to( unsigned from_row, unsigned to_row ) const
  {
    boolean_square_matrix other = *this;

    for ( auto c = 0u; c < dimension; ++c )
    {
      other.set( to_row, c, at( from_row, c ) != at( to_row, c ) );
    }

    return other;
  }

  boolean_square_matrix sort() const
  {
    auto rows = get_row_vectors();

    std::sort( rows.begin(), rows.end(), []( const boost::dynamic_bitset<>& bs1, const boost::dynamic_bitset<>& bs2 ) { return bs1.to_ulong() < bs2.to_ulong(); } );

    return boolean_square_matrix( rows );
  }

  /* multiply a column vector */
  boost::dynamic_bitset<> multiply( const boost::dynamic_bitset<>& v ) const
  {
    boost::dynamic_bitset<> y( dimension );

    for ( auto r = 0u; r < dimension; ++r )
    {
      auto value = false;
      for ( auto c = 0u; c < dimension; ++c )
      {
        value = value != ( v[c] && at( r, c ) );
      }
      y[r] = value;
    }

    return y;
  }

  /* other can be sorted, but this must be as we want it
     we assume it's possible */
  std::pair<unsigned, unsigned> get_add_to_rows( const boolean_square_matrix& other ) const
  {
    const auto vectors = get_row_vectors();
    const auto ovectors = other.get_row_vectors();

    auto to = 0u, from = 0u;
    boost::dynamic_bitset<> to_row;
    boost::dynamic_bitset<> from_row;

    for ( const auto& r : index( vectors ) )
    {
      const auto it = std::find( ovectors.begin(), ovectors.end(), r.value );
      if ( it == ovectors.end() )
      {
        to = r.index;
        to_row = r.value;
      }
    }

    for ( const auto& r : ovectors )
    {
      const auto it = std::find( vectors.begin(), vectors.end(), r );
      if ( it == vectors.end() )
      {
        from_row = r;
      }
    }

    from_row ^= to_row;
    from = std::distance( vectors.begin(), std::find( vectors.begin(), vectors.end(), from_row ) );

    //std::cout << boost::format( "[i] added %s [%d] to %s [%d]" ) % to_string( from_row ) % from % to_string( to_row ) % to << std::endl;

    return {from, to};
  }

  std::pair<unsigned, unsigned> compute_swap_permutation( const boolean_square_matrix& other ) const
  {
    std::vector<unsigned> perm( 1u << dimension ); /* permutation of this matrix */
    std::vector<unsigned> perm2( 1u << dimension );

    boost::dynamic_bitset<> bs( dimension );

    do
    {
      perm[bs.to_ulong()] = multiply( bs ).to_ulong();

      inc( bs );
    } while ( bs.any() );

    do
    {
      perm2[other.multiply( bs ).to_ulong()] = bs.to_ulong();

      inc( bs );
    } while ( bs.any() );

    unsigned delta = 0u;
    unsigned theta = 0u;

    for ( auto i = 0u; i < perm.size(); ++i )
    {
      const auto j = perm2[perm[i]];
      if ( i < j )
      {
        delta = j - i;
        theta |= ( 1 << i );
      }
    }

    return {delta, theta};
  }

  inline unsigned long value() const
  {
    return matrix.to_ulong();
  }

  friend std::ostream& operator<<( std::ostream& os, const boolean_square_matrix& m )
  {
    for ( auto r = 0u; r < m.dimension; ++r )
    {
      for ( auto c = 0u; c < m.dimension; ++c )
      {
        if ( c > 0u )
        {
          os << " ";
        }
        os << m.at( r, m.dimension - c - 1 );
      }
      os << std::endl;
    }

    return os;
  }

private:
  boost::dynamic_bitset<> matrix;
  unsigned                dimension;
};

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

unsigned factorial( unsigned n )
{
  return ( n == 1u || n == 0u ) ? 1u : factorial( n - 1u ) * n;
}

unsigned compute_order( unsigned n )
{
  unsigned order = 1u;

  for ( unsigned i = 0; i < n; ++i )
  {
    order *= ( 1 << n ) - ( 1 << i );
  }

  return order;
}

int igraph_is_strongly_regular( const igraph_t *graph, igraph_bool_t *res, igraph_integer_t *k, igraph_integer_t *lambda, igraph_integer_t *mu )
{
  int err;
  *lambda = -1;
  *mu = -1;

  /* check g is undirected */
  if ( igraph_is_directed( graph ) ) { return IGRAPH_EINVAL; }

  /* special case if empty */
  if ( igraph_vcount( graph ) == 0u )
  {
    *res = 1;
    return IGRAPH_SUCCESS;
  }

  /* check same degree property */
  igraph_vector_t degrees;
  igraph_vector_init( &degrees, 0 );
  igraph_vs_t vertices;
  igraph_vs_all( &vertices );
  if ( ( err = igraph_degree( graph, &degrees, vertices, IGRAPH_ALL, 0 ) ) != IGRAPH_SUCCESS )
  {
    //return err;
  }
  igraph_vs_destroy( &vertices );

  *k = VECTOR( degrees )[0];
  for ( auto i = 1u; i < igraph_vector_size( &degrees ); ++i )
  {
    if ( VECTOR( degrees )[i] != *k )
    {
      igraph_vector_destroy( &degrees );
      std::cout << "[w] not all vertices have same degree" << std::endl;
      *res = 0;
      return IGRAPH_SUCCESS;
    }
  }

  igraph_vector_destroy( &degrees );

  /* check that neighbors have the same number of neighbors */
  for ( auto node = 0; node < igraph_vcount( graph ); ++node )
  {
    igraph_vector_t neis;
    igraph_vector_init( &neis, 0 );
    if ( ( err = igraph_neighbors( graph, &neis, node, IGRAPH_ALL ) ) != IGRAPH_SUCCESS )
    {
      // return err;
    }

    /* point to the first neighbor */
    auto cur = 0;
    auto conn = 0;
    while ( cur < igraph_vector_size( &neis ) && VECTOR( neis )[cur] < node + 1 ) ++cur;

    for ( auto onode = node + 1; onode < igraph_vcount( graph ); ++onode )
    {
      if ( onode == node ) continue;

      if ( cur < igraph_vector_size( &neis ) && VECTOR( neis )[cur] == onode )
      {
        /* connected */
        conn = 1;
        ++cur;
      }
      else
      {
        /* not connected */
        conn = 0;
      }

      igraph_vector_t oneis;
      igraph_vector_init( &oneis, 0 );
      igraph_neighbors( graph, &oneis, onode, IGRAPH_ALL );

      igraph_vector_t inter;
      igraph_vector_init( &inter, 0 );
      igraph_vector_intersect_sorted( &neis, &oneis, &inter );

      if ( conn )
      {
        if ( *lambda == -1 )
        {
          *lambda = igraph_vector_size( &inter );
        }
        else if ( *lambda != igraph_vector_size( &inter ) )
        {
          igraph_vector_destroy( &inter );
          igraph_vector_destroy( &oneis );
          igraph_vector_destroy( &neis );

          std::cout << "[w] not same lambda" << std::endl;
          *res = 0;
          return IGRAPH_SUCCESS;
        }
      }
      else
      {
        if ( *mu == -1 )
        {
          *mu = igraph_vector_size( &inter );
        }
        else if ( *mu != igraph_vector_size( &inter ) )
        {
          igraph_vector_destroy( &inter );
          igraph_vector_destroy( &oneis );
          igraph_vector_destroy( &neis );

          std::cout << "[w] not same mu" << std::endl;
          *res = 0;
          return IGRAPH_SUCCESS;
        }
      }

      igraph_vector_destroy( &inter );

      igraph_vector_destroy( &oneis );
    }

    igraph_vector_destroy( &neis );
  }

  *res = 1;
  return IGRAPH_SUCCESS;
}

struct enumerate_hamiltonians_is_equivalent
{
  enumerate_hamiltonians_is_equivalent( const std::vector<boolean_square_matrix>& node_to_matrix )
    : node_to_matrix( node_to_matrix )
  {
  }

  bool operator()( const vertex_t<graph_t<>>& vs, const vertex_t<graph_t<>>& vl ) const
  {
    if ( vs == 0u )
    {
      return vl == 0u;
    }
    if ( vs < prefix.size() && prefix[vs] != 0 )
    {
      return node_to_matrix[vl].row_sum() == prefix[vs];
    }
    else
    {
      return true;
    }
  }

private:
  //const std::vector<unsigned> prefix = {1, 2, 4, 3, 6, 7, 5, 3, 0, 0, 0, 0, 0, 0, 5, 0, 0, 0, 0, 0, 0, 7};
  //const std::vector<unsigned> prefix = {1, 2, 4, 3, 6, 7, 5, 3, 2, 4, 7, 1, 5, 6, 5, 4, 6, 1, 3, 7, 2, 7, 4, 1, 3, 5, 2, 6};
  const std::vector<unsigned> prefix = {7, 6, 5, 2, 4, 1, 3, 5, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 3};
  const std::vector<boolean_square_matrix>& node_to_matrix;
};

struct enumerate_hamiltonians_printer
{
public:
  enumerate_hamiltonians_printer( const graph_t<>& sg, const std::vector<boolean_square_matrix>& node_to_matrix )
    : sg( sg ),
      node_to_matrix( node_to_matrix )
  {
  }

  template<typename CorrespondenceMap1To2, typename CorrespondenceMap2To1>
  bool operator()( const CorrespondenceMap1To2& f, const CorrespondenceMap2To1& g ) const
  {
    std::vector<std::vector<unsigned>> parts( 4u, std::vector<unsigned>( 7u ) );

    for ( auto v = 0u; v < boost::num_vertices( sg ); ++v )
    {
      parts[v / 7][v % 7] = node_to_matrix[boost::get( f, v )].row_sum();
    }

    for ( auto& p : parts )
    {
      std::sort( p.begin(), p.end() );
      for ( auto j = 0u; j < 7u; ++j )
      {
        if ( p[j] != j + 1 )
        {
          return true;
        }
      }
    }

    for ( auto v = 0u; v < boost::num_vertices( sg ); ++v )
    {
      std::cout << " " << node_to_matrix[boost::get( f, v )].row_sum();
    }
    std::cout << std::endl;

    for ( auto v = 0u; v < boost::num_vertices( sg ); ++v )
    {
      std::cout << " " << boost::get( f, v );
    }
    std::cout << std::endl;

    for ( auto i = 0u; i < 4u; ++i )
    {
      std::cout << " " << node_to_matrix[boost::get( f, 7 * i )].row_sum();
    }
    std::cout << std::endl << std::endl;

    return true;
  }

private:
  const graph_t<>& sg;
  const std::vector<boolean_square_matrix>& node_to_matrix;
};

void enumerate_hamiltonians( const igraph_t* graph, const std::vector<boolean_square_matrix>& node_to_matrix )
{
  graph_t<> g;
  for ( auto i = 0; i < igraph_vcount( graph ); ++i )
  {
    add_vertex( g );
  }

  igraph_es_t es;
  igraph_es_all( &es, IGRAPH_EDGEORDER_ID );

  igraph_eit_t eit;
  igraph_eit_create( graph, es, &eit );

  while ( !IGRAPH_EIT_END( eit ) )
  {
    const auto eid = IGRAPH_EIT_GET( eit );

    igraph_integer_t from, to;
    igraph_edge( graph, eid, &from, &to );

    add_edge( from, to, g );

    IGRAPH_EIT_NEXT( eit );
  }

  igraph_eit_destroy( &eit );
  igraph_es_destroy( &es );

  const auto rg = ring_graph( boost::num_vertices( g ) );
  boost::vf2_subgraph_mono( rg, g,
                            enumerate_hamiltonians_printer( rg, node_to_matrix ),
                            boost::vertex_order_by_mult( rg ),
                            boost::vertices_equivalent( enumerate_hamiltonians_is_equivalent( node_to_matrix ) ) );
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

void enumerate_linear_transformations( unsigned n, const properties::ptr& settings, const properties::ptr& statistics )
{
  /* settings */
  const auto abstract    = get( settings, "abstract",    true );
  const auto hamiltonian = get( settings, "hamiltonian", false );
  const auto hamilenum   = get( settings, "hamilenum",   false );
  const auto frompath    = get( settings, "frompath",    std::string() );
  const auto dotname     = get( settings, "dotname",     std::string() );
  const auto verbose     = get( settings, "verbose",     false );

  std::vector<boolean_square_matrix> node_to_matrix;
  std::unordered_map<unsigned long, unsigned> matrix_to_node;
  std::vector<unsigned> edge_to_operation;

  igraph_t graph, * p_g = &graph;

  auto order = compute_order( n );
  if ( abstract )
  {
    order /= factorial( n );
  }
  L( "[i] order = " << order );
  igraph_empty( p_g, order, IGRAPH_UNDIRECTED );

  boost::dynamic_bitset<> bs( n * n );

  /* create vertices */
  foreach_bitset( n * n, [&matrix_to_node, &node_to_matrix, n, abstract, verbose]( const boost::dynamic_bitset<>& bs ) {
      boolean_square_matrix m( bs, n );
      if ( m.determinant() != 0u && ( !abstract || m.is_sorted() ) )
      {
        matrix_to_node[m.value()] = node_to_matrix.size();
        node_to_matrix.push_back( m );
      }
    } );

  /* create edges */
  for ( size_t v = 0; v < node_to_matrix.size(); ++v )
  {
    const auto& bm = node_to_matrix[v];

    /* adding */
    for ( auto from_row = 0u; from_row < n; ++from_row )
    {
      for ( auto to_row = 0u; to_row < n; ++to_row )
      {
        if ( from_row == to_row ) continue;

        auto other = bm.add_to( from_row, to_row );
        if ( abstract )
        {
          other = other.sort();
        }

        const auto w = matrix_to_node[other.value()];
        if ( w > v )
        {
          igraph_add_edge( p_g, v, w );

          igraph_integer_t eid;
          igraph_get_eid( p_g, &eid, v, w, 0, 0 );
          edge_to_operation.push_back( from_row * 3 + to_row );
        }
      }
    }

    /* swapping */
    if ( !abstract )
    {
      for ( unsigned row2 = 1u; row2 < n; ++row2 )
      {
        for ( unsigned row1 = 0u; row1 < row2; ++row1 )
        {
          const auto other = bm.swap( row1, row2 );
          const auto w = matrix_to_node[other.value()];
          if ( w > v )
          {
            igraph_add_edge( p_g, v, w );
          }
        }
      }
    }
  }

  /* check some properties */
  igraph_bool_t b_connected;
  igraph_is_connected( p_g, &b_connected, IGRAPH_STRONG );
  L( "[i] is strongly connected: " << b_connected );

  igraph_bool_t b_strongly_regular;
  igraph_integer_t k, lambda, mu;
  assert( !igraph_is_strongly_regular( p_g, &b_strongly_regular, &k, &lambda, &mu ) );
  L( "[i] is strongly regular: " << b_strongly_regular );

  /* Hamiltonian path */
  if ( hamiltonian )
  {
    igraph_t rgraph, * p_rg = &rgraph;
    igraph_ring( p_rg, order, 0, 0, 1 );

    igraph_bool_t b_iso;
    igraph_vector_t map;
    igraph_vector_init( &map, 0 );
    igraph_subisomorphic_lad( p_rg, p_g, nullptr, &b_iso, &map, nullptr, 0, 0 );
    L( "[i] found isomorphism: " << b_iso );

    igraph_vector_print( &map );
    igraph_vector_destroy( &map );
    igraph_destroy( p_rg );
  }

  if ( hamilenum )
  {
    enumerate_hamiltonians( p_g, node_to_matrix );
  }

  /* derive delta-swap sequence from path */
  if ( !frompath.empty() )
  {
    std::cout << "[i] derive delta-swap sequence from path: " << frompath << std::endl;
    std::vector<unsigned> path;
    parse_string_list( path, frompath, " " );

    unsigned delta, theta;
    const auto& swaps = tt_store::i().swaps( n );

    auto curm = node_to_matrix[path[0u]];

    std::cout << curm;
    for ( auto i = 0u; i < swaps.size(); ++i )
    {
      auto nextm = curm.swap( swaps[i], swaps[i] + 1 );
      std::tie( delta, theta ) = nextm.compute_swap_permutation( curm );
      std::cout << boost::format( "[i] swap %d with %d (%d, %d)" ) % swaps[i] % ( swaps[i] + 1 ) % delta % theta << std::endl;
      std::cout << nextm;
      curm = nextm;
    }

    auto next = 1u;
    while ( next < path.size() )
    {
      // const auto v = std::min( path[next - 1], path[next] );
      // const auto w = std::max( path[next - 1], path[next] );
      // igraph_integer_t eid;
      // igraph_get_eid( p_g, &eid, v, w, 0, 0 );
      // const auto op = edge_to_operation[eid];
      // const auto to = op % n;
      // const auto from = ( op - to ) / n;
      // std::cout << boost::format( "[i] add %d to %d" ) % from % to << std::endl;

      unsigned from_row, to_row;
      std::tie( from_row, to_row ) = curm.get_add_to_rows( node_to_matrix[path[next]] );

      auto nextm = curm.add_to( from_row, to_row );
      std::tie( delta, theta ) = nextm.compute_swap_permutation( curm );
      std::cout << boost::format( "[i] add %d to %d (%d, %d)" ) % from_row % to_row % delta % theta << std::endl;
      std::cout << nextm;
      curm = nextm;

      for ( auto i = 0u; i < swaps.size(); ++i )
      {
        auto nextm = curm.swap( swaps[i], swaps[i] + 1 );
        std::tie( delta, theta ) = nextm.compute_swap_permutation( curm );
        std::cout << boost::format( "[i] swap %d with %d (%d, %d)" ) % swaps[i] % ( swaps[i] + 1 ) % delta % theta << std::endl;
        std::cout << nextm;
        curm = nextm;
      }
      ++next;
    }
  }

  /* row value inspection */
  if ( false )
  {
    const auto from_sum = 2u;
    const auto to_sum = 3u;
    for ( const auto& bm : node_to_matrix )
    {
      if ( bm.row_sum() == from_sum )
      {
        std::cout << "[i] matrix with row sum " << from_sum << ": " << std::endl << bm;
        //std::cout << "[i] adjacent matrices have row sums:";

        igraph_vector_t neis;
        igraph_vector_init( &neis, 0 );
        igraph_neighbors( p_g, &neis, matrix_to_node[bm.value()], IGRAPH_ALL );

        for ( auto i = 0u; i < igraph_vector_size( &neis ); ++i )
        {
          const auto& om = node_to_matrix[VECTOR( neis )[i]];
          if ( om.row_sum() == to_sum )
          {
            std::cout << "[i] connects to: " << std::endl << om;
          }
        }
        //std::cout << std::endl;
        igraph_vector_destroy( &neis );
      }
    }
  }

  if ( !dotname.empty() )
  {
    auto * fp = fopen( dotname.c_str(), "w" );
    igraph_write_graph_dot( p_g, fp );
    fclose( fp );
  }

  igraph_destroy( p_g );
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#ifndef TUPLE_CAT_HANA_HPP
#define TUPLE_CAT_HANA_HPP

#include <boost/hana/at.hpp>
#include <boost/hana/tuple.hpp>

#include <cstddef>
#include <type_traits>
#include <utility>

//
// Implementation adapted from Hana
//

template <typename Tuple>
struct tuple_size;

template <typename ...T>
struct tuple_size<boost::hana::tuple<T...>> {
    static constexpr auto value = sizeof...(T);
};

template <typename ...T>
boost::hana::tuple<T&&...> forward_as_tuple(T&& ...t) {
    return {static_cast<T&&>(t)...};
}

namespace mystd {
    template <typename InputIt, typename T>
    constexpr T accumulate(InputIt first, InputIt last, T init) {
        for (; first != last; ++first)
            init += *first;
        return init;
    }

    template <typename T, std::size_t Size>
    struct array {
        T elems_[Size > 0 ? Size : 1];

        constexpr T& operator[](std::size_t n)
        { return elems_[n]; }

        constexpr T const& operator[](std::size_t n) const
        { return elems_[n]; }

        constexpr std::size_t size() const noexcept
        { return Size; }

        constexpr T* begin() noexcept             { return elems_; }
        constexpr T const* begin() const noexcept { return elems_; }
        constexpr T* end() noexcept               { return elems_ + Size; }
        constexpr T const* end() const noexcept   { return elems_ + Size; }
    };
}

template <typename ...Tuples>
struct tuple_cat_type;

template <>
struct tuple_cat_type<> {
    using type = boost::hana::tuple<>;
};

template <typename Tuple>
struct tuple_cat_type<Tuple> {
    using type = Tuple;
};

template <typename ...T, typename ...U, typename ...Tuples>
struct tuple_cat_type<boost::hana::tuple<T...>,
                      boost::hana::tuple<U...>,
                      Tuples...>
{
    using type = typename tuple_cat_type<
        boost::hana::tuple<T..., U...>,
        Tuples...
    >::type;
};

// sample(flatten_indices-1)
template <std::size_t ...Lengths>
struct flatten_indices {
    static constexpr std::size_t lengths[] = {Lengths..., 0};
    static constexpr auto flat_length =
        mystd::accumulate(lengths, lengths + sizeof...(Lengths), 0);

    template <bool Inner>
    static constexpr auto compute() {
        mystd::array<std::size_t, flat_length> indices{};
        for (std::size_t index = 0, i = 0; i < sizeof...(Lengths); ++i)
            for (std::size_t j = 0; j < lengths[i]; ++j, ++index)
                indices[index] = (Inner ? i : j);
        return indices;
    }

    static constexpr auto inner = compute<true>();
    static constexpr auto outer = compute<false>();
// end-sample

// sample(flatten_indices-2)
    template <typename Result, typename Tuples, std::size_t ...i>
    static constexpr Result
    apply(Tuples tuples, std::index_sequence<i...>) {
        return Result{
            boost::hana::at_c<outer[i]>(
                boost::hana::at_c<inner[i]>(tuples)
            )...
        };
    }
};
// end-sample

// sample(tuple_cat)
template <typename ...Tuples>
constexpr auto tuple_cat(Tuples&& ...tuples) {
    using Indices = flatten_indices<
        tuple_size<std::remove_reference_t<Tuples>>::value...
    >;
    using Result = typename tuple_cat_type<
        std::remove_reference_t<Tuples>...
    >::type; // implementation not relevant
    return Indices::template apply<Result>(
        ::forward_as_tuple(std::forward<Tuples>(tuples)...),
        std::make_index_sequence<Indices::flat_length>{}
    );
}
// end-sample

#endif


#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <fmt/format.h>
#include <string>
#include <iomanip>
#include <cctype>
#include <boost/variant.hpp>
#include <unordered_map>
#include <range/v3/all.hpp>
#include <queue>
#include <optional>
#include <vector>
struct Output
{
};
struct Bot
{
  std::optional<int> low_;
  std::optional<int> high_;
  std::priority_queue<int> q_;
};
struct Factory
{
  std::vector<Bot> bots;
  int startid_;
  Factory() : bots(500) {}
  bool abletran_{ true };
  std::optional<int> validid_;
  int n = 0;
  int total = 1;

  void transfer(int i)
  {
    auto &bot = bots[i];
    auto big = bot.q_.top();
    bot.q_.pop();
    auto small = bot.q_.top();
    if (i == 49 || i == 159 || i == 115) {
      if (n != 3) {
        total *= small;
        if (n == 2) {
          fmt::print("total:{}\n", total);
        }
        ++n;
      } else {
        return;
      }
    }
    bot.q_.pop();
    std::tuple result{ small, big, i };
    //fmt::print("id:{},pair:{},{}\n", i, small, big);

    //if (small == 17 && big == 61) {
    //  if (!validid_) {
    //    *validid_ = std::get<2>(result);
    //    fmt::print("validid:{}", i);
    //  }
    //  return;
    //}
    if (bot.low_) {
      bots[*bot.low_].q_.push(small);
      if (bots[*bot.low_].q_.size() == 2) {
        //fmt::print("transfer low\n");
        transfer(*bot.low_);
      }
    }
    if (bot.high_) {
      bots[*bot.high_].q_.push(big);
      if (bots[*bot.high_].q_.size() == 2) {
        //fmt::print("transfer high\n");
        transfer(*bot.high_);
      }
    }
    //result;
  }
};

int main(int argc, char **argv)
{
  if (argc > 2) {
    std::ifstream init(argv[1]);
    int value, botid;
    int startbotid;
    Factory f;
    while (init >> value >> botid) {
      f.bots[botid].q_.push(value);
      if (f.bots[botid].q_.size() == 2) {
        f.startid_ = botid;
      }
    }
    std::ifstream tran(argv[2]);
    int low, high;

    std::string loutput;
    std::string houtput;
    std::string ignore;
    while (tran >> ignore >> botid
           >> ignore >> ignore >> ignore >> loutput >> low
           >> ignore >> ignore >> ignore >> houtput >> high) {
      if (loutput != "output") {
        f.bots[botid].low_ = low;
      }
      if (houtput != "output") {
        f.bots[botid].high_ = high;
      }
    }
    //for (; f.abletran_;) {
    f.transfer(f.startid_);
    //fmt::print("tranid:{}\n", id);
    //if (l == 17 && h == 61) {
    //  fmt::print("id:{}\n", id);
    //  break;
    //}
    //}
  }
}


#ifndef AIKIDO_CONSTRAINT_DART_FRAMEDIFFERENTIABLE_HPP_
#define AIKIDO_CONSTRAINT_DART_FRAMEDIFFERENTIABLE_HPP_

#include <Eigen/Dense>
#include <dart/dynamics/dynamics.hpp>

#include "aikido/constraint/Differentiable.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"

namespace aikido {
namespace constraint {
namespace dart {

/// A pose constraint on _jacobianNode's transform
/// w.r.t. MetaSkeletonState of _jacobianNode.
/// _poseConstraint is
///     1) Differentiable
///     2) in SE3.
///     2) constrains _jacobianNode's pose in World Frame.
class FrameDifferentiable : public Differentiable
{
public:
  /// Constructor.
  /// \param _metaSkeletonStateSpace StateSpace whose state
  ///        defines _jacobianNode's transform.
  /// \param _metaskeleton MetaSkeleton to test with
  /// \param _jacobianNode The frame being constrained.
  /// \param _poseConstraint Constraint on _jacobian. This should be
  ///        in SE3.
  FrameDifferentiable(
      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
      ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
      ::dart::dynamics::ConstJacobianNodePtr _jacobianNode,
      DifferentiablePtr _poseConstraint);

  // Documentation inherited.
  std::size_t getConstraintDimension() const override;

  // Documentation inherited.
  void getValue(const statespace::StateSpace::State* _s, Eigen::VectorXd& _out)
      const override;

  /// Get jacobian of poseConstraint w.r.t. generalized coordinates.
  /// \param _s State to be evaluated at.
  /// \param[out] _out _m x numDofs, where m is the number of constraints.
  void getJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::MatrixXd& _out) const override;

  /// Get both Value and Jacobian evaluated at _s.
  /// This is more efficient than calling getValue and getJacobian separately
  /// because this sets MetaSkeleton's position only once.
  /// \param _s State to be evaluated at.
  /// \param[out] _val Value of constraints.
  /// \param[out] _jac Jacobian of constraints.
  void getValueAndJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::VectorXd& _val,
      Eigen::MatrixXd& _jac) const override;

  // Documentation inherited.
  std::vector<ConstraintType> getConstraintTypes() const override;

  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;

private:
  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  ::dart::dynamics::ConstJacobianNodePtr mJacobianNode;
  DifferentiablePtr mPoseConstraint;
};

} // namespace dart
} // namespace constraint
} // namespace aikido

#endif // AIKIDO_CONSTRAINT_DART_FRAMEDIFFERENTIABLE_HPP_


/*
 *  Copyright 2015 Aldebaran. All rights reserved.
 *
 */
#include <almath/scenegraph/urdfrigidbodysystemtraverser.h>
#include <almath/scenegraph/urdfeigen.h>
#include <boost/optional.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

namespace Builder = AL::RigidBodySystemBuilder;
typedef Builder::Interface<double>::Pose Pose;
typedef Builder::Interface<double>::BodyMass BodyMass;

namespace AL {

static const urdf::Array3d unit_x = {{1., 0., 0.}};
static const urdf::Array3d unit_y = {{0., 1., 0.}};
static const urdf::Array3d unit_z = {{0., 0., 1.}};

class UrdfRigidBodySystemTraverser : public urdf::RobotTree::JointConstVisitor {
 public:
  UrdfRigidBodySystemTraverser(Builder::Interface<double> &builder,
                               const urdf::RobotTree &parser)
      : builder(builder), parser(parser), removed_root_link(false) {}

  bool _discover(const std::string &parent_link_name,
                 const std::string &link_name, const Pose &origin,
                 Builder::JointType type, const urdf::ptree &link_pt,
                 const boost::optional<std::string> &joint_name);
  bool discover(const urdf::ptree &joint_el);

 private:
  Builder::Interface<double> &builder;
  const urdf::RobotTree &parser;
  boost::optional<std::string> urdf_root_link;
  bool removed_root_link;
};

bool UrdfRigidBodySystemTraverser::_discover(
    const std::string &parent_link_name, const std::string &link_name,
    const Pose &origin, Builder::JointType type, const urdf::ptree &link_pt,
    const boost::optional<std::string> &joint_name =
        boost::optional<std::string>()) {
  urdf::Link link(link_pt);
  if (!link.inertial()) return false;  // stop exploring this branch
  BodyMass mass;
  Math::urdfInertialToEigen(*(link.inertial()), mass.mass, mass.center_of_mass,
                            mass.rotational_inertia);
  if (joint_name)
    builder.add(parent_link_name, link_name, origin, type, mass, *joint_name);
  else
    builder.add(parent_link_name, link_name, origin, type, mass);
  return true;
}

bool UrdfRigidBodySystemTraverser::discover(const urdf::ptree &joint_el) {
  urdf::Joint joint(joint_el);
  Builder::JointType type;
  switch (joint.type()) {
    case urdf::Joint::fixed: {
      builder.add(joint.parent_link(), joint.child_link(),
                  Math::toEigenTransform(joint.origin()), joint.name());
      return true;
    }
    case urdf::Joint::prismatic:
    case urdf::Joint::planar:
      throw std::runtime_error("unsupported joint type");
    case urdf::Joint::revolute:
    case urdf::Joint::continuous: {
      const urdf::Array3d axis = joint.axis();
      if (axis == unit_x) {
        type = Builder::JointType::Rx;
        break;
      }
      if (axis == unit_y) {
        type = Builder::JointType::Ry;
        break;
      }
      if (axis == unit_z) {
        type = Builder::JointType::Rz;
        break;
      }
      throw std::runtime_error("unsupported joint axis");
    }
    case urdf::Joint::floating: {
      type = Builder::JointType::FreeFlyer;
      break;
    }
  }
  _discover(joint.parent_link(), joint.child_link(),
            Math::toEigenTransform(joint.origin()), type,
            parser.link(joint.child_link()), joint.name());
  return true;
}

void buildRigidBodySystemFromUrdf(
    RigidBodySystemBuilder::Interface<double> &builder, urdf::ptree &pt,
    bool remove_root_joint, bool make_continuous_joints_fixed) {
  urdf::RobotTree parser(pt.get_child("robot"));
  if (remove_root_joint) parser.rm_root_joint();
  if (make_continuous_joints_fixed) makeContinuousJointsFixed(parser);
  makeMasslessJointsFixed(parser);
  UrdfRigidBodySystemTraverser visitor(builder, parser);
  visitor._discover("", parser.root_link(), Pose::Identity(),
                    Builder::JointType::FreeFlyer,
                    parser.link(parser.root_link()));
  parser.traverse_joints(visitor);
}

void buildRigidBodySystemFromUrdf(
    RigidBodySystemBuilder::Interface<double> &builder, std::istream &is,
    bool remove_root_joint, bool make_continuous_joints_fixed) {
  AL::urdf::ptree pt;
  boost::property_tree::xml_parser::read_xml(is, pt);
  buildRigidBodySystemFromUrdf(builder, pt, remove_root_joint,
                               make_continuous_joints_fixed);
}
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_DEFINITION_IDIV_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_IDIV_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/function/definition/ceil.hpp>
#include <boost/simd/function/idivceil.hpp>
#include <boost/simd/function/definition/floor.hpp>
#include <boost/simd/function/idivfloor.hpp>
#include <boost/simd/function/definition/round.hpp>
#include <boost/simd/function/idivround.hpp>
#include <boost/simd/function/definition/round2even.hpp>
#include <boost/simd/function/idivround2even.hpp>
#include <boost/simd/function/definition/fix.hpp>
#include <boost/simd/function/idivfix.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, idiv_, boost::dispatch::elementwise_<idiv_>);
  }
  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, idiv_);
  }
  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::idiv_,idiv);
  }

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  template < typename T>
  BOOST_FORCEINLINE auto idiv(T const& a, T const& b )
    BOOST_NOEXCEPT_DECLTYPE(bs::idivfix(a, b))
  {
    return bs::idivfix(a, b);
  }

#define BOOST_SIMD_IDIV_WITH_OPTION(option)                      \
  template < typename T>                                         \
  BOOST_FORCEINLINE auto idiv(T const& a, T const& b             \
                             , bd::functor<bs::tag::option##_> const& ) \
    BOOST_NOEXCEPT_DECLTYPE(bs::idiv##option(a, b))              \
  {                                                              \
    return idiv##option(a, b);                                   \
  }                                                              \
/**/

BOOST_SIMD_IDIV_WITH_OPTION(ceil)
BOOST_SIMD_IDIV_WITH_OPTION(fix)
BOOST_SIMD_IDIV_WITH_OPTION(floor)
BOOST_SIMD_IDIV_WITH_OPTION(round)
BOOST_SIMD_IDIV_WITH_OPTION(round2even)

} }

#endif


#include <tiny.h>
#include <narrow.h>

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

typedef tiny::MathTypes<float>   M;
typedef M::real_type             T;
typedef M::vector3_type          V;
typedef M::quaternion_type       Q;
typedef M::coordsys_type         C;

class MyObject : public narrow::Object< M > {};

typedef narrow::Geometry< M >          geometry_type;

typedef geometry_type::box_type        box_type;
typedef geometry_type::convex_type     convex_type;
typedef geometry_type::sphere_type     sphere_type;
typedef geometry_type::tetramesh_type  tetramesh_type;

class MyCallback
  : public geometry::ContactsCallback<V>
{
public:

  bool m_hit;

  MyCallback()
  : m_hit(false)
  {}

  void operator()(
                  V const & point
                  , V const & normal
                  , V::real_type const & distance
                  )
  {
    m_hit = true;
  }

};

/**
 * Test Two Shapes Function.
 * This is a convenience function that makes it more easy to write test cases.
 */
template<typename shape_typeA, typename shape_typeB>
inline void do_test(
                    V posA
                    , V posB
                    , Q rotA
                    , Q rotB
                    , shape_typeA const & shapeA
                    , shape_typeB const & shapeB
                    , bool const & should_overlap
                    )
{
  narrow::System<M> system;

  size_t const geoA_idx = system.create_geometry();
  size_t const geoB_idx = system.create_geometry();

  MyObject objA;
  MyObject objB;

  objA.set_geometry_idx( geoA_idx );
  objB.set_geometry_idx( geoB_idx );

  geometry_type  & geoA = system.get_geometry(objA);
  geometry_type  & geoB = system.get_geometry(objB);

  geoA.add_shape( shapeA );
  geoB.add_shape( shapeB );

  // The part in the block below is specific for tetrahedra  meshes.
  //
  // Their BVH needs to be created in the "object" prior to using
  // them (as pre-processing before simulation) by using make_kdop_bvh. During
  // run-time while simulating their BVH must be updated to reflect positional
  // changes. This is done using the update_kdop_bvh.
  {
    std::vector< narrow::KDopBvhUpdateWorkItem< M > > work_pool;

    if( geoA.m_tetramesh.has_data())
    {
      narrow::make_kdop_bvh( system.params(), objA, geoA);
      narrow::KDopBvhUpdateWorkItem<M> const work = narrow::KDopBvhUpdateWorkItem<M>( objA, geoA, posA, rotA );

      work_pool.push_back(work);
    }
    if( geoB.m_tetramesh.has_data())
    {
      narrow::make_kdop_bvh( system.params(), objB, geoB);

      narrow::KDopBvhUpdateWorkItem<M> const work = narrow::KDopBvhUpdateWorkItem<M>( objB, geoB, posB, rotB );
      work_pool.push_back(work);
    }

    if( work_pool.size() )
    {
      narrow::update_kdop_bvh(work_pool, narrow::sequential() );
    }
  }

  MyCallback callback;

  narrow::TestPair<M> test_pair = narrow::TestPair<M>( objA, objB, posA, rotA, posB, rotB, callback );

  std::vector<narrow::TestPair<M> > test_pairs;
  test_pairs.push_back(test_pair);

  narrow::dispatch_collision_handlers( system, test_pairs );

  BOOST_CHECK_EQUAL(callback.m_hit, should_overlap);

}

BOOST_AUTO_TEST_SUITE(narrow);

BOOST_AUTO_TEST_CASE(dispatch_sphere_box_test)
{
  sphere_type shapeA;
  shapeA.radius()    = 1.0f;
  shapeA.transform() = C::identity();

  box_type shapeB;
  shapeB.half_extent()(0) = 1.0f;
  shapeB.half_extent()(1) = 1.0f;
  shapeB.half_extent()(2) = 1.0f;

  shapeB.transform() = C::identity();

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_sphere_sphere_test)
{
  sphere_type shapeA;
  shapeA.radius()    = 1.0f;
  shapeA.transform() = C::identity();

  sphere_type shapeB;
  shapeB.radius()    = 1.0f;
  shapeB.transform() = C::identity();

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_box_box_test)
{
  box_type shapeA;
  shapeA.half_extent()(0) = 1.0f;
  shapeA.half_extent()(1) = 1.0f;
  shapeA.half_extent()(2) = 1.0f;
  shapeA.transform()      = C::identity();

  box_type shapeB;
  shapeB.half_extent()(0) = 1.0f;
  shapeB.half_extent()(1) = 1.0f;
  shapeB.half_extent()(2) = 1.0f;
  shapeB.transform()      = C::identity();

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_box_sphere_test)
{
  box_type shapeA;
  shapeA.half_extent()(0) = 1.0f;
  shapeA.half_extent()(1) = 1.0f;
  shapeA.half_extent()(2) = 1.0f;
  shapeA.transform()      = C::identity();

  sphere_type shapeB;
  shapeB.radius()    = 1.0f;
  shapeB.transform() = C::identity();

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_tetmesh_tetmesh_test)
{
  mesh_array::T3Mesh surfaceA;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sXA;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sYA;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZA;

  mesh_array::make_box<M>( 2.0f, 2.0f, 2.0f, surfaceA, sXA, sYA, sZA);

  mesh_array::T4Mesh mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z;

  mesh_array::tetgen(surfaceA, sXA, sYA, sZA, mesh, X, Y, Z);

  tetramesh_type shapeA;
  tetramesh_type shapeB;

  shapeA.set_tetramesh_shape(mesh, X, Y, Z);
  shapeB.set_tetramesh_shape(mesh, X, Y, Z);

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_sphere_tetmesh_test)
{
  sphere_type shapeA;
  shapeA.radius()    = 1.0f;
  shapeA.transform() = C::identity();

  tetramesh_type shapeB;

  mesh_array::T3Mesh surface;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  mesh_array::make_box<M>( 2.0f, 2.0f, 2.0f, surface, sX, sY, sZ);

  mesh_array::T4Mesh mesh;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> X;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Y;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Z;

  mesh_array::tetgen(surface, sX, sY, sZ, mesh, X, Y, Z);

  shapeB.set_tetramesh_shape(mesh, X, Y, Z);

  {
    V posA = V::make(  1.0f, 0.0f, 0.0f);
    V posB = V::make( -0.9f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make( 1.1f, 0.0f, 0.0f);
    V posB = V::make(-1.1f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_CASE(dispatch_convex_convex_test)
{
  convex_type shapeA;
  shapeA.data().add_point( V::make( 1.0f,  1.0f,  1.0f ) );
  shapeA.data().add_point( V::make(-1.0f,  1.0f,  1.0f ) );
  shapeA.data().add_point( V::make( 1.0f, -1.0f,  1.0f ) );
  shapeA.data().add_point( V::make(-1.0f, -1.0f,  1.0f ) );
  shapeA.data().add_point( V::make( 1.0f,  1.0f, -1.0f ) );
  shapeA.data().add_point( V::make(-1.0f,  1.0f, -1.0f ) );
  shapeA.data().add_point( V::make( 1.0f, -1.0f, -1.0f ) );
  shapeA.data().add_point( V::make(-1.0f, -1.0f, -1.0f ) );
  shapeA.transform() = C::identity();

  convex_type shapeB;
  shapeB.data().add_point( V::make( 1.0f,  1.0f,  1.0f ) );
  shapeB.data().add_point( V::make(-1.0f,  1.0f,  1.0f ) );
  shapeB.data().add_point( V::make( 1.0f, -1.0f,  1.0f ) );
  shapeB.data().add_point( V::make(-1.0f, -1.0f,  1.0f ) );
  shapeB.data().add_point( V::make( 1.0f,  1.0f, -1.0f ) );
  shapeB.data().add_point( V::make(-1.0f,  1.0f, -1.0f ) );
  shapeB.data().add_point( V::make( 1.0f, -1.0f, -1.0f ) );
  shapeB.data().add_point( V::make(-1.0f, -1.0f, -1.0f ) );
  shapeB.transform() = C::identity();

  {
    V posA = V::make(-1.0f, 0.0f, 0.0f);
    V posB = V::make( 1.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    // 2015-11-30 Kenny: Convex types not yet supported
    //    do_test( posA, posB, rotA, rotB, shapeA, shapeB, true);
  }

  {
    V posA = V::make(-2.0f, 0.0f, 0.0f);
    V posB = V::make( 2.0f, 0.0f, 0.0f);
    Q rotA = Q::identity();
    Q rotB = Q::identity();
    // 2015-11-30 Kenny: Convex types not yet supported
    // do_test( posA, posB, rotA, rotB, shapeA, shapeB, false);
  }
}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.

#ifndef poplibs_test_GeneralMatrixAdd_hpp
#define poplibs_test_GeneralMatrixAdd_hpp

#include <boost/multi_array.hpp>

namespace poplibs_test {
namespace axpby {

/*
 * Computes matD = alpha * op(matA) + beta * op(matB)
 *
 * where op(matA) = A     if transposeA = false
 *       op(matA) = A'    if transposeA = true
 *
 *
 *       op(matB) = B     if transposeB = false
 *       op(matB) = B'    if transposeB = true
 *
 * Matrix dimensions of op(A) must be equal to op(B)
 */

void add(const boost::multi_array_ref<double, 3> matA,
         const boost::multi_array_ref<double, 3> matB,
         boost::multi_array_ref<double, 3> matC, float alpha = 1.0,
         float beta = 1.0);

void add(const boost::multi_array_ref<double, 2> matA,
         const boost::multi_array_ref<double, 2> matB,
         boost::multi_array_ref<double, 2> matC, float alpha = 1.0,
         float beta = 1.0, bool transposeA = false, bool transposeB = false);

void add(const boost::multi_array_ref<double, 1> matA,
         const boost::multi_array_ref<double, 1> matB,
         boost::multi_array_ref<double, 1> matC, float alpha = 1.0,
         float beta = 1.0);

} // End namespace axpby.
} // End namespace poplibs_test.

#endif // poplibs_test_GeneralMatrixAdd_hpp


#include "contour_optimization.h"

#include "session_data.h"
#include "frame_data.h"
#include "focal_grid.h"
#include "image_segmentation.h"
#include "model_class.h"

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <string>
#include <stdint.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <list>
#include <thread>
#include <chrono>
#include <algorithm>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <armadillo>

#include <nlopt.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/algorithm.h>
#include <CGAL/assertions.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Polygon_set_2.h>

#define PI 3.14159265

using namespace std;

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

typedef CGAL::Exact_predicates_inexact_constructions_kernel KI;
//typedef CGAL::Exact_predicates_exact_constructions_kernel KE;

typedef KI::Point_2 				   Point_2;
typedef CGAL::Polygon_2<KI>            Polygon_2;
typedef CGAL::Polygon_with_holes_2<KI> Polygon_with_holes_2;
typedef CGAL::Polygon_set_2<KI> 	   Polygon_set_2;

typedef struct {
	FocalGrid* fg;
	ContourOpt* opt;
	ModelClass* mdl;
	int N_cam;
	const vector<list<Polygon_with_holes_2>> body_dest;
	const vector<list<Polygon_with_holes_2>> wing_L_dest;
	const vector<list<Polygon_with_holes_2>> wing_R_dest;
} cost_func_data;

double cost_function(const vector<double> &x, vector<double> &grad, void *data) {

	clock_t start;
	double duration;

	start = clock();

	cost_func_data *d = reinterpret_cast<cost_func_data*>(data);
	FocalGrid* fg = d->fg;
	ContourOpt* opt = d->opt;
	ModelClass* mdl = d->mdl;
	int N_cam = d->N_cam;
	//vector<list<Polygon_with_holes_2>> body_dest = d->body_dest;
	//vector<list<Polygon_with_holes_2>> wing_L_dest = d->wing_L_dest;
	//vector<list<Polygon_with_holes_2>> wing_R_dest = d->wing_R_dest;

	vector<double> state_now;
	state_now.push_back(x[0]);
	state_now.push_back(x[1]);
	state_now.push_back(x[2]);
	state_now.push_back(x[3]);
	state_now.push_back(x[4]);
	state_now.push_back(x[5]);
	state_now.push_back(x[6]);
	state_now.push_back(x[7]);
	state_now.push_back(x[8]);
	state_now.push_back(x[9]);
	state_now.push_back(x[10]);
	state_now.push_back(x[11]);
	state_now.push_back(x[12]);
	state_now.push_back(x[13]);
	state_now.push_back(x[14]);

	vector<list<Polygon_with_holes_2>> body_src;
	vector<list<Polygon_with_holes_2>> wing_L_src;
	vector<list<Polygon_with_holes_2>> wing_R_src;

	vector<list<Polygon_with_holes_2>> body_dest;
	vector<list<Polygon_with_holes_2>> wing_L_dest;
	vector<list<Polygon_with_holes_2>> wing_R_dest;

	// Convert state x to affine matrix transforms:
	cout << "return src state" << endl;
	vector<double> src_state = mdl->ReturnSRCState(state_now);
	cout << "size src state" << src_state.size() << endl;

	cout << "get src contours" << endl;
	for (int i=0; i<N_cam; i++) {
		cout << "get silhouette" << endl;
		vector<Polygon_2> m_silhouette = mdl->ReturnSilhouette2(*fg,src_state,i);
		cout << "size silhouette" << m_silhouette.size() << endl;
		vector<Polygon_2> m_body;
		for (int j=0; j<3; j++) {
			m_body.push_back(m_silhouette[j]);
		}
		list<Polygon_with_holes_2> body_contour_poly = opt->CalcUnion(m_body);
		body_src.push_back(body_contour_poly);
		body_dest.push_back(d->body_dest[i]);
		vector<Polygon_2> m_wing_L;
		m_wing_L.push_back(m_silhouette[3]);
		list<Polygon_with_holes_2> wing_L_contour_poly = opt->CalcUnion(m_wing_L);
		list<Polygon_with_holes_2> wing_L_complement = opt->CalcComplement(body_contour_poly, wing_L_contour_poly);
		wing_L_src.push_back(wing_L_complement);
		wing_L_dest.push_back(d->wing_L_dest[i]);
		vector<Polygon_2> m_wing_R;
		m_wing_R.push_back(m_silhouette[4]);
		list<Polygon_with_holes_2> wing_R_contour_poly = opt->CalcUnion(m_wing_R);
		list<Polygon_with_holes_2> wing_R_complement = opt->CalcComplement(body_contour_poly, wing_R_contour_poly);
		wing_R_src.push_back(wing_R_complement);
		wing_R_dest.push_back(d->wing_R_dest[i]);
	}

	// Calculate the cost by calculating the symmetric difference per view per polygon list:
	double cost = 0.0;

	list<Polygon_with_holes_2> body_diff;
	list<Polygon_with_holes_2> wing_L_diff;
	list<Polygon_with_holes_2> wing_R_diff;

	cout << "body src vec size: " << body_src.size() << endl;
	cout << "body dest vec size: " << body_dest.size() << endl;

	for (int i=0; i<N_cam; i++) {
		cout << "i " << i << endl;
		if (body_dest[i].size()>0 && body_src[i].size()>0) {
			//body_diff.clear();
			cout << "Calc symmetric difference body " << endl;
			list<Polygon_with_holes_2> body_diff = opt->CalcSymmetricDifference(body_dest[i],body_src[i]);
			if (body_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it1 = body_diff.begin(); it1 != body_diff.end(); ++it1) {
					cout << "area: " << it1->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		/*
		if (wing_L_dest[i].size()>0 && wing_L_src[i].size()>0) {
			//wing_L_diff.clear();
			cout << "Calc symmetric difference wing_L " << endl;
			list<Polygon_with_holes_2> wing_L_diff = opt->CalcSymmetricDifference(wing_L_dest[i],wing_L_src[i]);
			if (wing_L_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it2 = wing_L_diff.begin(); it2 != wing_L_diff.end(); ++it2) {
					cout << "area: " << it2->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		cout << "dest polygons size " << wing_R_dest[i].size() << endl;
		cout << "src polygons size "<< wing_R_src[i].size() << endl;
		if (wing_R_dest[i].size()>0 && wing_R_src[i].size()>0) {
			//wing_R_diff.clear();
			cout << "wing_R_dest" << endl;
			cout << wing_R_dest[i].front() << endl;
			cout << "wing_R_src" << endl;
			cout << wing_R_src[i].front() << endl;
			cout << "Calc symmetric difference wing_R " << endl;
			list<Polygon_with_holes_2> wing_R_diff = opt->CalcSymmetricDifference(wing_R_dest[i],wing_R_src[i]);
			cout << "wing_R_diff size " << wing_R_diff.size() << endl;
			if (wing_R_diff.size() > 0) {
				for (list<Polygon_with_holes_2>::iterator it3 = wing_R_diff.begin(); it3 != wing_R_diff.end(); ++it3) {
					cout << "area: " << it3->outer_boundary().area() << endl;
					//cost += it->outer_boundary().area();
				}
			}
		}
		*/
	}

	cout << "cost: " << cost << endl;
	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';

	return cost;
}

double quat_constraint_body(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2));
	return (1.0-q_norm);
}

double quat_constraint_wing_L(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2));
	return (1.0-q_norm);
}

double quat_constraint_wing_R(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	double q_norm = sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2));
	return (1.0-q_norm);
}

/*
void Optimize(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ContourOpt &opt) {

	int N_cam = frame_in.N_cam;

	cost_func_data cost_data = {fg, opt, mdl, N_cam, opt.body_dest_polygons, opt.wing_L_dest_polygons, opt.wing_R_dest_polygons};

    vector<double> lb(15);
    lb[0] = -1.0;
	lb[1] = -1.0;
	lb[2] = -1.0;
	lb[3] = -1.0;
	lb[4] = -10.0;
	lb[5] = -10.0;
	lb[6] = -10.0;
	lb[7] = -1.0;
	lb[8] = -1.0;
	lb[9] = -1.0;
	lb[10] = -1.0;
	lb[11] = -1.0;
	lb[12] = -1.0;
	lb[13] = -1.0;
	lb[14] = -1.0;

	vector<double> ub(15);
	ub[0] = -1.0;
	ub[1] = -1.0;
	ub[2] = -1.0;
	ub[3] = -1.0;
	ub[4] = -10.0;
	ub[5] = -10.0;
	ub[6] = -10.0;
	ub[7] = -1.0;
	ub[8] = -1.0;
	ub[9] = -1.0;
	ub[10] = -1.0;
	ub[11] = -1.0;
	ub[12] = -1.0;
	ub[13] = -1.0;
	ub[14] = -1.0;

	vector<double> x(15);
	x[0] = frame_in.init_state(0,0);
	x[1] = frame_in.init_state(1,0);
	x[2] = frame_in.init_state(2,0);
	x[3] = frame_in.init_state(3,0);
	x[4] = frame_in.init_state(4,0);
	x[5] = frame_in.init_state(5,0);
	x[6] = frame_in.init_state(6,0);
	x[7] = frame_in.init_state(0,1);
	x[8] = frame_in.init_state(1,1);
	x[9] = frame_in.init_state(2,1);
	x[10] = frame_in.init_state(3,1);
	x[11] = frame_in.init_state(0,2);
	x[12] = frame_in.init_state(1,2);
	x[13] = frame_in.init_state(2,2);
	x[14] = frame_in.init_state(3,2);

	nlopt::opt opt_alg(nlopt::LN_COBYLA, 15);
	opt_alg.set_lower_bounds(lb);
	opt_alg.set_upper_bounds(ub);
	opt_alg.set_min_objective(cost_function, &cost_data);
	opt_alg.add_equality_constraint(quat_constraint_body, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_L, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_R, NULL, 1e-6);
	opt_alg.set_xtol_rel(1e-5);

	cout << " -------------------- " << endl;
	cout << "x start: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	double minf;

	nlopt::result result = opt_alg.optimize(x, minf);

	cout << "x opt: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	cout << "quaternion norm body: " << sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2)) << endl;
	cout << "quaternion norm wing L: " << sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2)) << endl;
	cout << "quaternion norm wing R: " << sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2)) << endl;
	cout << " -------------------- " << endl;
	cout << endl;
}
*/

ContourOpt::ContourOpt() {
	// Empty
}

void ContourOpt::OptimizeState(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ImagSegm &seg) {
	ContourOpt::FindDestinationContour(fg, frame_in, seg);
	ContourOpt::FindInitContour(fg, frame_in, mdl, seg);
	ContourOpt::Optimize(fg, frame_in, mdl);
}

void ContourOpt::Optimize(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl) {

	int N_cam = frame_in.N_cam;

	cost_func_data cost_data = {&fg, this, &mdl, N_cam, body_dest_polygons, wing_L_dest_polygons, wing_R_dest_polygons};

    vector<double> lb(15);
    lb[0] = -1.0;
	lb[1] = -1.0;
	lb[2] = -1.0;
	lb[3] = -1.0;
	lb[4] = -10.0;
	lb[5] = -10.0;
	lb[6] = -10.0;
	lb[7] = -1.0;
	lb[8] = -1.0;
	lb[9] = -1.0;
	lb[10] = -1.0;
	lb[11] = -1.0;
	lb[12] = -1.0;
	lb[13] = -1.0;
	lb[14] = -1.0;

	vector<double> ub(15);
	ub[0] = 1.0;
	ub[1] = 1.0;
	ub[2] = 1.0;
	ub[3] = 1.0;
	ub[4] = 10.0;
	ub[5] = 10.0;
	ub[6] = 10.0;
	ub[7] = 1.0;
	ub[8] = 1.0;
	ub[9] = 1.0;
	ub[10] = 1.0;
	ub[11] = 1.0;
	ub[12] = 1.0;
	ub[13] = 1.0;
	ub[14] = 1.0;

	vector<double> x(15);
	x[0] = frame_in.init_state(0,0);
	x[1] = frame_in.init_state(1,0);
	x[2] = frame_in.init_state(2,0);
	x[3] = frame_in.init_state(3,0);
	x[4] = frame_in.init_state(4,0);
	x[5] = frame_in.init_state(5,0);
	x[6] = frame_in.init_state(6,0);
	x[7] = frame_in.init_state(0,1);
	x[8] = frame_in.init_state(1,1);
	x[9] = frame_in.init_state(2,1);
	x[10] = frame_in.init_state(3,1);
	x[11] = frame_in.init_state(0,2);
	x[12] = frame_in.init_state(1,2);
	x[13] = frame_in.init_state(2,2);
	x[14] = frame_in.init_state(3,2);

	nlopt::opt opt_alg(nlopt::LN_COBYLA, 15);
	opt_alg.set_lower_bounds(lb);
	opt_alg.set_upper_bounds(ub);
	opt_alg.set_min_objective(cost_function, &cost_data);
	opt_alg.add_equality_constraint(quat_constraint_body, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_L, NULL, 1e-6);
	opt_alg.add_equality_constraint(quat_constraint_wing_R, NULL, 1e-6);
	opt_alg.set_xtol_rel(1e-5);

	cout << " -------------------- " << endl;
	cout << "x start: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	double minf;

	nlopt::result result = opt_alg.optimize(x, minf);

	cout << "x opt: " << x[0] << ", " << x[1] << ", " << x[2] << ", " << x[3] << ", " << x[4]
		<< ", " << x[5] << ", " << x[6] << endl;
	cout << x[7] << ", " << x[8] << ", " << x[9] << ", " << x[10] << endl;
	cout << x[11] << ", " << x[12] << ", " << x[13] << ", " << x[14] << endl;

	cout << "quaternion norm body: " << sqrt(pow(x[0],2)+pow(x[1],2)+pow(x[2],2)+pow(x[3],2)) << endl;
	cout << "quaternion norm wing L: " << sqrt(pow(x[7],2)+pow(x[8],2)+pow(x[9],2)+pow(x[10],2)) << endl;
	cout << "quaternion norm wing R: " << sqrt(pow(x[11],2)+pow(x[12],2)+pow(x[13],2)+pow(x[14],2)) << endl;
	cout << " -------------------- " << endl;
	cout << endl;
}

void ContourOpt::FindDestinationContour(FocalGrid &fg, frame_data &frame_in, ImagSegm &seg) {

	clock_t start;
	double duration;

	start = clock();

	body_dest_polygons.clear();
	wing_L_dest_polygons.clear();
	wing_R_dest_polygons.clear();
	body_dest_contours.clear();
	wing_L_dest_contours.clear();
	wing_R_dest_contours.clear();

	int N_cam = frame_in.N_cam;

	// Get the body and wing pointclouds
	arma::uvec body_pt_ids   = arma::find(frame_in.body_and_wing_pcls.row(3) == 1.0);
	arma::uvec wing_L_pt_ids = arma::find(frame_in.body_and_wing_pcls.row(3) == 2.0);
	arma::uvec wing_R_pt_ids = arma::find(frame_in.body_and_wing_pcls.row(3) == 3.0);

	arma::Mat<double> body_pcl = frame_in.body_and_wing_pcls.cols(body_pt_ids);
	arma::Mat<double> wing_L_pcl = frame_in.body_and_wing_pcls.cols(wing_L_pt_ids);
	arma::Mat<double> wing_R_pcl = frame_in.body_and_wing_pcls.cols(wing_R_pt_ids);

	//arma::Mat<double> means = ContourOpt::KMeans(body_pcl, 3, 10);

	//cout << means << endl;

	vector<arma::Col<int>> body_proj = fg.FocalGrid::ProjectCloud2Frames(body_pcl);
	vector<arma::Col<int>> wing_L_proj = fg.FocalGrid::ProjectCloud2Frames(wing_L_pcl);
	vector<arma::Col<int>> wing_R_proj = fg.FocalGrid::ProjectCloud2Frames(wing_R_pcl);

	vector<vector<arma::Mat<double>>> contour_vec = ContourOpt::FindContours(body_proj, wing_L_proj, wing_R_proj, frame_in, seg);

	for (int i=0; i<N_cam; i++) {
		body_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][0]));
		wing_L_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][1]));
		wing_R_dest_polygons.push_back(ContourOpt::Convert2PolygonWithHoles(contour_vec[i][2]));
		body_dest_contours.push_back(contour_vec[i][0]);
		wing_L_dest_contours.push_back(contour_vec[i][1]);
		wing_R_dest_contours.push_back(contour_vec[i][2]);
	}

	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';

}

arma::Mat<double> ContourOpt::KMeans(arma::Mat<double> &pcl_in, int N_clusters, int N_iter) {

	arma::Mat<double> data = pcl_in.rows(0,2);

	arma::Mat<double> means;

	bool status =arma::kmeans(means,data,N_clusters,arma::random_subset,N_iter,false);

	if (status==false) {
		cout << "clustering failed" << endl;
	}

	return means;
}

void ContourOpt::FindInitContour(FocalGrid &fg, frame_data &frame_in, ModelClass &mdl, ImagSegm &seg) {

	clock_t start;
	double duration;

	start = clock();

	body_init_contours.clear();
	wing_L_init_contours.clear();
	wing_R_init_contours.clear();

	int N_cam = frame_in.N_cam;

	// Get model pcl's in initial state:

	vector<arma::Mat<double>> M_init_vec = mdl.ReturnInitState(frame_in);

	for (int i=0; i<N_cam; i++) {
		vector<Polygon_2> m_silhouette = mdl.ReturnSilhouette(fg, M_init_vec,fg.CalculateViewVector(i),i);
		vector<Polygon_2> m_body;
		for (int j=0; j<3; j++) {
			m_body.push_back(m_silhouette[j]);
		}
		list<Polygon_with_holes_2> body_contour_poly = ContourOpt::CalcUnion(m_body);
		body_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(body_contour_poly));
		vector<Polygon_2> m_wing_L;
		m_wing_L.push_back(m_silhouette[3]);
		list<Polygon_with_holes_2> wing_L_contour_poly = ContourOpt::CalcUnion(m_wing_L);
		list<Polygon_with_holes_2> wing_L_complement = ContourOpt::CalcComplement(body_contour_poly, wing_L_contour_poly);
		wing_L_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(wing_L_complement));
		vector<Polygon_2> m_wing_R;
		m_wing_R.push_back(m_silhouette[4]);
		list<Polygon_with_holes_2> wing_R_contour_poly = ContourOpt::CalcUnion(m_wing_R);
		list<Polygon_with_holes_2> wing_R_complement = ContourOpt::CalcComplement(body_contour_poly, wing_R_contour_poly);
		wing_R_init_contours.push_back(ContourOpt::ConvertPolygonWithHoles2Mat(wing_R_complement));
	}

	duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
    cout<<"printf: "<< duration <<'\n';
}

vector<vector<arma::Mat<double>>> ContourOpt::FindContours(vector<arma::Col<int>> &body_proj, vector<arma::Col<int>> &wing_L_proj, vector<arma::Col<int>> &wing_R_proj, frame_data &frame_in, ImagSegm &seg) {

	int N_cam = frame_in.N_cam;

	vector<vector<arma::Mat<double>>> contour_vec_out;

	vector<arma::Mat<double>> contour_vec_now;

	arma::Mat<double> contour_now;

	arma::Col<int> proj_body_img;
	arma::Col<int> proj_wing_L_img;
	arma::Col<int> proj_wing_R_img;

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

		contour_vec_now.clear();

		int N_row = get<0>(frame_in.image_size[i]);
		int N_col = get<1>(frame_in.image_size[i]);

		proj_body_img = body_proj[i];
		proj_wing_L_img = wing_L_proj[i]%(1-body_proj[i]);
		proj_wing_R_img = wing_R_proj[i]%(1-body_proj[i]);

		contour_vec_now.push_back(seg.FindContours(proj_body_img,N_row,N_col));
		contour_vec_now.push_back(seg.FindContours(proj_wing_L_img,N_row,N_col));
		contour_vec_now.push_back(seg.FindContours(proj_wing_R_img,N_row,N_col));

		contour_vec_out.push_back(contour_vec_now);
	}
	return contour_vec_out;
}

arma::Mat<double> ContourOpt::CalculateContour(arma::Mat<double> &pts_in, double d_u, double d_v) {

	int N_pts = pts_in.n_cols;

	// find starting point (minimum u index):

	arma::Col<double> uv_start = pts_in.col(arma::index_min(pts_in.row(0)));

	arma::Col<double> uv_now = uv_start;

	arma::Col<double> uv_prev = uv_start;

	arma::uvec inds_u_plus;
	arma::uvec inds_u_min;
	arma::uvec inds_v_plus;
	arma::uvec inds_v_min;

	arma::uvec inds_neighbors;

	double theta_now;

	double theta_min;

	int theta_min_ind;

	arma::Mat<double> contour_pts;

	contour_pts = uv_start;

	int iter = 0;

	int N_contour_pts = 0;

	bool contour_closed = false;

	while (iter<N_pts && contour_closed==false) {

		inds_u_plus = arma::find(pts_in.row(0)>(uv_now(0)-d_u));
		inds_u_min = arma::find(pts_in.row(0)<(uv_now(0)+d_u));
		inds_v_plus = arma::find(pts_in.row(1)>(uv_now(1)-d_v));
		inds_v_min = arma::find(pts_in.row(1)<(uv_now(1)+d_v));

		inds_neighbors = arma::intersect(arma::intersect(inds_u_plus,inds_u_min),arma::intersect(inds_v_plus,inds_v_min));

		if (inds_neighbors.n_rows>0) {

			theta_min = 2*PI;
			theta_min_ind = -1;

			for (int i=0; i<inds_neighbors.n_rows; i++) {
				theta_now = atan2(pts_in(1,inds_neighbors(i))-uv_prev(1),pts_in(0,inds_neighbors(i))-uv_prev(0));
				if (isnan(theta_now)==0) {
					if (theta_now<theta_min) {
						theta_min = theta_now;
						theta_min_ind = i;
					}
				}
			}

			uv_prev = uv_now;

			uv_now = pts_in.col(inds_neighbors(theta_min_ind));

			contour_pts.insert_cols(N_contour_pts,uv_now);
			N_contour_pts++;
			if (uv_now(0)==uv_start(0) && uv_now(1)==uv_start(1)) {
				contour_closed = true;
			}
		}
		else {
			iter = N_pts;
			cout << "could not close contour" << endl;
		}
		iter++;
	}

	return contour_pts;
}

list<Polygon_with_holes_2> ContourOpt::CalcUnion(vector<Polygon_2> &polygons_in) {

	int N_c = polygons_in.size();

	Polygon_set_2 S;

	for (int i=0; i<N_c; i++) {
		if (i==0) {
			S.insert(polygons_in[i]);
		}
		else {
			S.join(polygons_in[i]);
		}
	}

	//S.union()

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));

	return res;
}

list<Polygon_with_holes_2> ContourOpt::CalcComplement(list<Polygon_with_holes_2> &pol_A, list<Polygon_with_holes_2> &pol_B) {

	Polygon_set_2 S;

	int iter_1 =0;
	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
		//cout << "pol B area " << it_1->outer_boundary().area() << endl;
		if (iter_1==0) {
			S.insert(*it_1);
		}
		else {
			S.join(*it_1);
		}
		iter_1++;
	}

	int iter_2 = 0;
	for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
		//cout << "pol A area " << it_2->outer_boundary().area() << endl;
		S.difference(*it_2);
	}

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));

	return res;
}

list<Polygon_with_holes_2> ContourOpt::CalcSymmetricDifference(list<Polygon_with_holes_2> &pol_A, list<Polygon_with_holes_2> &pol_B) {

	/*
	Polygon_set_2 S;

	int iter_1 =0;
	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
		if (iter_1==0) {
			S.insert(*it_1);
		}
		else {
			S.join(*it_1);
		}
		iter_1++;
	}

	int iter_2 = 0;
	for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
		S.symmetric_difference(*it_2);
	}

	list<Polygon_with_holes_2> res;
	S.polygons_with_holes (back_inserter (res));
	*/
	//cout << "caclulating symmetric difference" << endl;
	list<Polygon_with_holes_2> res_out;

	//if (!pol_A.empty() && !pol_B.empty()){
  	for (list<Polygon_with_holes_2>::iterator it_1=pol_B.begin(); it_1 !=pol_B.end(); ++it_1) {
  		for (list<Polygon_with_holes_2>::iterator it_2=pol_A.begin(); it_2 !=pol_A.end(); ++it_2) {
  			//if (CGAL::do_intersect(*it_1, *it_2)) {
  			//cout << "do intersect " << CGAL::do_intersect(*it_1, *it_2) << endl;
			//cout << "area pol_A " << it_2->outer_boundary().area() << endl;
			//cout << "area pol_B " << it_1->outer_boundary().area() << endl;
			CGAL::symmetric_difference (*it_1, *it_2, back_inserter(res_out));
			//}
			//else {
			//	cout << "do not intersect " << endl;
			//}
		}
	}
	//}

	cout << "Res size: " << res_out.size() << endl;

	return res_out;
}

arma::Mat<double> ContourOpt::ConvertPolygonWithHoles2Mat(list<Polygon_with_holes_2> &Pol_list_in) {

	int N_poly = Pol_list_in.size();
	int N_v;
	arma::Mat<double> mat_out;

	if (N_poly>0) {
		int iter = 0;
		vector<double> output_iterator;
		for (list<Polygon_with_holes_2>::iterator it=Pol_list_in.begin(); it !=Pol_list_in.end(); ++it) {
			//cout << it->is_unbounded() << endl;
			N_v = it->outer_boundary().size();
			arma::Mat<double> curr_pol_mat(3,N_v);
			for (int j=0; j<N_v; j++) {
				curr_pol_mat(0,j) = static_cast<double>(it->outer_boundary().vertex(j).x());
				curr_pol_mat(1,j) = static_cast<double>(it->outer_boundary().vertex(j).y());
				curr_pol_mat(2,j) = iter+1;
			}
			if (iter==0) {
				mat_out = curr_pol_mat;
			}
			else {
				mat_out = arma::join_rows(mat_out, curr_pol_mat);
			}
			iter++;
		}
	}
	else {
		mat_out.zeros(3,1);
	}

	return mat_out;
}

arma::Mat<double> ContourOpt::ConvertPolygon2Mat(Polygon_2 &Pol_in) {

	int N_v = Pol_in.size();

	arma::Mat<double> mat_out;
	mat_out.zeros(3,N_v);

	//int k=0;

	//for (VertexIterator vi = Pol_in.vertices_begin(); vi != Pol_in.vertices_end(); ++vi) {
	for (int i=0; i<N_v; i++) {

		mat_out(0,i) = static_cast<double>(Pol_in.vertex(i).x());
		mat_out(1,i) = static_cast<double>(Pol_in.vertex(i).y());
		mat_out(2,i) = 1.0;

		//k++;
	}

	return mat_out;
}

list<Polygon_with_holes_2> ContourOpt::Convert2PolygonWithHoles(arma::Mat<double> &contour_mat) {

	int N_contours = contour_mat.row(2).max();

	arma::uvec temp_inds;
	arma::Mat<double> temp_mat;

	list<Polygon_with_holes_2> poly_list_out;

	for (int i=0; i<N_contours; i++) {
		temp_inds = arma::find(contour_mat.row(2)==i+1);
		temp_mat = contour_mat.cols(temp_inds);
		Polygon_with_holes_2 t_poly(ContourOpt::Convert2Polygon(temp_mat));
		poly_list_out.push_back(t_poly);
	}

	return poly_list_out;
}

Polygon_2 ContourOpt::Convert2Polygon(arma::Mat<double> &contour_mat) {

	// Run through the points and construct a Polygon_2 object:
	Polygon_2 P;

	int N_pts = contour_mat.n_cols;

	//for (int i=0; i<N_pts; i++) {
	for (int i=(N_pts-1); i>=0; i--) {
		P.push_back(Point_2 (contour_mat(0,i),contour_mat(1,i)));
	}

	return P;
}

p::list ContourOpt::ReturnInitContour(int cam_nr) {

	p::list init_contour_list;

	arma::Mat<double> body_c = body_init_contours[cam_nr];
	arma::Mat<double> wing_L_c = wing_L_init_contours[cam_nr];
	arma::Mat<double> wing_R_c = wing_R_init_contours[cam_nr];

	arma::uvec found_ids;
	int N_found;

	for (int i=0; i<body_c.row(2).max(); i++) {
		found_ids = arma::find(body_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = body_c(0,found_ids(j));
				c_array[1][j] = body_c(1,found_ids(j));
				c_array[2][j] = 1;
			}
			init_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_L_c.row(2).max(); i++) {
		found_ids = arma::find(wing_L_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_L_c(0,found_ids(j));
				c_array[1][j] = wing_L_c(1,found_ids(j));
				c_array[2][j] = 2;
			}
			init_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_R_c.row(2).max(); i++) {
		found_ids = arma::find(wing_R_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_R_c(0,found_ids(j));
				c_array[1][j] = wing_R_c(1,found_ids(j));
				c_array[2][j] = 3;
			}
			init_contour_list.append(c_array);
		}
	}

	return init_contour_list;
}

p::list ContourOpt::ReturnDestContour(int cam_nr) {

	p::list dest_contour_list;

	arma::Mat<double> body_c = body_dest_contours[cam_nr];
	arma::Mat<double> wing_L_c = wing_L_dest_contours[cam_nr];
	arma::Mat<double> wing_R_c = wing_R_dest_contours[cam_nr];

	arma::uvec found_ids;
	int N_found;

	for (int i=0; i<body_c.row(2).max(); i++) {
		found_ids = arma::find(body_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = body_c(0,found_ids(j));
				c_array[1][j] = body_c(1,found_ids(j));
				c_array[2][j] = 1;
			}
			dest_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_L_c.row(2).max(); i++) {
		found_ids = arma::find(wing_L_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_L_c(0,found_ids(j));
				c_array[1][j] = wing_L_c(1,found_ids(j));
				c_array[2][j] = 2;
			}
			dest_contour_list.append(c_array);
		}
	}

	for (int i=0; i<wing_R_c.row(2).max(); i++) {
		found_ids = arma::find(wing_R_c.row(2)==i+1);
		N_found = found_ids.n_rows;
		if (N_found>1) {
			p::tuple shape = p::make_tuple(3,N_found);
			np::dtype dtype = np::dtype::get_builtin<double>();
			np::ndarray c_array = np::zeros(shape,dtype);
			for (int j=0; j<N_found; j++) {
				c_array[0][j] = wing_R_c(0,found_ids(j));
				c_array[1][j] = wing_R_c(1,found_ids(j));
				c_array[2][j] = 3;
			}
			dest_contour_list.append(c_array);
		}
	}

	return dest_contour_list;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_DEC_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DEC_S_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/is_not_equal.hpp>
#include <boost/simd/function/if_dec.hpp>
#include <boost/simd/function/saturated.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD_IF( dec_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::arithmetic_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &, A0 const& a0) const BOOST_NOEXCEPT
    {
      return if_dec(is_not_equal(a0, Valmin<A0>()), a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF( dec_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::saturated_tag
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &, A0 const& a0) const BOOST_NOEXCEPT
    {
      return a0-One<A0>();
    }
  };
} } }

#endif