#include<numeric>
#include "Road_segmentation.h"


using namespace RoadStateMonitor;

size_t CRoad_segmentation::id = 0;

/*
použitie Cannyho detektoru s dopomocou trackbarov pre presnejšiu segmentáciu
*/
Mat CRoad_segmentation::Canny_track(const Mat& input) const
{
	cv::namedWindow("Canny");
	cv::resizeWindow("Canny", 500, 500);
	int tresh1 = 0;
	int tresh2 = 0;
	cv::createTrackbar("Tresh1", "Canny", 0, 255);
	cv::createTrackbar("Tresh2", "Canny", 0, 255);
	cv::Mat canny_mat;
	while (1)
	{
		int tresh1 = cv::getTrackbarPos("Tresh1", "Canny");
		int tresh2 = cv::getTrackbarPos("Tresh2", "Canny");

		cv::Canny(input, canny_mat, tresh1, tresh2, 3);
		cv::imshow("Canny", canny_mat);
		char key = cv::waitKey(1) & 0xFF;
		if (key == ('q'))
		{
			cv::destroyWindow("Canny");
			break;
		}
	}
	return canny_mat;
}

/*
automatická voľba prahov Cannyho detektoru
*/
Mat CRoad_segmentation::Canny(const Mat& input) const
{
	cv::Mat tresh, canny_mat, gray;
	cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY);

	double high_tresh{};

	if (road_indexes.empty())
	{
		double otsu_thresh_val = cv::threshold(gray, tresh, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);

		high_tresh = otsu_thresh_val;
	}

	cv::Canny(input, canny_mat, high_tresh / 2, high_tresh);
	//cv::imshow("Canny", canny_mat);
	//cv::waitKey();
	return canny_mat;
}


/*
segmentácia vozovky Houghovou transformáciou
*/
void CRoad_segmentation::Hough_segmentation(const Mat& input)
{
	//cv::imwrite("HOUGH1.2.png", input);
	Mat canny = CRoad_segmentation::Canny_track(input);
	//cv::imwrite("HOUGH2.2.png", canny);
	//cv::dilate(canny, canny, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)), cv::Point(-1, -1), 1);

	//cv::erode(canny, canny, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)), cv::Point(-1, -1), 1);


	//Mat canny = CRoad_segmentation::Canny(input);
	std::vector<cv::Vec4i> lines;
	cv::HoughLinesP(canny, lines, 1, CV_PI / 180, 100, 100, 30);
	Mat out = input.clone();
	for (size_t i = 0; i < lines.size(); i++)
	{
		cv::Vec4i l = lines[i];
		cv::line(out, cv::Point(l[0], l[1]), cv::Point(l[2], l[3]), cv::Scalar(0, 255, 0), 2, cv::LINE_4);
	}

	finish_segmentation(canny, input);
	
}


Mat CRoad_segmentation::HSV_split(const Mat& input) const
{
	vector<Mat> hsv;
	Mat input_HSV;
	cvtColor(input, input_HSV, cv::COLOR_BGR2HSV);
	split(input_HSV, hsv);
	return hsv[2]; //2
}

/*
segmentácia vozovky použitím HSV modelu pomocou trackbaru
*/

void CRoad_segmentation::HSV_segmentation(const Mat& input)
{
	Mat hsv = HSV_split(input);
	
	cv::namedWindow("HSV");
	cv::resizeWindow("HSV", 500, 500);

	cv::createTrackbar("LowH", "HSV", 0, 179); //Hue (0 - 179)
	cv::createTrackbar("HighH", "HSV", 0, 179);
	

	Mat out;
	while (1)
	{
		int lHue = cv::getTrackbarPos("LowH", "HSV");
		int hHue = cv::getTrackbarPos("HighH", "HSV");



		cv::inRange(hsv, cv::Scalar(lHue), cv::Scalar(hHue), out);
		cv::imshow("HSV", out);
		char key = cv::waitKey(1) & 0xFF;
		if (key == ('q'))
		{
			cv::destroyWindow("HSV");
			break;
		}
	}
	cv::morphologyEx(out, out, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)), cv::Point(-1, -1), 1);
	//cv::imshow("binary", out);
	finish_segmentation(out, input);
}


