#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>

using namespace cv;
using namespace std;

#define EPSILON 1E-5


cv::Point2f center(0, 0);

cv::Point2f computeIntersect(cv::Vec4i a,
	cv::Vec4i b)
{
	int x1 = a[0], y1 = a[1], x2 = a[2], y2 = a[3], x3 = b[0], y3 = b[1], x4 = b[2], y4 = b[3];
	float denom;

	if (float d = ((float)(x1 - x2) * (y3 - y4)) - ((y1 - y2) * (x3 - x4)))
	{
		cv::Point2f pt;
		pt.x = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / d;
		pt.y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) / d;
		return pt;
	}
	else
		return cv::Point2f(-1, -1);
}

void sortCorners(std::vector<cv::Point2f>& corners,
	cv::Point2f center)
{
	std::vector<cv::Point2f> top, bot;

	for (int i = 0; i < corners.size(); i++)
	{
		if (corners[i].y < center.y)
			top.push_back(corners[i]);
		else
			bot.push_back(corners[i]);
	}
	corners.clear();

	if (top.size() == 2 && bot.size() == 2) {
		cv::Point2f tl = top[0].x > top[1].x ? top[1] : top[0];
		cv::Point2f tr = top[0].x > top[1].x ? top[0] : top[1];
		cv::Point2f bl = bot[0].x > bot[1].x ? bot[1] : bot[0];
		cv::Point2f br = bot[0].x > bot[1].x ? bot[0] : bot[1];


		corners.push_back(tl);
		corners.push_back(tr);
		corners.push_back(br);
		corners.push_back(bl);
	}
}

struct Ray
{
	cv::Vec3f origin;
	cv::Vec3f direction;

};



bool linePlaneIntersection(Vec3d* contact, Vec3d ray, Vec3d rayOrigin,
	Vec3d normal, Vec3d coord) {
	// get d value
	float d = normal.dot(coord);

	if (normal.dot(ray) == 0) {
		return false; // No intersection, the line is parallel to the plane
	}

	// Compute the X value for the directed line ray intersecting the plane
	float x = (d - normal.dot(rayOrigin)) / normal.dot(ray);

	// output contact point
	*contact = rayOrigin + normalize(ray)*x; //Make sure your ray vector is normalized
	return true;
}

cv::Point2f snapPoint;
bool isClicked = false;

void onmouse(int event, int x, int y, int flags, void* param)
{
	if (event == CV_EVENT_LBUTTONDOWN)
	{
		snapPoint.x = x;
		snapPoint.y = y;

		//Mat &img = *((Mat*)(param)); // 1st cast it back, then deref
		//circle(img, Point(x, y), 50, Scalar(0, 255, 0), 1);
		isClicked = true;
	}
}
std::vector<cv::Point3d> Generate3DPoints()
{
	std::vector<cv::Point3d> points;

	double x, y, z;

	x = .5; y = .5; z = -.5;
	points.push_back(cv::Point3d(x, y, z));

	x = .5; y = .5; z = .5;
	points.push_back(cv::Point3d(x, y, z));

	x = -.5; y = .5; z = .5;
	points.push_back(cv::Point3d(x, y, z));

	x = -.5; y = .5; z = -.5;
	points.push_back(cv::Point3d(x, y, z));

	x = .5; y = -.5; z = -.5;
	points.push_back(cv::Point3d(x, y, z));

	x = -.5; y = -.5; z = -.5;
	points.push_back(cv::Point3d(x, y, z));

	x = -.5; y = -.5; z = .5;
	points.push_back(cv::Point3d(x, y, z));

	/*
	for(unsigned int i = 0; i < points.size(); ++i)
	{
	std::cout << points[i] << std::endl;
	}
	*/
	return points;
}

