#include "LineContinuityAndRGBDetection.h"
# define max(a, b, c)  a > b ? a > c ? a : c : b > c ? b : c
# define min(a, b, c)  a < b ? a < c ? a : c : b < c ? b : c
# define SET_BIT(x,n)   (x|(1U<<(n-1)))  //把x的第n位置1
# define CLEAR_BIT(x,n)   (x&~(1U<<(n-1)))  //把x的第n位清零
LineContinuityAndRGBDetection::LineContinuityAndRGBDetection()
{
}

int LineContinuityAndRGBDetection::isContinuous(const cv::Mat& image)
{
	cv::Mat thre;
	if (image.channels() != 1)
	{
		cv::Mat mv[3];
		cv::split(image, mv);
		cv::Mat gray = mv[0] | mv[1] | mv[2];
		cv::threshold(gray, thre, 70, 255, cv::THRESH_BINARY);
	}
	else
		cv::threshold(image, thre, 70, 255, cv::THRESH_BINARY);
	std::vector<std::vector<cv::Point>> contours;
	std::vector<cv::Vec4i> hierarchy;
	myFindContours(thre, contours, hierarchy, cv::RETR_TREE);
	return filterBlock(contours, hierarchy, cv::Size(image.cols, image.rows));
}


int LineContinuityAndRGBDetection::RGB_Judge(const cv::Mat& image)
{
	std::vector<cv::Scalar> RGB_Color;
	unsigned int RGB = 0;
	bool is_red = false;
	bool is_green = false;
	bool is_blue = false;
	cv::Mat mv[3];
	cv::split(image, mv);

	//R
	cv::Mat mask_r = (~mv[0]) & (~mv[1]) & mv[2];
	cv::threshold(mask_r, mask_r, 127, 255, cv::THRESH_BINARY);
	cv::Scalar color_r = cv::mean(image, mask_r);
	double rb = color_r[0];
	double rr = color_r[2];
	color_r[0] = rr;
	color_r[2] = rb;
	RGB_Color.push_back(color_r);

	//G
	cv::Mat mask_g = (~mv[0]) & mv[1] & (~mv[2]);
	cv::threshold(mask_g, mask_g, 127, 255, cv::THRESH_BINARY);
	cv::Scalar color_g = cv::mean(image, mask_g);
	double gb = color_g[0];
	double gr = color_g[2];
	color_g[0] = gr;
	color_g[2] = gb;
	RGB_Color.push_back(color_g);

	//B
	cv::Mat mask_b = mv[0] & (~mv[1]) & (~mv[2]);
	cv::threshold(mask_b, mask_b, 127, 255, cv::THRESH_BINARY);
	cv::Scalar color_b = cv::mean(image, mask_b);
	double bb = color_b[0];
	double br = color_b[2];
	color_b[0] = br;
	color_b[2] = bb;
	RGB_Color.push_back(color_b);

	//转换成HSV空间体系
	double max, min;
	std::vector<cv::Scalar> HSV_Color(RGB_Color.size());
	for (int i = 0; i < RGB_Color.size(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			RGB_Color[i][j] /= 255;
		}
		max = max(RGB_Color[i][0], RGB_Color[i][1], RGB_Color[i][2]);
		min = min(RGB_Color[i][0], RGB_Color[i][1], RGB_Color[i][2]);
		if (max == RGB_Color[i][0])//R最大
		{
			HSV_Color[i][0] = (60 * (RGB_Color[i][1] - RGB_Color[i][2]) / min) / 2;//H分量转换成opencv h分量，需除以2
		}
		else if (max == RGB_Color[i][1])//G最大
		{
			HSV_Color[i][0] = (120 + 60 * (RGB_Color[i][2] - RGB_Color[i][0]) / (max - min)) / 2;
		}
		else if (max == RGB_Color[i][2])//B最大
		{
			HSV_Color[i][0] = (240 + 60 * (RGB_Color[i][0] - RGB_Color[i][1]) / (max - min)) / 2;
		}
		if (HSV_Color[i][0] < 0)
		{
			HSV_Color[i][0] += 360 / 2;
		}
		HSV_Color[i][1] = (max - min) / max * 255;//S分量转换成opencv s分量，需乘以255
		HSV_Color[i][2] = max * 255;//V分量转换成opencv v分量，需乘以255

		//红色H分量范围156->180，根据实际扫描图片，改为130->180
		if (((HSV_Color[i][0] > 0 && HSV_Color[i][0] < 10) || (HSV_Color[i][0] > 130 && HSV_Color[i][0] < 180)) && (HSV_Color[i][1] > 43 && HSV_Color[i][1] < 255) && (HSV_Color[i][2] > 46 && HSV_Color[i][2] < 255))
		{
			is_red = true;
		}
		if ((HSV_Color[i][0] > 35 && HSV_Color[i][0] < 77) && (HSV_Color[i][1] > 43 && HSV_Color[i][1] < 255) && (HSV_Color[i][2] > 46 && HSV_Color[i][2] < 255))
		{
			is_green = true;
		}
		if ((HSV_Color[i][0] > 100 && HSV_Color[i][0] < 124) && (HSV_Color[i][1] > 43 && HSV_Color[i][1] < 255) && (HSV_Color[i][2] > 46 && HSV_Color[i][2] < 255))
		{
			is_blue = true;
		}
	}

	RGB = is_red ? SET_BIT(RGB, 1) : CLEAR_BIT(RGB, 1);
	RGB = is_green ? SET_BIT(RGB, 2) : CLEAR_BIT(RGB, 2);
	RGB = is_blue ? SET_BIT(RGB, 3) : CLEAR_BIT(RGB, 3);

	return RGB;
}


