#include "Detector.hpp"

using namespace cv;

void Detector::detect(Mat thresholdFrame, Mat &output){
    /* TASK: Try to understand this data structure! */
    contour_vector_t contours;

    /* TASK: Research the different parameters it takes and play around with them */
    findContours(thresholdFrame, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

    //DEBUG
    //drawContours(imgFiltered, contours, -1, Scalar(0, 255, 0), 4);

    // size is always positive, so unsigned int -> size_t; if you have not
    // initialized the vector it is -1, hence crash
    for (size_t k = 0; k < contours.size(); k++) {

      contour_t approx_contour;
      approxPolyDP(contours[k], approx_contour, arcLength(contours[k], true) * 0.02, true);

      // Convert to a usable rectangle
      Rect r = boundingRect(approx_contour);

      // TASK: 4 Corners -> How to color them in the frame?
      if(approx_contour.size() != 4){
        continue;
      }

      // --- Filter tiny ones --- If the found contour is too small (20 ->
      // pixels, frame.cols - 10 to prevent extreme big contours)
      if (r.width < 20 || r.height < 20 || r.width > output.cols - 10 || r.height > output.cols - 10) {
         continue;
      }

      //DEBUG
      //rectangle(filteredFrame, r, Scalar(0,0,255),4);

      drawRectangle(output, approx_contour);
    }
  }

void Detector::drawRectangle(Mat &output, contour_t contour){
      // -> Cleaning done!

      // 1 -> 1 contour, we have a closed contour, true -> closed , 4 ->
      // thickness
      polylines(output, contour, true, Scalar(0,0,255), THICKNESS_VALUE);

      // -----------------------------

      // --- Process Corners ---
      for (size_t i = 0; i < 4; i++) {
        // Render the corners, 3 -> Radius, -1 filled circle
        circle(output, contour[i], 3, CV_RGB(0, 255, 0), -1);

        // Euclidic distance, 7 -> parts, both directions dx and dy
        /* TASK: How to achieve that? */
        double dx = (double)(contour[(i+1)%4].x - contour[i].x) / 7;
        double dy = (double)(contour[(i+1)%4].y - contour[i].y) / 7;

        // First point already rendered, now the other 6 points
        for (size_t j = 1; j < 7; j++) {
          // Position calculation
          double px = dx * j + contour[i].x;
          double py = dy * j + contour[i].y;


          Point p;
          p.x = (int)px;
          p.y = (int)py;


          cv::Point2d correction;
          correction = sobelFilter.getSubPixelPoint(dx, dy, contour.at(i), p, output);

          correction.x += p.x;
          correction.y += p.y;

          circle(output, correction, 3, Scalar(255,0,0), -1);
          //circle(output, p, 3, CV_RGB(0, 0, 255));
        }

        sobelFilter.drawStrips(output);
      }
}
