//
// Created by UnknownObject on 2022/9/21.
// Copyright (c) 2022 UnknownNetworkService. All rights reserved.
//

#include "traffic_light.h"

namespace uns
{
	int TrafficLight::GetCircle(cv::Mat& img)
	{
		cv::Mat gray;
		cvtColor(img, gray, cv::COLOR_BGR2GRAY);
		GaussianBlur(gray, gray, cv::Size(9, 9), 2, 2);  //平滑滤波
		//检测圆形
		std::vector<cv::Vec3f> circles;
		double dp = 2.5; //
		double minDist = 10;  //两个圆心之间的最小距离
		double  param1 = 100;  //Canny边缘检测的较大阈值
		double  param2 = 100;  //累加器阈值
		int min_radius = 20;  //圆形半径的最小值
		int max_radius = 200;  //圆形半径的最大值
		HoughCircles(gray, circles, cv::HOUGH_GRADIENT, dp, minDist, param1, param2,min_radius, max_radius);
		int circle_r_max = 0;
		cv::Point max_circle_center = cv::Point(0, 0);
		for (size_t i = 0; i < circles.size(); i++)
		{
			int radius = cvRound(circles[i][2]);
			//circle(img, Point(cvRound(circles[i][0]), cvRound(circles[i][1])), radius, Scalar(0, 0, 0), 4, 8, 0);
			if (radius > circle_r_max)
			{
				circle_r_max = radius;
				max_circle_center = cv::Point(cvRound(circles[i][0]), cvRound(circles[i][1]));
			}
		}
		CutImage(img, circle_r_max, cv::Point(max_circle_center.y, max_circle_center.x));
		return 0;
	}
	
	bool TrafficLight::CheckRGB(cv::Vec3b point)
	{
		if (point[0] < 240)
			if (point[1] < 240)
				if (point[2] < 240)
					return true;
		return false;
	}
	
	int TrafficLight::tri_max(int a, int b, int c)
	{
		return std::max(std::max(a, b), c);
	}
	
	void TrafficLight::ImageProcess(cv::Mat& image)
	{
		for (int i = 0; i < image.rows; i++)
		{
			for (int j = 0; j < image.cols; j++)
			{
				if (CheckRGB(image.at<cv::Vec3b>(i, j)))
				{
					image.at<cv::Vec3b>(i, j)[0] = 255;
					image.at<cv::Vec3b>(i, j)[1] = 255;
					image.at<cv::Vec3b>(i, j)[2] = 255;
				}
			}
		}
		return;
	}
	
	bool TrafficLight::ApproximatelyEqual(int a, int b, int offset)
	{
		if (a == b)
			return true;
		if (((a + offset) >= b) && ((a - offset) <= b))
			return true;
		else
			return false;
	}
	
	void TrafficLight::CutImage(cv::Mat& image,int radius,cv::Point center)
	{
		cv::Mat result(radius * 2, radius * 2, image.type(), cv::Scalar(0, 0, 0));
		cv::Point start(center.x - radius, center.y - radius);
		int x_end = center.x + radius;
		int y_end = center.y + radius;
		for (int i = start.x; i < x_end; i++)
		{
			for (int j = start.y; j < y_end; j++)
			{
				result.at<cv::Vec3b>(i - start.x, j - start.y)[0] = image.at<cv::Vec3b>(i, j)[0];
				result.at<cv::Vec3b>(i - start.x, j - start.y)[1] = image.at<cv::Vec3b>(i, j)[1];
				result.at<cv::Vec3b>(i - start.x, j - start.y)[2] = image.at<cv::Vec3b>(i, j)[2];
			}
		}
		result.copyTo(image);
		return;
	}
	
	void TrafficLight::CountMax(cv::Vec3b data, int& rmax, int& gmax, int& bmax)
	{
		if ((data[0] == data[1]) && (data[1] == data[2]))
			return;
		uchar _max = tri_max(data[0], data[1], data[2]);
		if (_max == data[0])
			bmax++;
		if (_max == data[1])
			gmax++;
		if (_max == data[2])
			rmax++;
		return;
	}
	
	Light::TL_COLOR TrafficLight::GetColor(cv::Mat &image, int offset)
	{
		ImageProcess(image);
		GetCircle(image);
		int rmax_count = 0, gmax_count = 0, bmax_count = 0;
		for (int i = 0; i < image.rows; i++)
			for (int j = 0; j < image.cols; j++)
				CountMax(image.at<cv::Vec3b>(i, j), rmax_count, gmax_count, bmax_count);
		if (ApproximatelyEqual(rmax_count, gmax_count, offset))
			return Light::TL_COLOR::Yellow;
		int max_max = tri_max(rmax_count, gmax_count, bmax_count);
		if (max_max == rmax_count)
			return Light::TL_COLOR::Red;
		else if ((max_max == gmax_count) || (max_max == bmax_count))
			return Light::TL_COLOR::Green;
		return Light::TL_COLOR::Yellow;
	}
	
	bool TrafficLight::MuchLarger(int a, int b, double rate)
	{
		return (a >= (b * rate));
	}
	