/*
segmentácia vozovky Cannyho detektorom
*/
void CRoad_segmentation::Canny_segmentation(const Mat& input)
{
	Mat canny = CRoad_segmentation::Canny_track(input);
	//cv::imshow("canny", canny);
	//cv::imwrite("CANNY1.png", input);
	//cv::imwrite("CANNY2.png", canny);
	//Mat canny = CRoad_segmentation::Canny(input);
	cv::morphologyEx(canny, canny, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)), cv::Point(-1, -1), 1);
	cv::bitwise_not(canny, canny);
	//cv::imshow("negation", canny);
	//cv::imwrite("CANNY3.png", canny);
	finish_segmentation(canny, input);

}

/*
posledný krok segmentácie - vyhľadanie kontúr a uloženie oblasti vozovky a jej indexov
*/
void CRoad_segmentation::finish_segmentation(const Mat& binary_input, const Mat& input)
{
	Mat out_binary;
	//cv::morphologyEx(binary_input, out_binary, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)), cv::Point(-1, -1), 1);
	//cv::imshow("out_binary2", out_binary);
	Mat copy_input = input.clone();
	segmented_image = Contours(binary_input, copy_input);
	set_indexes(segmented_image);
	set_segmented_road(input);
	//cv::imshow("input3", input);
}


/*
vyhľadávanie kontúr a ošetrenie voči označeniu oblohy ako vozovky
*/
Mat CRoad_segmentation::Contours(const Mat& binary_input, const Mat& input) const
{
	std::vector<std::vector<cv::Point>> conts;
	cv::findContours(binary_input, conts, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

	std::vector<int> indexes(conts.size());
	std::iota(indexes.begin(), indexes.end(), 0);

	sort(indexes.begin(), indexes.end(), [&conts](int lhs, int rhs)
		{
			return conts[lhs].size() > conts[rhs].size();
		});

	

	int tmp{}, index{};
	for (int i = 0; i < conts.size(); i++)
	{

		for (auto p : conts[indexes[i]])
		{

			if (p.y <= (binary_input.cols / 5))
			{
				tmp++;
			}
		}

		
		if (tmp >= conts[indexes[i]].size() * 2 /*/5*/)
		{
			tmp = 0;
			continue;
		}
		else
		{
			index = i;
			break;
		}
	}
	Mat out = input;

	drawContours(out, conts, static_cast<int>(indexes[index]), cv::Scalar(0, 255, 0), cv::FILLED, 0);
	
	//cv::imshow("out", out);
	//cv::imwrite("HSVcont[2].png", out);
	return out;
}

void CRoad_segmentation::set_indexes(const Mat& segment_input)
{
	for (int i{}; i < segment_input.rows; ++i)
		for (int j{}; j < segment_input.cols; ++j)
			if ((segment_input.at<cv::Vec3b>(i, j)[0] == 0) && (segment_input.at<cv::Vec3b>(i, j)[1] == 255) && (segment_input.at<cv::Vec3b>(i, j)[2] == 0))
				road_indexes.push_back(cv::Point(i, j));
}

void CRoad_segmentation::set_segmented_road(const Mat& input)
{
	Mat gray;
	cv::cvtColor(input, gray, cv::COLOR_BGR2GRAY);
	const int n = static_cast<int>(sqrt(road_indexes.size())+1);

	Mat road(n, n, CV_8UC1);

	int i{}, j{};
	for (auto& a : road_indexes)
	{
		road.at<uchar>(i, j) = gray.at<uchar>(a.x, a.y);
		j++;
		if (j == road.cols)
		{
			j = 0;
			i++;
			if (i == road.rows)
			{
				for(int r{i}; r < road.rows; ++i)
					for (int c{j}; c < road.cols; ++c)
						road.at<uchar>(r, c) = gray.at<uchar>(road_indexes[road_indexes.size()/2]);
			}
		}
	}
	segmented_road = road;
	//return cesta;
}
void CRoad_segmentation::set_segmented_image(const Mat& input)
{
	segmented_image = input;
	for (const auto& point : road_indexes)
	{
		segmented_image.at<cv::Vec3b>(point.x, point.y)[0] = 0;
		segmented_image.at<cv::Vec3b>(point.x, point.y)[1] = 255;
		segmented_image.at<cv::Vec3b>(point.x, point.y)[2] = 0;
	}
}
void CRoad_segmentation::segmentation(const Mat& input, ESegment_method method)
{
	if (method == ESegment_method::HSV) HSV_segmentation(input);
	else if (method == ESegment_method::CANNY) Canny_segmentation(input);
	else Hough_segmentation(input);
}





