#include "KinectGrabThread.hpp"
#include "MainWindow.hpp"

namespace lfb
{

KinectGrabThread::KinectGrabThread( MainWindow& app )
    : QThread()
    , mApp( app )
    , mEnvironment()
    , mDevice( mEnvironment.createDevice< lfb::Device >( 0 ) )
    , mRGBBuffer( FREENECT_VIDEO_RGB_SIZE )
    , mDepthBuffer( FREENECT_VIDEO_RGB_SIZE )
{
    mDevice.setDepthFormat( FREENECT_DEPTH_11BIT );
    mDevice.setVideoFormat( FREENECT_VIDEO_RGB );
    mDevice.startVideo();
    mDevice.startDepth();

    connect(this, SIGNAL(ImagesGrabbed(const std::vector< uint8_t >&, const std::vector< uint8_t >&)), &app,
            SLOT(RefreshKinectData( const std::vector< uint8_t >&, const std::vector< uint8_t >& )));

    QTimer* timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(GrabImages()));
    timer->start( 200 ); // 25 FPS
}

KinectGrabThread::~KinectGrabThread()
{
    mDevice.stopVideo();
    mEnvironment.deleteDevice( 0 );
}

void KinectGrabThread::LockImages()
{
    mMutex.lock();
}

void KinectGrabThread::UnlockImages()
{
    mMutex.unlock();
}

void KinectGrabThread::run()
{
    exec();
}

void KinectGrabThread::GrabImages()
{
    LockImages();
    mDevice.GetRGBImage( mRGBBuffer );
    mDevice.GetDepthImage( mDepthBuffer );
    UnlockImages();
    emit ImagesGrabbed( mRGBBuffer, mDepthBuffer );

}

}