/// Point Grey Research Bumblebee XB3 acquisition component.
///
/// created @date 2012-07-25
/// @author Marek Kurdej
/// @version $Id: $

#include "BumblebeeXB3.h"

#include <cassert>
#include <iomanip>
#include <iostream>
#include <PGRFlyCaptureStereo.h>
#include <qdir.h>
#include <qscopedpointer.h>
#include <string>
#include <sstream>

#include "Pacpus/kernel/ComponentFactory.h"
#include "Pacpus/kernel/DbiteException.h"
#include "Pacpus/kernel/DbiteFileTypes.h"
#include "Pacpus/kernel/Log.h"

using namespace std;

namespace pacpus {

DECLARE_STATIC_LOGGER("pacpus.base.BumblebeeXB3");

/// Construct the factory
ComponentFactory<BumblebeeXB3> sFactory("BumblebeeXB3"); 

static const size_t kMaxFilepathLength = 128; // TODO: should be same for all images

/// Macro to check, report on, and handle Flycapture API error codes.
#define _HANDLE_FLYCAPTURE_ERROR( description, error )	\
{ \
    if ( error != FLYCAPTURE_OK ) { \
    LOG_FATAL( \
        "*** Flycapture Error '" << flycaptureErrorToString( error ) \
        << "' at line " << __LINE__ \
        << ":\n\t" << description \
    );	\
    } \
}

/// Macro to check, report on, and handle Triclops API error codes.
#define _HANDLE_TRICLOPS_ERROR( description, error )	\
{ \
    if ( error != TriclopsErrorOk ) { \
    LOG_FATAL( \
        "*** Triclops Error '" << triclopsErrorToString( error ) \
        << "' at line " << __LINE__ \
        << " :\n\t" << description \
    );	\
    } \
}

BumblebeeXB3::BumblebeeXB3(QString name)
    : ComponentBase(name)
    , mTriclops(NULL)
    , mFlycapture(NULL)
    , mIsRecording(false)
    , mVerbosityLevel(0)
    , mShMemLeft(NULL)
    //, mShMemTop(NULL)
    , mShMemRight(NULL)
{
}

BumblebeeXB3::~BumblebeeXB3()
{
}

ComponentBase::COMPONENT_CONFIGURATION BumblebeeXB3::configureComponent(XmlComponentConfig config)
{
    if (config.getProperty("recording") != QString::null) {
        mIsRecording = (0 < config.getProperty("recording").toInt());
    }
    if (config.getProperty("verbose") != QString::null) {
        mVerbosityLevel = config.getProperty("verbose").toInt();
    }

    return ComponentBase::CONFIGURED_OK;
}

void BumblebeeXB3::startActivity()
{
    TriclopsError te;
    FlyCaptureError fe;

    // Open the camera
    fe = flycaptureCreateContext( &mFlycapture );
    _HANDLE_FLYCAPTURE_ERROR( "flycaptureCreateContext()", fe );
    if ( fe != FLYCAPTURE_OK ) {
        return;
    }

    // Initialize the Flycapture context, device no. 0
    fe = flycaptureInitialize( mFlycapture, 0 );
    _HANDLE_FLYCAPTURE_ERROR( "flycaptureInitialize()", fe );
    if ( fe != FLYCAPTURE_OK ) {
        return;
    }
    
    // Save the camera's calibration file, and return the path 
    char * szCalFile;
    fe = flycaptureGetCalibrationFileFromCamera( mFlycapture, &szCalFile );
    _HANDLE_FLYCAPTURE_ERROR( "flycaptureGetCalibrationFileFromCamera()", fe );
    if ( fe != FLYCAPTURE_OK ) {
        return;
    }

    LOG_DEBUG("FlyCapture camera configuration file: " << szCalFile);

    // Create a Triclops context from the cameras calibration file
    te = triclopsGetDefaultContextFromFile( &mTriclops, szCalFile );
    _HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", te );
    if (te != TriclopsErrorOk) {
        return;
    }

    // Get camera information
    FlyCaptureInfoEx pInfo;
    fe = flycaptureGetCameraInfo( mFlycapture, &pInfo );
    _HANDLE_FLYCAPTURE_ERROR( "flycatpureGetCameraInfo()", fe );
    if ( fe != FLYCAPTURE_OK ) {
        return;
    }

    // Get pixel format
    if (pInfo.CameraType == FLYCAPTURE_COLOR) {
        mPixelFormat = FLYCAPTURE_RAW16;
    } else {
        mPixelFormat = FLYCAPTURE_MONO16;
    }

    // Get max image size
    mMaxRows = mMaxCols = 0;

    switch (pInfo.CameraModel) {
    case FLYCAPTURE_BUMBLEBEE2:
        {
            unsigned long ulValue;
            const unsigned long ulRegister = 0x1F28;
            flycaptureGetCameraRegister( mFlycapture, ulRegister, &ulValue );
            
            const unsigned long ulMask = 0x2;
            if ( ( ulValue & ulMask ) == 0 ) {
                // Hi-res BB2
                mMaxCols = 1024;
                mMaxRows = 768;
            } else {
                // Low-res BB2
                mMaxCols = 640;
                mMaxRows = 480;
            }
        }    
        break;

    case FLYCAPTURE_BUMBLEBEEXB3:
        mMaxCols = 1280;
        mMaxRows = 960;
        break;

    default:
        te = TriclopsErrorInvalidCamera;
        _HANDLE_TRICLOPS_ERROR( "triclopsCheckCameraModel()", te );
        break;
    }

    // Create shared memory segments
    mMaxImageSize = mMaxCols * mMaxRows * 4 /* RGBA = 32bpp */;
    mShMemLeft = new ShMem((/*ComponentBase::componentName*/name() + "-left").toStdString().c_str(), mMaxImageSize);
    //mShMemTop = new ShMem((ComponentBase::componentName + "-top").toStdString().c_str(), mMaxImageSize);
    mShMemRight = new ShMem((/*ComponentBase::componentName*/name() + "-right").toStdString().c_str(), mMaxImageSize);

    // Create output files for recording
    if (mIsRecording) {
        try {
            QString dbtFileName_ = /*ComponentBase::componentName*/name() + "-left.dbt";
            mDbtLeft.open(dbtFileName_.toStdString(), WriteMode, STEREO_LEFT_IMAGE, kMaxFilepathLength);
        } catch (DbiteException & e) {
            LOG_ERROR("error opening dbt file: " << e.what());
        }
        //try {
        //    QString dbtFileName_ = ComponentBase::componentName + "-top.dbt";
        //    mDbtTop.open(dbtFileName_.toStdString(), WriteMode, STEREO_TOP_IMAGE, kMaxFilepathLength);
        //} catch (DbiteException & e) {
        //    LOG_ERROR("error opening dbt file: " << e.what());
        //}
        try {
            QString dbtFileName_ = /*ComponentBase::componentName*/name() + "-right.dbt";
            mDbtRight.open(dbtFileName_.toStdString(), WriteMode, STEREO_RIGHT_IMAGE, kMaxFilepathLength);
        } catch (DbiteException & e) {
            LOG_ERROR("error opening dbt file: " << e.what());
        }
    }

    // Run thread
    /*ComponentBase::*/THREAD_ALIVE = true;
    // execute main thread loop run()
    start();
}

void BumblebeeXB3::stopActivity()
{
    /*ComponentBase::*/THREAD_ALIVE = false;

    // wait the thread termination
    if (!wait(1000)) {
        LOG_ERROR(name() << ":The thread doesn't respond for 1 second, it has been terminated");
        terminate();
    }

    TriclopsError te;
    FlyCaptureError fe;

    // Close the camera
    if (mFlycapture) {
        fe = flycaptureStop( mFlycapture );
        _HANDLE_FLYCAPTURE_ERROR( "flycaptureStop()", fe );
    }

    // Destroy the Triclops context
    if (mTriclops) {
        te = triclopsDestroyContext( mTriclops ) ;
        mTriclops = NULL;
        _HANDLE_TRICLOPS_ERROR("triclopsDestroyContext()", te);
    }

    // Destroy the Flycapture context
    if (mFlycapture) {
        fe = flycaptureDestroyContext( mFlycapture );
        mFlycapture = NULL;
        _HANDLE_FLYCAPTURE_ERROR( "flycaptureDestroyContext()", fe );
    }
    
    // Close DBT files
    if (mIsRecording) {
        mDbtLeft.close();
        //mDbtTop.close();
        mDbtRight.close();
    }

    // Delete shared memory segments
    delete mShMemLeft; mShMemLeft = NULL;
    //delete mShMemTop; mShMemTop = NULL;
    delete mShMemRight; mShMemRight = NULL;
}

void BumblebeeXB3::run()
{
    TriclopsError te;
    FlyCaptureError fe;

    // Start transferring images from the camera to the computer
    const unsigned int uiMode = 3;
    const unsigned int uiImagePosLeft = 0, uiImagePosTop = 0;
    const float fBandwidth = 100; // percent
    fe = flycaptureStartCustomImage(mFlycapture, uiMode, uiImagePosLeft, uiImagePosTop, mMaxCols, mMaxRows, fBandwidth, mPixelFormat);
    _HANDLE_FLYCAPTURE_ERROR( "flycaptureStartCustomImage()", fe );
    if (fe != FLYCAPTURE_OK) {
        return;
    }

    // set rectified resolution
    te = triclopsSetResolution( mTriclops, mMaxRows, mMaxCols );
    _HANDLE_TRICLOPS_ERROR( "triclopsSetResolution()", te );
    if (te != TriclopsErrorOk) {
        return;
    }

    FlyCaptureImage flycaptureImage;

    size_t imageCount = 0;

    QDir outputDirectory;
    if (mIsRecording) {
        outputDirectory.mkdir(/*ComponentBase::componentName*/name());
        outputDirectory.setPath(/*ComponentBase::componentName*/name());
    }

    const char * const imagePrefix = "rectified";
    const char * const imageNameDelimiter = "-";
    const char * const imageExtension = (mPixelFormat == FLYCAPTURE_RAW16) ? ".ppm" : ".pgm";

    const char kFillCharacter = '0';
    const int kNumberWidth = 5;

    /////////////////////
    // Grab an image from the camera
    fe = flycaptureGrabImage2( mFlycapture, &flycaptureImage );
    _HANDLE_FLYCAPTURE_ERROR( "flycaptureGrabImage()", fe );
    if (fe != FLYCAPTURE_OK) {
        return;
    }

    // Extract information from the FlycaptureImage
    int imageCols = flycaptureImage.iCols;
    int imageRows = flycaptureImage.iRows;
    int imageRowInc = flycaptureImage.iRowInc;
    int iSideBySideImages = flycaptureImage.iNumImages;
    LOG_INFO("imageCols = " << imageCols);
    LOG_INFO("imageRows = " << imageRows);
    LOG_INFO("imageRowInc = " << imageRowInc);
    LOG_INFO("iSideBySideImages = " << iSideBySideImages);
    
    /////////////////////
    // Create buffers for holding the color and mono images
    QScopedArrayPointer<unsigned char> rowIntColor(new unsigned char[ imageCols * imageRows * iSideBySideImages * 4 /* RGBA = 32bpp */ ]);

    // Pointers to positions in the color buffer that correspond to the beginning
    // of the red, green and blue sections
    unsigned char * redColor = NULL;
    unsigned char * greenColor = NULL;
    unsigned char * blueColor = NULL;
    
    redColor = rowIntColor.data();
    if (flycaptureImage.iNumImages == 2) {
        greenColor = redColor + ( 4 * imageCols );
        blueColor = redColor + ( 4 * imageCols );
    }
    if (flycaptureImage.iNumImages == 3) {
        greenColor = redColor + ( 4 * imageCols );
        blueColor = redColor + ( 2 * 4 * imageCols );
    }

    /////////////////////
    // Create a temporary FlyCaptureImage for preparing the stereo image
    FlyCaptureImage tempImage;
    tempImage.pData = rowIntColor.data();
    
    // Timestamp
    road_time_t time = 0;
    road_timerange_t tr = 0;

    /////////////////////
    while (/*ComponentBase::*/THREAD_ALIVE) {
        /////////////////////
        // Grab an image from the camera
        fe = flycaptureGrabImage2( mFlycapture, &flycaptureImage );
        _HANDLE_FLYCAPTURE_ERROR( "flycaptureGrabImage()", fe );
        if (fe != FLYCAPTURE_OK) {
            continue;
        }

        // Get timestamp from capture
        unsigned long timeStampSeconds = flycaptureImage.timeStamp.ulSeconds;
        unsigned long timeStampMicroSeconds = flycaptureImage.timeStamp.ulMicroSeconds;
        time = static_cast<road_time_t>(timeStampSeconds) * 1000 * 1000 + timeStampMicroSeconds;
        
        /////////////////////
        // Convert the pixel interleaved raw data to row interleaved format
        fe = flycapturePrepareStereoImage( mFlycapture, flycaptureImage, NULL, &tempImage );
        _HANDLE_FLYCAPTURE_ERROR( "flycapturePrepareStereoImage()", fe );
        if (fe != FLYCAPTURE_OK) {
            continue;
        }
        
        /////////////////////
        // Use the row interleaved images to build up a packed TriclopsInput.
        // A packed triclops input will contain a single image with 32 bpp.
        TriclopsInput colorInputRight;
        te = triclopsBuildPackedTriclopsInput(
            imageCols, imageRows, imageRowInc * 4,
            timeStampSeconds, timeStampMicroSeconds,
            redColor, &colorInputRight );
        _HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
        if (te != TriclopsErrorOk) {
            continue;
        }

        //TriclopsInput colorInputTop;
        //te = triclopsBuildPackedTriclopsInput(
        //    imageCols, imageRows, imageRowInc * 4,
        //    timeStampSeconds, timeStampMicroSeconds,
        //    greenColor, &colorInputTop );
        //_HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
        //if (te != TriclopsErrorOk) {
        //    continue;
        //}

        TriclopsInput colorInputLeft;
        te = triclopsBuildPackedTriclopsInput(
            imageCols, imageRows, imageRowInc * 4,
            timeStampSeconds, timeStampMicroSeconds,
            blueColor, &colorInputLeft );
        _HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
        if (te != TriclopsErrorOk) {
            continue;
        }

        /////////////////////
        // Rectify the color image
        TriclopsPackedColorImage colorImageRight;
        te = triclopsRectifyPackedColorImage(mTriclops, TriCam_RIGHT, &colorInputRight,  &colorImageRight);
        _HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
        if (te != TriclopsErrorOk) {
            continue;
        }

        // TriclopsPackedColorImage colorImageTop;
        //te = triclopsRectifyPackedColorImage(mTriclops, TriCam_TOP, &colorInputTop,  &colorImageTop);
        //_HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
        //if (te != TriclopsErrorOk) {
        //    continue;
        //}

        TriclopsPackedColorImage colorImageLeft;
        te = triclopsRectifyPackedColorImage(mTriclops, TriCam_LEFT, &colorInputLeft,  &colorImageLeft);
        _HANDLE_TRICLOPS_ERROR( "triclopsRectifyPackedColorImage()", te );
        if (te != TriclopsErrorOk) {
            continue;
        }

        /////////////////////
        // Copy images to shared memory
        mShMemLeft->write(colorImageLeft.data, mMaxImageSize);
        //mShMemTop->write(colorImageTop.data, mMaxImageSize);
        mShMemRight->write(colorImageRight.data, mMaxImageSize);
        
        /////////////////////
        // Write images to disk
        if (mIsRecording) {
            // Save the color rectified image to file
            {
                stringstream imageNameRightSs; imageNameRightSs << imagePrefix << imageNameDelimiter << "right" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
                string imageNameRight = outputDirectory.filePath(imageNameRightSs.str().c_str()).toStdString();
                te = triclopsSavePackedColorImage(&colorImageRight, const_cast<char *>(imageNameRight.c_str()));
                _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
                try {
                    mDbtRight.writeRecord(time, tr, imageNameRight.c_str(), kMaxFilepathLength);
                } catch (DbiteException & e) {
                    LOG_ERROR("error writing data: " << e.what());
                }
            }

            //{
            //    stringstream imageNameTopSS; imageNameTopSS << imagePrefix << imageNameDelimiter << "top" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
            //    string imageNameTop = outputDirectory.filePath(imageNameTopSS.str().c_str()).toStdString();
            //    te = triclopsSavePackedColorImage(&colorImageTop, const_cast<char *>(imageNameTop.c_str()));
            //    _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
            //    try {
            //        mDbtTop.writeRecord(time, tr, imageNameTop.c_str(), kMaxFilepathLength);
            //    } catch (DbiteException & e) {
            //        LOG_ERROR("error writing data: " << e.what());
            //    }
            //}

            {
                stringstream imageNameLeftSs; imageNameLeftSs << imagePrefix << imageNameDelimiter << "left" << imageNameDelimiter << setfill(kFillCharacter) << setw(kNumberWidth) << imageCount << imageExtension;
                string imageNameLeft = outputDirectory.filePath(imageNameLeftSs.str().c_str()).toStdString();
                te = triclopsSavePackedColorImage(&colorImageLeft, const_cast<char *>(imageNameLeft.c_str()));
                _HANDLE_TRICLOPS_ERROR( "triclopsSavePackedColorImage()", te );
                try {
                    mDbtLeft.writeRecord(time, tr, imageNameLeft.c_str(), kMaxFilepathLength);
                } catch (DbiteException & e) {
                    LOG_ERROR("error writing data: " << e.what());
                }
            }
        }
        
        LOG_TRACE("image no. " << imageCount << " acquired successfully");
        ++imageCount;
    }

    LOG_INFO("finished acquisition");
}

} // namespace pacpus