#include "opencv2/opencv.hpp"
#include <math.h>
#include <algorithm>
#include "math.hpp"

int filter(const cv::Mat& img, cv::Mat output, int seuil) {
	bool detect = false;
	uchar R, G, B;
	int rows = img.rows;
	int cols = img.cols;
	int dim = img.channels();
	int indexNB;

	for (int index=0,indexNB=0;index<dim*rows*cols;index+=dim,indexNB++) {
		detect=0;
		B = img.data[index    ];
		G = img.data[index + 1];
		R = img.data[index + 2];

		if ((R>G) && (R>B))
			if (((R-B)>=seuil) || ((R-G)>=seuil))
				detect=1;
		if (detect==1)
			output.data[indexNB]=255;
		else
			output.data[indexNB]=0;
	}
}

int main(int argc, char** argv) {
	int seuil=80;
	int cmax = 10;
	int N = 5000;

	if (argc > 1) {
		seuil = atol(argv[1]);
	}

	char detect;
	cv::VideoCapture cap(0); 
	if(!cap.isOpened())  
		return -1;
	cv::namedWindow("Image",1);
	cv::namedWindow("Detection",1);
	cv::namedWindow("Contours",1);
	cv::namedWindow("New Contours",1);
	while(true) {
		int X, Y, DIM, index;
		unsigned int numc;
		uchar R, G, B;
		std::vector<std::vector<cv::Point>> contours;
		std::vector<cv::Vec4i> hierarchy;
		cv::Mat frame;
		cap >> frame;
		X=frame.rows;
		Y=frame.cols;
		cv::Mat binaire(X,Y,CV_8UC1);
		cv::imshow("Image", frame);
		cv::GaussianBlur(frame, frame, cv::Size(7,7), 1.5, 1.5);
		X=frame.rows;
		Y=frame.cols;
		DIM=frame.channels();

		filter(frame, binaire, seuil);

		cv::imshow("Detection", binaire);
		cv::findContours(binaire, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
		cv::Mat Dessin = cv::Mat::zeros(X,Y, CV_8UC1);
		cv::Mat new_contour_image = cv::Mat::zeros(X,Y, CV_8UC1);

    if (contours.size() > 0) {
		  std::vector<std::vector<cv::Point>> contrs;

      int id = math::max_cont(contours);
      contrs.push_back(contours[id]);

      std::cout << "Number of countours: "
                << contours.size()
                << "; Index of biggest contour: "
                << id
                << std::endl;

      contrs.push_back(math::simplify_contour(contrs[0], cmax));

      cv::drawContours(Dessin, contrs, 0, 255);
      cv::drawContours(new_contour_image, contrs, 1, 255);
    }
		cv::imshow("Contours", Dessin);
		cv::imshow("New Contours", new_contour_image);
		if(cv::waitKey(30) == 27) {
			break;
		}
	}
	return 0;
}