#include "limb_manipulation/region_growing_rgb_segmentation.h"
#include <ros/console.h>

#include "perception/box_fitter.h"
#include "perception/object.h"
#include "visualization_msgs/Marker.h"
#include "pcl/segmentation/extract_clusters.h"
#include <pcl/filters/extract_indices.h>
#include "geometry_msgs/Pose.h"
#include "simple_grasping/shape_extraction.h"
#include "shape_msgs/SolidPrimitive.h"

#include <boost/lexical_cast.hpp>

namespace manipulation {

Segmenter::Segmenter(const ros::Publisher& filtered_cloud_pub, const ros::Publisher& colored_cloud_pub, const ros::Publisher& marker_pub)
  : filtered_cloud_pub_(filtered_cloud_pub), colored_cloud_pub_(colored_cloud_pub), marker_pub_(marker_pub) {}

void Segmenter::JointStateCallback(const sensor_msgs::JointState& msg) {
  head_tilt_joint_value_ = msg.position[5];
  torso_lift_joint_value_ = msg.position[2];
}

void Segmenter::Callback(const sensor_msgs::PointCloud2& msg) {
  SegmentPointCloud(msg);
}

void Segmenter::SegmentPointCloud(const sensor_msgs::PointCloud2& msg) {
  // parameters
  int reg_distance, reg_point_color, reg_region_color, min_cluster_size, max_cluster_size;
  ros::param::param("reg_distance", reg_distance, 10);
  ros::param::param("reg_point_color", reg_point_color, 6);
  ros::param::param("reg_region_color", reg_region_color, 5);
  ros::param::param("min_cluster_size", min_cluster_size, 60);
  ros::param::param("max_cluster_size", max_cluster_size, 10000);

  // pass through filter
  pcl::search::Search <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud_unfiltered (new pcl::PointCloud <pcl::PointXYZRGB>);
  pcl::fromROSMsg (msg, *cloud_unfiltered);

  pcl::IndicesPtr indices (new std::vector <int>);
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
  pcl::removeNaNFromPointCloud(*cloud_unfiltered, *cloud, *indices);

  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud (cloud);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(0.0, 1.0);
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr filtered_cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
  pass.filter (*filtered_cloud);

  // publish the filtered point cloud
  // sensor_msgs::PointCloud2 filtered_cloud_msg;
  // pcl::toROSMsg (*filtered_cloud, filtered_cloud_msg);
  // filtered_cloud_pub_.publish(filtered_cloud_msg);

  // color segmentation
  pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
  reg.setInputCloud (filtered_cloud);
  // reg.setIndices (indices);
  reg.setSearchMethod (tree);
  reg.setDistanceThreshold (reg_distance);  // determine whether the point is in the neighboring or not
  reg.setPointColorThreshold (reg_point_color);
  reg.setRegionColorThreshold (reg_region_color);
  reg.setMinClusterSize (min_cluster_size);
  reg.setMaxClusterSize (max_cluster_size);

  std::vector <pcl::PointIndices> object_indices;
  reg.extract (object_indices);
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();

  // convert to sensor message
  sensor_msgs::PointCloud2 cloud_out;
  pcl::toROSMsg (*colored_cloud, cloud_out);

  // **********************************
  // // transform point cloud
  // cloud_out.header.frame_id = "head_camera_rgb_frame";

  // tf::TransformListener tf_listener;
  // tf::StampedTransform transform;
  // try {
  //   tf_listener.waitForTransform("base_link", cloud_out.header.frame_id,
  //                                ros::Time(0), ros::Duration(5.0));
  //   tf_listener.lookupTransform("base_link", cloud_out.header.frame_id,
  //                               ros::Time(0), transform);
  // } catch (tf::LookupException& e) {
  //   ROS_ERROR("%s", e.what());
  // } catch (tf::ExtrapolationException& e) {
  //   ROS_ERROR("%s", e.what());
  // }

  // sensor_msgs::PointCloud2 colored_cloud_transformed;
  // pcl_ros::transformPointCloud("base_link", cloud_out, colored_cloud_transformed, tf_listener);

  // colored_cloud_pub_.publish(colored_cloud_transformed);
  // **********************************
  // transform point cloud
  tf::Transform rotation_transform;
  rotation_transform.setOrigin( tf::Vector3(0.17, 0.02, 1.03 + torso_lift_joint_value_) );
  tf::Quaternion q;
  q.setRPY(-1.5707 - head_tilt_joint_value_, 0, -1.5707);
  rotation_transform.setRotation(q);

  // // Eigen::Affine3f rotation_transform = Eigen::Affine3f::Identity();
  // // Define a translation of 2.5 meters on the x axis.
  // rotation_transform.translation() << 2.5, 0.0, 0.0;
  // // The same rotation matrix as before; theta radians around Z axis
  // rotation_transform.rotate (Eigen::AngleAxisf (1.5707, Eigen::Vector3f::UnitZ()));

  sensor_msgs::PointCloud2 colored_cloud_transformed;
  pcl_ros::transformPointCloud("base_link", rotation_transform, cloud_out, colored_cloud_transformed);
  // **********************************

  // publish the color-segmented cloud
  colored_cloud_pub_.publish(colored_cloud_transformed);

  // convert ROS message to pcl PointCloud type for labelling
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud_with_transformation (new pcl::PointCloud <pcl::PointXYZRGB>);
  pcl::fromROSMsg (colored_cloud_transformed, *colored_cloud_with_transformation);
  // label the markers
  SegmentMarker(&object_indices, colored_cloud_with_transformation);

}

void Segmenter::SegmentMarker(std::vector <pcl::PointIndices>* object_indices, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud) {
  // parameters
  int size_min, size_max;
  ros::param::param("size_min", size_min, 3);
  ros::param::param("size_max", size_max, 10);

  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  extract.setInputCloud(cloud);

  int num = 0;  // for the id# of visualization markers

  for (size_t i = 0; i < object_indices->size(); ++i) {
    // reify indices into a point cloud of the object
    pcl::PointIndices::Ptr indices(new pcl::PointIndices);
    *indices = object_indices->at(i);

    if (indices->indices.size() > size_min && indices->indices.size() < size_max) {
      pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
      // fill in object_cloud using indices
      extract.setIndices(indices);
      extract.setNegative(false);
      extract.filter(*object_cloud);

      // find the center of the object
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid (*object_cloud, centroid);

      // publish a marker
      visualization_msgs::Marker object_marker;
      object_marker.ns = "objects";
      object_marker.id = num;
      object_marker.header.frame_id = "base_link";
      object_marker.type = visualization_msgs::Marker::SPHERE;

      object_marker.pose.position.x = centroid[0];
      object_marker.pose.position.y = centroid[1];
      object_marker.pose.position.z = centroid[2];
      object_marker.scale.x = 0.05;
      object_marker.scale.y = 0.05;
      object_marker.scale.z = 0.05;
      object_marker.color.g = 1;
      object_marker.color.a = 1;
      marker_pub_.publish(object_marker);

      num++;

    }

    /*
    // Fit every region into a bounding box, and only label markers with specific sizes
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

    // fill in object_cloud using indices
    extract.setIndices(indices);
    extract.setNegative(false);
    extract.filter(*object_cloud);

    // bounding box
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_cloud_out(new pcl::PointCloud<pcl::PointXYZRGB>());
    shape_msgs::SolidPrimitive shape;
    geometry_msgs::Pose pose;
    simple_grasping::extractShape(*object_cloud, *object_cloud_out, shape, pose);

    if (shape.type == shape_msgs::SolidPrimitive::BOX) {
      double x = shape.dimensions[0], y = shape.dimensions[1], z = shape.dimensions[2];

      if (z > 0.5) {
        std::cout << num << std::endl;
        std::cout << "x: " << x << std::endl;
        std::cout << "y: " << y << std::endl;
        std::cout << "z: " << z << std::endl;
        std::cout << std::endl;

        // sensor_msgs::PointCloud2 object_cloud_transformed;
        // pcl::toROSMsg(*object_cloud, object_cloud_transformed);
        // colored_cloud_pub_.publish(object_cloud_transformed);

        // find the center of the object
        Eigen::Vector4f centroid;
        pcl::compute3DCentroid (*object_cloud, centroid);

        // publish a marker
        visualization_msgs::Marker object_marker;
        object_marker.ns = "objects";
        object_marker.id = num;
        object_marker.header.frame_id = "base_link";
        object_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

        object_marker.text = boost::lexical_cast<std::string>(num);
        object_marker.pose.position.x = centroid[0];
        object_marker.pose.position.y = centroid[1];
        object_marker.pose.position.z = centroid[2];
        object_marker.scale.x = 0.05;
        object_marker.scale.y = 0.05;
        object_marker.scale.z = 0.05;
        object_marker.color.g = 1;
        object_marker.color.a = 1;
        marker_pub_.publish(object_marker);

        num++;
      }
    }
    */
  }
}
}  // namespace manipulation