void LineContinuityAndRGBDetection::myFindContours(const cv::Mat& src, std::vector<std::vector<cv::Point>>& contours, std::vector<cv::Vec4i>& hierarchy, int retr, int method, cv::Point offset)
{
#if CV_VERSION_REVISION <= 6
	CvMat c_image = src;
#else
	CvMat c_image;
	c_image = cvMat(src.rows, src.cols, src.type(), src.data);
	c_image.step = src.step[0];
	c_image.type = (c_image.type & ~cv::Mat::CONTINUOUS_FLAG) | (src.flags & cv::Mat::CONTINUOUS_FLAG);
#endif
	cv::MemStorage storage(cvCreateMemStorage());
	CvSeq* _ccontours = nullptr;

#if CV_VERSION_REVISION <= 6
	cvFindContours(&c_image, storage, &_ccontours, sizeof(CvContour), retr, method, CvPoint(offset));
#else
	cvFindContours(&c_image, storage, &_ccontours, sizeof(CvContour), retr, method, CvPoint{ offset.x, offset.y });
#endif
	if (!_ccontours)
	{
		contours.clear();
		return;
	}
	cv::Seq<CvSeq*> all_contours(cvTreeToNodeSeq(_ccontours, sizeof(CvSeq), storage));
	size_t total = all_contours.size();
	contours.resize(total);

	cv::SeqIterator<CvSeq*> it = all_contours.begin();
	for (size_t i = 0; i < total; i++, ++it)
	{
		CvSeq* c = *it;
		reinterpret_cast<CvContour*>(c)->color = static_cast<int>(i);
		int count = c->total;
		int* data = new int[static_cast<size_t>(count * 2)];
		cvCvtSeqToArray(c, data);
		for (int j = 0; j < count; j++)
		{
			contours[i].push_back(cv::Point(data[j * 2], data[j * 2 + 1]));
		}
		delete[] data;
	}

	hierarchy.resize(total);
	it = all_contours.begin();
	for (size_t i = 0; i < total; i++, ++it)
	{
		CvSeq* c = *it;
		int h_next = c->h_next ? reinterpret_cast<CvContour*>(c->h_next)->color : -1;
		int h_prev = c->h_prev ? reinterpret_cast<CvContour*>(c->h_prev)->color : -1;
		int v_next = c->v_next ? reinterpret_cast<CvContour*>(c->v_next)->color : -1;
		int v_prev = c->v_prev ? reinterpret_cast<CvContour*>(c->v_prev)->color : -1;
		hierarchy[i] = cv::Vec4i(h_next, h_prev, v_next, v_prev);
	}

	storage.release();
}


cv::RotatedRect LineContinuityAndRGBDetection::getBoundingRect(const std::vector<cv::Point>& contour)
{
	if (contour.empty()) return {};

	cv::RotatedRect rect = minAreaRect(contour);
	if (rect.angle < -45)
	{
		rect.angle += 90;
		float temp = rect.size.width;
		rect.size.width = rect.size.height;
		rect.size.height = temp;
	}
	if (rect.angle > 45)
	{
		rect.angle -= 90;
		float temp = rect.size.width;
		rect.size.width = rect.size.height;
		rect.size.height = temp;
	}

	return rect;
}


