/**
* 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 "matutils.h"
#include <opencv2/core/ocl.hpp>
#include <iostream>
#include <math.h>
#include <assert.h>
#include "uvosunwrap/log.h"

void sanityCheckMap(cv::Mat& mat, const float min, const float max, float minValue, float maxValue)
{
	for(int y = 0; y < mat.rows; y++)
	{
		float* col = mat.ptr<float>(y);
		for(int x = 0; x < mat.cols; ++x)
		{
			if(col[x] < min) 
				col[x] = minValue;
			else if(col[x] > max)
				col[x] = maxValue;
		}
	}
}

void thompsonTauTest(const std::vector<float>& in, std::vector<size_t>& outliers, float criticalValue)
{
	float mean = 0;
	size_t n = 0;
	for(size_t i = 0; i < in.size(); ++i)
	{
		bool found = false;
		for(size_t j = 0; j < outliers.size() && !found; ++j) 
			if(outliers[j]==i) found = true;
		if(!found)
		{
			mean+=in[i];
			++n;
		}
	}
	mean/=n;
	
	float sd = 0;
	for(size_t i = 0; i < in.size(); ++i)
	{
		bool found = false;
		for(size_t j = 0; j < outliers.size() && !found; ++j) 
			if(outliers[j]==i) found = true;
		if(!found) sd+=pow(in[i]-mean,2);
	}
	sd = sqrt(sd/(n-1));
	float rej = (criticalValue*(n-1))/(sqrt(n)*sqrt(n-2+criticalValue));
	bool removed = false;
	for(size_t i = 0; i < in.size() && !removed; ++i)
	{
		bool found = false;
		for(size_t j = 0; j < outliers.size() && !found; ++j) 
			if(outliers[j]==i) found = true;
		if(!found)
		{
			if(abs((in[i]-mean)/sd) > rej)
			{
				Log(Log::DEBUG)<<__func__<<": "<<i<<" is outlier mean: "<<mean<<" sd: "<<sd<<" n: "<<n<<'\n';
				outliers.push_back(i);
				removed = true;
			}
		}
	}
	if(removed) thompsonTauTest(in, outliers, criticalValue);
}

std::vector<cv::Point2f>::iterator getTopLeft(std::vector<cv::Point2f>& points)
{
	return std::min_element(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){
		return sqrt(a.y*a.y+a.x*a.x) < sqrt(b.y*b.y+b.x*b.x);
	});
}

std::vector<cv::Point2f>::iterator getBottomRight(std::vector<cv::Point2f>& points)
{
	return std::max_element(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){
		return sqrt(a.y*a.y+a.x*a.x) < sqrt(b.y*b.y+b.x*b.x);
	});
}

double distance(const cv::Point2f& a, const cv::Point2f& b)
{
	return sqrt((a.y-b.y)*(a.y-b.y)+(a.x-b.x)*(a.x-b.x));
}

bool findClosest(size_t& xIndex, size_t& yIndex, 
				 const cv::Point2f point, const std::vector< std::vector<cv::Point2f> >& array, 
				 float xTolerance, float yTolerance)
{	
	size_t rowBelow = 0;
	while(rowBelow < array.size() && !array[rowBelow].empty() && array[rowBelow][0].y < point.y) ++rowBelow;
	
	if(rowBelow == array.size()) rowBelow = array.size()-1;
	
	size_t rightAbove = 0;
	size_t rightBelow = 0;
	
	while(rightBelow < array[rowBelow].size() && array[rowBelow][rightBelow].x < point.x ) ++rightBelow;
	
	float distRB = distance(point, array[rowBelow][rightBelow]);
	float distLB = rightBelow > 0 ? distance(point, array[rowBelow][rightBelow-1]) : std::numeric_limits<float>::max();
	float distRA = std::numeric_limits<float>::max();
	float distLA = std::numeric_limits<float>::max();
	
	if(rowBelow > 0)
	{
		while(rightAbove < array[rowBelow-1].size() && array[rowBelow-1][rightAbove].x < point.x ) ++rightAbove;
		distRA = distance(point, array[rowBelow-1][rightAbove]);
		if(rightAbove > 0)  distLA = distance(point, array[rowBelow-1][rightAbove-1]);
	}
	
	float* min = &distRB;
	if(distLB < *min) min = &distLB;
	if(distRA < *min) min = &distRA;
	if(distLA < *min) min = &distLA;
	
	if(min == &distRB)
	{
		yIndex = rowBelow;
		xIndex = rightBelow;
	} 
	else if(min == &distLB)
	{
		yIndex = rowBelow;
		xIndex = rightBelow-1;
	} 
	else if(min == &distRA)
	{
		yIndex = rowBelow-1;
		xIndex = rightBelow;
	} 
	else if(min == &distLA)
	{
		yIndex = rowBelow-1;
		xIndex = rightBelow-1;
	}
	else return false;
	return abs(array[yIndex][xIndex].x - point.x) < xTolerance && abs(array[yIndex][xIndex].y - point.y) < yTolerance;
}

bool findClosest(size_t& index, const cv::Point2f point, const std::vector<cv::Point2f>& array, float xTolerance, float yTolerance)
{
	float minDistance = std::numeric_limits<float>::max();

	bool found;
	for(size_t i = 0; i < array.size(); ++i)
	{
		const auto& arrayPoint = array[i];
		if(abs(arrayPoint.x - point.x < xTolerance) && 
		   abs(arrayPoint.y - point.y < yTolerance) &&
		   distance(point, arrayPoint) < minDistance)
		{
			index = i;
			found = true;
		}
	}
	return found;
}

void interpolateMissing(cv::Mat& mat)
{
	assert(mat.type() == CV_32FC1);
	
	for(int y = 0; y < mat.rows; y++)
	{
		float* col = mat.ptr<float>(y);
		for(int x = 0; x < mat.cols; ++x)
		{
			if(col[x] < 0) 
			{
				int closestA = -1;
				int closestB = -1;
				int dist = std::numeric_limits<int>::max();
				for(int i = 0; i < mat.cols; ++i)
				{
					if(i != closestA && col[i] >= 0 && abs(i-x) <= dist)
					{
						closestB = closestA;
						closestA = i;
						dist = abs(i-x);
					}
				}
				if(closestA < 0 || closestB < 0)
				{
					closestA = -1;
					closestB = -1;
					dist = std::numeric_limits<int>::max();
					for(int i = mat.cols-1; i >= 0; --i)
					{
						if(i != closestA && col[i] >= 0 && abs(i-x) <= dist)
						{
							closestB = closestA;
							closestA = i;
							dist = abs(i-x);
						}
					}
				}
				float slope = (col[closestB] - col[closestA])/(closestB-closestA);
				col[x] = col[closestA] - (closestA-x)*slope;
			}
		}
	}
}

void fillMissing(cv::Mat& mat)
{
	assert(mat.type() == CV_32FC1);
	
	bool finished = true;
	for(int y = 0; y < mat.rows; y++)
	{
		float* col = mat.ptr<float>(y);
		for(int x = 0; x < mat.cols; ++x)
		{
			if(col[x] < 0 && col[x] > -2) 
			{
				if(y > 0 && mat.at<float>(y-1,x) >= 0)
				{
					col[x] = mat.at<float>(y-1,x);
					finished = false;
				}
				else if(y < mat.rows-1 && mat.at<float>(y+1,x) >= 0)
				{
					col[x] = mat.at<float>(y+1,x);
					finished = false;
				}
				if(col[x] > 0 && ((x+1 < mat.cols && col[x] > col[x+1]) || (x > 0 && col[x] < col[x-1])))
					col[x] = -2;
			}
		}
	}
	if(!finished) fillMissing(mat);
}

bool findDeadSpace(const cv::Mat& mat, cv::Rect& roi)
{
	assert(mat.type() == CV_32FC1);
	
	if(mat.cols < 2 || mat.rows < 2) 
		return false;
	
	float centerTop = mat.at<float>(0,mat.cols/2);
	float centerLeft = mat.at<float>(mat.rows/2,0);
	float centerBottom = mat.at<float>(mat.rows-1,mat.cols/2);
	float centerRight = mat.at<float>(mat.rows/2,mat.cols-1);
	
	int left = 0;
	int right = mat.cols-1;
	int top = 0;
	int bottom = mat.rows-1;
	for(; left < mat.cols && mat.at<float>(mat.rows/2,left) == centerLeft; ++left);
	for(; top < mat.rows && mat.at<float>(top,mat.cols/2) == centerTop; ++top);
	for(; right > left && mat.at<float>(mat.rows/2,right) == centerRight; --right);
	for(; bottom > top && mat.at<float>(bottom,mat.cols/2) == centerBottom; --bottom);
	
	
	roi.x=left > 0 ? left : 0;
	roi.y=top > 0 ? top : 0;
	roi.width = right-roi.x;
	roi.height = bottom-roi.y;
	
	if(roi.width < 0 || roi.height < 0)
		return false;
	
	Log(Log::DEBUG)<<"Reduced Roi: "<<roi;
	return true;
}

bool deleteEmptyCols(cv::Mat& mat)
{
	assert(mat.type() == CV_32FC1);

	std::vector<size_t> cols;
	cols.reserve(mat.cols);
	
	for(int x = 0; x < mat.cols; x++)
	{
		bool empty = true;
		for(int y = 0; y < mat.rows; y++)
		{
			if(mat.at<float>(x,y) > 0)
			{
				empty = false;
				break;
			}
		}
		
		if(!empty)
			cols.push_back(x);
	}
	
	if(mat.cols < static_cast<long int>(cols.size()))
	{
		cv::Mat tmp(cv::Size(cols.size(), mat.rows), CV_32FC1);

		for(auto& col : cols)
		{
			cv::Rect roi(cv::Point2i(col, 0), cv::Size(1, mat.rows));
			mat.copyTo(tmp(roi));
		}
		mat.release();
		mat = tmp;
		
		return true;
	}
	
	return false;
}

void removeEmptyFrontBackCols(cv::Mat& mat)
{
	assert(mat.type() == CV_32FC1);

	int front = 0;
	int back = 0;
	for(int y = 0; y < mat.rows; ++y)
	{
		if(mat.at<float>(y,0) >= 0) ++front;
		if(mat.at<float>(y,mat.cols-1) >= 0) ++back;
	}
	Log(Log::DEBUG)<<__func__<<" front: "<<front<<" back "<<back;
	cv::Rect roi;
	bool frontRej = (front < mat.rows/2);
	bool backRej  = (back < mat.rows/2);
	roi.x=frontRej;
	roi.y=0;
	roi.width = mat.cols - backRej - frontRej;
	roi.height = mat.rows;
	mat = mat(roi);
	if(frontRej || backRej) removeEmptyFrontBackCols(mat);
}

