#include <opencv2/opencv.hpp>
#include <opencv2/face.hpp>

#include <iostream>

#include <graphics.hpp>
#include <paths.hpp>
#include <args.hpp>
#include <cv.hpp>
#include <eye.hpp>
#include <modelpart.hpp>

cv::Ptr<cv::face::Facemark> facemark;
cv::CascadeClassifier haarFaceDetector;
cv::dnn::Net dnnFaceDetector;
cv::VideoCapture vid;
cv::Mat frame, gray, small;

void initCV() {
	haarFaceDetector = cv::CascadeClassifier (resolvePath("cvdata/haarcascade_frontalface_alt2.xml"));
	dnnFaceDetector = cv::dnn::readNetFromCaffe(
			resolvePath("cvdata/deploy.prototxt"),
			resolvePath("cvdata/res10_300x300_ssd_iter_140000_fp16.caffemodel") );

	facemark = cv::face::FacemarkLBF::create();
	facemark->loadModel (resolvePath("cvdata/lbfmodel.yaml"));

	vid = cv::VideoCapture (0);

	if(!vid.isOpened()) {std::cout << "Camera not opened!" << std::endl;}
}

void dnnFaceDetect(cv::Mat inFrame, std::vector<cv::Rect>* faces) {
	cv::Mat inputBlob = cv::dnn::blobFromImage(inFrame, 1.0f, cv::Size(300, 300), cv::Scalar(104, 177, 123, 0), false, false);

	dnnFaceDetector.setInput(inputBlob, "data");
	cv::Mat output = dnnFaceDetector.forward("detection_out");
	cv::Mat detection(output.size[2], output.size[3], CV_32F, output.ptr<float>());

	for (int i = 0; i < detection.rows; i++) {
		float confidence = detection.at<float>(i, 2);

		if (confidence > 0.75f) {
			int x1 = detection.at<float>(i, 3) * inFrame.cols;
			int y1 = detection.at<float>(i, 4) * inFrame.rows;
			int x2 = detection.at<float>(i, 5) * inFrame.cols;
			int y2 = detection.at<float>(i, 6) * inFrame.rows;

			cv::Point2f pt1(x1, y1);
			cv::Point2f pt2(x2, y2);

			faces->push_back(cv::Rect(pt1, pt2));
		}
	}
}

//process image and send controls to graphics
void cvFrame() {
	vid.read(frame);

	if(frame.empty()) return;

	cv::cvtColor (frame, gray, cv::COLOR_BGR2GRAY);

	std::vector<cv::Rect> faces;

	if (optData.useHaar) {
		//downsample image for face detection, works too slow on full res
		cv::pyrDown (gray, small);
		cv::pyrDown (small, small);

		haarFaceDetector.detectMultiScale(small, faces);
	} else {
		dnnFaceDetect(frame, &faces);
	}

	//get biggest face
	int biggestFace = 0;
	int biggestArea = 0;
	for (int i = 0; i < faces.size(); i++) {
		//convert face region to full res, because we perform facemark on full res
		if (optData.useHaar) {
			faces[i] = cv::Rect (faces[i].x * 4, faces[i].y * 4, faces[i].width * 4, faces[i].height * 4);
		}

		int iArea = faces[i].area();
		if (iArea > biggestArea) {
			biggestFace = i;
			biggestArea = iArea;
		}
		
		//force ROI to be square
		faces[i] = cv::Rect(faces[i].x + faces[i].width / 2 - faces[i].height / 2,
				faces[i].y,
				faces[i].height,
				faces[i].height);

		cv::rectangle (frame, faces[i], cv::Scalar (255, 255, 0));
	}

	//TODO: make a pointer to faces[biggestFace] and use that

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

	if (facemark->fit (frame, faces, landmarks)) {
		for (int i = 0; i < landmarks[biggestFace].size(); i++) {
			cv::circle (frame, landmarks[biggestFace][i], 2, cv::Scalar (255, 255, 255));
		}
		cv::circle(frame, cv::Point2f(
			(landmarks[biggestFace][2].x + landmarks[biggestFace][14].x) / 2,
			(landmarks[biggestFace][2].y + landmarks[biggestFace][14].y) / 2
					), 6, cv::Scalar(0, 0, 255));
		cv::circle (frame, landmarks[biggestFace][30], 6, cv::Scalar (0, 255, 255));
		cv::circle (frame, landmarks[biggestFace][66], 3, cv::Scalar (0, 255, 0));
		cv::circle (frame, landmarks[biggestFace][62], 3, cv::Scalar (0, 255, 0));

		//get ROI for eyes
		float eyeWidth = landmarks[biggestFace][45].x - landmarks[biggestFace][42].x;
		cv::Rect eyeRect(landmarks[biggestFace][42].x,
				landmarks[biggestFace][42].y - eyeWidth / 2, eyeWidth, eyeWidth);

		cv::rectangle(frame, eyeRect, cv::Scalar(255, 255, 255));

		cv::Mat eyeROI;
		eyeROI = gray(eyeRect);

		glm::vec2 eyeVector = eyeDirection(eyeROI);

		//send control information to graphics
		float faceSize = landmarks[biggestFace][14].x - landmarks[biggestFace][2].x;

		struct FaceData faceData;
		faceData.positions[BIND_NULL] = glm::vec2(0.0f, 0.0f);
		faceData.positions[BIND_HEAD] = glm::vec2(
			(landmarks[biggestFace][2].x + landmarks[biggestFace][14].x) / 2
				* 2 / (float)frame.cols - 1,
			(landmarks[biggestFace][2].y + landmarks[biggestFace][14].y) / 2
				* 2 / (float)frame.rows - 1
			);
		faceData.positions[BIND_FACE] = glm::vec2(
			landmarks[biggestFace][30].x * 2 / (float)frame.cols - 1,
			landmarks[biggestFace][30].y * 2 / (float)frame.rows - 1
			);
		faceData.positions[OFFSET_EYES] = eyeVector;

		faceData.triggers[TRIGGER_NULL] = false;
		faceData.triggers[TRIGGER_MOUTH_OPEN] = 
		(landmarks[biggestFace][66].y - landmarks[biggestFace][62].y) / faceSize > 0.04f;

		faceData.headRotation = atanf(
			(float)(landmarks[biggestFace][14].y - landmarks[biggestFace][2].y) /
			(float)(landmarks[biggestFace][2].x - landmarks[biggestFace][14].x));
		faceData.scale = faceSize * 6 / (float)frame.cols;


		cv::line(frame, cv::Point(50,50), cv::Point(50,50) + cv::Point(eyeVector.x * 25, eyeVector.y * 25), cv::Scalar(255,128,128));
		cv::circle(frame, cv::Point(50,50), 6, cv::Scalar(255,128,128));

		updateModel(faceData);
	}
}

void cvShowFrame() {
	if(frame.empty()) return;

	cv::imshow("Video Input", frame);
	cv::waitKey(1);
}