#include "modelVerfication.h"

//#define VIEW_DEBUG_IMG
modelVerfication::modelVerfication(double tarStddevVal, double tarMeanVal) :
	tarStddev(tarStddevVal)
	, tarMean(tarMeanVal)
{
	
}

modelVerfication::~modelVerfication()
{

}

double disOfPoint(const Point2f& p1, const Point2f& p2)
{
	return norm(p1 - p2);
}

float modelVerfication::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;
}
bool modelVerfication::objectVerification(const Mat &img, Mat& baseImage, double modelValidThresh, int endAngle, double & s)
{
	int diff = abs(img.cols - baseImage.cols);
	if (diff >=5)
	{
		qDebug() << "model verification : model possibly is not match";
		s = 365;
		return false;
	}
	if (baseImage.type()!=CV_32FC1)
	{
		baseImage.convertTo(baseImage, CV_32FC1);
	}
	Mat resizedImage(baseImage.size(), baseImage.type(), Scalar::all(0));
	if (img.size()!= baseImage.size())
	{
		resize(img, resizedImage, baseImage.size());
	}
	else
	{
		resizedImage = img;
	}
	
	Mat mask = genMask(resizedImage, Point(resizedImage.cols / 2, resizedImage.rows / 2), -1, -1, CV_8UC1);
	m32fMaskImg = genMask(resizedImage, Point2f(resizedImage.cols / 2, resizedImage.rows / 2));
	preProcessImage(resizedImage, mask, tarMean, tarStddev, 256);
	RData* pData = new RData();
	rotateMatchData(resizedImage, baseImage, pData, 1, 0, endAngle);

	double result = 0;
	Mat rstImg, showImage, Ibase;

	if (pData->mDisValVec.empty())
	{
		delete pData;
		qDebug() << "model verification : the distance of model is empty";
		result = FLT_MAX;
	}
	else
	{
		size_t bestIndex = min_element(pData->mDisValVec.begin(), pData->mDisValVec.end()) - pData->mDisValVec.begin();

		result = pData->mDisValVec[bestIndex];
		rstImg = pData->mRImgVec[bestIndex];
#ifdef VIEW_DEBUG_IMG
		showImage = rstImg / 255;
		Ibase = baseImage / 255;
#endif // VIEW_DEBUG_IMG
		qDebug() << "model verification : found the minimal value model:" << result;
		delete pData;
	}

	if (result < modelValidThresh)
	{
		qDebug() << "model verification : image is valid";
		s = result;
		return true;
	}
	else
	{
		qDebug() << "model verification : image is not valid";
		s = result;
		return false;
	}


	//Mat image = croppedImage / 255.0;
}
Mat modelVerfication::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 modelVerfication::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;
}

void ImageCompareModel2::operator()(const cv::Range& range) const
{
	int i0 = range.start;
	int i1 = range.end;
	assert(abs(i1 - i0) == 1);
	model->parallelDetect(i0, m_pData, templ);
}

void modelVerfication::parallelDetect(int index, void *p, Mat templ)
{
	RData *pData = (RData *)p;
	Mat t = getRotationMatrix2D(pData->mCenter, pData->angle(index), 1.0);
	Mat rImg;
	warpAffine(pData->mImgSrc, rImg, t, pData->mImgSrc.size());
	if (rImg.size() != templ.size())
	{
		resize(rImg, rImg, templ.size());
	}
	Mat imgRes = abs(templ - rImg);
	imgRes = imgRes.mul(m32fMaskImg).mul(weightMat);
	float s = sum(weightMat).val[0];
	double val = norm(imgRes) / s;
	pData->mDisValVec[index] = val;
	pData->mRImgVec[index] = rImg;
}

void modelVerfication::rotateMatchData(const Mat& _img, const Mat &baseImage, RData* pData, float angleStep, float startAngle, float endAngle)
{
	Mat img = _img.clone();
	Point2f center(img.cols / 2.0, img.rows / 2.0);
	int nNum = (endAngle - startAngle) / angleStep;
	RData& data = *pData;
	data.init(_img, center, angleStep, nNum);
	data.mStartAngle = startAngle;
	data.mEndAngle = endAngle;
	qDebug() << "start parallel test";
	parallel_for_(Range(0, nNum), ImageCompareModel2(this, pData, baseImage));
}

void modelVerfication::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 & dilatedMask;
	//imgMask.convertTo(imgMask, CV_32FC1);
	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 modelVerfication::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 modelVerfication::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;
		}
	}

#ifdef DEBUG_VIEW_INTERNAL_MAT
	Mat viewRetMat = retMat / 255.0;
#endif 

	return retMat;
}