#include<pxcsensemanager.h>
#include<pxcsession.h>
#include<pxcfacemodule.h>
#include<util_render.h>
#include<opencv2\opencv.hpp>

using namespace cv;
using namespace std;

int main() 
{
    PXCSenseManager *psm =PXCSenseManager::CreateInstance();
    if (!psm)
    {
        cout << "Create the PXCSenseManager error!!!" << endl;
        exit(-1);
    }

    if(psm->EnableFace()==0);
        cout<<"Enable the Face Module OK!"<<endl;//Waitting for be verificated

    psm->Init();

    PXCFaceModule *faceModule  = psm->QueryFace();
    if (faceModule ==NULL)//If return NULL,the corresponding address is sixtheen zeros.That means the result is NULL
    {
        //Another way to judge is :!faceModule
        cout << "Unable to query FaceModule!!!" << endl;
        exit(-1);
    }

    PXCFaceConfiguration *cfg = faceModule ->CreateActiveConfiguration();//Create a dynamic configuration of face.
    if (cfg==0)
    {
        cout << "Enable to create FaceConfiguration!!!" << endl;
        exit(-1);
    }

    cfg->detection.isEnabled = TRUE;
    cfg->EnableAllAlerts();
    cfg->ApplyChanges();

    PXCFaceData *facedata = faceModule->CreateOutput();

    PXCImage *colorIm;
    PXCImage::ImageData color_data;
    PXCImage::ImageInfo color_info;
    pxcI32 nfaces_temp;

    while (psm->AcquireFrame(true)>=PXC_STATUS_NO_ERROR)
    {
        if (psm->AcquireFrame(true) < PXC_STATUS_NO_ERROR)break;//There are some ambiguities.
        facedata->Update();

        PXCCapture::Sample *sample = psm->QuerySample();
        colorIm = sample->color;

        if (colorIm->AcquireAccess(PXCImage::ACCESS_READ,PXCImage::PIXEL_FORMAT_RGB24,&color_data))
        {
            cout << "Cann't get the color images!!!" << endl;
            exit(-1);
        }

        color_info = sample->color->QueryInfo();
        Mat color(Size(color_info.width, color_info.height), CV_8UC3, (void*)color_data.planes[0], color_data.pitches[0] / sizeof(uchar));
        pxcI32 nfaces = facedata->QueryNumberOfDetectedFaces();

        if (nfaces_temp!=nfaces){cout << "Queried " << nfaces << "  faces..." << endl;}
        nfaces_temp = nfaces;

        for (pxcI32 i = 0; i < nfaces; i++)
        {
            PXCFaceData::Face *trakedface = facedata->QueryFaceByIndex(i);
            PXCFaceData::DetectionData *detectiondata = trakedface->QueryDetection();

            if (detectiondata==NULL)
            {
                cout << "Unable to get the detection data!!!" << endl;
                exit(-1);
            }

            PXCRectI32 rect;
            detectiondata->QueryBoundingRect(&rect);
            
            Rect cvrect = Rect(rect.x, rect.y, rect.w,rect.h);
            rectangle(color,cvrect,Scalar(0,0,255),1,8);

            putText(color, "ID:" + to_string(i), Point(rect.x + rect.w / 2, rect.y - rect.h / 20), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(255, 255, 0));
        }

        colorIm->ReleaseAccess(&color_data);
        putText(color, to_string(nfaces) + "  faces in the field of view.", Point(color.rows / 20, color.cols / 40), CV_FONT_HERSHEY_COMPLEX, 0.5, Scalar(255, 0, 0));

        psm->ReleaseFrame();
        imshow("test", color);
        waitKey(1);//waitkey():This function is pretty important,code  will collapse without it 
    }

    facedata->Release();
    cfg->Release();
    psm->Close();
    psm->Release();
    return 0;
}