#include "../Header/Detection_classes.h"

using namespace cv;
using namespace std;

Manual_ROI::Manual_ROI() {
	cout << "Pnt_ROI object created." << endl;
}

Manual_ROI::~Manual_ROI(void) {
	cout << "Pnt_ROI object desructed." << endl;
}

void Manual_ROI::Manual_ROI_init(int user_pnt_num) {
	pnt_num = user_pnt_num;
	current_idx = 0;
	pnt_ROI = std::vector<Point2f>(pnt_num);
	cout << "Pnt_ROI object created." << endl;
}

myKeyPnt::myKeyPnt() {
	cout << "myKeyPnt# object created." << endl;
}

myKeyPnt::~myKeyPnt(void) {
	cout << "myKeyPnt# " << keypnt_idx << " object desructed." << endl;
}

void myKeyPnt::configKeyPnt(int usr_window_size_h, unsigned int usr_idx, Point2f* pntInfo) {
	window_size_h = usr_window_size_h;
	keypnt_idx = usr_idx;
	keypnt = pntInfo;
	cout << "myKeyPnt# " << keypnt_idx << " object configured." << endl;
}

void myKeyPnt::ROI_range_casting(int row, int col) {
	// return an array[4], with the window xmin, xl, ymin, yl
	int xmin = (int)keypnt->x - window_size_h;
	int xmax = (int)keypnt->x + window_size_h;
	int ymin = (int)keypnt->y - window_size_h;
	int ymax = (int)keypnt->y + window_size_h;

	if (xmin < 0) { window_info.x = 0;} 
	else { window_info.x = xmin; }

	if (xmax > col - 1) { window_info.width = col - 1 - xmin; }
	else { window_info.width = 2 * window_size_h; }

	if (ymin < 0) { window_info.y = 0; }
	else { window_info.y = ymin; }

	if (ymax > row - 1) { window_info.height = row - 1 - ymin;}
	else { window_info.height = 2 * window_size_h;}

}

