#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <stdio.h>
#include <libfreenect.h>
#include "libfreenect_cv.h"
#include <iostream>


IplImage *GlViewColor(IplImage *depth)
{
        static IplImage *image = 0;
        if (!image) image = cvCreateImage(cvSize(640,480), 8, 3);
        unsigned char *depth_mid = (unsigned char*)image->imageData;
        int i;
        for (i = 0; i < 640*480; i++) {
                int lb = ((short *)depth->imageData)[i] % 256;
                //int lb = 0;
                int ub = ((short *)depth->imageData)[i] / 256;
                switch (ub) {
                        case 0:
                                depth_mid[3*i+2] = 255;
                                depth_mid[3*i+1] = 255-lb;
                                depth_mid[3*i+0] = 255-lb;
                                break;
                        case 1:
                                depth_mid[3*i+2] = 255;
                                depth_mid[3*i+1] = lb;
                                depth_mid[3*i+0] = 0;
                                break;
                        case 2:
                                depth_mid[3*i+2] = 255-lb;
                                depth_mid[3*i+1] = 255;
                                depth_mid[3*i+0] = 0;
                                break;
                        case 3:
                                depth_mid[3*i+2] = 0;
                                depth_mid[3*i+1] = 255;
                                depth_mid[3*i+0] = lb;
                                break;
                        case 4:
                                depth_mid[3*i+2] = 0;
                                depth_mid[3*i+1] = 255-lb;
                                depth_mid[3*i+0] = 255;
                                break;

                        case 5:
                                depth_mid[3*i+2] = 0;
                                depth_mid[3*i+1] = 0;
                                depth_mid[3*i+0] = 255-lb;
                                break;
                        default:
                                depth_mid[3*i+2] = 0;
                                depth_mid[3*i+1] = 0;
                                depth_mid[3*i+0] = 0;
                                break;
                }
        }
        //std::cout << image->widthStep << " ";
        depth_mid[240*1920+320*3+0] = 255;
        depth_mid[240*1920+320*3+1] = 255;
        depth_mid[240*1920+320*3+2] = 255;

        return image;
}

IplImage *getRange(IplImage *depth, unsigned int lower, unsigned int higher)
{
    IplImage *single = 0;
    if (!single) single = cvCreateImage(cvSize(640,480), 8, 1);
    unsigned char *depth_mid = (unsigned char*)single->imageData;

    //if (!dC) dC=100/(-0.00307 * (((short *)depth->imageData)[320*240])  + 3.33);
    for (unsigned int i = 0; i < 640*480; i++) {
            int imgd = (((short *)depth->imageData)[i]);
            depth_mid[i] = 100/(-0.00307 * imgd + 3.33);
            if (!( depth_mid[i]>(lower) &&  depth_mid[i]<(higher) ) )
                depth_mid[i]=0;
    }
    depth_mid[240*640+320] = 255;
    return single;
}

IplImage *cutRange(IplImage *depth)
{
    IplImage *single = 0;
    if (!single) single = cvCreateImage(cvSize(640,480), 8, 1);
    unsigned char *depth_mid = (unsigned char*)single->imageData;
    unsigned int min=200;
    for (unsigned int i = 0; i < 640*480; i++) {
            int imgd = (((short *)depth->imageData)[i]);
            depth_mid[i] = 100/(-0.00307 * imgd + 3.33);
            if (depth_mid[i]<min)
                min = depth_mid[i];
    }

    for (unsigned int i = 0; i < 640*480; i++) {
        if (!( depth_mid[i]>(min-10) &&  depth_mid[i]<(min+5) ) )
            depth_mid[i]=0;
    }
    depth_mid[240*640+320] = 255;
    return single;
}


int main()
{
    int dC=0;

        while (cvWaitKey(10) < 0) {
                /*IplImage *image = freenect_sync_get_rgb_cv(0);
                if (!image) {
                    printf("Error: Kinect not connected?\n");
                    return -1;
                }
                cvCvtColor(image, image, CV_RGB2BGR);*/
                IplImage *depth = freenect_sync_get_depth_cv(0);
                if (!depth) {
                    printf("Error: Kinect not connected?\n");
                    return -1;
                }
                //cvShowImage("RGB", image);
                CvFont font;
                cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1, CV_AA);
                /*
                static IplImage *single = 0;
                if (!single) single = cvCreateImage(cvSize(640,480), 8, 1);
                unsigned char *depth_mid = (unsigned char*)single->imageData;
                std::cout << single->widthStep << " " ;*/


                IplImage *single = 0;
                //if (!single) single = getRange(depth,60, 70);
                if (!single) single = cutRange(depth);

                //if (!dC)
                    //dC=100/(-0.00307 * (((short *)depth->imageData)[240*1*320])  + 3.33);
                unsigned char *depth_mid = (unsigned char*)single->imageData;
                dC= int(depth_mid[240+320*640]);
                std::cout << " dC " << dC << " " << "img " << single->widthStep << std::endl;
                std::stringstream txt;
                txt << dC;
                std::string toto=txt.str();
                //cvShowImage("RGB", getRange(depth,dC-5, dC+5));
                cvShowImage("Range", single);
                cvPutText(depth, toto.c_str(), cvPoint(10, 130), &font, cvScalar(255, 255, 255, 0));
                cvShowImage("Depth", GlViewColor(depth));

                /*std::cout << int(depth_mid[240*640+320]) << " ";
                std::cout << int(depth_mid[320*240]) << " ";
                std::cout << int(depth->imageData[320*240]) << " ";
                */


                IplImage *testpic = 0;
                if (!testpic) testpic = cvCreateImage(cvSize(640,480), 8, 1);
                unsigned char *test_mid = (unsigned char*)testpic->imageData;
                for (unsigned int i = 640*40; i < (640*440 & 0x ); i++)
                    test_mid[i] = 100;
                cvShowImage("testING", testpic);

        }
        return 0;
}