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

#include <graphics.hpp>

#include <iostream>

int main () {
	initGraphics();

	cv::CascadeClassifier faceDetector ("lbpcascade_frontalface_improved.xml");

	cv::Ptr<cv::face::Facemark> facemark = cv::face::FacemarkLBF::create();
	facemark->loadModel ("lbfmodel.yaml");

	cv::VideoCapture vid (0);
	
	cv::Mat frame, gray, small;

	while (vid.read(frame)) {
		cv::cvtColor (frame, gray, cv::COLOR_BGR2GRAY);
		//downsample image for face detection, works too slow on full res
		cv::pyrDown (gray, small);
		cv::pyrDown (small, small);

		std::vector<cv::Rect> faces;
		faceDetector.detectMultiScale(small, 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
			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;
			}

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

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

			//send control information to graphics
			updateModel(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
				),
				glm::vec2(
				landmarks[biggestFace][30].x * 2 / (float)frame.cols - 1,
				landmarks[biggestFace][30].y * 2 / (float)frame.rows - 1
				),
				landmarks[biggestFace][66].y - landmarks[biggestFace][62].y > 5);
		}

		graphicsFrame ();

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