#include "highgui.h"
#include <stdio.h>

#include <string.h>
#include <math.h>
#include <libusb.h>

#include "libfreenect.h"

#include <pthread.h>

#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>

#define CV_NO_BACKWARD_COMPATIBILITY

#include <opencv/cv.h>
#include <opencv/highgui.h>


// written by Arne Bernin <arne@alamut.de>
// Nov 13, 2010


/*
 * Makefile for ubuntu, assumes that libfreenect.a is in /usr/local/lib, and libfreenect.h is in /usr/local/include
 *
 * make sure that libfreenect.h includes the extern C stuff:
 *


***************************************************************************************************************************
* libfreenect.h
***************************************************************************************************************************

#ifdef __cplusplus
extern "C" {
#endif

void cams_init(libusb_device_handle *d, depthcb depth_cb, rgbcb rgb_cb);

#ifdef __cplusplus
}
#endif



***************************************************************************************************************************
* end of libfreenect.h
***************************************************************************************************************************


***************************************************************************************************************************
* Makefile
***************************************************************************************************************************
 CXXFLAGS = -O2 -g -Wall -fmessage-length=0 `pkg-config opencv --cflags ` -I /usr/include/libusb-1.0 -I /usr/local/include


 OBJS =     freenectopencv.o

LIBS =    `pkg-config opencv --libs` -L/usr/local/lib -lfreenect

TARGET =    kinectopencv

$(TARGET):  $(OBJS)
    $(CXX) -o $(TARGET) $(OBJS) $(LIBS)

all:    $(TARGET)

clean:
    rm -f $(OBJS) $(TARGET)


***************************************************************************************************************************
* End of Makefile
***************************************************************************************************************************



*/




#define FREENECTOPENCV_WINDOW_D "Depthimage"
#define FREENECTOPENCV_WINDOW_N "Normalimage"
#define FREENECTOPENCV_RGB_DEPTH 3
#define FREENECTOPENCV_DEPTH_DEPTH 1
#define FREENECTOPENCV_RGB_WIDTH 640
#define FREENECTOPENCV_RGB_HEIGHT 480
#define FREENECTOPENCV_DEPTH_WIDTH 640
#define FREENECTOPENCV_DEPTH_HEIGHT 480



IplImage* depthimg = 0;
IplImage* rgbimg = 0;
pthread_mutex_t mutex_depth = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mutex_rgb = PTHREAD_MUTEX_INITIALIZER;
pthread_t cv_thread;






// callback for depthimage, called by libfreenect
void depthimg_callback(uint16_t *buf, int width, int height)
{

    // convert depth image to 8bit
    cv::Mat depth8;
    cv::Mat depth = cv::Mat( width, height, CV_16UC1, buf );
    depth.convertTo(depth8, CV_8UC1, 1.0/8.0);

    // lock mutex for opencv depth image
    pthread_mutex_lock( &mutex_depth );
    // copy image to opencv buffer
    memcpy(depthimg->imageData, depth8.data, width*height);
    // unlock mutex
    pthread_mutex_unlock( &mutex_depth );

}

// callback for rgbimage, called by libfreenect

void rgbimg_callback(uint8_t *buf, int width, int height)
{
    // lock mutex for opencv rgb image
    pthread_mutex_lock( &mutex_rgb );
    // copy image to opencv buffer
    memcpy(rgbimg->imageData, buf, width*height*FREENECTOPENCV_RGB_DEPTH);
    // unlock mutex
    pthread_mutex_unlock( &mutex_rgb );

}


/*
 * thread for displaying the opencv content
 */
void *cv_threadfunc (void *ptr) {
    cvNamedWindow( FREENECTOPENCV_WINDOW_D, CV_WINDOW_AUTOSIZE );
    cvNamedWindow( FREENECTOPENCV_WINDOW_N, CV_WINDOW_AUTOSIZE );
    depthimg = cvCreateImage(cvSize(FREENECTOPENCV_DEPTH_WIDTH, FREENECTOPENCV_DEPTH_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_DEPTH_DEPTH);
    rgbimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);


    // use image polling
    while (1) {
        //lock mutex for depth image
        pthread_mutex_lock( &mutex_depth );
        // show image to window
        cvShowImage(FREENECTOPENCV_WINDOW_D, depthimg);
        //unlock mutex for depth image
        pthread_mutex_unlock( &mutex_depth );

        //lock mutex for rgb image
        pthread_mutex_lock( &mutex_rgb );
        // show image to window
        cvShowImage(FREENECTOPENCV_WINDOW_N, rgbimg);
        //unlock mutex
        pthread_mutex_unlock( &mutex_rgb );

        // wait for quit key
        if( cvWaitKey( 15 )==27 )
                break;

    }
    pthread_exit(NULL);

}


int main(int argc, char **argv)
{

    int res = 0;
    libusb_device_handle *dev;
    printf("Kinect camera test\n");

    libusb_init(NULL);

    dev = libusb_open_device_with_vid_pid(NULL, 0x45e, 0x2ae);
    if (!dev) {
        printf("Could not open device\n");
        return 1;
    }

    // create opencv display thread
    res = pthread_create(&cv_thread, NULL, cv_threadfunc, (void*) depthimg);
    if (res) {
        printf("pthread_create failed\n");
        return 1;
    }

    // set callback functions
    cams_init(dev, depthimg_callback, rgbimg_callback);
    printf("init done\n");

    // main loop, forever
    while(libusb_handle_events(NULL) == 0);
}