#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/objdetect/objdetect.hpp>

using namespace cv;
using namespace std;

/**
 * @brief Detect and draw faces and eyes on an image using Haar cascades.
 * 
 * @param img Input image where detection is performed.
 * @param cascade Haar cascade classifier for face detection.
 * @param nestedCascade Haar cascade classifier for eye detection.
 * @param scale Downscaling factor to speed up detection.
 * @param tryflip Whether to also detect faces in horizontally flipped image.
 */
void detectAndDraw(Mat& img, CascadeClassifier& cascade,
                   CascadeClassifier& nestedCascade,
                   double scale, bool tryflip)
{
    vector<Rect> faces, faces2;
    const static Scalar colors[] = {
        CV_RGB(0,0,255), CV_RGB(0,128,255), CV_RGB(0,255,255),
        CV_RGB(0,255,0), CV_RGB(255,128,0), CV_RGB(255,255,0),
        CV_RGB(255,0,0), CV_RGB(255,0,255)
    };

    Mat gray, smallImg(cvRound(img.rows/scale), cvRound(img.cols/scale), CV_8UC1);
    cvtColor(img, gray, COLOR_BGR2GRAY);
    resize(gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR);
    equalizeHist(smallImg, smallImg);

    cascade.detectMultiScale(smallImg, faces, 1.1, 2,
                             CASCADE_SCALE_IMAGE, Size(30, 30));

    if (tryflip)
    {
        flip(smallImg, smallImg, 1);
        cascade.detectMultiScale(smallImg, faces2, 1.1, 2,
                                 CASCADE_SCALE_IMAGE, Size(30, 30));
        for (const Rect& r : faces2)
            faces.push_back(Rect(smallImg.cols - r.x - r.width, r.y, r.width, r.height));
    }

    int i = 0;
    for (const Rect& r : faces)
    {
        Mat smallImgROI;
        vector<Rect> nestedObjects;
        Point center;
        Scalar color = colors[i % 8];
        int radius;

        double aspect_ratio = (double)r.width / r.height;
        if (0.75 < aspect_ratio && aspect_ratio < 1.3)
        {
            center.x = cvRound((r.x + r.width * 0.5) * scale);
            center.y = cvRound((r.y + r.height * 0.5) * scale);
            radius   = cvRound((r.width + r.height) * 0.25 * scale);
            circle(img, center, radius, color, 3, 8, 0);
        }
        else
        {
            rectangle(img, Point(cvRound(r.x * scale), cvRound(r.y * scale)),
                      Point(cvRound((r.x + r.width - 1) * scale),
                            cvRound((r.y + r.height - 1) * scale)),
                      color, 3, 8, 0);
        }

        if (nestedCascade.empty())
            continue;

        smallImgROI = smallImg(r);
        nestedCascade.detectMultiScale(smallImgROI, nestedObjects, 1.1, 2,
                                       CASCADE_SCALE_IMAGE, Size(30, 30));
        for (const Rect& nr : nestedObjects)
        {
            center.x = cvRound((r.x + nr.x + nr.width * 0.5) * scale);
            center.y = cvRound((r.y + nr.y + nr.height * 0.5) * scale);
            radius   = cvRound((nr.width + nr.height) * 0.25 * scale);
            circle(img, center, radius, color, 3, 8, 0);
        }
        i++;
    }

    imwrite("./demo26_result.jpg", img);
}

/**
 * @brief Load classifiers and perform face + eye detection on a test image.
 * 
 * @return int Exit code: 0 if success.
 */
int face_and_eye_det()
{
    Mat frame;
    CascadeClassifier cascade, nestedCascade;
    cascade.load("./haarcascade_frontalface_alt.xml");
    nestedCascade.load("./haarcascade_eye.xml");

    frame = imread("./1.bmp");
    detectAndDraw(frame, cascade, nestedCascade, 2, false);
    return 0;
}

void setup() {
  // put your setup code here, to run once:
  face_and_eye_det();
}

void loop() {
  // put your main code here, to run repeatedly:

}
