#include <iostream>
#include "opencv2/opencv.hpp"
#include "opencv2/videoio.hpp"
#include "opencv2/highgui.hpp"

#define FPS 30.0
bool isDiscardData = true;
int countDiscard = 0;
const double nyquistFreq = FPS / 2.0;
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(){
	
	int sampleIdBuffer = 0;
	int BUFFER_DURATION= 10;
    std::cout<<"PPG, j'adore gitbash même si avec github desktop c'est mieux"<< std::endl;
    cv::VideoCapture cap;
    cap.open(0);
    if (!cap.isOpened())
    {
    std::cerr << "[ERROR] Unable to open camera!" << std::endl;
    return -2;
    }
    cv::CascadeClassifier faceDetector;
    if( !faceDetector.load("./haarcascade_frontalface_alt.xml"))
    {
    std::cerr << "[ERROR] Unable to load face cascade" << std::endl;
    return -1;
    };
    cv::Mat greenSignal(1, FPS*BUFFER_DURATION, CV_64F);
    while (true)
    {
    	if (cv::waitKey(1000.0/FPS) >= 0)
        {
            break;
        }
        if (isDiscardData)
        {
            countDiscard++;
            std::cout <<"countDiscard= " << countDiscard<< std::endl;
            if (countDiscard == 1*FPS)

            isDiscardData = false;
        }
        else
        {
            // create a matrix to store the image from the cam
            cv::Mat frame, frame_gray;
            bool isBufferFull = false;
            // wait for a new frame from camera and store it into 'frame'
            cap.read(frame);
            // check if we succeeded
            if (frame.empty())
            {
                std::cerr << "[ERROR] blank frame grabbed" << std::endl;
                break;
            }
            std::vector<cv::Rect> faceRectangles;
            cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
            faceDetector.detectMultiScale(frame_gray, faceRectangles, 1.1, 3, 0,  cv::Size(20, 20));
            cv::rectangle(frame, faceRectangles[0], cv::Scalar(0, 0, 255), 1, 1, 0);
            cv::Rect foreheadROI;
            foreheadROI = faceRectangles[0];
            foreheadROI.height *= 0.3;
            cv::Mat frame_forehead = frame(foreheadROI);
            cv::Scalar avg_forehead = mean(frame_forehead);
            if (!isBufferFull)
            {
            	greenSignal.at<double>(0, sampleIdBuffer) = avg_forehead[1] ;
    		sampleIdBuffer++;
    		std::cout << "BUFFER" << sampleIdBuffer << std::endl;
    		if (sampleIdBuffer == FPS*BUFFER_DURATION)
    		{
    			isBufferFull = true;
    			sampleIdBuffer=0;
			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]);
			}
			int range[2] = {0, (int)(FPS*BUFFER_DURATION)};
			cv::imshow("green", plotGraph(greenSignalNormalized, range));
			cv::Mat greenFFT;
			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);
			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));
			}
			cv::imshow("FFT module green", plotGraph(greenFFTModule, range));
			
			// Find the peaks in the green signal in the frequency range of 0 to 4 Hz
			std::vector<int> peakLocs;
			double Fs = FPS;
			double df = Fs/greenSignalNormalized.size(); // Frequency resolution
			int fStart = 0; // Index of first frequency component that corresponds to 0 Hz
			int fEnd = (int) (3/df); // Index of last frequency component that corresponds to 4 Hz
			for (int i = fStart + 1; i < fEnd - 1; i++) {
			    if (greenFFTModule[i] > greenFFTModule[i-1] && greenFFTModule[i] > greenFFTModule[i+1]) {
				peakLocs.push_back(i);
			    }
			}

			// Calculate heart rate based on peak locations
			double sumIntervals = 0.0;

			for (int i = 1; i < peakLocs.size(); i++) {
			    sumIntervals += (peakLocs[i] - peakLocs[i-1]) / FPS;
			}
			std::cout << "sum interval: " << sumIntervals << std::endl;
			std::cout << "pealocksize: " << peakLocs.size() << std::endl;
			double meanInterval = sumIntervals / (peakLocs.size() - 1);
			std::cout << "mean interval: " << meanInterval << std::endl;
			double heartRate = (60.0 / meanInterval);
			std::cout << "Heart rate (no findPeaks): " << heartRate << " bpm" << std::endl;


    			
    		}
    		}


	
            
            
            

            cv::rectangle(frame, foreheadROI, cv::Scalar(0, 255, 0), 2);
            cv::imshow("Color", frame);

        }
        
        
    }


return 0;
}