#include <iostream>
#include <cstdlib>
#include <pthread.h>
#include <libfreenect.h>
#include <sys/time.h>

using namespace std;

void *depthThreadFunction(void *args);
void depthCallback(freenect_device *freenectDevice, void *depthAsVoid, uint32_t timestamp);

freenect_context *freenectContext;
freenect_device *freenectDevice;
pthread_mutex_t depthMutex = PTHREAD_MUTEX_INITIALIZER;
void *depthThreadFunction();
bool endDepthThread;

int main(int argc, char **argv)
{
    pthread_t depthThread;

    if (freenect_init(&freenectContext, NULL) < 0)
    {
        cout << "freenect_init() failed" << endl;

        return 1;
    }

    freenect_set_log_level(freenectContext, FREENECT_LOG_DEBUG);
    freenect_select_subdevices(freenectContext, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));

    int deviceCount = freenect_num_devices(freenectContext);
    cout << "Number of devices found: " << deviceCount << endl;

    int userDeviceNumber = 0;
    if (argc > 1)
        userDeviceNumber = atoi(argv[1]);

    if (deviceCount < 1)
    {
        freenect_shutdown(freenectContext);

        return 1;
    }

    if (freenect_open_device(freenectContext, &freenectDevice, userDeviceNumber) < 0)
    {
        cout << "Could not open device" << endl;
        freenect_shutdown(freenectContext);

        return 1;
    }

    freenect_set_depth_callback(freenectDevice, depthCallback);
    freenect_set_depth_mode(freenectDevice, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_MM));
    freenect_start_depth(freenectDevice);

    endDepthThread = false;
    pthread_create(&depthThread, 0, &depthThreadFunction, 0);

    pthread_join(depthThread, 0);

    freenect_stop_depth(freenectDevice);
    freenect_shutdown(freenectContext);

    return 0;
}

void *depthThreadFunction(void *args)
{
    while (endDepthThread == false && freenect_process_events(freenectContext) >= 0)
    {
    }

    return 0;
}

void depthCallback(freenect_device *freenectDevice, void *depthAsVoid, uint32_t timestamp)
{
    pthread_mutex_lock(&depthMutex);

    short *depthData = (short*)depthAsVoid;

    pthread_mutex_unlock(&depthMutex);
}