#include "CaliValve.h"
#define VIEW_INTERNAL_MAT
using namespace luffy_base;
CaliValve::CaliValve()
{
}


CaliValve::~CaliValve()
{
}
double disOfPoint(const Point2f& p1, const Point2f& p2)
{
	return norm(p1 - p2);
}
void CaliValve::preProcessImage(Mat& img, const Mat& mask, double dstMean, double dstStddev, int highlightsThreshold)
{

	if (img.type() != CV_32FC1)
	{
		img.convertTo(img, CV_32FC1);
	}

	Mat gaussImg;
	GaussianBlur(img, gaussImg, Size(3, 3), 5.0);
	img = gaussImg;

	Mat dilatedMask;
	dilate(mask, dilatedMask, Mat::ones(Size(3, 3), CV_32FC1));
	Mat hightlightsMask = img < highlightsThreshold;
	Mat imgMask = hightlightsMask & (mask > 0);
	
	Scalar meanScalar, stddevScalar;
	meanStdDev(img, meanScalar, stddevScalar, imgMask);
	img = (img - meanScalar.val[0]) * dstStddev / stddevScalar.val[0] + dstMean;
	imgMask.convertTo(imgMask, CV_32FC1);
	imgMask /= 255.0;
	Mat imgNorm = cocentricNorm(img, Point2f(img.cols / 2.0, img.rows / 2.0),
		imgMask, 125);
#ifdef DEBUG_VIEW_INTERNAL_MAT
	Mat vImgNorm = imgNorm / 255.0;
#endif
	img = imgNorm;
}

cv::Mat CaliValve::genMask(const Mat& img, Point2f center, float innerR /*= -1*/, float outterR /*= -1*/, int type /*= CV_32FC1*/)
{
	Mat mask(img.size(), CV_8UC1);
	mask.setTo(0);
	if (innerR == -1)
	{
		// default is 30
		innerR = img.rows*0.178;
	}
	if (outterR == -1)
	{
		// default is max radius - 10
		outterR = img.rows*0.425;
	}
	circle(mask, center, outterR, Scalar(255), -1);
	circle(mask, center, innerR, Scalar(0), -1);
	if (type != CV_8UC1)
	{
		mask.convertTo(mask, type);
		mask /= 255;
	}
	return mask;
}

cv::Mat CaliValve::cocentricNorm(Mat& img, Point2f center, const Mat& weightMat, float dstMeanVal)
{
	assert(weightMat.empty() || weightMat.type() == CV_32FC1);

	int w = img.cols;
	int h = img.rows;
	vector<Point2f> corners;
	corners.push_back(Point2f(0, 0));
	corners.push_back(Point2f(0, h));
	corners.push_back(Point2f(w, h));
	corners.push_back(Point2f(w, 0));
	vector<double> cornerDisVec;
	for_each(corners.begin(), corners.end(), [&](const Point2f& pt)
	{
		double dis = disOfPoint(center, pt);
		cornerDisVec.push_back(dis);
	});

	auto farthestCornerDis = max_element(cornerDisVec.begin(), cornerDisVec.end());
	float maxRadius = *farthestCornerDis;

	int radiusNum = floorf(maxRadius);
	//radiusNum = 20;
	float radiusStep = (maxRadius / radiusNum);
	Mat cocentricSumMat = Mat::zeros(1, radiusNum, CV_32FC1);
	float* pSumData = (float*)cocentricSumMat.data;
	Mat cocentricWeightSumMat = Mat::zeros(1, radiusNum, CV_32FC1);
	float* pWeightSumData = (float*)cocentricWeightSumMat.data;
	Mat radiusMat(img.rows, img.cols, CV_32FC1);

	for (int y = 0; y < h; y++)
	{
		const Mat& imgRow = img.row(y);
		float* pImgRowData = (float*)imgRow.data;
		float* pRadiusRowData = (float*)radiusMat.row(y).data;

		float* pWeightRowData = NULL;
		if (!weightMat.empty())
		{
			pWeightRowData = (float*)weightMat.row(y).data;
		}

		for (int x = 0; x < w; x++)
		{
			//std::cout << x << " " << y << std::endl;
			float weight;
			if (pWeightRowData)
			{
				weight = pWeightRowData[x];
			}
			else
			{
				weight = 1.0;
			}
			float val = pImgRowData[x] * weight;
			float radius = disOfPoint(Point2f(x, y), center);
			pRadiusRowData[x] = radius;
			int radiusIdx0 = (int)(radius / radiusStep);
			assert(radiusIdx0 >= 0);
			int radiusIdx1 = radiusIdx0 + 1;
			if (radiusIdx0 >= radiusNum - 1)
			{
				pSumData[radiusNum - 1] += val;
				pWeightSumData[radiusNum - 1] += weight;
			}
			else
			{
				float s = (radius - radiusStep*radiusIdx0) / radiusStep;
				pSumData[radiusIdx0] += val*s;
				pSumData[radiusIdx1] += val*(1 - s);
				pWeightSumData[radiusIdx0] += s*weight;
				pWeightSumData[radiusIdx1] += (1 - s)*weight;
			}
		}
		//         CvPlot::plot<float>("sum", pSumData, radiusNum);
		//         CvPlot::plot<float>("count", pCountData, radiusNum);
		//         waitKey();
	}

	for (int i = 0; i < radiusNum; ++i)
	{
		//float radius = (i*radiusStep + radiusStep) / 2;
		if (pWeightSumData[i] == 0)
		{

		}
		else
		{
			pSumData[i] /= pWeightSumData[i];
		}
	}

	Mat retMat = Mat::zeros(img.rows, img.cols, img.type());
	for (int y = 0; y < h; y++)
	{
		float* pImgRowData = (float*)img.row(y).data;
		float* pRetRowData = (float*)retMat.row(y).data;
		float* pRadiusData = (float*)radiusMat.row(y).data;
		for (int x = 0; x < w; x++)
		{
			float val = pImgRowData[x];
			float radius = pRadiusData[x];
			float mean = interpolate(pSumData, radiusNum, radiusStep, radius);
			if (mean == 0)
			{
				continue;
			}
			float newVal = (float)val * dstMeanVal / mean;
			pRetRowData[x] = newVal;
		}
	}

	return retMat;
}