#pragma once

#include <crab/support/debug.hpp>
#include <crab/support/os.hpp>
#include <crab/types/variable.hpp>

#include <boost/optional.hpp>

namespace crab {

// Very simple language for expressing constraints over reference variables.
template <typename Number, typename VariableName> class reference_constraint {
public:
  using number_t = Number;
  using variable_t = variable<number_t, VariableName>;
  using reference_constraint_t = reference_constraint<number_t, VariableName>;

private:
  using opt_var_t = boost::optional<variable_t>;
  using cst_kind_t = enum {
    REF_EQ,
    REF_LT,
    REF_LEQ,
    REF_GT,
    REF_GEQ,
    REF_DISEQ
  };

  // m_lhs should be defined except if the constraints is a
  // contradiction/tautology.
  opt_var_t m_lhs; // if !m_lhs then NULL
  opt_var_t m_rhs; // if !m_rhs then NULL
  number_t m_offset;
  cst_kind_t m_kind;

  cst_kind_t swap_operand(cst_kind_t op) const {
    switch (op) {
    case REF_LT:
      return REF_GT;
    case REF_LEQ:
      return REF_GEQ;
    case REF_GT:
      return REF_LT;
    case REF_GEQ:
      return REF_LEQ;
    default:
      // case REF_EQ:
      // case REF_DISEQ:
      return op;
    }
  }

  void swap_constraint() {
    std::swap(m_lhs, m_rhs);
    m_kind = swap_operand(m_kind);
    if (m_offset != number_t(0)) {
      m_offset = -(m_offset);
    }
  }
  reference_constraint(opt_var_t lhs, opt_var_t rhs, cst_kind_t kind)
      : m_lhs(lhs), m_rhs(rhs), m_offset(0), m_kind(kind) {
    if (!m_lhs && m_rhs) { // normalize
      swap_constraint();
    }
  }

  reference_constraint(opt_var_t lhs, opt_var_t rhs, number_t offset,
                       cst_kind_t kind)
      : m_lhs(lhs), m_rhs(rhs), m_offset(offset), m_kind(kind) {
    if (!m_lhs && m_rhs) { // normalize
      swap_constraint();
    }
  }

public:
  reference_constraint() : m_kind(REF_EQ) {}

  // return true iff null != null or null < null
  bool is_contradiction() const {
    return (!m_lhs && !m_rhs && (m_kind == REF_DISEQ || m_kind == REF_LT));
  }

  // return true iff null == null or null <= null
  bool is_tautology() const {
    return (!m_lhs && !m_rhs && (m_kind == REF_EQ || m_kind == REF_LEQ));
  }

  bool is_equality() const { return m_kind == REF_EQ; }

  bool is_disequality() const { return m_kind == REF_DISEQ; }

  bool is_less_or_equal_than() const { return m_kind == REF_LEQ; }

  bool is_less_than() const { return m_kind == REF_LT; }

  bool is_greater_or_equal_than() const { return m_kind == REF_GEQ; }

  bool is_greater_than() const { return m_kind == REF_GT; }

  // return true iff  p REL_OP null
  bool is_unary() const { return (m_lhs && !m_rhs); }

  // return true iff p REL_OP q + offset
  bool is_binary() const { return (m_lhs && m_rhs); }

  variable_t lhs() const {
    if (!m_lhs)
      CRAB_ERROR("reference_constraint lhs is null");
    return *m_lhs;
  }

  variable_t rhs() const {
    if (!m_rhs)
      CRAB_ERROR("reference_constraint rhs is null");
    return *m_rhs;
  }

  std::vector<variable_t> variables() const {
    std::vector<variable_t> res;
    if (m_lhs) {
      res.push_back(*m_lhs);
    }
    if (m_rhs) {
      res.push_back(*m_rhs);
    }
    return res;
  }

  number_t offset() const { return m_offset; }

  reference_constraint_t negate() const {
    if (is_contradiction()) {
      return mk_true();
    } else if (is_tautology()) {
      return mk_false();
    } else if (is_unary()) {
      if (is_equality()) {
        // p == NULL --> p != NULL
        return mk_not_null(lhs());
      } else if (is_disequality()) {
        // p != NULL --> p == NULL
        return mk_null(lhs());
      } else if (is_less_or_equal_than()) {
        // p <= NULL --> p > NULL
        return reference_constraint_t(m_lhs, opt_var_t(), REF_GT);
      } else if (is_less_than()) {
        // p < NULL --> p >= NULL
        return reference_constraint_t(m_lhs, opt_var_t(), REF_GEQ);
      } else if (is_greater_or_equal_than()) {
        // p >= NULL --> p < NULL
        return reference_constraint_t(m_lhs, opt_var_t(), REF_LT);
      } else if (is_greater_than()) {
        // p > NULL --> p <= NULL
        return reference_constraint_t(m_lhs, opt_var_t(), REF_LEQ);
      }
    } else if (is_binary()) {
      if (is_equality()) {
        // p == q + k --> p != q + k
        return mk_not_eq(lhs(), rhs(), offset());
      } else if (is_disequality()) {
        // p != q + k --> p = q + k
        return mk_eq(lhs(), rhs(), offset());
      } else if (is_less_or_equal_than()) {
        // p <= q + k --> p > q + k  --> q < p - k
        return mk_lt(rhs(), lhs(), -(offset()));
      } else if (is_less_than()) {
        // p < q + k  --> p >= q + k --> q <= p - k
        return mk_le(rhs(), lhs(), -(offset()));
      } else if (is_greater_or_equal_than()) {
        // p >= q + k --> p < q + k  --> q > p - k
        return reference_constraint_t(m_rhs, m_lhs, -(offset()), REF_GT);
      } else if (is_greater_than()) {
        // p > q + k  --> p <= q + k --> q >= p - k
        return reference_constraint_t(m_rhs, m_lhs, -(offset()), REF_GEQ);
      }
    }
    CRAB_ERROR("reference_constraints::negate: unsupported case ", *this);
  }

  static reference_constraint_t mk_true() { return reference_constraint_t(); }

  static reference_constraint_t mk_false() {
    return reference_constraint_t(opt_var_t(), opt_var_t(), REF_DISEQ);
  }

  /* constraint a variable with null */
  static reference_constraint_t mk_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_null:", v, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_EQ);
  }

