#include "sensors_com.h"
#include "udp_interface.h"
#include "communication.h"
extern "C" {
#include "log.h"
}

#include <opencv2/objdetect.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/aruco.hpp>

#include <iostream>
#include <stdio.h>
#include <ctime>
#include <math.h>
#include <signal.h>
#include <pthread.h>

using namespace std;
using namespace cv;

#define IMG_PREVIEW
#define FRAME_WIDTH   320
#define FRAME_HEIGHT  240



void interrupt_handler(int sig);


VideoCapture stream(0);
pthread_t communication_thread;
Point the_center(0, 0);
extern object_position_t object_pos;

int main( void )
{
  char buf[256];
  sprintf(buf, "OpenCV version %d.%d", CV_MAJOR_VERSION, CV_MINOR_VERSION);

  log_info(buf);

  signal(SIGINT, interrupt_handler);
  Mat frame_cam(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3);
  Ptr<aruco::Dictionary> marker_dictionary = aruco::getPredefinedDictionary(0);
  aruco::DetectorParameters parameters;
  vector<int> marker_ids, rejected_candidates;
  vector<vector<Point_<float>>> marker_corners;  // Point2f
  vector<vector<Point3f>> rotation_vectors;
  vector<vector<Point3f>> translation_vectors;

  stream.set(CV_CAP_PROP_FRAME_WIDTH, FRAME_WIDTH);
  stream.set(CV_CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT);
  stream.set(CV_CAP_PROP_FORMAT, CV_8UC3);

  if (!stream.isOpened())
  {
    log_error("Failed to open video stream...exiting");
    return 1;
  }
  else
  {
    log_info("Video stream has been opened");
  }

  const char serial_port[] = "/dev/ttyS0";
  if (sensors_data_init(serial_port, 921600) == -1)
  {
    log_error("Failed to open serial port: %s, exiting", serial_port);
    //return 1;
  }
  else
  {
    log_info("Serial port %s has been opened", serial_port);
  }

  if (udp_interface_init() == -1)
  {
    log_error("Error initilising UDP interface...exiting");
    return 1;
  }
  else
  {
    log_info("UDP inteface initilised", serial_port);
  }

  pthread_create(
    &communication_thread,
    NULL,
    communication_handler_thread,
    NULL
  );

  while (true)
  {
    stream.grab();
    stream.retrieve(frame_cam);
    if (frame_cam.empty() == true)
    {
      log_info("Couldn't read new image...exiting");
      return 1;
    }

    /*
     * FRAME PROCESSING
     */

    aruco::detectMarkers(
      frame_cam,
      marker_dictionary,
      marker_corners,
      marker_ids,
      &parameters,
      rejected_candidates
    );
    aruco::drawDetectedMarkers(frame_cam, marker_corners, marker_ids);

    /*
    // pose estimation requires properly calibrated cam
    aruco::estimatePoseSingleMarkers(
      marker_corners,
      0.05,                                  // size of markers side in meters
      camera_matrix, dist_coeffs,            // cam calibration params
      rotation_vectors, translation_vectors  // rotation, translation vectors
    );
    for (uint8_t i = 0; i < marker_ids.size(); ++i) {
      aruco::drawAxis(
        frame_cam,
        camera_matrix, dist_coeffs,
        rotation_vectors[i], translation_vectors[i],
        0.1                                  // length of the axis
      );
    }
    */

#ifdef IMG_PREVIEW
    imshow("Video Capture", frame_cam);

    if (waitKey(10) >= 0)
    {
      communication_handler_thread_stop();
      stream.release();
      break;
    }
#endif

  }
  return 0;
}


void interrupt_handler(int sig)
{
  log_warn("SIGINT...exiting");
  communication_handler_thread_stop();
  pthread_join(communication_thread, NULL);
  sensors_data_cleanup();
  stream.release();
  exit(0);
}