#include "matutils.h"
#include <opencv2/core/ocl.hpp>
#include <iostream>

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;
		}
	}
}

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, 
				 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;
}


void interpolateMissing(cv::Mat& mat)
{
	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)
{
	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);
}