  static reference_constraint_t mk_not_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_not_null:", v,
                 " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_DISEQ);
  }

  static reference_constraint_t mk_le_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_LEQ);
  }

  static reference_constraint_t mk_lt_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_LT);
  }

  static reference_constraint_t mk_ge_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_GEQ);
  }

  static reference_constraint_t mk_gt_null(variable_t v) {
    if (!v.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v), opt_var_t(), REF_GT);
  }

  /* constraint two variables */
  static reference_constraint_t mk_eq(variable_t v1, variable_t v2,
                                      number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_eq:", v1, " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_eq:", v2, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset, REF_EQ);
  }

  static reference_constraint_t mk_not_eq(variable_t v1, variable_t v2,
                                          number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_not_eq:", v1,
                 " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_not_eq:", v2,
                 " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset,
                                  REF_DISEQ);
  }

  static reference_constraint_t mk_lt(variable_t v1, variable_t v2,
                                      number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v1, " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v2, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset, REF_LT);
  }

  static reference_constraint_t mk_le(variable_t v1, variable_t v2,
                                      number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v1, " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v2, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset,
                                  REF_LEQ);
  }

  static reference_constraint_t mk_gt(variable_t v1, variable_t v2,
                                      number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v1, " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_lt:", v2, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset, REF_GT);
  }

  static reference_constraint_t mk_ge(variable_t v1, variable_t v2,
                                      number_t offset = number_t(0)) {
    if (!v1.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v1, " must be a reference");
    }
    if (!v2.get_type().is_reference()) {
      CRAB_ERROR("reference_constraint::mk_leq:", v2, " must be a reference");
    }
    return reference_constraint_t(opt_var_t(v1), opt_var_t(v2), offset,
                                  REF_GEQ);
  }

  bool lexicographical_compare(const reference_constraint_t &o) const {
    return (m_kind < o.m_kind  &&
	    m_offset < o.m_offset &&
	    m_lhs < o.m_lhs &&
	    m_rhs < o.m_rhs);
  }

  void write(crab_os &o) const {
    if (is_contradiction()) {
      o << "false";
    } else if (is_tautology()) {
      o << "true";
    } else {
      assert(m_lhs);
      o << lhs();
      switch (m_kind) {
      case REF_EQ:
        o << " == ";
        break;
      case REF_DISEQ:
        o << " != ";
        break;
      case REF_LEQ:
        o << " <= ";
        break;
      case REF_LT:
        o << " < ";
        break;
      case REF_GEQ:
        o << " => ";
        break;
      case REF_GT:
        o << " > ";
        break;
      default:;
        // unreachable
      }
      if (!m_rhs)
        o << "NULL_REF";
      else
        o << rhs();

      if (m_offset != 0) {
        o << " + " << m_offset;
      }
    }
  }
};

template <typename Number, typename VariableName>
inline crab_os &
operator<<(crab_os &o, const reference_constraint<Number, VariableName> &cst) {
  cst.write(o);
  return o;
}
} // end namespace crab


/*
Copyright (c) 2017 Theodoros Chondrogiannis
*/

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>
#include <fstream>
#include <iostream>

#include "kspwlo_ref/algorithms/kspwlo.hpp"
#include "kspwlo_ref/model/graph.hpp"

#include "kspwlo_ref/exploration/error_metrics.hpp"

#include <margot.hpp>

using namespace std;

int main(int argc, char **argv) {
  // Initialize Margot
  margot::init();

  string graphFile = "";
  unsigned int k = 0;
  double theta = -1;
  string algo = "";
  RoadNetwork *rN = 0;

  NodeID source = 0, target = 100;

  int opt = -1;
  while ((opt = getopt(argc, argv, "f:S:D:k:s:a:")) != -1) {
    switch (opt) {
    case 'f':
      graphFile = string(optarg);
      break;
    case 'S':
      source = stoi(string(optarg));
      break;
    case 'D':
      target = stoi(string(optarg));
      break;
    case 'k':
      k = stoi(string(optarg));
      break;
    case 's':
      theta = stof(string(optarg));
      break;
    case 'a':
      algo = string(optarg);
      break;
    }
  }

  // Input checking
  if (graphFile == "") {
    cerr << "Wrong arguments. Define graph file correctly." << endl;
    exit(1);
  }

  if (k < 1) {
    cerr << "Define k between [1,+inf)" << endl;
    exit(2);
  }

  if (theta < 0 || theta > 1) {
    cerr << "Define theta between [0,1]" << endl;
    exit(3);
  }

  if (source == target) {
    cerr << "Source and target are the same node" << endl;
    exit(4);
  }

  // Loading road network
  rN = new RoadNetwork(graphFile.c_str());

  vector<Path> result;

  margot::parameter_space_exploration::start_monitor();
  if (boost::iequals(algo, "op")) {
    result = onepass(rN, source, target, k, theta);
  } else if (boost::iequals(algo, "mp")) {
    result = multipass(rN, source, target, k, theta);
  } else if (boost::iequals(algo, "opplus")) {
    result = onepass_plus(rN, source, target, k, theta);
  } else if (boost::iequals(algo, "svp")) {
    result = svp_plus(rN, source, target, k, theta);
  } else if (boost::iequals(algo, "esx")) {
    result = esx(rN, source, target, k, theta);
  }
  margot::parameter_space_exploration::stop_monitor();

  auto errors = margot::compute_errors(result, *rN, source, target);

  float total_distance = errors.total_distance;
  float average_distance = errors.average_distance;
  float decision_edges = errors.decision_edges;

  margot::parameter_space_exploration::monitor::total_distance_monitor.push(
      total_distance);
  margot::parameter_space_exploration::monitor::average_distance_monitor.push(
      average_distance);
  margot::parameter_space_exploration::monitor::decision_edges_monitor.push(
      decision_edges);
  margot::parameter_space_exploration::log();

  cout << source << "\t" << target << "\t[" << result[0].length;
  for (unsigned int j = 1; j < result.size(); j++) {
    cout << "," << result[j].length;
  }
  cout << "]" << endl;

  delete rN;
  return 0;
}


#pragma once

#include <Eigen/Dense>

#include <merely3d/color.hpp>

#include "shader.hpp"

// TODO: Remove this, can we somehow forward-declare a typedef?
typedef int GLint;

