#include <eye.hpp>

// This pupil tracking algorithm is based on the paper
// "ACCURATE EYE CENTRE LOCALISATION BY MEANS OF GRADIENTS"
// by Fabian Timm  and  Erhardt Barth. Thanks!

float objectiveFunction(cv::Point2f c, cv::Mat gX, cv::Mat gY) {
	float sum = 0;
	for(int xx = 0; xx < gX.cols; xx++) {
		for(int xy = 0; xy < gX.rows; xy++) {
			cv::Point2f x(xx, xy);
			cv::Point2f g(gX.at<float>(xx, xy), gY.at<float>(xx, xy)); // gradient
			double gMag = std::sqrt(g.x * g.x + g.y * g.y);
			if(gMag < 200.0) continue;      // ignore gradients in homogenous regions
			//g = g / gMag;
			cv::Point2f d = x - c;  // displacement
			d /= std::sqrt(d.x * d.x + d.y * d.y);  // normalize d

			float dotProduct = d.y * g.x + d.x * g.y;

			dotProduct = std::max(0.0f, dotProduct);

			sum += dotProduct * dotProduct;
		}
	}
	sum /= gX.rows * gX.cols;

	return sum;
}

cv::Point2f derivativeFunction(cv::Point2f c, cv::Mat gX, cv::Mat gY) {
	cv::Point2f sum(0,0);

	for(int xx = 0; xx < gX.cols; xx++) {
		for(int xy = 0; xy < gX.rows; xy++) {
			cv::Point2f x(xy, xx);
			cv::Point2f g(gY.at<float>(xx, xy), gX.at<float>(xx, xy)); // gradient
			float gMag = std::sqrt(g.x * g.x + g.y * g.y);
			if(gMag < 200.0f) continue;	// ignore gradients in homogenous regions
			//g = g / gMag;
			cv::Point2f d = x - c;	// displacement
			float n = std::sqrt(d.x * d.x + d.y * d.y);
			if (n == 0) continue;
			//d /= n;	// normalize d
			float e = d.x * g.y + d.y * g.x;

			//float dotProduct = d.y * g.x + d.x * g.y;

			//e = std::max(0.0f, e);

			//sum += dotProduct * dotProduct;// / (gray.rows * gray.cols);

			sum += (d * (e * e) - g * e * (n * n)) / (n * n * n * n);
		}
	}

	return sum;
}

glm::vec2 eyeDirection(cv::Mat roi) {
	cv::Mat gX, gY, eyeMat;
	if(roi.rows > 32) {
		cv::pyrDown(roi, eyeMat, cv::Size(roi.cols / 2, roi.rows / 2));
	} else {
		cv::GaussianBlur(roi, eyeMat, cv::Size(7,7), 1);
	}

	cv::Scharr(eyeMat, gX, CV_32F, 1, 0);
	cv::Scharr(eyeMat, gY, CV_32F, 0, 1);

	float stepSize = eyeMat.rows / 10;
	float maxVal = 0;

	cv::Point2f irisPosition;
	for(int i = 0; i < 32; i++) {
		cv::Point2f c(std::rand() % eyeMat.cols, std::rand() % eyeMat.rows); //start at a random point
		
		float prevVal = 0;
		for(int j = 0; j < 8; j++) {
			cv::Point2f gradient = derivativeFunction(c, gX, gY);	// calculate gradient
			gradient /= std::sqrt(gradient.x * gradient.x +
					gradient.y * gradient.y);		// normalize

			for(int k = 0; k < 6; k++) {
				cv::Point2f newC = c + gradient * stepSize;
				if (newC.x < 0 || newC.x > eyeMat.cols || newC.y < 0 || newC.y > eyeMat.rows) continue;

				float newVal = objectiveFunction(newC, gX, gY);
				if (newVal > prevVal) {
					//c += sum * 3;// * 2 / (gray.rows * gray.cols);
					c = newC;
					prevVal = newVal;
					break;
					//cv::drawMarker(frame, c, cv::Scalar(0,0,255));
				} else {
					stepSize /= 2;
				}
			}
		}

		if(prevVal > maxVal) {
			maxVal = prevVal;
			irisPosition = cv::Point2f(c.y, c.x); // no idea why but the coordinates are flipped
		}
	}

	cv::drawMarker(eyeMat, irisPosition, cv::Scalar(128,128,128));
	
	// convert result to vector for graphics
	return glm::vec2(
			(irisPosition.x - eyeMat.rows / 2.0) / (eyeMat.rows / 2.0),
			(irisPosition.y - eyeMat.rows / 2.0) / (eyeMat.rows / 2.0));
}