float CaliValve::interpolate(float* pY, int n, float stepX, float x)
{
	int lIdx = (int)(x / stepX);
	int rIdx = lIdx + 1;
	if (rIdx > n - 1)
	{
		return pY[n - 1];
	}
	assert(lIdx >= 0 && lIdx < n && rIdx >= 0 && rIdx < n);
	float s = (x - lIdx*stepX) / stepX;
	float ly = pY[lIdx];
	float ry = pY[rIdx];
	return ly + (ry - ly)*s;
}


Mat CaliValve::extractForegroundWheel(const Mat &background, const Mat &src)
{
	Mat resizedGroundImage = background.clone();
	if (resizedGroundImage.size() != src.size())
	{
		resize(background, resizedGroundImage, src.size());
	}

	return (src - resizedGroundImage);
}

Mat CaliValve::findWheelObject(Mat src, Mat backGroundImg, int thresh)
{


	if (src.empty() || backGroundImg.empty() || src.cols < 500)
	{
		return Mat();
	}
	assert(backGroundImg.type() == CV_8UC1);
	const cv::Size size = cv::Size(416, floor(416.0 / src.cols * src.rows));
	Mat resizedImage;
	resizedImage.setTo(0);
	resize(src, resizedImage, size);
	Mat foregroundImg = extractForegroundWheel(backGroundImg, resizedImage);
	using namespace luffy_base;
	Mat imgBinary;
	imgBinary.setTo(0);
	luffy_threshold::Threshold(foregroundImg, imgBinary, thresh);//0421
	//luffy_threshold::Threshold(imgTmp, imgBinary, nThres);

	Mat dilatedImgBin;
	dilate(imgBinary, dilatedImgBin, Mat::ones(7, 7, CV_32FC1));
	erode(dilatedImgBin, imgBinary, Mat::ones(7, 7, CV_32FC1));
	//openOper(imgBinary, Mat::ones(1, 13, CV_32FC1));

	vector<vector<Point>> conts;
	cv::findContours(imgBinary, conts, RETR_EXTERNAL, CHAIN_APPROX_NONE);
	imgBinary.setTo(0);
	for (int i = 0; i < conts.size(); i++) {
		const vector<Point> &pt = conts.at(i);
		if (pt.size() < 20) {
			continue;
		}
		Rect rt = boundingRect(pt);
		if (rt.width < 5 || rt.height < 5) {
			continue;
		}
		drawContours(imgBinary, conts, i, Scalar::all(255), -1);
	}
	Mat hit; vector<Point> pts;
	luffy_hit::firstHit4Circle(imgBinary, hit, pts, Point(size.width / 2, size.height / 2), 0, size.width / 2, 360, luffy_hit::emHitOut2In);

	//luffy_imageProc::RansacParam rs(0.02, 2.5, 70, 100, 220);
	luffy_imageProc::RansacParam rs(0.01, 3, 150, 100, 240);//0421
	vector<Point> pts2 = luffy_imageProc::fitModelbyRansac(pts, luffy_imageProc::emModelCircle, &rs);
#ifdef _DEBUG
	Mat imgColor;
	cv::cvtColor(resizedImage, imgColor, CV_GRAY2BGR);
	for (int i = 0; i < pts.size(); i++) {
		imgColor.at<cv::Vec3b>(pts.at(i))[0] = 255;//B  
		imgColor.at< cv::Vec3b >(pts.at(i))[1] = 0;//G  
		imgColor.at< cv::Vec3b >(pts.at(i))[2] = 0;//R  
	}
	for (int i = 0; i < pts2.size(); i++) {
		imgColor.at<cv::Vec3b>(pts2.at(i))[0] = 0;//B  
		imgColor.at< cv::Vec3b >(pts2.at(i))[1] = 0;//G  
		imgColor.at< cv::Vec3b >(pts2.at(i))[2] = 255;//R  
	}
#endif

	float fRadius;
	Point2f ptCenter;
	bool bFind = luffy_imageProc::lsCircleFit(pts2, fRadius, ptCenter);

	if (!bFind) {
		return Mat();
	}
	Mat dst;
	const int nOffset = 1;
	fRadius += nOffset;
	Rect rt(ptCenter.x - fRadius + nOffset, ptCenter.y - fRadius + nOffset, 2 * fRadius, 2 * fRadius);
	rt &= Rect(0, 0, resizedImage.cols, resizedImage.rows);
	resizedImage(rt).copyTo(dst);
	Mat finalDst(dst.size(), dst.type(), Scalar::all(0));
	cv::circle(finalDst, Point(finalDst.cols / 2, finalDst.rows / 2), fRadius, Scalar::all(1), -1);
	dst = dst.mul(finalDst);
	return dst;
}