namespace merely3d
{
    class MeshShader
    {
    public:
        void set_model_transform(const Eigen::Affine3f & model);
        void set_view_transform(const Eigen::Affine3f & view);
        void set_projection_transform(const Eigen::Matrix4f & projection);
        void set_normal_transform(const Eigen::Matrix3f & transform);
        void set_reference_transform(const Eigen::Matrix3f & transform);
        void set_object_color(const Color & color);
        void set_light_color(const Color & color);
        void set_light_direction(const Eigen::Vector3f & direction);
        void set_camera_position(const Eigen::Vector3f & position);
        void set_pattern_grid_size(float size);

        void use();

        static MeshShader create_in_context();

    private:
        explicit MeshShader(ShaderProgram && shader)
            : shader(std::move(shader))
        {}

        GLint projection_loc = 0;
        GLint model_loc = 0;
        GLint view_loc = 0;
        GLint normal_transform_loc = 0;
        GLint object_color_loc = 0;
        GLint light_color_loc = 0;
        GLint light_dir_loc = 0;
        GLint camera_pos_loc = 0;
        GLint reference_transform_loc = 0;
        GLint pattern_grid_size_loc = 0;

        ShaderProgram shader;
    };

    class LineShader
    {
    public:
        void set_model_transform(const Eigen::Affine3f & model);
        void set_view_transform(const Eigen::Affine3f & view);
        void set_projection_transform(const Eigen::Matrix4f & projection);
        void set_object_color(const Color & color);

        void use();

        static LineShader create_in_context();

    private:
        explicit LineShader(ShaderProgram && shader)
            : shader(std::move(shader))
        {}

        GLint projection_loc = 0;
        GLint model_loc = 0;
        GLint view_loc = 0;
        GLint object_color_loc = 0;

        ShaderProgram shader;
    };

    class ParticleShader
    {
    public:
        void set_view_transform(const Eigen::Affine3f & view);
        void set_projection_transform(const Eigen::Matrix4f & projection);
        void set_viewport_dimensions(float width, float height);
        void set_near_plane_dist(float dist);
        void set_light_color(const Color & color);
        void set_light_eye_direction(const Eigen::Vector3f & direction);

        void use();

        static ParticleShader create_in_context();

    private:
        explicit ParticleShader(ShaderProgram && shader)
            : shader(std::move(shader))
        {}

        GLint projection_loc = 0;
        GLint inv_projection_loc = 0;
        GLint view_loc = 0;
        GLint viewport_width_loc = 0;
        GLint viewport_height_loc = 0;
        GLint near_plane_dist_loc = 0;
        GLint light_color_loc = 0;
        GLint light_eye_dir_loc = 0;

        ShaderProgram shader;
    };

    class ShaderCollection
    {
    public:
        MeshShader &     mesh_shader();
        LineShader &     line_shader();
        ParticleShader & particle_shader();

        static ShaderCollection create_in_context();

    private:
        ShaderCollection(MeshShader && mesh_shader,
                         LineShader && line_shader,
                         ParticleShader && particle_shader)
            : _mesh_shader(std::move(mesh_shader)),
              _line_shader(std::move(line_shader)),
              _particle_shader(std::move(particle_shader))
        {}

        MeshShader      _mesh_shader;
        LineShader      _line_shader;
        ParticleShader  _particle_shader;
    };

}


//
// Created by nvidia on 6/13/18.
//

#include <deque>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/edge_list.hpp>
#include "SsdSingleShotSuperClassifier.hpp"
#include "superpixel_seg.hpp"
#include "SpVec.hpp"

using namespace std;
using namespace cv;
using namespace cv::ximgproc;
namespace cove {

SsdSingleShotSuperClassifier::SsdSingleShotSuperClassifier(Ptr<SuperpixelLSC> &&x, InputArray _cielab,
                                                           int _regionSize, double _ssdEpsilon) :
        g(x->getNumberOfSuperpixels()),
        super(x),
        cielab(_cielab.getMat()) {
    super->getLabels(labels);
    labels.convertTo(labels, CV_8UC1);
    regionSize = _regionSize;
    ssdEpsilon = _ssdEpsilon;
}

void SsdSingleShotSuperClassifier::getFloorMask(OutputArray _dst) {
    buildGraph();

#ifdef DEBUG
    boost::print_graph(g,
                       "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789!@#$%^&*()-=[];',._+{}:\"<>/?");
    cout << flush;
#endif
//    Mat mapped_labels;
//    labels.convertTo(mapped_labels, CV_8UC3);
//    SHOW_ALWAYS("labels", mapped_labels);
//    SHOW_ALWAYS("cielab", cielab);

    fillTrainingData();
    generateFeatureVectors();

//    printVecSpVec(pixelVecs);
    prepareSsdClassifier();

    auto &&floorLabels = doBfs();

    // compute the floor mask
    _dst.createSameSize(labels, CV_8U);
    Mat dst = _dst.getMat();
    dst = Mat::zeros(labels.size(), CV_8U);
    for (int label : floorLabels) {
        Mat temp;
        inRange(labels, Scalar(label), Scalar(label), temp);
        bitwise_or(dst, temp, dst);
    }
    SHOW("floor", dst);
}

void SsdSingleShotSuperClassifier::buildGraph() {
    CV_Assert(labels.channels() == 1);

    Mat tmp;
    Mat d;

    vector<pair<int, int>> offsets;
    offsets.push_back(make_pair(1, 0));
    offsets.push_back(make_pair(0, 1));
    for (auto pair: offsets) {
        labels.copyTo(tmp);
        translateImg(tmp, pair.first, pair.second);
        SHOW("translated", tmp);
        d = labels - tmp;

        int nRows = tmp.rows;
        int nCols = tmp.cols;

        if (tmp.isContinuous()) {
            CV_Assert(d.isContinuous());
            CV_Assert(labels.isContinuous());
            nCols *= nRows;
            nRows = 1;
        }

        int i, j;
        uchar *l, *dd, *pp;
        for (i = 0; i < nRows; ++i) {
            pp = tmp.ptr<uchar>(i);
            dd = d.ptr<uchar>(i);
            l = labels.ptr<uchar>(i);
            for (j = 0; j < nCols; ++j) {
                if (dd[j] == 0) continue;
                boost::add_edge(l[j], pp[j], g);
            }
        }
    }
}

void SsdSingleShotSuperClassifier::fillTrainingData(int rows, int cols) {
    trainingLabels.clear();
    Size s = labels.size();
    int radius = regionSize / 2;
    for (int ydiff = 0; ydiff < rows; ydiff++) {
        trainingLabels.push_back(labels.at<unsigned char>(s.height - radius - ydiff * regionSize, s.width / 2));
        for (int xdiff = 1; xdiff <= cols / 2; xdiff += 1) {
            trainingLabels.push_back(labels.at<unsigned char>(s.height - radius - ydiff * regionSize,
                                                              s.width / 2 + xdiff * regionSize));
            trainingLabels.push_back(labels.at<unsigned char>(s.height - radius - ydiff * regionSize,
                                                              s.width / 2 - xdiff * regionSize));
        }
    }
}

void SsdSingleShotSuperClassifier::generateFeatureVectors() {
    int numPx = super->getNumberOfSuperpixels();
    pixelVecs = vector<SpVec>(numPx);
    for (int i = 0; i < numPx; i++) {
        pixelVecs.at(i) = move(getParams(labels, cielab, i));
    }
}

void SsdSingleShotSuperClassifier::prepareSsdClassifier() {
    vector<SpVec> trainingVecs;
    for (int idx: trainingLabels) {
        trainingVecs.push_back(pixelVecs.at(idx));
    }
//    printVecSpVec(trainingVecs);
    trainingMean = vecAverage(trainingVecs);
    populationStdDev = vecStdDev(pixelVecs);
      cout << "MEAN\nDEV" << endl;
//    printSpVec(mean);
//    printSpVec(stdDev);
    vector<double> ssdTrainVals(trainingVecs.size(), 0.0);
    for (int idx: trainingLabels) {
        ssdTrainVals.push_back(normSsd(pixelVecs.at(idx), trainingMean, populationStdDev));
    }
    maxTrainingSsd = *max_element(ssdTrainVals.begin(), ssdTrainVals.end());
}

inline bool SsdSingleShotSuperClassifier::satisfiesSsd(const SpVec &v) {
    return normSsd(v, trainingMean, populationStdDev) <= maxTrainingSsd + ssdEpsilon;
}

vector<int> SsdSingleShotSuperClassifier::doBfs() {
    vector<int> floorLabels;
    if (trainingLabels.empty()) {
        return floorLabels;
    }
    vector<bool> visited(boost::num_vertices(g), false);
    deque<int> toVisit;
    toVisit.push_back(trainingLabels.at(0));

    while (!toVisit.empty()) {
        int node = toVisit.front();
        toVisit.pop_front();

        visited.at(node) = true;
        if (!satisfiesSsd(pixelVecs.at(node))) continue;

        floorLabels.push_back(node);

        auto its = boost::out_edges(node, g);
        for (; its.first != its.second; ++(its.first)) {
            int connected = boost::target(*(its.first), g);
            if (visited.at(connected)) continue;
            toVisit.push_back(connected);
        }
    }

    return floorLabels;
}
}


