#include <cstdlib>
#include <iostream>
#include <algorithm>
#include "opencv2/core/core.hpp"




using std::cerr; using std::abort;
using std::min; using std::max;


/*! Find a point set in a mask that is nearest (in l^\infty metric) to a given point. */
/*!
	\arg `image` is a boolean mask, must be of type `CV_8C` and 2 dimensional, 
		nonzero value means true.
	
	\arg `center` is the point.
	
	\arg `output` becomes the nearest point found.

	\returns true iff a point is found, false if `image` is all false.
*/
bool
FindNearestPoint(cv::Mat image, cv::Point center, cv::Point &output) {
	if (CV_8U != image.type() || 2 != image.dims) {
		cerr << "error: wrong type of matrix passed to FindNearestPoints (type=" << image.type() << ", dims=" << image.dims << ").\n";
		abort();
	}
	int sy = image.rows, sx = image.cols;
	int cy = center.y, cx = center.x;
	int ldx = cx < 0 ? -cx : sx <= cx ? cx - sx : 0;
	int ldy = cy < 0 ? -cy : sy <= cy ? cy - sy : 0;
	int ld = max(ldx, ldy);
	int hdx = cx < 0 ? sx - cx : sx <= cx ? cx : max(cx, sx - cx);
	int hdy = cy < 0 ? sy - cy : sy <= cy ? cy : max(cy, sy - cy);
	int hd = max(hdx, hdy);
	for (int d = ld; d <= hd; d++) {
		int ay = cy - d;
		int ly = max(0, ay);
		int by = cy + d;
		int hy = min(sy - 1, by);
		int ax = cx - d;
		int lx = max(0, ax);
		int bx = cx + d;
		int hx = min(sx - 1, bx);
		if (0 <= ax && ax < sx)
			for (int y = ly; y <= hy; y++)
				if (0 != image.at<unsigned char>(y, ax))
					{ output = cv::Point(ax, y); return true; }
		if (0 <= bx && bx < sx)
			for (int y = ly; y <= hy; y++)
				if (0 != image.at<unsigned char>(y, bx))
					{ output = cv::Point(bx, y); return true; }
		if (0 <= ay && ay < sy)
			for (int x = lx; x <= hx; x++)
				if (0 != image.at<unsigned char>(ay, x))
					{ output = cv::Point(x, ay); return true; }
		if (0 <= by && by < sy)
			for (int x = lx; x <= hx; x++)
				if (0 != image.at<unsigned char>(by, x))
					{ output = cv::Point(x, by); return true; }
	}
	output = cv::Point(0, 0);
	return false;
}