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

using namespace std;
using namespace cv;

void wykryjIWyswietl( Mat frame ); //

CascadeClassifier face_cascade;
CascadeClassifier eyes_cascade;

int main( int argc, const char** argv )
{
    CommandLineParser parser(argc, argv,
                             "{help h||}"
                             "{face_cascade|data/haarcascades/haarcascade_frontalface_alt.xml|Path to face cascade.}"
                             "{eyes_cascade|data/haarcascades/haarcascade_eye_tree_eyeglasses.xml|Path to eyes cascade.}"
                             "{camera|0|Camera device number.}");
    parser.about( "\nJest to demonstracyjny program wykorzystujący klasę cv::CascadeClassifier do"
                  "wykrywania obiektów (Face + eyes) w strumieniu wideo.\n"
                  "Można wykorzystać funkcjonalności Haar lub LBP.\n\n" );
    parser.printMessage();
    String face_cascade_name = samples::findFile( parser.get<String>("face_cascade") );
    String eyes_cascade_name = samples::findFile( parser.get<String>("eyes_cascade") );
    
    //-- 1. Ładowanie klasyfikatora kaskadowego - Haar
    if( face_cascade.load( face_cascade_name ) == 0 )
    {
        cout << "Błąd wczytywania klasyfikatora twarzy! \n";
        return -1;
    };
    if( eyes_cascade.load( eyes_cascade_name ) == 0 )
    {
        cout << "Błąd wczytywania klasyfikatora oczu! \n";
        return -1;
    };
    
    int camera_device = parser.get<int>("camera");
    VideoCapture capture;
    
    //-- 2. Otwieranie strumienia wideo 
    capture.open( camera_device );
    if ( capture.isOpened() == 0 )
    {
        cout << "Nie udało się otworzyć strumienia wideo \n";
        return -1;
    }
    
    Mat frame;
    while ( capture.read(frame) )
    {
        if( frame.empty() )
        {
            cout << "Brak ramki. -- Wyjście z pętli! \n"; 
            break; //wyjście z pętli
        }
        
        //-- 3. Stosowanie klasyfikatorów do bieżącej ramki wideo 
        wykryjIWyswietl( frame ); //Główna funkcja, zawiera główny algorytm 
        if( waitKey(10) == 27 ) break; // wyjście z pętli
    }
    return 0;
}

void wykryjIWyswietl( Mat frame )
{
    Mat frame_gray;
    cvtColor( frame, frame_gray, COLOR_BGR2GRAY );
    equalizeHist( frame_gray, frame_gray );
    
    //-- Wykrywaj twarze
    std::vector<Rect> faces;
    face_cascade.detectMultiScale( frame_gray, faces );
    for ( size_t i = 0; i < faces.size(); i++ )
    {
        Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 );
        ellipse( frame, center, Size( faces[i].width/2, faces[i].height/2 ), 0, 0, 360, Scalar( 255, 0, 255 ), 4 );
        Mat faceROI = frame_gray( faces[i] );
        
        //-- Dla każdej twarzy, szukaj oczu (wykrywaj) 
        std::vector<Rect> eyes;
        eyes_cascade.detectMultiScale( faceROI, eyes );
        for ( size_t j = 0; j < eyes.size(); j++ )
        {
            Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 );
            int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
            circle( frame, eye_center, radius, Scalar( 255, 0, 0 ), 4 );
        }
    }
    
    //-- Prezentowanie wyniku pracy algorytmu 
    imshow( "Przechwycony obraz - Wykrywanie twarzy", frame );
}