// CobaOpencv.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <windows.h>
#include <NuiApi.h>
#include <NuiImageCamera.h>
//#include"NuiImageBuffer.h"
#include <opencv\cv.h>
#include <opencv\cxcore.h>
#include <opencv\highgui.h>
#include <opencv2\imgproc\imgproc.hpp>
#include <opencv2\imgcodecs\imgcodecs.hpp>
#include <opencv2\ml.hpp>
#include <opencv2\objdetect.hpp>
#include <opencv2\opencv.hpp>
#include <time.h>

using namespace std;
using namespace cv;

interface INuiInstance;

#define COLOR_WIDTH 640
#define COLOR_HIGHT 480
#define DEPTH_WIDTH 640
#define DEPTH_HIGHT 480
#define SKELETON_WIDTH 640
#define SKELETON_HIGHT 480
#define CHANNEL 3

BYTE buf[DEPTH_WIDTH*DEPTH_HIGHT*CHANNEL];

int drawColor(HANDLE h, IplImage* color)
{
    const NUI_IMAGE_FRAME * pImageFrame = NULL;
    HRESULT hr = NuiImageStreamGetNextFrame(h, 0, &pImageFrame);

    if (FAILED(hr))
    {
        cout << "Get Image Frame Failed" << endl;
        return -1;
    }

    INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
    NUI_LOCKED_RECT LockedRect;
    pTexture->LockRect(0, &LockedRect, NULL, 0);

    if (LockedRect.Pitch != 0)
    {
        BYTE * pBuffer = (BYTE*)LockedRect.pBits;
        cvSetData(color, pBuffer, LockedRect.Pitch);
    }

    //cvShowImage("color image", color);
    NuiImageStreamReleaseFrame(h, pImageFrame);
    return 0;
}

int drawDepth(HANDLE h, IplImage* depth)
{
    const NUI_IMAGE_FRAME * pImageFrame = NULL;
    HRESULT hr = NuiImageStreamGetNextFrame(h, 0, &pImageFrame);
    if (FAILED(hr))
    {
        cout << "Get Image Frame Failed" << endl;
        return -1;
    }

    //  temp1 = depth;
    INuiFrameTexture * pTexture = pImageFrame->pFrameTexture;
    NUI_LOCKED_RECT LockedRect;
    pTexture->LockRect(0, &LockedRect, NULL, 0);
    if (LockedRect.Pitch != 0)
    {
        USHORT * pBuff = (USHORT*)LockedRect.pBits;
        for (int i = 0; i<DEPTH_WIDTH*DEPTH_HIGHT; i++)
        {
            BYTE index = pBuff[i] & 0x07;
            USHORT realDepth = (pBuff[i] & 0xFFF8) >> 3;
            BYTE scale = 255 - (BYTE)(256 * realDepth / 0x0fff); //realDepth = jarak, dibawa ke 12 bit
            buf[CHANNEL*i] = buf[CHANNEL*i + 1] = buf[CHANNEL*i + 2] = 0;

            float jarak;

            switch (index)
            {
            case 0:
                buf[CHANNEL*i] = scale / 2;
                buf[CHANNEL*i + 1] = scale / 2;
                buf[CHANNEL*i + 2] = scale / 2;
                break;

            case 1:
                buf[CHANNEL*i] = scale;
                break;

            case 2:
                buf[CHANNEL*i + 1] = scale;
                break;

            case 3:
                buf[CHANNEL*i + 2] = scale;
                break;

            case 4:
                buf[CHANNEL*i] = scale;
                buf[CHANNEL*i + 1] = scale;
                break;

            case 5:
                buf[CHANNEL*i] = scale;
                buf[CHANNEL*i + 2] = scale;
                break;

            case 6:
                buf[CHANNEL*i + 1] = scale;
                buf[CHANNEL*i + 2] = scale;
                break;

            case 7:
                buf[CHANNEL*i] = 255 - scale / 2;
                buf[CHANNEL*i + 1] = 255 - scale / 2;
                buf[CHANNEL*i + 2] = 255 - scale / 2;
                break;
            }
            jarak = (3.2*scale/256)+0.8;
            //printf("Jarak : %f\n",jarak);
        }
        cvSetData(depth, buf, DEPTH_WIDTH*CHANNEL);
    }

    NuiImageStreamReleaseFrame(h, pImageFrame);
    //cvShowImage("depth image", depth);
    return 0;

}

