/**
* Lubricant Detecter
* Copyright (C) 2021 Carl Klemm
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* version 3 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the
* Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
* Boston, MA  02110-1301, USA.
*/

#include <opencv2/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui.hpp>
#include "uvosunwrap/harris.h"
#include "uvosunwrap/log.h"
#include "matutils.h"
#include "drawing.h"

static float detimineXPitch(const std::vector<cv::Point2f>& row, float fuzz = 1.3f)
{
	std::vector<float> xRowDists;
	for(size_t i = 0; i < row.size()-1; ++i)
		xRowDists.push_back(abs(row[i+1].x-row[i].x));
	float xMinDist = *std::min(xRowDists.begin(), xRowDists.end());
	
	Log(Log::DEBUG)<<__func__<<": xMinDist "<<xMinDist;
	
	float meanXDistAccum = 0;
	size_t validCount = 0;
	for(size_t i = 0; i < xRowDists.size(); ++i)
	{
		if(xRowDists[i] < xMinDist*fuzz)
		{
			++validCount;
			meanXDistAccum+=xRowDists[i];
		}
	}
	return meanXDistAccum/validCount;
}

static std::vector<cv::RotatedRect> fitEllipses(std::vector< std::vector<cv::Point2f > >& rows, bool remove = true)
{
	if(rows.empty()) return std::vector<cv::RotatedRect>();
	
	std::vector<cv::RotatedRect> ellipses;
	ellipses.reserve(rows.size());
	for(size_t i = 0; i < rows.size(); ++i) 
	{
		if(rows[i].size() > 4) ellipses.push_back(cv::fitEllipse(rows[i]));
		else 
		{ 
			rows.erase(rows.begin()+i);
			i--;
		}
	}
	return ellipses;
}

static bool sanityCheckElipses(std::vector< std::vector<cv::Point2f > >& rows, std::vector<cv::RotatedRect>& elipses)
{
	if(rows.size() != elipses.size() && elipses.size() > 1) return false;

	for(size_t i = 0; i < elipses.size(); ++i)
	{
		float angDiff = fmod(elipses[i].angle,90);
		if(angDiff < 90-5 && angDiff > 5)
		{
			elipses.erase(elipses.begin()+i);
			rows.erase(rows.begin()+i);
			--i;
		}
	}
	
	std::vector<float> widths;
	std::vector<float> heights;
	
	for(auto& elipse : elipses)
	{
		widths.push_back(elipse.size.width);
		heights.push_back(elipse.size.height);
	}
	
	std::vector<size_t> outliersW;
	std::vector<size_t> outliersH;
	thompsonTauTest(widths, outliersW, 2);
	thompsonTauTest(heights, outliersH, 2);
	
	std::vector< std::vector<cv::Point2f > > rowsReduced;
	std::vector<cv::RotatedRect> elipsesReduced;

	for(size_t i = 0; i < elipses.size(); ++i)
	{
		bool found = false;
		for(size_t j = 0; j < outliersW.size() && !found; ++j) 
			if(outliersW[j]==i) found = true;
		for(size_t j = 0; j < outliersH.size() && !found; ++j) 
			if(outliersH[j]==i) found = true;
		if(!found)
		{
			rowsReduced.push_back(rows[i]);
			elipsesReduced.push_back(elipses[i]);
		}
	}
	elipses = elipsesReduced;
	rows = rowsReduced;
	return true;
}

static std::vector< std::vector<cv::Point2f > > sortPointsIntoRows(std::vector<cv::Point2f>& points)
{
	
	if(points.size() < 6) return std::vector< std::vector<cv::Point2f> >();
	
	cv::Point2f topLeft(*getTopLeft(points));
	cv::Point2f bottomRight(*getBottomRight(points));
	
	Log(Log::DEBUG)<<"topLeft "<<topLeft.x<<' '<<topLeft.y;
	Log(Log::DEBUG)<<"bottomRight "<<bottomRight.x<<' '<<bottomRight.y;
	
	float fuzz = (bottomRight.x-topLeft.x)*0.05f;
	
	for(size_t i = 0; i < points.size(); ++i)
	{
		if(points[i].x < topLeft.x-fuzz || points[i].y < topLeft.y-fuzz || 
			points[i].x > bottomRight.x+fuzz || points[i].y > bottomRight.y+fuzz)
			points.erase(points.begin()+i);
	}
	
	std::sort(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.y < b.y;});
	
	double accumulator = points[0].y+points[1].y;
	size_t accuCount = 2;
	float sigDist = (bottomRight.y-topLeft.y) / 20;
	
	std::vector< std::vector<cv::Point2f> > result(1);
	result.back().push_back(points[0]);
	result.back().push_back(points[1]);
	
	for(size_t i = 2; i < points.size(); ++i)
	{
		if( points[i].y - accumulator/accuCount > sigDist )
		{
			if(points.size() - i - 3 < 0) break;
			else 
			{
				accumulator = points[i+1].y+points[i+2].y;
				accuCount = 2;
			}
			result.push_back(std::vector<cv::Point2f>());
		}
		result.back().push_back(points[i]);
		accumulator += points[i].y;
		++accuCount;
	}
	
	for(auto& row : result)  
		std::sort(row.begin(), row.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.x < b.x;});
	return result;
}