bool CaliValve::cutValve(Mat & img2Rect, Mat &imgTemplate, Rect rtCut, int nAngleMax)
{
	if (rtCut.x < 0) {
		imgTemplate.create(Size(rtCut.width, rtCut.height), img2Rect.type());
		Rect rtLeft(nAngleMax + rtCut.x, rtCut.y, -rtCut.x, rtCut.height);
		Rect rtRight(0, rtCut.y, rtCut.width - rtLeft.width, rtCut.height);

		img2Rect(rtLeft).copyTo(imgTemplate(Rect(0, 0, rtLeft.width, rtLeft.height)));
		img2Rect(rtRight).copyTo(imgTemplate(Rect(rtLeft.width, 0, rtRight.width, rtLeft.height)));
		return true;
	}
	else if (rtCut.x + rtCut.width >= nAngleMax) {
		imgTemplate.create(Size(rtCut.width, rtCut.height), img2Rect.type());

		Rect rtLeft(rtCut.x, rtCut.y, nAngleMax-rtCut.x, rtCut.height);
		Rect rtRight(0, rtCut.y, rtCut.width - rtLeft.width, rtCut.height);

		img2Rect(rtLeft).copyTo(imgTemplate(Rect(0, 0, rtLeft.width, rtLeft.height)));
		img2Rect(rtRight).copyTo(imgTemplate(Rect(rtLeft.width, 0, rtRight.width, rtLeft.height)));
		return true;
	}

	luffy_math::checkRoiRect(Size(img2Rect.cols, img2Rect.rows), rtCut);
	img2Rect(rtCut).copyTo(imgTemplate);

	return true;
}
bool CaliValve::detect(Mat & imgSrc, InputParam &paramIn, OutputParam &paramOut, Mat &imgDst /*= Mat()*/)
{
	Record_List valueRecords = paramIn.valueROI.records;//气门芯位置
	Record_List barRecords = paramIn.barROI.records;//
	Record_List startRecords = paramIn.startROI.records;
	int valueCircleCount = valueRecords.size();
	int barCircleCount = barRecords.size();
	if (valueCircleCount == 0 && barCircleCount ==0) {
		return false;
	}
	luffy_base::luffyCircle valueCircle;
	valueCircle.ptCenter = Point2f(0, 0);
	valueCircle.fRadius = 10000000000000.0;
	for (int i = 0; i < valueCircleCount; i++) {
		Item_List item = valueRecords.at(i);
		Feature_List feature = item.at(0);

		if (1 == feature.first) {
			int size = feature.second.size();
			if (size < 3)
			{
				continue;
			}
			Point2f p = Point2f(feature.second.at(0), feature.second.at(1));
			float r = abs(feature.second.at(2));
			if (r < valueCircle.fRadius)
			{
				valueCircle.fRadius = r;
				valueCircle.ptCenter = p;
			}
			continue;
		}
	}

	luffy_base::luffyCircle barCircle;
	for (int i = 0; i < barCircleCount; i++) {
		Item_List circleItem = barRecords.at(i);
		Feature_List circleFeature = circleItem.at(0);

		if (1 == circleFeature.first) {
			int size = circleFeature.second.size();
			if (size < 3)
			{
				continue;
			}
			barCircle.ptCenter = Point2f(circleFeature.second.at(0), circleFeature.second.at(1));
			barCircle.fRadius = abs(circleFeature.second.at(2));
			break;
		}

	}

	luffy_base::luffyCircle startCircle;
	int startCircleCount = startRecords.size();
	for (int i = 0; i < startCircleCount; i++) {
		Item_List circleItem = startRecords.at(i);
		Feature_List circleFeature = circleItem.at(0);

		if (1 == circleFeature.first) {
			int size = circleFeature.second.size();
			if (size < 3)
			{
				continue;
			}
			startCircle.ptCenter = Point2f(circleFeature.second.at(0), circleFeature.second.at(1));
			startCircle.fRadius = abs(circleFeature.second.at(2));
			break;
		}

	}
	//绘制气门芯标定的圆区域
	cv::circle(imgDst, valueCircle.ptCenter, valueCircle.fRadius, LP_COLOR_RED, 2);
	cv::circle(imgDst, valueCircle.ptCenter, 2, LP_COLOR_BLUE, 2);
	//绘制辐条标定的圆区域
	cv::circle(imgDst, barCircle.ptCenter, barCircle.fRadius, LP_COLOR_RED, 2);
	cv::circle(imgDst, barCircle.ptCenter, 2, LP_COLOR_BLUE, 2);
	//绘制参考点的圆区域
	cv::line(imgDst,cv::Point(startCircle.ptCenter.x- startCircle.fRadius,startCircle.ptCenter.y), cv::Point(startCircle.ptCenter.x+ startCircle.fRadius, startCircle.ptCenter.y),LP_COLOR_BLUE, 2);
	cv::line(imgDst, cv::Point(startCircle.ptCenter.x, startCircle.ptCenter.y - startCircle.fRadius), cv::Point(startCircle.ptCenter.x, startCircle.ptCenter.y + startCircle.fRadius), LP_COLOR_BLUE, 2);
	cv::circle(imgDst, startCircle.ptCenter, startCircle.fRadius, LP_COLOR_GREEN, 2);
	cv::circle(imgDst, startCircle.ptCenter, 2, LP_COLOR_GREEN, 2);
	//绘制参考的与圆心的直线
	cv::line(imgDst, startCircle.ptCenter, paramIn.ptCenter, LP_COLOR_GREEN, 2);
	//绘制圆心点
	cv::circle(imgDst, paramIn.ptCenter, 2, LP_COLOR_RED, 2);
	//计算气门芯中点与圆心中点的距离
	float fRadius = luffy_math::disofPoints(valueCircle.ptCenter, paramIn.ptCenter);
	int offset = 30;
	int nImgHeight = valueCircle.fRadius * 2 + 2 * offset;
	Mat img2Rect;
	luffy_math::polar2rect(imgSrc, img2Rect, paramIn.ptCenter, fRadius - nImgHeight / 2, fRadius + nImgHeight / 2, paramIn.nAngleMax);
	
	if (paramIn.flagCircle > 0)
	{
		qWarning() << "outPut param flagCircle :" << paramIn.flagCircle;
		paramOut.nValveOffset = luffy_math::caculAngle(paramIn.ptCenter, barCircle.ptCenter) / 360.0 * paramIn.nAngleMax;
		Rect rTargetRoi(paramOut.nValveOffset - barCircle.fRadius, offset, 2 * barCircle.fRadius, 2 * valueCircle.fRadius);
		cutValve(img2Rect, paramOut.barTemplate, rTargetRoi, paramIn.nAngleMax);
		paramOut.fValveWidth = valueCircle.fRadius;
		paramOut.fValveDis = fRadius;
		paramOut.withinOffset = 0;
	}
	else
	{
		//<! cut valve//逆时针计算角度 0点在右侧
		qWarning() << "outPut param flagCircle :" << paramIn.flagCircle;
		paramOut.nValveOffset = luffy_math::caculAngle(paramIn.ptCenter, valueCircle.ptCenter) / 360.0 * paramIn.nAngleMax;
		paramOut.nStartOffset = luffy_math::caculAngle(paramIn.ptCenter, startCircle.ptCenter) / 360.0 * paramIn.nAngleMax;
		float barCircleOffset = luffy_math::caculAngle(paramIn.ptCenter, barCircle.ptCenter) / 360 * paramIn.nAngleMax;

		Rect rbarRoi(barCircleOffset - barCircle.fRadius, offset, 2 * barCircle.fRadius, 2 * valueCircle.fRadius);
		Rect rtRoi(paramOut.nValveOffset - valueCircle.fRadius, offset, 2 * valueCircle.fRadius, 2 * valueCircle.fRadius);
		cutValve(img2Rect, paramOut.imgTemplate, rtRoi, paramIn.nAngleMax);
		cutValve(img2Rect, paramOut.barTemplate, rbarRoi, paramIn.nAngleMax);

		if (paramIn.barNum <= 0)
		{
			qWarning() << "barNum cannot be negative or ZERO";
			return false;
		}

		int T = paramIn.nAngleMax / paramIn.barNum;
		if ((paramOut.nValveOffset - barCircleOffset > -T && paramOut.nValveOffset - barCircleOffset < 0) ||
			(paramOut.nValveOffset - barCircleOffset >(paramIn.barNum - 1) * T)
			)
		{
			barCircleOffset = barCircleOffset - T;
		}

		int interval = luffy_math::mod(paramOut.nValveOffset - barCircleOffset, paramIn.nAngleMax);
		paramOut.withinOffset = interval;
		paramOut.fValveWidth = valueCircle.fRadius;
		paramOut.fValveDis = fRadius;
	}
	
	///////for test
	//
	//Mat circleRoi;
	//float startx = realCircle.ptCenter.x - realCircle.fRadius;
	//float starty = realCircle.ptCenter.y - realCircle.fRadius;
	//imgSrc(Rect(startx, starty, realCircle.fRadius * 2, realCircle.fRadius * 2)).copyTo(circleRoi);
	//paramOut.imgTemplate = circleRoi;
	if (paramIn.ifClasify)
	{
		Mat grayImage;
		grayImage.setTo(0);
		colorConvert(paramOut.background, grayImage);
		Mat gray = imgSrc.clone();
		Mat foreGround = findWheelObject(gray, grayImage, paramIn.backgroundThresh);
		if (foreGround.empty())
		{
			return false;
		}
		int repeatNum = paramIn.barNum;
		Mat weightMat(foreGround.size(), foreGround.type());
		selfRotateMin(foreGround, weightMat, repeatNum);
		Mat mask = genMask(foreGround, Point(foreGround.cols / 2, foreGround.rows / 2), -1, -1, CV_32FC1);
		weightMat.convertTo(weightMat, CV_32FC1);
		weightMat = weightMat.mul(mask);
		weightMat.setTo(0, weightMat < 20);
		preProcessImage(foreGround, mask, 127.0, 20.0, 256);
#ifdef VIEW_INTERNAL_MAT
		Mat m_weightMat = weightMat / 255.0;
		Mat baseImage = foreGround / 255.0;
#endif
		paramOut.baseImage = foreGround;
		paramOut.weightMat = weightMat;
		paramOut.background = grayImage;
	}
	else
	{
		paramOut.baseImage = Mat();
		paramOut.weightMat = Mat();
	}
	
	return true;
}

void CaliValve::selfRotateMin(const Mat& src, Mat &dst, int repeatNum)
{
	Point2f center(src.cols / 2.0, src.rows / 2.0);
	float angleStep = 360.0 / repeatNum;
	Mat dstMat = Mat::ones(src.size(), src.type())*255;
	for (int i = 0; i < repeatNum; ++i)
	{
		Mat rotateParamMat = getRotationMatrix2D(center, angleStep*i, 1.0);
		Mat rImg;
		warpAffine(src, rImg, rotateParamMat, src.size(), INTER_CUBIC, BORDER_CONSTANT);
		Mat dialitedMat;
		dilate(rImg, dialitedMat, Mat::ones(3, 3, CV_32FC1));
		dstMat = min(dstMat, dialitedMat);
	}
	dst = dstMat;
}


void CaliValve::colorConvert(const Mat& src, Mat & dst)
{
	if (src.channels() == 1)
	{
		dst = src.clone();
	}
	else if (src.channels() == 3)
	{
		cvtColor(src, dst, CV_RGB2GRAY);
	}
	else if (src.channels() == 4)
	{
		cvtColor(src, dst, CV_RGBA2GRAY);
	}
}