#ifndef STAN_MATH_TORSTEN_GENERALODEMODEL_BDF_HPP
#define STAN_MATH_TORSTEN_GENERALODEMODEL_BDF_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/PKModel/functors/general_functor.hpp>
#include <stan/math/torsten/PKModel/PKModel.hpp>
#include <stan/math/torsten/PKModel/Pred/Pred1_general.hpp>
#include <stan/math/torsten/PKModel/Pred/PredSS_general.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace torsten {

/**
 * Computes the predicted amounts in each compartment at each event
 * for a general compartment model, defined by a system of ordinary
 * differential equations. Uses the stan::math::integrate_ode_bdf
 * function.
 *
 * <b>Warning:</b> This prototype does not handle steady state events.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalars for the model parameters.
 * @tparam T5 type of scalars for the bio-variability parameters.
 * @tparam T6 type of scalars for the model tlag parameters.
 * @tparam F type of ODE system function.
 * @param[in] f functor for base ordinary differential equation that defines
 *            compartment model.
 * @param[in] nCmt number of compartments in model
 * @param[in] pMatrix parameters at each event
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * @param[in] rel_tol relative tolerance for the Boost ode solver
 * @param[in] abs_tol absolute tolerance for the Boost ode solver
 * @param[in] max_num_steps maximal number of steps to take within
 *            the Boost ode solver
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 * FIX ME: currently have a dummy msgs argument. Makes it easier
 * to expose to stan grammar files, because I can follow more closely
 * what was done for the ODE integrator. Not ideal.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                    const int nCmt,
                    const std::vector<T0>& time,
                    const std::vector<T1>& amt,
                    const std::vector<T2>& rate,
                    const std::vector<T3>& ii,
                    const std::vector<int>& evid,
                    const std::vector<int>& cmt,
                    const std::vector<int>& addl,
                    const std::vector<int>& ss,
                    const std::vector<std::vector<T4> >& pMatrix,
                    const std::vector<std::vector<T5> >& biovar,
                    const std::vector<std::vector<T6> >& tlag,
                    std::ostream* msgs = 0,
                    double rel_tol = 1e-10,
                    double abs_tol = 1e-10,
                    long int max_num_steps = 1e8) {  // NOLINT(runtime/int)
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;

  // check arguments
  static const char* function("generalOdeModel_bdf");
  torsten::pmetricsCheck(time, amt, rate, ii, evid, cmt, addl, ss,
                pMatrix, biovar, tlag, function);

  // Construct dummy matrix for last argument of pred
  Matrix<T4, Dynamic, Dynamic> dummy_system;
  vector<Matrix<T4, Dynamic, Dynamic> >
    dummy_systems(1, dummy_system);

  typedef general_functor<F> F0;

  return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
              pMatrix, biovar, tlag, nCmt, dummy_systems,
              Pred1_general<F0>(F0(f), rel_tol, abs_tol,
                                max_num_steps, msgs, "bdf"),
              PredSS_general<F0>(F0(f), rel_tol, abs_tol,
                                 max_num_steps, msgs, "bdf", nCmt));

  // // check arguments
  // static const char* function("generalOdeModel_bdf");
  // pmetricsCheck(time, amt, rate, ii, evid, cmt, addl, ss,
  //               pMatrix, biovar, tlag, function);
  //
  // // Construct dummy matrix for last argument of pred
  // Matrix<T4, Dynamic, Dynamic> dummy_system;
  // vector<Matrix<T4, Dynamic, Dynamic> >
  //   dummy_systems(1, dummy_system);
  //
  // typedef general_functor<F> F0;
  //
  // return Pred(time, amt, rate, ii, evid, cmt, addl, ss,
  //             pMatrix, biovar, tlag, nCmt, dummy_systems,
  //             Pred1_general<F0>(F0(f), rel_tol, abs_tol,
  //                               max_num_steps, msgs, "rk45"),
  //             PredSS_general<F0>(F0(f), rel_tol, abs_tol,
  //                                max_num_steps, msgs, "bdf", nCmt));
}

/**
 * Overload function to allow user to pass an std::vector for
 * pMatrix.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix and biovar.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix, biovar, and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* pMatrix and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<T4>& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T4> > vec_pMatrix(1, pMatrix);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              vec_pMatrix, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* biovar.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, vec_biovar, tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
* Overload function to allow user to pass an std::vector for
* biovar and tlag.
*/
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<T5>& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T5> > vec_biovar(1, biovar);
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, vec_biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