int LineContinuityAndRGBDetection::filterBlock(const std::vector<std::vector<cv::Point>>& contours, const std::vector<cv::Vec4i>& hierarchy, const cv::Size& imageSize, double distance)
{
	std::vector<std::vector<cv::Point>> filter_contours = contours;
	std::vector<cv::RotatedRect> rects;

	for (size_t i = 0; i < filter_contours.size(); i++)
	{
		cv::RotatedRect rect = getBoundingRect(filter_contours[i]);
		rects.push_back(rect);
	}

	for (size_t i = 0; i < rects.size(); i++)
	{
		if (rects[i].size.width > (imageSize.width * 7 / 10) &&
			rects[i].size.height > (imageSize.height * 7 / 10))
		{
			filter_contours.erase(filter_contours.begin() + i);
			rects.erase(rects.begin() + i);
			i--;
			continue;
		}
		if (rects[i].size.width > (imageSize.width * 49 / 50) ||
			rects[i].size.height > (imageSize.height * 49 / 50))
		{
			filter_contours.erase(filter_contours.begin() + i);
			rects.erase(rects.begin() + i);
			i--;
			continue;
		}
		if (rects[i].size.width < 300 || rects[i].size.height < 300)
		{
			filter_contours.erase(filter_contours.begin() + i);
			rects.erase(rects.begin() + i);
			i--;
			continue;
		}
	}

	if (filter_contours.size() == 0)//交叉线过短、无交叉线情况，返回0
	{
		return 0;
	}

	std::vector<std::vector<cv::Point>> output(filter_contours.size());
	std::vector<std::vector<cv::Point>> hull(filter_contours.size());
	for (int i = 0; i < filter_contours.size(); i++)
	{
		cv::approxPolyDP(filter_contours[i], output[i], 100, true);//简化轮廓，获取轮廓端点
		cv::convexHull(output[i], hull[i], true, true);
	}


	cv::Vec4i LineA;
	cv::Vec4i LineB;
	cv::Point2f cross_point;
	double distance1;
	double distance2;
	double distance3;
	double distance4;
	double cross_width;
	for (int i = 0; i < hull.size(); i++)
	{
		if (hull[i].size() != 4)//两两端点相差距离小于100
		{
			return 1;
		}
		LineA = cv::Vec4i(hull[i][0].x, hull[i][0].y, hull[i][2].x, hull[i][2].y);
		LineB = cv::Vec4i(hull[i][1].x, hull[i][1].y, hull[i][3].x, hull[i][3].y);
		cross_point = getCrossPoint(LineA, LineB);
		distance1 = getDistance(hull[i][0], cross_point);
		distance2 = getDistance(hull[i][1], cross_point);
		distance3 = getDistance(hull[i][2], cross_point);
		distance4 = getDistance(hull[i][3], cross_point);
		cross_width = abs(hull[i][2].x - hull[i][0].x);
		if (abs(distance1 - distance3) > distance || abs(distance2 - distance4) > distance || cross_width < imageSize.width * 0.8)//两两交叉线相差距离超过100，交叉线宽度小于图片宽度*0.8，认为断线，返回1
		{
			return 1;
		}
	}
	return 2;//无断线，正常情况返回2
}


/* @function 求两点之间距离 */
double LineContinuityAndRGBDetection::getDistance(cv::Point pointO, cv::Point pointA)
{
	double distance;
	distance = sqrt((powf((pointO.x - pointA.x), 2) + powf((pointO.y - pointA.y), 2)));//求两点之间距离
	return distance;
}


/*函数功能：求两条直线交点*/
/*输入：两条Vec4i类型直线*/
/*返回：Point2f类型的点*/
cv::Point2f LineContinuityAndRGBDetection::getCrossPoint(cv::Vec4i LineA, cv::Vec4i LineB)
{
	double ka, kb;
	ka = (double)(LineA[3] - LineA[1]) / (double)(LineA[2] - LineA[0]); //求出LineA斜率
	kb = (double)(LineB[3] - LineB[1]) / (double)(LineB[2] - LineB[0]); //求出LineB斜率

	cv::Point2f crossPoint;
	crossPoint.x = (ka * LineA[0] - LineA[1] - kb * LineB[0] + LineB[1]) / (ka - kb);
	crossPoint.y = (ka * kb * (LineA[0] - LineB[0]) + ka * LineB[1] - kb * LineA[1]) / (ka - kb);
	return crossPoint;
}