/*void histogram(string const& name, Mat img)
{
    cvtColor(img, img, cv::COLOR_BGR2HSV);
    int hbins = 30, sbins = 32;
    int histSize[] = { hbins };

    float hranges[] = { 0, 180 };
    float sranges[] = {0, 255};
    const float* ranges[] = { hranges, sranges }; //, vranges
    MatND hist;

    int channels[] = {0,1};

    calcHist(&img, 1, channels, Mat(), hist, 2, histSize, ranges, true, false);
    double maxVal = 0;
    minMaxLoc(hist, 0, &maxVal, 0, 0);

    int scale = 10;
    Mat histImg = Mat::zeros(sbins*scale, hbins * 10, CV_8UC3);

    for (int h = 0; h < hbins; h++)
        for (int s = 0; s < sbins; s++)
        {
        float binVal = hist.at<float>(h, s);
        int intensity = cvRound(binVal * 255 / maxVal);
        rectangle(histImg, Point(h*scale, s*scale),
            Point((h + 1)*scale - 1, (s + 1)*scale - 1),
            Scalar::all(intensity),
            CV_FILLED);
        }
    cv::imshow(name, histImg);
}*/

void histogram(string const& name, Mat img)
{
    cvtColor(img, img, cv::COLOR_BGR2HSV);
    // Set histogram bins count
    int hbins = 30; // sbins = 32; //vbins = 30
    int histSize[] = {hbins}; //sbins, vbins

    // Set ranges for histogram bins
    float hranges[] = {0, 180};
    //float sranges[] = {0, 255};
    //float vranges[] = {0, 255};
    const float* ranges[] = {hranges}; //sranges, vranges

    // create matrix for histogram
    MatND h_hist;
    int channels[] = { 0 };

    // create matrix for histogram visualization
    //int scale = 10;
    //Mat histImg = Mat::zeros(sbins*scale, hbins*10, CV_8UC3);

    calcHist(&img, 1, channels, Mat(), h_hist, 1, histSize, ranges, true, false);
    //calcHist(&img, 1, channels, Mat(), s_hist, 2, histSize, ranges, true, false);

    // Draw the histograms for H, S and V
    int hist_w = 512; int hist_h = 400;
    int bin_w = cvRound((double)hist_w / hbins);

    Mat histImage(hist_h, hist_w, CV_8UC3, Scalar(0,0,0));

    //Normalize
    normalize(img, img, 0, 1, NORM_MINMAX, -1, Mat() );
    //normalize(img, img, 0, 1, NORM_MINMAX, -1, Mat());

    //Draw for each channel
    for (int i = 1; i < hbins; i++){
        line(histImage, Point(bin_w*(i - 1), hist_h - cvRound(h_hist.at<float>(i - 1))),
            Point(bin_w*(i), hist_h - cvRound(h_hist.at<float>(i))),
            Scalar(255, 0, 0), 2, 8, 0);
    }
    /*double maxVal = 0;
    minMaxLoc(h_hist, 0, &maxVal, 0, 0);

    // visualize each bin
    for (int h = 0; h < hbins; h++)
        for (int s = 0; s < sbins; s++)
            {
                float binVal = hist.at<float>(h, s);
                int intensity = cvRound(binVal * 255 / maxVal);
                cv::line(histImg, cv::Point(h*scale, s*scal),
                    cv::Point((h + 1)*scale - 1), (s + 1) * scale - 1),
                    cv::Scalar::all(intensity),
                    CV_FILLED);
            }
    */
    cv::imshow(name, histImage);
}

//PCAawal
void read_csv(const string& filename, vector<Mat>& images, vector<int>& labels) {
    std::ifstream file(filename.c_str(), ifstream::in);
    if (!file)
        throw std::exception();
    std::string line, path, classlabel;
    // For each line in the given file:
    while (std::getline(file, line)) {
        // Get the current line:
        std::stringstream liness(line);
        // Split it at the semicolon:
        std::getline(liness, path, ';');
        std::getline(liness, classlabel);
        // And push back the data into the result vectors:
        images.push_back(imread(path, IMREAD_GRAYSCALE));
        labels.push_back(atoi(classlabel.c_str()));
    }
}
// Normalizes a given image into a value range between 0 and 255.
Mat norm_0_255(const Mat& src) {
    // Create and return normalized image:
    Mat dst;
    switch (src.channels()) {
    case 1:
        cv::normalize(src, dst, 0, 255, NORM_MINMAX, CV_8UC1);
        break;
    case 3:
        cv::normalize(src, dst, 0, 255, NORM_MINMAX, CV_8UC3);
        break;
    default:
        src.copyTo(dst);
        break;
    }
    return dst;
}