/**
 * Overload function to allow user to pass an std::vector for
 * tlag.
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename boost::math::tools::promote_args<T0, T1, T2, T3,
  typename boost::math::tools::promote_args<T4, T5, T6>::type>::type,
  Eigen::Dynamic, Eigen::Dynamic>
generalOdeModel_bdf(const F& f,
                     const int nCmt,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& pMatrix,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<T6>& tlag,
                     std::ostream* msgs = 0,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6) {  // NOLINT(runtime/int)
  std::vector<std::vector<T6> > vec_tlag(1, tlag);

  return generalOdeModel_bdf(f, nCmt,
                              time, amt, rate, ii, evid, cmt, addl, ss,
                              pMatrix, biovar, vec_tlag,
                              msgs, rel_tol, abs_tol, max_num_steps);
}

}

#endif


#include "random.h"
#include <boost/random.hpp>
#include <time.h>

static boost::mt19937 g_random_gen((uint32_t)time(0));

Random::Random(int min, int max)
    : m_ivalue(0)
{
    boost::uniform_int<> dist(min, max);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<>> g(g_random_gen, dist);
    m_ivalue = g();
}

Random::operator int() const
{
    return m_ivalue;
}

Random::operator int()
{
    return m_ivalue;
}


#include <utils/utilities.hpp>
#include <iostream>
#include <stdio.h>

#include <fstream>
#include <algorithm>
#include <Eigen/Eigenvalues>
#include <string>

using namespace std;

namespace sejong{
  std::string pretty_string(sejong::Vector const & vv)
  {
    ostringstream os;
    pretty_print(vv, os, "", "", true);
    return os.str();
  }

  void saveVector(const Eigen::VectorXd & vec_, std::string name_, bool b_param){
        std::string file_name;
        cleaningFile(name_, file_name, b_param);

        std::ofstream savefile(file_name.c_str(), std::ios::app);
        for (int i(0); i < vec_.rows(); ++i){
            savefile<<vec_(i)<< "\t";
        }
        savefile<<"\n";
        savefile.flush();
    }

    void saveVector(double * _vec, std::string _name, int size, bool b_param){
        std::string file_name;
        cleaningFile(_name, file_name, b_param);
        std::ofstream savefile(file_name.c_str(), std::ios::app);

        for (int i(0); i < size; ++i){
            savefile<<_vec[i]<< "\t";
        }
        savefile<<"\n";
        savefile.flush();
    }

    void saveVector(const std::vector<double> & _vec, std::string _name, bool b_param){
        std::string file_name;
        cleaningFile(_name, file_name, b_param);
        std::ofstream savefile(file_name.c_str(), std::ios::app);
        for (int i(0); i < _vec.size(); ++i){
            savefile<<_vec[i]<< "\t";
        }
        savefile<<"\n";
        savefile.flush();
    }

    void cleaningFile(std::string  _file_name, std::string & _ret_file, bool b_param){
        if(b_param)
            _ret_file += "asdf";
        else
            _ret_file += "/home/apptronik/ros/DracoNodelet/AnkleKneeController/ExperimentData/";

        _ret_file += _file_name;
        _ret_file += ".txt";

        std::list<std::string>::iterator iter = std::find(gs_fileName_string.begin(), gs_fileName_string.end(), _file_name);
        if(gs_fileName_string.end() == iter){
            gs_fileName_string.push_back(_file_name);
            remove(_ret_file.c_str());
        }
    }

  std::string pretty_string(sejong::Quaternion const & qq)
  {
    ostringstream os;
    pretty_print(qq, os, "", "", true);
    return os.str();
  }

  std::string pretty_string(sejong::Matrix const & mm, std::string const & prefix)
  {
    ostringstream os;
    pretty_print(mm, os, "", prefix);
    return os.str();
  }

  void pretty_print(sejong::Vector const & vv, std::ostream & os,
                    std::string const & title, std::string const & prefix,
                    bool nonl)
  {
    pretty_print((sejong::Matrix const &) vv, os, title, prefix, true, nonl);
  }

  void pretty_print(sejong::Vect3 const & vv, std::ostream & os,
                    std::string const & title, std::string const & prefix, bool nonl){
    pretty_print((sejong::Matrix const &) vv, os, title, prefix, true, nonl);
  }

  void pretty_print(sejong::Quaternion const & qq, std::ostream & os,
                    std::string const & title, std::string const & prefix,
                    bool nonl)
  {
    pretty_print(qq.coeffs(), os, title, prefix, true, nonl);
  }

  std::string pretty_string(double vv)
  {
    static int const buflen(32);
    static char buf[buflen];
    memset(buf, 0, sizeof(buf));
    /*#ifndef WIN32
      if (isinf(vv)) {
      snprintf(buf, buflen-1, " inf    ");
      }
      else if (isnan(vv)) {
      snprintf(buf, buflen-1, " nan    ");
      }
      else if (fabs(fmod(vv, 1)) < 1e-9) {
      snprintf(buf, buflen-1, "%- 7d  ", static_cast<int>(rint(vv)));
      }
      else {
      snprintf(buf, buflen-1, "% 6.6f  ", vv);
      }
      #else // WIN32*/
    snprintf(buf, buflen-1, "% 6.6f  ", vv);
    //#endif // WIN32
    string str(buf);
    return str;
  }

  void pretty_print(const std::vector<double> & _vec, const char* title){
    printf("%s: ", title);
    for( int i(0); i< _vec.size(); ++i){
      printf("% 6.4f, \t", _vec[i]);
    }
    printf("\n");
  }
  void pretty_print(const std::vector<int> & _vec, const char* title){
    printf("%s: ", title);
    for( int i(0); i< _vec.size(); ++i){
      printf("%d, \t", _vec[i]);
    }
    printf("\n");
  }

  void pretty_print(const double * _vec, const char* title, int size){
    printf("%s: ", title);
    for(int i(0); i< size; ++i){
      printf("% 6.4f, \t", _vec[i]);
    }
    printf("\n");
  }
  void pretty_print(sejong::Matrix const & mm, std::ostream & os,
                    std::string const & title, std::string const & prefix,
                    bool vecmode, bool nonl)
  {
    char const * nlornot("\n");
    if (nonl) {
      nlornot = "";
    }
    if ( ! title.empty()) {
      os << title << nlornot;
    }
    if ((mm.rows() <= 0) || (mm.cols() <= 0)) {
      os << prefix << " (empty)" << nlornot;
    }
    else {
      // if (mm.cols() == 1) {
      //   vecmode = true;
      // }

      if (vecmode) {
        if ( ! prefix.empty())
          os << prefix;
        for (int ir(0); ir < mm.rows(); ++ir) {
          os << pretty_string(mm.coeff(ir, 0));
        }
        os << nlornot;

      }
      else {

        for (int ir(0); ir < mm.rows(); ++ir) {
          if ( ! prefix.empty())
            os << prefix;
          for (int ic(0); ic < mm.cols(); ++ic) {
            os << pretty_string(mm.coeff(ir, ic));
          }
          os << nlornot;
        }

      }
    }
  }

  void printVectorSequence(const std::vector<sejong::Vector> & seq, string _name){
    std::cout<<_name<<":\n";
    for (int i(0); i< seq.size(); ++i){
      for (int j(0); j< seq[i].rows(); ++j){
        printf("% 6.4f, \t", seq[i][j]);
      }
      std::cout<<"\n";
    }
  }

  double generator_truncated_white_noise(double mean, double var, double min, double max){
    double ret;
    do{
      ret = generator_white_noise(mean, var);
    } while (ret <min || max < ret );

    return ret;
  }

  double generator_white_noise(double mean, double var){

    static bool hasSpare = false;
    static double rand1, rand2;

    if(hasSpare){
      hasSpare = false;
      return sqrt(var*rand1)*sin(rand2) + mean;
    }
    hasSpare = true;

    rand1 = rand() / ((double ) RAND_MAX);
    if(rand1 < 1e-100) rand1 = 1e-100;
    rand1 = -2*log(rand1);
    rand2 = rand() / ((double ) RAND_MAX) * M_PI * 2.;

    return mean + sqrt(var*rand1)*cos(rand2);
  }
  double generator_gamma_noise(double a, double b){
    /* assume a > 0 */
     if (a < 1)      {
       double u = rand()/((double)RAND_MAX);
       return generator_gamma_noise (1.0 + a, b) * pow (u, 1.0 / a);
     }

     {
       double x, v, u;
       double d = a - 1.0 / 3.0;
       double c = (1.0 / 3.0) / sqrt (d);

       while (true){
         do{
           x = generator_white_noise (0., 1.0);
           v = 1.0 + c * x;
         }
         while (v <= 0);

         v = v * v * v;
         u = rand()/((double)RAND_MAX);

         if (u < 1 - 0.0331 * x * x * x * x)  break;
         if (log (u) < 0.5 * x * x + d * (1 - v + log (v)))  break;
       }
       return b * d * v;
     }
  }

  void sqrtm(const sejong::Matrix & mt, sejong::Matrix & sqrt_mt){
    Eigen::EigenSolver<sejong::Matrix> es(mt, true);
    Eigen::MatrixXcd V = es.eigenvectors();
    Eigen::VectorXcd Dv = es.eigenvalues();
    Eigen::MatrixXcd sqrtD(mt.cols(), mt.rows());
    sqrtD = Dv.cwiseSqrt().asDiagonal();
    Eigen::MatrixXcd tmp_sqrt_mt = V*sqrtD*V.inverse();

    sqrt_mt = tmp_sqrt_mt.real();
  }

  double smooth_changing(double ini, double end, double moving_duration, double curr_time){
    double ret;
    ret = ini + (end - ini)*0.5*(1-cos(curr_time/moving_duration * M_PI));
    if(curr_time>moving_duration){
      ret = end;
    }
    return ret;
  }

  double smooth_changing_vel(double ini, double end, double moving_duration, double curr_time){
    double ret;
    ret = (end - ini)*0.5*(M_PI/moving_duration)*sin(curr_time/moving_duration*M_PI);
    if(curr_time>moving_duration){
      ret = 0.0;
    }
    return ret;
  }
  double smooth_changing_acc(double ini, double end, double moving_duration, double curr_time){
    double ret;
    ret = (end - ini)*0.5*(M_PI/moving_duration)*(M_PI/moving_duration)*cos(curr_time/moving_duration*M_PI);
    if(curr_time>moving_duration){
      ret = 0.0;
    }
    return ret;
  }

  double MinMaxBound(double value, double min, double max){
    if(value > max){
      return max;
    }
    else if (value < min){
      return min;
    }
    return value;
  }
  bool MinMaxCheck(double value, double min, double max, bool b_exit){
    if(value > max){
      if(b_exit) exit(0);
      return true;
    }
    else if (value < min){
      if(b_exit) exit(0);
      return true;
    }
    return false;
  }

  //jh
  void read_file(std::string _file_name, std::vector<std::string> & _vec){
    ifstream  InputFile(_file_name.c_str());
    std::string tempstring;
    if(!InputFile.is_open()){
      cout << "Data file load error... check the data file" << endl;
      exit(0);
    }
    else{
      while(!InputFile.eof()){
        InputFile.clear();
        getline(InputFile,tempstring);
        _vec.push_back(tempstring);
      }
      InputFile.close();
    }
  }

  void split_string(std::string* str_array, std::string strTarget, std::string strTok ){
    int nCutPos = 0;
    int nIndex = 0;
    while ((nCutPos = strTarget.find_first_of(strTok)) != strTarget.npos){
      if (nCutPos > 0){
        str_array[nIndex++] = strTarget.substr(0, nCutPos);
      }
      strTarget = strTarget.substr(nCutPos+1);
    }
    if(strTarget.length() > 0){
      str_array[nIndex++] = strTarget.substr(0, nCutPos);
    }
  }

  sejong::Matrix crossmat(sejong::Vect3 r){
      sejong::Matrix ret(3, 3);
      ret << 0          , -1 * r[2]   , r[1],
             r[2]       ,  0          , -1 * r[0],
             -1 * r[1]  , r[0]        , 0;

      return ret;
  }

  void hStack(const Eigen::MatrixXd a,
              const Eigen::MatrixXd b,
              Eigen::MatrixXd & ab){
      if ( a.rows() != b.rows() ) {
          std::cout << "[hStack] Matrix Size is Wrong" << std::endl;
          exit(0);
      }
      ab.resize(a.rows(), a.cols() + b.cols());
      ab << a, b;
  }

  void vStack(const Eigen::MatrixXd a,
              const Eigen::MatrixXd b,
              Eigen::MatrixXd & ab){
      if ( a.cols() != b.cols() ) {
          std::cout << "[vStack] Matrix Size is Wrong" << std::endl;
          exit(0);
      }
      ab.resize(a.rows() + b.rows(), a.cols());
      ab << a, b;
  }

  void vStack(const Eigen::VectorXd a,
              const Eigen::VectorXd b,
              Eigen::VectorXd & ab){
      ab.resize(a.size() + b.size());
      ab << a, b;
  }
  //jh
}


