#include "opencv2/objdetect.hpp"
#include "opencv2/highgui/highgui_c.h"
#include <stdio.h>

#ifdef HAVE_CVCONFIG_H
#include <cvconfig.h>
#endif
#ifdef HAVE_TBB
#include "tbb/task_scheduler_init.h"
#endif

using namespace cv;

static void help()
{
    printf( "This program demonstrated the use of the latentSVM detector.\n"
            "It reads in a trained object model and then uses that to detect the object in an image\n"
            "Call:\n"
            "./latentsvmdetect [<image_filename> <model_filename> [<threads_number>]]\n"
            "  The defaults for image_filename and model_filename are cat.jpg and cat.xml respectively\n"
            "  Press any key to quit.\n");
}

const char* model_filename = "cat.xml";
const char* image_filename = "cat.jpg";
int   tbbNumThreads = -1;

static void detect_and_draw_objects( IplImage* image, CvLatentSvmDetector* detector, int numThreads = -1)
{
    CvMemStorage* storage = cvCreateMemStorage(0);
    CvSeq* detections = 0;
    int i = 0;
    int64 start = 0, finish = 0;
#ifdef HAVE_TBB
    tbb::task_scheduler_init init(tbb::task_scheduler_init::deferred);
    if (numThreads > 0)
    {
        init.initialize(numThreads);
        printf("Number of threads %i\n", numThreads);
    }
    else
    {
        printf("Number of threads is not correct for TBB version");
        return;
    }
#endif

    start = cvGetTickCount();
    detections = cvLatentSvmDetectObjects(image, detector, storage, 0.5f, numThreads);
    finish = cvGetTickCount();
    printf("detection time = %.3f\n", (float)(finish - start) / (float)(cvGetTickFrequency() * 1000000.0));

#ifdef HAVE_TBB
    init.terminate();
#endif
    for( i = 0; i < detections->total; i++ )
    {
        CvObjectDetection detection = *(CvObjectDetection*)cvGetSeqElem( detections, i );
        float score         = detection.score;
        CvRect bounding_box = detection.rect;
        cvRectangle( image, cvPoint(bounding_box.x, bounding_box.y),
                     cvPoint(bounding_box.x + bounding_box.width,
                            bounding_box.y + bounding_box.height),
                     CV_RGB(cvRound(255.0f*score),0,0), 3 );
    }
    cvReleaseMemStorage( &storage );
}

int main(int argc, char* argv[])
{
    help();
    if (argc > 2)
    {
        image_filename = argv[1];
        model_filename = argv[2];
        if (argc > 3)
        {
            tbbNumThreads = atoi(argv[3]);
        }
    }
    IplImage* image = cvLoadImage(image_filename);
    if (!image)
    {
        printf( "Unable to load the image\n"
                "Pass it as the first parameter: latentsvmdetect <path to cat.jpg> <path to cat.xml>\n" );
        return -1;
    }
    CvLatentSvmDetector* detector = cvLoadLatentSvmDetector(model_filename);
    if (!detector)
    {
        printf( "Unable to load the model\n"
                "Pass it as the second parameter: latentsvmdetect <path to cat.jpg> <path to cat.xml>\n" );
        cvReleaseImage( &image );
        return -1;
    }
    detect_and_draw_objects( image, detector, tbbNumThreads );
    cvNamedWindow( "test", 0 );
    cvShowImage( "test", image );
    cvWaitKey(0);
    cvReleaseLatentSvmDetector( &detector );
    cvReleaseImage( &image );
    cvDestroyAllWindows();

    return 0;
}