//some code taken from file with this header:

/*
-- Georgia Tech 2016 Spring
--
-- This is a sample code to show how to use the libfreenet2 with OpenCV
--
-- The code will streams RGB, IR and Depth images from an Kinect sensor.
-- To use multiple Kinect sensor, simply initial other "listener" and "frames"
-- This code refered from sample code provided from libfreenet2: Protonect.cpp
-- https://github.com/OpenKinect/libfreenect2
-- and another discussion from: http://answers.opencv.org/question/76468/opencvkinect-onekinect-for-windows-v2linuxlibfreenect2/
-- Contact: Chih-Yao Ma at <cyma@gatech.edu>
*/




#include "opencv2/opencv.hpp"
#include "opencv2/videoio.hpp"
#include "opencv2/highgui.hpp"



#include <iostream>
#include <stdio.h>
#include <iomanip>
#include <time.h>
#include <signal.h>

#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/logger.h>



bool protonect_shutdown = false; // Whether the running application should shut down.

void sigint_handler(int s)
{
  protonect_shutdown = true;
}


int main()
{
    std::cout << "Streaming from Kinect One sensor!" << std::endl;

    //! [context]
    libfreenect2::Freenect2 freenect2;
    libfreenect2::Freenect2Device *dev = 0;
    libfreenect2::PacketPipeline *pipeline = 0;
    //! [context]

    //! [discovery]
    if(freenect2.enumerateDevices() == 0)
    {
        std::cout << "no device connected!" << std::endl;
        return -1;
    }

    std::string serial = freenect2.getDefaultDeviceSerialNumber();

    std::cout << "SERIAL: " << serial << std::endl;

    if(pipeline)
    {
        //! [open]
        dev = freenect2.openDevice(serial, pipeline);
        //! [open]
    } else {
        dev = freenect2.openDevice(serial);
    }

    if(dev == 0)
    {
        std::cout << "failure opening device!" << std::endl;
        return -1;
    }

    signal(SIGINT, sigint_handler);
    protonect_shutdown = false;

    //! [listeners]
    libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color);
    libfreenect2::FrameMap frames;
    dev->setColorFrameListener(&listener);
    //! [listeners]

    //! [start]
    dev->start();

    std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
    std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;
    //! [start]

    cv::Mat rgbmat;
    
    cv::CascadeClassifier faceDetector;
    if(!faceDetector.load("./haarcascade_frontalface_alt.xml")){
        std::cerr <<"[ERROR] Unable to load face cascade" << std::endl;
        return -1;
    }
    
    
    
    
    bool isDiscardData = true;
    int countDiscard = 0;
    const int DISCARD_DURATION = 2;//maybe do a float?
    const int FPS = 30;//can actually switch to 15fps in low light, but for this application it is not very important

    std::cout << "beginning loop" << std::endl;
    while(!protonect_shutdown)
    {
        int64 start = cv::getTickCount();//gotta check var type is it appropriate
        
        listener.waitForNewFrame(frames);
        libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
        cv::Mat(rgb->height, rgb->width, CV_8UC4, rgb->data).copyTo(rgbmat);

        if(cv::waitKey(1) >= 0){//the listener waits for frames, there is no need to limit speed ourselves

            protonect_shutdown = true;
        }

        listener.release(frames);
        
        
        
        

        
        if(isDiscardData){
            countDiscard++;
            cv::imshow("rgb", rgbmat);
            double fps = cv::getTickFrequency() / (cv::getTickCount() - start);
            std::cout << "FPS : " << fps << std::endl;
            if(countDiscard == DISCARD_DURATION*FPS){
                isDiscardData = false;
                std::cout << "not discarding data anymore" << std::endl;
            }
            
            continue;
        }
        
        cv::Mat frame_gray;
        cvtColor( rgbmat, frame_gray, cv::COLOR_BGR2GRAY );
        equalizeHist(frame_gray, frame_gray);
        
        std::vector<cv::Rect> faceRectangles;
        faceDetector.detectMultiScale(frame_gray, faceRectangles, 1.1, 3, 0, cv::Size(150, 150));
        //all default parameters besides setting a min face size to be valid
        
        /*
        printf("%zd face(s) are found.\n", faceRectangles.size());
        for (int i = 0; i < faceRectangles.size(); i++) {
            cv::Rect r = faceRectangles[i];
            printf("a face is found at Rect(%d,%d,%d,%d).\n", r.x, r.y, r.width, r.height);
        }
        */
        
        if(faceRectangles.empty() == false){//THIS WAS AN INFURIATING BUG
            cv::rectangle(rgbmat, faceRectangles[0], cv::Scalar(100, 0, 255), 1, 1, 0);
            
            cv::Rect foreheadROI;
            foreheadROI = faceRectangles[0];
            foreheadROI.height *= 0.3;
            //foreheadROI.width *= 0.8;
            cv::rectangle(rgbmat, foreheadROI, cv::Scalar(50, 200, 50), 1, 1, 0);
        }
        
        
        
        
        
        
        
        
        cv::imshow("rgb", rgbmat);
        //cv::imshow("grayscale", frame_gray);
        
        double fps = cv::getTickFrequency() / (cv::getTickCount() - start);
        std::cout << "FPS : " << fps << std::endl;
        
    }
    //! [loop end]

    //! [stop]
    dev->stop();
    dev->close();
    //! [stop]

    std::cout << "Streaming Ends!" << std::endl;
    return 0;
}