// Converts the images given in src into a row matrix.
Mat asRowMatrix(const vector<Mat>& src, int rtype, double alpha = 1, double beta = 0) {
    // Number of samples:
    size_t n = src.size();
    // Return empty matrix if no matrices given:
    if (n == 0)
        return Mat();
    // dimensionality of (reshaped) samples
    size_t d = src[0].total();
    // Create resulting data matrix:
    Mat data(n, d, rtype);
    // Now copy data:
    for (int i = 0; i < n; i++) {
        //
        if (src[i].empty()) {
            string error_message = format("Image number %d was empty, please check your input data.", i);
            CV_Error(CV_StsBadArg, error_message);
        }
        // Make sure data can be reshaped, throw a meaningful exception if not!
        if (src[i].total() != d) {
            string error_message = format("Wrong number of elements in matrix #%d! Expected %d was %d.", i, d, src[i].total());
            CV_Error(CV_StsBadArg, error_message);
        }
        // Get a hold of the current row:
        Mat xi = data.row(i);
        // Make reshape happy by cloning for non-continuous matrices:
        if (src[i].isContinuous()) {
            src[i].reshape(1, 1).convertTo(xi, rtype, alpha, beta);
        }
        else {
            src[i].clone().reshape(1, 1).convertTo(xi, rtype, alpha, beta);
        }
    }
    return data;
}
//PCAakhir