static std::vector<DetectedPoint> harrisGenerateCoordinates(std::vector<cv::Point2f>& points, bool verbose)
{
	std::vector< std::vector<cv::Point2f > > rows = sortPointsIntoRows(points);
	Log(Log::INFO)<<"Found "<<rows.size()<<" rows";
	if(rows.size() < 2)
	{
		Log(Log::ERROR)<<"Error creating map, insufficant rows detected";
		return std::vector<DetectedPoint>();
	}

	std::vector< cv::RotatedRect > ellipses = fitEllipses(rows);
	Log(Log::INFO)<<"Found "<<ellipses.size()<<" ellipses. rows reduced to "<<rows.size();
	if(ellipses.size() < 3)
	{
		Log(Log::ERROR)<<"Error creating map, insufficant ellipses detected";
		return std::vector<DetectedPoint>();
	}

	sanityCheckElipses(rows, ellipses);

	if(rows.size() < 2 || rows.size() != ellipses.size()) {
		Log(Log::ERROR)<<__func__<<": rows < 2 or rows != ellipses";
		return std::vector<DetectedPoint>();
	}

	std::vector<cv::Point2i> coordinates;

	coordinates.assign(points.size(), cv::Point2i());

	float meanYdist = 0;
	for(size_t i = 0; i < ellipses.size()-1; ++i)
	{
		meanYdist += ellipses[i+1].center.y-ellipses[i].center.y;
	}
	meanYdist = meanYdist/ellipses.size();

	float meanXdist = 0;
	float xMin = std::numeric_limits<float>::max();
	float xMeanMin = 0;
	float xMax = std::numeric_limits<float>::min();
	
	for(auto& row : rows)
	{
		meanXdist+=detimineXPitch(row);
		xMeanMin+=row.front().x;
		if(xMin > row.front().x )
			xMin = row.front().x;
		if(xMax < row.back().x)
			xMax = row.back().x;
	}
	meanXdist/=rows.size();
	Log(Log::DEBUG)<<__func__<<": meanXdist "<<meanXdist;
	Log(Log::DEBUG)<<__func__<<": meanYdist "<<meanYdist;
	xMeanMin = xMeanMin / rows.size();
	
	Log(Log::DEBUG)<<__func__<<": Grid: xMin "<<xMin;
	Log(Log::DEBUG)<<__func__<<": Grid: grid xMax "<<xMax;
	Log(Log::DEBUG)<<__func__<<": Grid: grid xMeanMin "<<xMeanMin;
	
	size_t xGridSize = static_cast<size_t>(std::lround(abs((xMax-xMin)/meanXdist))+1);
	
	std::vector<DetectedPoint> dpoints;
	
	for(size_t y = 0; y < rows.size(); ++y)
	{
		Log(Log::DEBUG)<<__func__<<": Proc row "<<y;
		
		for(size_t x = 0; x < xGridSize; ++x)
		{
			cv::Point2f tmp(x*meanXdist+xMin, y*meanYdist);
			size_t closest;
			bool found = 	findClosest(closest, tmp, 
											   points, 
											   (2*meanXdist)/5, (2*meanYdist)/5);
			
			Log(Log::DEBUG)<<__func__<<": found: "<<found<<' '<<x<<'x'<<y;
			
			if(found)
				dpoints.push_back(DetectedPoint(points[closest], cv::Point2i(x,y)));
		}
	}
	return dpoints;
}

std::vector<DetectedPoint> harrisDetectPoints(cv::Mat& image, const cv::Mat& mask, 
											 int blockSize, int apature, float detectorParameter,
											 float minSize, bool verbose)
{
	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
	
	//detect corners
	cv::Mat corners;
	cv::cornerHarris(gray, corners, blockSize, apature, detectorParameter);
	cv::normalize(corners, corners, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
	cv::convertScaleAbs( corners, corners );
	cv::threshold(corners, corners, 50, 255, cv::THRESH_BINARY);
	cv::Mat cornersMasked;
	if(mask.data && mask.size == corners.size) corners.copyTo(cornersMasked, mask);
	else corners.copyTo(cornersMasked);
	
	if(verbose)
	{
		cv::imshow( "Viewer", cornersMasked );
		cv::waitKey(0);
	}
	
	//get middle of corners
	cv::SimpleBlobDetector::Params blobParams;
	blobParams.filterByArea = true;
	blobParams.minArea = minSize;
	blobParams.maxArea = 500;
	blobParams.filterByColor = false;
	blobParams.blobColor = 255;
	blobParams.filterByInertia = false;
	blobParams.filterByConvexity = false;
	cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(blobParams);
	std::vector<cv::KeyPoint> keypoints;
	blobDetector->detect(cornersMasked, keypoints);
	
	std::vector<cv::Point2f> points;
	cv::KeyPoint::convert(keypoints, points);
	
	return harrisGenerateCoordinates(points, verbose);
}