#pragma once
#include <armadillo>
#include <window_manager.hpp>

//Experimental
namespace ImPlot{
	void ShowDemo_Heatmaps();
	void ShowDemo_Help();
	void ShowDemo_Config();
	void ShowDemo_DigitalPlots();
	void ShowDemoWindow(bool* open);
}

namespace arm_simu{

	class Plot{
		static void PlotterCallback(arm_simu::WinPointer*,arm_simu::Extra);
		public:
			static void LinePlot(arma::vec x,arma::vec y,const char* title);
	};

}


// GTest
#include <gtest/gtest.h>

//cupoch_conversions
#include "cupoch_conversions/cupoch_conversions.h"

// cupoch
#include <cupoch/cupoch.h>

// ROS
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

// Boost
#include <boost/make_shared.hpp>

using namespace cupoch;
using namespace Eigen;

TEST(ConversionFunctions, cupochToRos_uncolored)
{
  auto cupoch_pc = std::make_shared<cupoch::geometry::PointCloud>();
  thrust::host_vector<Eigen::Vector3f> cupoch_pc_points_host;
  for (int i = 0; i < 5; ++i)
  {
    cupoch_pc_points_host.push_back(Eigen::Vector3f((0.5 * i), (i * i), (10.5 * i)));
  }
  cupoch_pc->SetPoints(cupoch_pc_points_host);

  sensor_msgs::PointCloud2 ros_pc2;
  cupoch_conversions::cupochToRos(cupoch_pc, ros_pc2, "cupoch_frame");
  EXPECT_EQ(ros_pc2.height * ros_pc2.width, cupoch_pc->GetPoints().size());
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_x(ros_pc2, "x");
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_y(ros_pc2, "y");
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_z(ros_pc2, "z");
  for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z)
  {
    const Eigen::Vector3f &point = cupoch_pc->GetPoints()[i];
    EXPECT_EQ(*ros_pc2_x, (0.5 * i));
    EXPECT_EQ(*ros_pc2_y, (i * i));
    EXPECT_EQ(*ros_pc2_z, (10.5 * i));
  }
}

