//C++

#include <iostream>
//include those opencv2 files in our program
#include "opencv2/opencv.hpp"
#include "opencv2/videoio.hpp" 
#include "opencv2/highgui.hpp"

int FPS=10; //FPS variable. FPS is the framerate of your video, aka your recording device's
int DISCARD_DURATION=5;
bool isDiscardData=true;
int countDiscard=0;

bool isBufferFull = false; //buffer variables to initialise before main();
int sampleIdBuffer = 0;
int BUFFER_DURATION= 15;

//Display normalised signal
template <typename T>
cv::Mat plotGraph(std::vector<T>& vals, int YRange[2])
{
	auto it = minmax_element(vals.begin(), vals.end());
	float scale = 1./ceil(*it.second - *it.first);
	float bias = *it.first;
	int rows = YRange[1] - YRange[0] + 1;
	cv::Mat image = 255*cv::Mat::ones( rows, vals.size(), CV_8UC3 );
	image.setTo(255);
	for (int i = 0; i < (int)vals.size()-1; i++)
	{
		cv::line(image, cv::Point(i, rows - 1 - (vals[i] -
		bias)*scale*YRange[1]), cv::Point(i+1, rows - 1 - (vals[i+1] -
		bias)*scale*YRange[1]), cv::Scalar(255, 0, 0), 1);
	}
	return image;
}

int main(){ 
	//Print "PPG algorithm" to terminal
	//Note to self: std::endl; returns to line in terminal; use it everytime when done printing something.
	std::cout << "PPG algorithm"<< std::endl;
	
	cv::VideoCapture cap;
	cap.open(0);
	if (!cap.isOpened())
	{
		//Check if we can access the camera
		std::cerr<<"[ERROR] unable to open camera!"<<std::endl;
		return -2;
	}
		
	cv::CascadeClassifier faceDetector; 
	if(!faceDetector.load("./haarcascade_frontalface_alt.xml"))//Testing to see if cascade_frontalface.xml is available (necessary for the program to work)
	{
		std::cerr<<"[ERROR] Unable to load face cascade"<<std::endl; 
		return -1;
	};	
		
	while (true)
	{
		//Create a matrix to store the image from the cam
		cv::Mat frame;
		//Wait for a new frame from camera and store it into "frame"
		cap.read(frame);
	
		//Check if we succeeded
		if (frame.empty()) //If the camera records a blank frame, returns an error.
		{
			std::cerr<<"[ERROR] Blank frame grabbed"<<std::endl;
			break;
		}
		
		if(isDiscardData) //This function delays the beginning of the analysis of the data to avoid processing frames taken during white balancing.
		{
			countDiscard++;
			if (countDiscard == DISCARD_DURATION*FPS)
			{
				isDiscardData=false;
			}
		}
		else
		{
			cv::Mat frame_gray;
			cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
			//cv::imshow("Gray", frame_gray); //Shows frame in greyscale, disabled if as comment
			std::vector<cv::Rect> faceRectangles;
			faceDetector.detectMultiScale(frame_gray, faceRectangles, 1.1, 3, 0, cv::Size(20,20)); //Detects face
			
			if (faceRectangles.size() > 0)
				cv::rectangle(frame, faceRectangles[0], cv::Scalar(0,0,255),1,1,0);

			cv::Rect foreheadROI; //create a forehead ROI equal to the face ROI slightly moved upward.
			if (faceRectangles.size() > 0)
			{
				foreheadROI = faceRectangles[0];
				foreheadROI.height *= 0.3;

				cv::Mat frame_forehead = frame(foreheadROI);
				cv::Scalar avg_forehead = mean(frame_forehead); //calculates mean of object frame_forehead
			

				//Buffer of average value for the green channel over the forehead ROI
				cv::Mat greenSignal(1, FPS*BUFFER_DURATION, CV_64F); 
				if (!isBufferFull)
				{
					std::cout << "sampleIdBuffer= " << sampleIdBuffer << " / " << FPS*BUFFER_DURATION << std::endl;
					greenSignal.at<double>(0, sampleIdBuffer) = avg_forehead[1] ;
					sampleIdBuffer++;
					if (sampleIdBuffer == FPS*BUFFER_DURATION)
					{
					isBufferFull = true;
					std::cout<<"greenSignal= "<<greenSignal<<std::endl;
					}
				}
				else
				{

					//Normalisation of our signal
					std::vector<double> greenSignalNormalized; 
					cv::Scalar mean, stddev;
					cv::meanStdDev(greenSignal, mean, stddev);
					for (int l_sample=0; l_sample < FPS*BUFFER_DURATION; l_sample++)
					{
						greenSignalNormalized.push_back((greenSignal.at<double>(0, l_sample)-mean[0])/stddev[0]);
					}
					//This is used in the main function to display the signal
					int range[2] = {0, (int)(FPS*BUFFER_DURATION)};
					cv::imshow("green", plotGraph(greenSignalNormalized, range));

					cv::Mat greenFFT; //Fast Fourier Transform
					std::vector<double> greenFFTModule;
					cv::dft(greenSignalNormalized,greenFFT,cv::DFT_ROWS|cv::DFT_COMPLEX_OUTPUT);
					cv::Mat planes[] = {cv::Mat::zeros(greenSignalNormalized.size(),1, CV_64F),
					cv::Mat::zeros(greenSignalNormalized.size(),1, CV_64F)};
					cv::split(greenFFT, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I))
					greenFFTModule.clear();
					for (int l=0; l < planes[1].cols; l++)
					{
						double moduleFFT = pow(planes[1].at<double>(0,l),2) +
						pow(planes[0].at<double>(0,l),2);
						greenFFTModule.push_back(sqrt(moduleFFT));
					}
					// display green FFT
					cv::imshow("FFT module green", plotGraph(greenFFTModule, range));
					
					//Find max Value and index of said value from FFT 
					float maxValue=-1;
					int indexValue=0;
					for(auto i=0.5*(BUFFER_DURATION);i<(4*BUFFER_DURATION);i++)
					{
						if(greenFFTModule[i]>maxValue)
						{
							maxValue=greenFFTModule[i];
							indexValue=i;
						}
					}
					//Calculate and print Heart Rate Beat Per Minute
					float HRBPM=(indexValue*60.0)/(BUFFER_DURATION);
					std::cout<<HRBPM << std::endl;
				}
			}
		}
		
		cv::imshow("Color", frame); //shows the colored frame
		if (cv::waitKey(1000.0/FPS)>=0) //Stops after 1000/FPS frames 
		{ 
			break;
		}
	}	
}