int main(int argc, char * argv[])
{
    IplImage* color = cvCreateImageHeader(cvSize(COLOR_WIDTH, COLOR_HIGHT), IPL_DEPTH_8U, 4);
    IplImage* depth = cvCreateImageHeader(cvSize(DEPTH_WIDTH, DEPTH_HIGHT), IPL_DEPTH_8U, CHANNEL);
    IplImage* skeleton = cvCreateImage(cvSize(SKELETON_WIDTH, SKELETON_HIGHT), IPL_DEPTH_8U, CHANNEL);
    HRESULT hr = NuiInitialize(NUI_INITIALIZE_FLAG_USES_DEPTH | NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_SKELETON);

    if (hr != S_OK)
    {
        cout << "NuiInitialize failed" << endl;
        return hr;
    }

    HANDLE h1 = CreateEvent(NULL, TRUE, FALSE, NULL);
    HANDLE h2 = NULL;
    hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_COLOR, NUI_IMAGE_RESOLUTION_640x480, 0, 2, h1, &h2);
    if (FAILED(hr))
    {
        cout << "Could not open image stream video" << endl;
        return hr;
    }

    HANDLE h3 = CreateEvent(NULL, TRUE, FALSE, NULL);
    HANDLE h4 = NULL;
    hr = NuiImageStreamOpen(NUI_IMAGE_TYPE_DEPTH, NUI_IMAGE_RESOLUTION_640x480, 0, 2, h3, &h4);
    if (FAILED(hr))
    {
        cout << "Could not open depth stream video" << endl;
        return hr;
    }

    HANDLE h5 = CreateEvent(NULL, TRUE, FALSE, NULL);
    hr = NuiSkeletonTrackingEnable(h5, 0);
    if (FAILED(hr))
    {
        cout << "Could not open skeleton stream video" << endl;
        return hr;
    }

    Mat tmplt, tmplt2, CropFace;
    Rect rekt(0, 0, 0, 0), rekt2(0, 0, 0, 0), rektg(0, 0, 0, 0);
    int flagtmplt = 0, flagtmplt2 = 0, flagtmpltc, flgfacetmplt = 0;

    int i = 0;
    char nama[10];

    //humandetection
    HOGDescriptor hog;
    String nn = "HOGHalfUpperBody72x96.xml";
    bool l = hog.load(nn);
    cout << l << endl;

    //face detection
    CascadeClassifier face_haar;
    String fc = ("haarcascade_frontalface_alt2.xml");
    bool f = face_haar.load(fc);
    cout << f << endl;

    int cntr=0;
    Mat his, his2, His_c, dst;
    vector<Mat> hsv_t2, hsv_t1, hsv_tc;

    while (1)
    {
        WaitForSingleObject(h1, INFINITE);
        drawColor(h2, color);
        WaitForSingleObject(h3, INFINITE);
        drawDepth(h4, depth);

        Mat cr = cvarrToMat(color);
        Mat dp = cvarrToMat(depth);
        Mat ImgNew, ImgDepth, CropImg;
        //resize(cr, ImgNew, Size(320, 240));
        //resize(m, ImgDepth, Size(320, 240));
        Mat gray, tgt;
        cvtColor(cr,gray, CV_BGR2GRAY);
        int method = 1;
        vector<Mat> db;

        //simpan gambar per frs
        //sprintf(nama,"%d.jpg",i++);
        //cv::imwrite(nama,m);

        //human detection
        vector< Rect > detections; //template deteksi
        vector< double > foundWeights;
        hog.detectMultiScale(dp, detections, foundWeights);

        //face detection
        vector<Rect> faces;
        face_haar.detectMultiScale(cr, faces, 1.1, 3, 0 | CV_HAAR_SCALE_IMAGE, Size(30, 30));

        cntr += 1;
        //humandetection
        for (size_t j = 0; j < detections.size(); j++)
        {
            if (foundWeights[j] > 0.6) {
                Scalar color = Scalar(0, foundWeights[j] * foundWeights[j] * 200, 0);
                rectangle(cr, detections[j], color, cr.cols / 400 + 1);

                int hgth = detections[j].height;
                int weght = detections[j].width;
                int hbg3 = hgth / 3;
                int wbg3 = weght / 3;
                Rect rec = Rect(wbg3, hbg3 * 2, wbg3, hbg3);
                rectangle(cr, rec, Scalar(255, 0, 255), 1);
                His_c = cr(rec);
                //cvtColor(His_c, His_c, CV_BGR2HSV);
                //resize(His_c, His_c, his.size());
                //split(His_c, hsv_tc);
                His_c.convertTo(His_c, CV_32F);
                histogram("Current Hist", His_c);
                flagtmpltc = 1;

                if (flagtmplt == 1 && cntr > 15){
                    his2 = His_c.clone();
                    //cvtColor(his2, his2, CV_BGR2HSV);
                    //resize(his2, his2, his.size());
                    //split(his2, hsv_t2);
                    resize(his2, his2, His_c.size());
                    his2.convertTo(his2, CV_32F);
                    histogram("Template 2", his2);
                    //cntr = 0;
                    flagtmplt2 = 1;
                }

                if (tmplt.empty() && flagtmplt==0){
                    Mat a = cr(detections[j]);
                    imwrite("halo.jpg", a);
                    tmplt = a.clone();
                    a.release();
                    Size s = tmplt.size();
                    int hgt = s.height;
                    int wght = s.width;
                    int hb3 = hgt / 3;
                    int wb3 = wght / 3;
                    rekt = Rect(wb3, hb3*2, wb3, hb3); //rekt
                    his = tmplt(rekt);
                    resize(his, his, His_c.size());
                    his.convertTo(his, CV_32F);
                    histogram("Template 1", his);
                    //cvtColor(his, his, CV_BGR2HSV);
                    //split(his, hsv_t1);
                    //imwrite("his_template", his);
                    flagtmplt = 1;
                }
                else{
                    rectangle(tmplt, rekt, Scalar(255, 0, 255), 1);
                    imshow("Template", tmplt);
                }


            }
        }

        if (flagtmplt == 1 && flagtmplt2 == 1 && cntr > 5){
            for (int i = 0; i < 2; i++)
            {
                int compare_method = i;
                double er1 = compareHist(His_c, his, compare_method);
                double er2 = compareHist(His_c, his2, compare_method);

                printf("error1 : %f, error2: %f", er1, er2 );
            }
        }

        /*if (flagtmplt == 1 && flagtmplt2 == 1 && cntr > 5){

            histogram("Hue t1", hsv_t1[0]);
            int Ht1 = countNonZero(hsv_t1[0]);
            int St1 = countNonZero(hsv_t1[1]);
            int Vt1 = countNonZero(hsv_t1[2]);
            int Ht2 = countNonZero(hsv_t2[0]); // H channel
            int St2 = countNonZero(hsv_t2[1]); // S channel
            int Vt2 = countNonZero(hsv_t2[2]); // V channel

            int Htc = countNonZero(hsv_tc[0]);
            int Stc = countNonZero(hsv_tc[1]);
            int Vtc = countNonZero(hsv_tc[2]);

            int EH1 = Ht2 - Htc;
            int ES1 = St2 - Stc;
            int EV1 = Vt2 - Vtc;

            int EH2 = Ht1 - Ht2;
            int ES2 = St1 - St2;
            int EV2 = Vt1 - Vt2;

            int EH_Now = EH2 - EH1;
            int ES_Now = ES2 - ES1;
            int EV_Now = EV2 - EV1;

            //if ((EH >= Ht1 || ES >= St1 || EV >= Vt1) || (EH >= Ht2 || ES >= St2 || EV >= Vt2))
            //if ((EH <= Ht1 || ES <= St1 || EV <= Vt1) && (EH <= Ht2 || ES <= St2 || EV <= Vt2))

            //printf("Warna: tidak %d\n", wr);
            if ((EH_Now <= Ht1 && ES_Now <= St1 && EV_Now <= Vt1) && (EH_Now <= Ht2 && ES_Now <= St2 && EV_Now <= Vt2))
            {
                    int wr = 1;
                printf("Warna EH : %d\n", EH_Now);
                printf("Warna: sama %d\n", wr);
            }
            else
            {
                printf("Warna EH_TidakSama : %d\n", EH_Now);
                int wr = 0;
                printf("Warna EH : %d\n",EH);
                printf("Warna Ht1 : %d\n",Ht1);
                printf("Warna Ht2 : %d\n",Ht2);
                printf("Warna ES : %d\n",ES);
                printf("Warna St2 : %d\n", St2);
                printf("Warna EV : %d\n", EV);
                printf("Warna Vt2 : %d\n", Vt2);
                printf("Warna: tidak %d\n", wr);
                printf("orang lain");
            }
        }*/

        //facedetection
        for (size_t i = 0; i < faces.size(); i++)
        {
            Rect r = faces[i];
            Mat faceROI = gray(faces[i]);
            int x = faces[i].x;
            int y = faces[i].y;
            int h = 0.1*y + faces[i].height;
            int w = faces[i].width;
            rectangle(cr, Point(x, y), Point(x + w, y + h), Scalar(255, 0, 255), 1, 4, 0);
            CropFace = cr(r);
            /*
            if (flgfacetmplt == 0){
                imwrite("template.jpg", CropFace);
                imshow("tmplt.jgp", CropFace);
                flgfacetmplt = 1;
            }
            if (flgfacetmplt == 1 && cntr > 2) {
                imwrite("current.jpg", CropFace);
                imshow("current.jpg", CropFace);
            }
            string prefix = "orl_faces//";
            Mat tmplt = imread("template.jpg", IMREAD_GRAYSCALE);
            Mat current = imread("current.jpg", IMREAD_GRAYSCALE);
            resize(current, current, tmplt.size());

            db.push_back(tmplt);
            db.push_back(current);

            // Build a matrix with the observations in row:
            Mat data = asRowMatrix(db, CV_32FC1);

            // Number of components to keep for the PCA:
            int num_components = 10;

            // Perform a PCA:
            PCA pca(data, Mat(), CV_PCA_DATA_AS_ROW, num_components);

            // And copy the PCA results:
            Mat mean = pca.mean.clone();
            Mat eigenvalues = pca.eigenvalues.clone();
            Mat eigenvectors = pca.eigenvectors.clone();

            // The mean face:
            imshow("avg", norm_0_255(mean.reshape(1, db[0].rows)));

            // The first three eigenfaces:
            imshow("pc1", norm_0_255(pca.eigenvectors.row(0)).reshape(1, db[0].rows));
            //imshow("pc2", norm_0_255(pca.eigenvectors.row(1)).reshape(1, db[0].rows));
            // imshow("pc3", norm_0_255(pca.eigenvectors.row(2)).reshape(1, db[0].rows));
            */
        }

        //print hasil
        imshow("Test", cr);
        //imshow("TestDepth", dp);
        //WaitForSingleObject(h5,INFINITE);
        //drawSkeleton(skeleton);
        //exit
        int c = cvWaitKey(1);
        if (c == 27 || c == 'q' || c == 'Q')
            break;
    }

    cvReleaseImageHeader(&depth);
    cvReleaseImageHeader(&color);
    cvReleaseImage(&skeleton);
    NuiShutdown();

    return 0;
}