	void TrafficLight::CountColor(const cv::Mat &img, double rate, int &r_cnt, int &g_cnt, int &y_cnt)
	{
		r_cnt = 0;
		g_cnt = 0;
		y_cnt = 0;
		for (int i = 0; i < img.rows; i++)
		{
			for (int j = 0; j < img.cols; j++)
			{
				cv::Vec3b color = img.at<cv::Vec3b>(i, j);
				if (MuchLarger(color[2], color[1], rate) && MuchLarger(color[2], color[1], rate))
					r_cnt++;
				else if (MuchLarger(color[1], color[0], rate) && MuchLarger(color[1], color[2], rate))
					g_cnt++;
				else if (ApproximatelyEqual(color[1], color[2], rate))
					y_cnt++;
			}
		}
	}
	
	Light::TL_COLOR TrafficLight::Reco(cv::Mat &img)
	{
		/*int r = 0, g = 0, y = 0;
		GetCircle(img);
		CountColor(img, 2.5, r, g, y);
		int max = tri_max(r, g, y);
		if (max == r)
			return Light::TL_COLOR::Red;
		else if (max == g)
			return Light::TL_COLOR::Green;
		else
			return Light::TL_COLOR::Yellow;*/
		ImageProcess(img);
		if(img.empty())
			return Light::TL_COLOR::Null;
		img = SplitImage(img);
		if(img.empty())
			return Light::TL_COLOR::Null;
		return GetImageColor(img, 1.3);
	}
	
	cv::Mat TrafficLight::SplitImage(const cv::Mat &img)
	{
		int max_image_area = 0;
		cv::Rect max_image_rect;
		uns::Images::Contour approx;
		uns::Images::Contours contours;
		cv::Mat shape_image(img.size(), CV_8UC1);
		for (int r = 0; r < img.rows; r++)
		{
			for (int c = 0; c < img.cols; c++)
			{
				cv::Vec3b color = img.at<cv::Vec3b>(r, c);
				if ((color[0] > 240) && (color[1] > 240) && (color[2] > 240))
					shape_image.at<uchar>(r, c) = 255;
				else
					shape_image.at<uchar>(r, c) = 0;
			}
		}
		cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(11, 11));
		cv::morphologyEx(shape_image, shape_image, cv::MORPH_CLOSE, kernel);
		findContours(shape_image, contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE); //轮廓查找
		for (const auto& contour : contours)
		{
			if (cv::contourArea(contour) >= (img.total() / 2.0))
				continue;
			cv::Rect target_rect = boundingRect(contour);
			if (target_rect.area() > max_image_area)
				max_image_rect = target_rect;
		}
		return img(max_image_rect);
	}
	
	Light::TL_COLOR TrafficLight::GetImageColor(const cv::Mat &img, double rate)
	{
		int cnt_r = 0, cnt_g = 0, cnt_y = 0;
		for (int r = 0; r < img.rows; r++)
		{
			for (int c = 0; c < img.cols; c++)
			{
				uns::Colors::CVColor color(img.at<cv::Vec3b>(r, c));
				if (MuchLarger(color.GetR(), color.GetG(), rate) && MuchLarger(color.GetR(), color.GetB(), rate))
					cnt_r++;
				else if (MuchLarger(color.GetG(), color.GetR(), rate) && MuchLarger(color.GetG(), color.GetB(), rate))
					cnt_g++;
				else if (MuchLarger(color.GetB(), color.GetR(), rate) && MuchLarger(color.GetB(), color.GetG(), rate))
					cnt_g++;
				else if (MuchLarger(color.GetR(), color.GetB(), rate) && MuchLarger(color.GetG(), color.GetB(), rate))
					cnt_y++;
			}
		}
		int cnt_max = tri_max(cnt_r, cnt_g, cnt_y);
		if (cnt_max == cnt_r)
			return Light::TL_COLOR::Red;
		else if (cnt_max == cnt_g)
			return Light::TL_COLOR::Green;
		else
			return Light::TL_COLOR::Yellow;
	}
};

extern "C" JNIEXPORT
jint JNICALL
Java_com_uns_maincar_cpp_1interface_TrafficLight_Recognize(JNIEnv *env, jclass _this, jobject image, jint offset)
{
	cv::Mat img;
	if(!BitmapToMat(env,image,img))
		return 4;
	uns::TrafficLight traffic_light;
	switch(traffic_light.Reco(img))
	{
		case uns::Light::TL_COLOR::Red:
			return 0;
		case uns::Light::TL_COLOR::Yellow:
			return 1;
		case uns::Light::TL_COLOR::Green:
			return 2;
		case uns::Light::TL_COLOR::Null:
			return 3;
	}
	return 5;
}

extern "C" JNIEXPORT
jstring JNICALL
Java_com_uns_maincar_cpp_1interface_EnvTest_TrafficLightTest(JNIEnv *env, jclass _this)
{
	std::string version = "TrafficLightReco Found, Version: " + std::string(TRAFFIC_LIGHT_RECO_VERSION);
	return env->NewStringUTF(version.c_str());
}