std::vector<cv::Point2d> Generate2DPoints()
{
	std::vector<cv::Point2d> points;

	float x, y;

	x = 282; y = 274;
	points.push_back(cv::Point2d(x, y));

	x = 397; y = 227;
	points.push_back(cv::Point2d(x, y));

	x = 577; y = 271;
	points.push_back(cv::Point2d(x, y));

	x = 462; y = 318;
	points.push_back(cv::Point2d(x, y));

	x = 270; y = 479;
	points.push_back(cv::Point2d(x, y));

	x = 450; y = 523;
	points.push_back(cv::Point2d(x, y));

	x = 566; y = 475;
	points.push_back(cv::Point2d(x, y));

	for (unsigned int i = 0; i < points.size(); ++i)
	{
		std::cout << points[i] << std::endl;
	}

	return points;
}

int main(int argc, char** argv)
{


	Mat src, src_copy, edges, dst;
	src = imread("freezeFrame__1508152029892.png", 0);

	namedWindow("My window", 1);

	imshow("My window", src);
	while (1 && isClicked == false)
	{
		setMouseCallback("My window", onmouse, &src); // pass address of img here
		cv::waitKey(10);
		while (isClicked != false)
		{
			isClicked = false;

			std::vector< cv::Point2f > corners;

			// maxCorners – The maximum number of corners to return. If there are more corners
			// than that will be found, the strongest of them will be returned
			int maxCorners = 10;

			// qualityLevel – Characterizes the minimal accepted quality of image corners;
			// the value of the parameter is multiplied by the by the best corner quality
			// measure (which is the min eigenvalue, see cornerMinEigenVal() ,
			// or the Harris function response, see cornerHarris() ).
			// The corners, which quality measure is less than the product, will be rejected.
			// For example, if the best corner has the quality measure = 1500,
			// and the qualityLevel=0.01 , then all the corners which quality measure is
			// less than 15 will be rejected.
			double qualityLevel = 0.01;

			// minDistance – The minimum possible Euclidean distance between the returned corners
			double minDistance = 20.;

			// mask – The optional region of interest. If the image is not empty (then it
			// needs to have the type CV_8UC1 and the same size as image ), it will specify
			// the region in which the corners are detected
			cv::Mat mask;

			// blockSize – Size of the averaging block for computing derivative covariation
			// matrix over each pixel neighborhood, see cornerEigenValsAndVecs()
			int blockSize = 3;

			// useHarrisDetector – Indicates, whether to use operator or cornerMinEigenVal()
			bool useHarrisDetector = false;

			// k – Free parameter of Harris detector
			double k = 0.04;

			cv::goodFeaturesToTrack(src, corners, maxCorners, qualityLevel, minDistance, mask, blockSize, useHarrisDetector, k);

			for (size_t i = 0; i < corners.size(); i++)
			{
				cv::circle(src, corners[i], 10, cv::Scalar(0, 255, 255), -1);
			}

			for (size_t i = 0; i < corners.size(); i++)
			{
				cv::Point2f pt1 = corners[i];
				for (int j = 0; j < corners.size(); j++)
				{
					cv::Point2f pt2 = corners[j];
					line(src, pt1, pt2, cv::Scalar(0, 255, 0));
				}
			}

			corners.push_back(snapPoint);

			Mat cameraIntrinsics(3, 3, CV_64FC1);

			cameraIntrinsics.at<double>(0, 0) = 1.6003814935684204;
			cameraIntrinsics.at<double>(0, 1) = 0;
			cameraIntrinsics.at<double>(0, 2) = -0.0021958351135253906;
			cameraIntrinsics.at<double>(1, 0) = 0;
			cameraIntrinsics.at<double>(1, 1) = 1.6003814935684204;
			cameraIntrinsics.at<double>(1, 2) = -0.0044271680526435375;
			cameraIntrinsics.at<double>(2, 0) = 0;
			cameraIntrinsics.at<double>(2, 1) = 0;
			cameraIntrinsics.at<double>(2, 2) = 1;




			Mat invCameraIntrinsics = cameraIntrinsics.inv();
			cout << "inv" << invCameraIntrinsics;
			std::vector<cv::Vec3d> pointsTransformed3D;

			std::vector<cv::Vec3d> points3D;
			for (int i = 0; i < corners.size(); i++)
			{
				cv::Vec3d pt;

				pt[2] = 1.0f;

				pt[0] = (corners[i].x / 1280) * 2 - 1;
				pt[1] = (corners[i].y / 720) * 2 - 1;

				points3D.push_back(pt);
			}
			cv::transform(points3D, pointsTransformed3D, invCameraIntrinsics);

			std::vector<Ray> rays;
			for (int i = 0; i < corners.size(); i++)
			{

				Ray ray;

				ray.origin = Vec3f(0, 0, 0);
				ray.direction = Vec3f(pointsTransformed3D[i][0], pointsTransformed3D[i][1], pointsTransformed3D[i][2]);

				rays.push_back(ray);
			}


			std::vector<cv::Vec3d> contacts;

			for (int i = 0; i < pointsTransformed3D.size(); i++)
			{
				Vec3d pt(pointsTransformed3D[i][0], pointsTransformed3D[i][1], pointsTransformed3D[i][2]);


				cv::Vec3d contact;
				if (linePlaneIntersection(&contact, rays[i].direction, rays[i].origin, normalize(Vec3f(0, 1, 0)), pt))
				{

					contacts.push_back(contact);
				}
			}


			Mat rotationMatrix(3, 3, CV_64FC1);

			rotationMatrix.at<double>(0, 0) = 0.9115078799790896;
			rotationMatrix.at<double>(0, 1) = -0.1883612409043686;
			rotationMatrix.at<double>(0, 2) = -0.3656137684237178;
			rotationMatrix.at<double>(1, 0) = -0.3046835686704949;
			rotationMatrix.at<double>(1, 1) = 0.2878667580409447;
			rotationMatrix.at<double>(1, 2) = -0.9079100465339108;
			rotationMatrix.at<double>(2, 0) = 0.2762631132059388;
			rotationMatrix.at<double>(2, 1) = 0.9389636694462479;
			rotationMatrix.at<double>(2, 2) = 0.2050022432604093;

			cv::Mat rVec(3, 1, CV_64FC1); // Rotation vector
			Rodrigues(rotationMatrix, rVec);
			double norm = cv::norm(rVec);

			double theta = (double)(sqrt(rVec.at<double>(0)*rVec.at<double>(0) + rVec.at<double>(1)*rVec.at<double>(1) + rVec.at<double>(2)*rVec.at<double>(2)) * 180 / 3.14567898726);

			cv::Mat tVec(3, 1, CV_64FC1); // Translation vector
			tVec.at<double>(0) = 21.408294677734375;
			tVec.at<double>(1) = 531.1319580078125;
			tVec.at<double>(2) = 705.74224853515625;

			cv::Mat distCoeffs(5, 1, CV_64FC1);   // Distortion vector
			distCoeffs.at<double>(0) = 0;
			distCoeffs.at<double>(1) = 0;
			distCoeffs.at<double>(2) = 0;
			distCoeffs.at<double>(3) = 0;
			distCoeffs.at<double>(4) = 0;

			std::vector<cv::Point2d> projectedPoints;
			std::vector < cv::Point3d> ContactPoints;

			for (int i = 0; i < contacts.size(); i++)
			{
				cv::Point3d pt;

				pt.x = contacts[i][0];
				pt.y = contacts[i][1];
				pt.z = contacts[i][2];

				ContactPoints.push_back(pt);
			}


			cv::projectPoints(ContactPoints, rVec, tVec, cameraIntrinsics, distCoeffs, projectedPoints);


			for (size_t i = 0; i < projectedPoints.size(); i++)
			{
				cv::Point2d pt;

				pt.x = projectedPoints[i].x* 1280;
				pt.y = projectedPoints[i].y* 720;

				cv::circle(src, pt, 10, cv::Scalar(255, 0, 255), -1);
			}

			imshow("My window", src);
		}
	}

cv:waitKey(0);
	return 0;
}