#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/world.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;

};



std::pair<bool, double>  linePlaneIntersection( Vec3f direction, Vec3f rayOrigin,
	Vec3f normal, Vec3f coord) {
	double denom = normalize(normal).dot(direction);
	double d = -(normalize(normal).dot(coord));

	if (std::abs(denom) < std::numeric_limits<double>::epsilon())
	{
		// Parallel
		return std::pair<bool, double>(false, (double)0);
	}
	else
	{
		double nom = normalize(normal).dot(normalize(rayOrigin)) + d;
		double t = -(nom / denom);
		return std::pair<bool, double>(t >= 0, (double)t);
	}

}

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



			corners.push_back(snapPoint);

			//Generate a camera intrinsic matrix
			Mat K = (Mat_<double>(3, 3)
				<< 1600, 0,src.cols/2,
				0, 1600,src.rows/2,
				0, 0, 1);



			Mat invCameraIntrinsics = K.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 );
				pt[1] = (corners[i].y );

				points3D.push_back(pt);
			}

			cv::transform(points3D, pointsTransformed3D, invCameraIntrinsics);


			Mat rot = (Mat_<double>(3, 3)
				<< 0.8744617, 0.2258282, -0.4293233,
				0.0608088, 0.8270180, 0.5588771,
				0.4812683, -0.5148232, 0.7094631);

			std::vector<cv::Point3d> pointsRotated;
			Mat translVec(3, 1, CV_64F);
			translVec.at<double>(0, 0) = 21.408294677734375;
			translVec.at<double>(1, 0) = 531.1319580078125;
			translVec.at<double>(2, 0) = 705.74224853515625;

			const Mat camera_translation = (-rot * translVec);
			cv::transform(pointsTransformed3D, pointsRotated, rot);

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

				Ray ray;

				ray.origin = Vec3f(camera_translation.at<double>(0,0), camera_translation.at<double>(1,0),
					camera_translation.at<double>(2,0));
				ray.direction = Vec3f(pointsRotated[i].x, pointsRotated[i].y,  pointsRotated[i].z);

				rays.push_back(ray);
			}
			std::vector<cv::Vec3f> contacts;

			for (int i = 0; i < pointsRotated.size(); i++)
			{
				Vec3f pt(0, 0,0);

				cv::Vec3f contact;
				std::pair<bool, double> test = linePlaneIntersection(rays[i].direction, rays[i].origin, Vec3f(0, 1, 0), pt);
				if (test.first == true)
				{
					cv::Vec3f contact(rays[i].origin + (rays[i].direction) * test.second);
					contacts.push_back(contact);
				}
			}



			std::vector<Vec6f> outputlines;
			for (int i = 0; i < contacts.size(); i++)
			{
				cv::Vec3f p1;
				p1[0] = contacts[i][0];
				p1[1] = contacts[i][1];
				p1[2] = contacts[i][2];
				for (int j = i + 1; j < contacts.size(); j++)
				{

					cv::Vec3f p2;
					p2[0] = contacts[j][0];
					p2[1] = contacts[j][1];
					p2[2] = contacts[j][2];

					for (int b = j + 1; b < contacts.size(); b++)
					{
						cv::Vec3f p3;
						p3[0] = contacts[b][0];
						p3[1] = contacts[b][1];
						p3[2] = contacts[b][2];

						std::vector<cv::Vec3f> lines;

						lines.push_back(p1);
						lines.push_back(p2);
						lines.push_back(p3);

						Vec6f lineOutput;
						cv::fitLine(lines, lineOutput, CV_DIST_L2, 0, 0.01, 0.01);

						outputlines.push_back(lineOutput);

					}
				}
			}

			/*
			std::vector<Ray> rays;
			for (int i = 0; i < pointsTransformed3D.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);
			}


			ViewSpacePoint.x = (2 * screenX) / screenWidth;
ViewSpacePoint.y = (2 * screenY) / screenHeight;
ViewSpacePoint.z = 1 / tan(cameraFieldOfViewInRadians / 2);

WorldPoint = ViewSpacePoint * inverseOfViewMatrix;


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

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

				cv::Vec3f contact;
				std::pair<bool, double> test = linePlaneIntersection(rays[i].direction, rays[i].origin, Vec3f(0, 1, 0), pt);
				if (test.first == true)
				{
					cv::Vec3f contact(rays[i].origin + (rays[i].direction) * test.second);
					contacts.push_back(contact);
				}
			}

		std::vector<cv::Vec3d> pointsOnPlane;
		std::vector<cv::Vec3d> pointsAbovePlane;
			for (int i = 0; i < contacts.size(); i++)
			{
				Vec3d sub = contacts[i];
				Vec3d normal = Vec3d(0, 1, 0);
				float d = -normal.dot(sub);
				pointsOnPlane.push_back(sub);
				if (-sub[1]  < std::numeric_limits<double>::epsilon())
				{

				}
				else
				{
					pointsAbovePlane.push_back(sub);
				}
			}

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

			for (int i = 0; i < pointsOnPlane.size(); i++)
			{
				Vec3d pt = pointsOnPlane[i];
				for (int j = 0; j < pointsAbovePlane.size(); j++ )
				{
					Vec3f direction = pointsAbovePlane[j] - pointsOnPlane[i] ;

				}
			}
			*/
			cv::Mat rvec, rMat = (cv::Mat_<double>(3, 3) << /* ( */1, 0, 0, 0, 1, 0, 0, 0, 1); //you had error here
			cv::Rodrigues(rMat, rvec);
			cv::Mat tvec = (cv::Mat_<double>(3, 1) <</* ( */ 0, 0, 0); //and here


			//Project the 3D Point onto 2D plane
			std::vector<cv::Point2d> points_2d;
			std::vector<cv::Point3d> points_3d;

			for (int i = 0; i < outputlines.size(); i++)
			{
				cv::Point3d pt;
				pt.x = (outputlines[i][0]);
				pt.y = (outputlines[i][1]);
				pt.z = (outputlines[i][2]);
				points_3d.push_back(pt);
			}

			projectPoints(points_3d, rvec, tvec, K, Mat(), points_2d);

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

				pt.x = points_2d[i].x;
				pt.y = points_2d[i].y;

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


			for (size_t i = 0; i < points_2d.size(); i++)
			{
				cv::Point2f pt1 = points_2d[i];
				for (int j = i + 1; j < points_2d.size(); j++)
				{
					cv::Point2f pt2 = points_2d[j];
					int length = 10;
					line(src, Point(pt2.x, pt2.y), Point(pt2.x + pt1.x * length, pt2.y + pt1.y * length), cv::Scalar(255, 0, 255), 3, 4);

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

cv:waitKey(0);
	return 0;
}