#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/String.h>
#include <dynamic_reconfigure/server.h>

#include <gesture_based_control/DescriptorsConfig.h>
#include "math.hpp"

void callback(gesture_based_control::DescriptorsConfig &config, uint32_t level, int& cmax, int& threshold) {
  cmax = config.cmax;
  threshold = config.threshold;
}

void callback(const sensor_msgs::ImageConstPtr& msg, int& threshold, int& cmax, ros::Publisher& order_pub, image_transport::Publisher& image_pub) {
  cv_bridge::CvImagePtr cv_ptr;
  std_msgs::String s;

  try {
    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
  }catch(cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return; 
  }

  cv::Mat& input = cv_ptr->image;
  cv::Mat binaire(input.rows, input.cols, CV_8UC1);
  math::filter(input, binaire, threshold);
  cv::GaussianBlur(input, input, cv::Size(7,7), 1.5, 1.5);
  
  std::vector<math::contour> contours;
  std::vector<cv::Vec4i> hierarchy;
  cv::findContours(binaire, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
  if (contours.size() != 0) {
    int index = math::max_cont(contours);
    math::csignal desc = math::descriptors(contours[index], cmax);
    //TODO: implémenter algo ml
    //TODO: publier résultat
    int r = 0;
    switch (r) {
      case 0:
        s.data = "left";
        break;
      case 1:
        s.data = "right";
        break;
      case 2:
        s.data = "stop";
        break;
      case 3:
        s.data = "forward";
        break;
      default:
        s.data = "None";
        break;
    }
    order_pub.publish(s);

    cv::drawContours(input, contours, index, 255);
  }
  image_pub.publish(cv_bridge::CvImage(msg->header, "rgb8", input).toImageMsg());
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "descripteur");

  int threshold = 25;
  int cmax = 10;

  ros::NodeHandle n;
  image_transport::ImageTransport it(n);

  ros::Publisher order_pub = n.advertise<std_msgs::String>("/order", 100);
  image_transport::Publisher image_pub = it.advertise("/image_out", 1);
  image_transport::Subscriber sub = it.subscribe("/image_raw", 1, boost::bind(callback, _1, boost::ref(threshold), boost::ref(cmax), boost::ref(order_pub), boost::ref(image_pub)));

  dynamic_reconfigure::Server<gesture_based_control::DescriptorsConfig> server;
  dynamic_reconfigure::Server<gesture_based_control::DescriptorsConfig>::CallbackType f;

  f = boost::bind(&callback, _1, _2, boost::ref(cmax), boost::ref(threshold));
  server.setCallback(f);
  
  ros::spin();
}