TEST(ConversionFunctions, cupochToRos_colored)
{
  auto cupoch_pc = std::make_shared<cupoch::geometry::PointCloud>();
  thrust::host_vector<Eigen::Vector3f> cupoch_pc_points_host;
  thrust::host_vector<Eigen::Vector3f> cupoch_pc_colors_host;
  for (int i = 0; i < 5; ++i)
  {
    cupoch_pc_points_host.push_back(Eigen::Vector3f((0.5 * i), (i * i), (10.5 * i)));
    cupoch_pc_colors_host.push_back(Eigen::Vector3f(2 * i / 255.0, 5 * i / 255.0, 10 * i / 255.0));
  }
  cupoch_pc->SetPoints(cupoch_pc_points_host);
  cupoch_pc->SetColors(cupoch_pc_colors_host);

  EXPECT_EQ(cupoch_pc_points_host.size(), cupoch_pc_colors_host.size());
  EXPECT_EQ(false, cupoch_pc->IsEmpty());
  EXPECT_EQ(true, cupoch_pc->HasColors());

  sensor_msgs::PointCloud2 ros_pc2;
  cupoch_conversions::cupochToRos(cupoch_pc, ros_pc2, "cupoch_frame");
  EXPECT_EQ(ros_pc2.height * ros_pc2.width, cupoch_pc->GetPoints().size());
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_x(ros_pc2, "x");
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_y(ros_pc2, "y");
  sensor_msgs::PointCloud2Iterator<float> ros_pc2_z(ros_pc2, "z");
  sensor_msgs::PointCloud2Iterator<uint8_t> ros_pc2_r(ros_pc2, "r");
  sensor_msgs::PointCloud2Iterator<uint8_t> ros_pc2_g(ros_pc2, "g");
  sensor_msgs::PointCloud2Iterator<uint8_t> ros_pc2_b(ros_pc2, "b");
  for (int i = 0; i < 5; i++, ++ros_pc2_x, ++ros_pc2_y, ++ros_pc2_z, ++ros_pc2_r, ++ros_pc2_g, ++ros_pc2_b)
  {
    const Eigen::Vector3f &point = cupoch_pc->GetPoints()[i];
    EXPECT_EQ(*ros_pc2_x, (0.5 * i));
    EXPECT_EQ(*ros_pc2_y, (i * i));
    EXPECT_EQ(*ros_pc2_z, (10.5 * i));
    const Eigen::Vector3f &color = cupoch_pc->GetPoints()[i];
    EXPECT_EQ(*ros_pc2_r, (int)(2 * i));
    EXPECT_EQ(*ros_pc2_g, (int)(5 * i));
    EXPECT_EQ(*ros_pc2_b, (int)(10 * i));
  }
}

TEST(ConversionFunctions, rosToCupoch_uncolored)
{
  sensor_msgs::PointCloud2 ros_pc2;
  ros_pc2.header.frame_id = "ros";
  ros_pc2.height = 1;
  ros_pc2.width = 5;
  ros_pc2.is_bigendian = false;
  ros_pc2.is_dense = true;
  sensor_msgs::PointCloud2Modifier modifier(ros_pc2);
  modifier.setPointCloud2FieldsByString(1, "xyz");
  modifier.resize(5 * 1);
  sensor_msgs::PointCloud2Iterator<float> mod_x(ros_pc2, "x");
  sensor_msgs::PointCloud2Iterator<float> mod_y(ros_pc2, "y");
  sensor_msgs::PointCloud2Iterator<float> mod_z(ros_pc2, "z");

  for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z)
  {
    *mod_x = 0.5 * i;
    *mod_y = i * i;
    *mod_z = 10.5 * i;
  }

  const sensor_msgs::PointCloud2ConstPtr &ros_pc2_ptr = boost::make_shared<sensor_msgs::PointCloud2>(ros_pc2);
  auto cupoch_pc = std::make_shared<cupoch::geometry::PointCloud>();
  cupoch_conversions::rosToCupoch(ros_pc2_ptr, cupoch_pc);

  auto cupoch_pc_points_host = cupoch_pc->GetPoints();
  EXPECT_EQ(ros_pc2_ptr->height * ros_pc2_ptr->width, cupoch_pc_points_host.size());
  EXPECT_EQ(cupoch_pc->HasColors(), false);
  for (unsigned int i = 0; i < 5; i++)
  {
    const Eigen::Vector3f &point = cupoch_pc_points_host[i];
    EXPECT_EQ(point(0), 0.5 * i);
    EXPECT_EQ(point(1), i * i);
    EXPECT_EQ(point(2), 10.5 * i);
  }
}

TEST(ConversionFunctions, rosToCupoch_colored)
{
  sensor_msgs::PointCloud2 ros_pc2;
  ros_pc2.header.frame_id = "ros";
  ros_pc2.height = 1;
  ros_pc2.width = 5;
  ros_pc2.is_bigendian = false;
  ros_pc2.is_dense = true;
  sensor_msgs::PointCloud2Modifier modifier(ros_pc2);
  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
  modifier.resize(5 * 1);

  sensor_msgs::PointCloud2Iterator<float> mod_x(ros_pc2, "x");
  sensor_msgs::PointCloud2Iterator<float> mod_y(ros_pc2, "y");
  sensor_msgs::PointCloud2Iterator<float> mod_z(ros_pc2, "z");
  sensor_msgs::PointCloud2Iterator<uint8_t> mod_r(ros_pc2, "r");
  sensor_msgs::PointCloud2Iterator<uint8_t> mod_g(ros_pc2, "g");
  sensor_msgs::PointCloud2Iterator<uint8_t> mod_b(ros_pc2, "b");

  for (int i = 0; i < 5; ++i, ++mod_x, ++mod_y, ++mod_z, ++mod_r, ++mod_g, ++mod_b)
  {
    *mod_x = 0.5 * i;
    *mod_y = i * i;
    *mod_z = 10.5 * i;
    *mod_r = 2 * i;
    *mod_g = 5 * i;
    *mod_b = 10 * i;
  }

  const sensor_msgs::PointCloud2ConstPtr &ros_pc2_ptr = boost::make_shared<sensor_msgs::PointCloud2>(ros_pc2);
  auto cupoch_pc = std::make_shared<cupoch::geometry::PointCloud>();
  cupoch_conversions::rosToCupoch(ros_pc2_ptr, cupoch_pc);

  auto cupoch_pc_points_host = cupoch_pc->GetPoints();
  auto cupoch_pc_colors_host = cupoch_pc->GetColors();
  EXPECT_EQ(ros_pc2_ptr->height * ros_pc2_ptr->width, cupoch_pc_points_host.size());
  EXPECT_EQ(cupoch_pc->HasColors(), true);
  for (unsigned int i = 0; i < 5; i++)
  {
    const Eigen::Vector3f &point = cupoch_pc_points_host[i];
    EXPECT_FLOAT_EQ(point(0), 0.5 * i);
    EXPECT_FLOAT_EQ(point(1), i * i);
    EXPECT_FLOAT_EQ(point(2), 10.5 * i);
    const Eigen::Vector3f &color = cupoch_pc_colors_host[i];
    EXPECT_FLOAT_EQ(color(0), 2 * i / 255.0);
    EXPECT_FLOAT_EQ(color(1), 5 * i / 255.0);
    EXPECT_FLOAT_EQ(color(2), 10 * i / 255.0);
  }
}