int myKeyPnt::detectROI(Mat *im_bgr, bool DEBUG) {

	bool LOCAL_DEBUG = 0; // local print and plot
	bool ELLIPSE_REFINE = 0; // refine ellipse (1) or not (0)

	/*if (LOCAL_DEBUG) {
		cout << "1 window_x: " << window_info.x << " window_y: " << window_info.y << endl;
		cout << "1 kpoint_x: " << keypnt->x << " kpoint_y: " << keypnt->y << endl;
	}*/

	Mat sub_img_bgr = (*im_bgr)(window_info); // linked to the original image (gray in BGR for debug visualize)

	Mat sub_img(sub_img_bgr.size(), CV_8UC1); // local image for ellipse detection (gray)
	cvtColor(sub_img_bgr, sub_img, CV_BGR2GRAY);
	Mat sub_img_refine = sub_img.clone(); // local image for ellipse refinement


	//ellipse refinement
	// ------------ Adaptive thresholding --------------------------------------
	int half_kernel_size = window_size_h/2;
	int minus_c = 30;// -30;// 11;
	//cv::adaptiveThreshold(sub_img, sub_img, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 2 * half_kernel_size + 1, minus_c);
	cv::adaptiveThreshold(sub_img, sub_img, 255, cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY, 51, minus_c); // fix window size for adaptive threshold
	if (LOCAL_DEBUG)
	{
		cv::imshow("Adaptive thresholding", sub_img);
	}

	// ------------ Contour extraction --------------------------------------
	std::vector<std::vector<cv::Point> > contours;
	cv::findContours(sub_img, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
	if (LOCAL_DEBUG)
	{

		for (size_t i = 0; i < contours.size(); i++)
			cv::drawContours(sub_img, contours, (int)i, cv::Scalar(0, 0, 255), 1, 8);
		cv::imshow("Contours", sub_img);
	}

	//------------Ellipse extraction--------------------------------------
	
	bool add_ellipse = 0;
	int contour_idx = 0;
	cv::RotatedRect box;
	float box_angle;
	cv::Mat pointsf;
	size_t count;
	const int count_thres = 50;//40
	int min_ellipse_size = 20; //3; Min ellipse size at 70cm distance is 20x20 pixels
	int max_ellipse_aspect_ratio = 3;
	double box_max;
	double box_min;
	double aspect_ratio;

	for (size_t i = 0; i < contours.size(); i++)
	{
		count = contours[i].size();
		if (count < count_thres)
			continue;

		cv::Mat(contours[i]).convertTo(pointsf, CV_32F);
		box = cv::fitEllipse(pointsf);
		box_angle = box.angle;
		/*if (box.size.width < box.size.height) {
			box_angle = 90. + box_angle;
		}*/

		// Plausibility checks
		box_max = std::max(box.size.width, box.size.height);
		box_min = std::min(box.size.width, box.size.height);
		aspect_ratio = box_max / box_min;
		
		/*if (LOCAL_DEBUG) {
			cout << "count: " << count << endl;
			cout << "box_min: " << box_min << "box_max: " << box_max << endl;
		}*/

		if (aspect_ratio > max_ellipse_aspect_ratio)
			continue;
		if (box_max < min_ellipse_size)
			continue;
		if (box.center.x < box_max /2 || box.center.x >= sub_img.cols- box_max / 2 || box.center.y < box_max / 2 || box.center.y >= sub_img.rows- box_max / 2)
			continue;
		// check blockage
		if (aspect_ratio > 2 && (box_angle < 40 || box_angle > 130))
			continue;

		add_ellipse = 1;
		contour_idx = (int)i;
		break;

	}
	if (add_ellipse) {
		// Assign new keypoint positions
		keypnt->x = (float)box.center.x + (float)window_info.x;
		keypnt->y = (float)box.center.y + (float)window_info.y;

		if (DEBUG) {
			ellipse(sub_img_bgr, box, cv::Scalar(0, 255, 0), 1, CV_AA);
			//ellipse(sub_img_bgr, box.center, box.size*0.5f, box.angle, 0, 360, cv::Scalar(0, 255, 255), 1, CV_AA);

			//// Put text
			//stringstream box_angle_text;
			//box_angle_text << (int)box_angle;
			//stringstream aspect_ratio_text;
			//aspect_ratio_text << (box_max/box_min);
			//if (box_angle < 70 || box_angle > 110) {
			//	putText(*im_bgr, box_angle_text.str(), Point((int)keypnt->x, (int)keypnt->y), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), 4);
			//	putText(*im_bgr, aspect_ratio_text.str(), Point((int)keypnt->x, (int)keypnt->y - 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), 4);
			//}
			//else {
			//	putText(*im_bgr, box_angle_text.str(), Point((int)keypnt->x, (int)keypnt->y), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 4);
			//	putText(*im_bgr, aspect_ratio_text.str(), Point((int)keypnt->x, (int)keypnt->y - 50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 4);
			//}
			
			
			if (LOCAL_DEBUG) {
				//cout << "2 kpoint_x: " << keypnt->x << " kpoint_y: " << keypnt->y << endl;

				ellipse(sub_img_bgr, box, cv::Scalar(0, 255, 0), 1, CV_AA);
				cv::drawContours(sub_img_bgr, contours, contour_idx, cv::Scalar(0, 0, 255), 1, 8);
				cv::imshow("Ellipses", sub_img_bgr);
				cv::waitKey(0);
			}

			rectangle(*im_bgr, window_info, Scalar(255, 0, 0), +1, 4);
			cv::circle(*im_bgr, Point((int)keypnt->x, (int)keypnt->y), 1, Scalar(0, 255, 0), 1, 4);
		}

		if (ELLIPSE_REFINE) {
			//------------Ellipse refinement--------------------------------------
			int min_matching_lines = 4;
			int sobel_winsize = 3;
			float gauss_smooth_sigma = 3.0;

			if (sobel_winsize % 2 == 0) {
				sobel_winsize++;
				std::cerr << " Sobel winsize changed to " << sobel_winsize << " (must be odd)" << std::endl;
			}

			// Sobel
			cv::Mat input_image_smooth;
			if (gauss_smooth_sigma > 0.0f) {
				cv::GaussianBlur(sub_img_refine, input_image_smooth, cv::Size(), gauss_smooth_sigma, gauss_smooth_sigma);
			}
			else {
				input_image_smooth = sub_img_refine.clone();
			}

			cv::Mat gradient_x(input_image_smooth.rows, input_image_smooth.cols, CV_16S);
			cv::Mat gradient_y(input_image_smooth.rows, input_image_smooth.cols, CV_16S);
			cv::Sobel(input_image_smooth, gradient_x, CV_16S, 1, 0, sobel_winsize);
			cv::Sobel(input_image_smooth, gradient_y, CV_16S, 0, 1, sobel_winsize);


			cv::Matx33d refined_ellipse;

			if (cv::runetag::ellipserefine(box, gradient_x, gradient_y, refined_ellipse))
			{
				cv::Point2d ellipse_center = cv::runetag::ellipseCenter(refined_ellipse);
				keypnt->x = (float)ellipse_center.x + (float)window_info.x;
				keypnt->y = (float)ellipse_center.y + (float)window_info.y;
				if (DEBUG) {
					cv::circle(sub_img_bgr, ellipse_center, 1, CV_RGB(255, 0, 0));
					if (LOCAL_DEBUG) {
						cv::imshow("Refined ellipses", sub_img_bgr);
						cv::waitKey(0);
					}
				}
			}
			else
			{
				std::cerr << "Ellipse refine failed" << std::endl;
			}
		}
		
		return 0;
		
	}
	return -1;
	
}