#include <iostream>

#include <PvSystem.h>
#include <PvDevice.h>
#include <PvDeviceGEV.h>
#include <PvStream.h>
#include <PvStreamGEV.h>

#include "StreamManager.hpp"

PvResult StreamManager::FindDevice(PvString *aConnectionID)
{
    PvResult lResult = PvResult();
    const PvDeviceInfo *lSelectedDI = NULL;
    PvSystem lSystem;

    lSystem.Find();

    // Detect, select device.
    std::vector<const PvDeviceInfo *> lDIVector;
    for (uint32_t i = 0; i < lSystem.GetInterfaceCount(); i++)
    {
        const PvInterface *lInterface = dynamic_cast<const PvInterface *>(lSystem.GetInterface(i));
        if (lInterface != NULL)
        {
            for (uint32_t j = 0; j < lInterface->GetDeviceCount(); j++)
            {
                const PvDeviceInfo *lDI = dynamic_cast<const PvDeviceInfo *>(lInterface->GetDeviceInfo(j));
                if (lDI != NULL)
                {
                    lDIVector.push_back(lDI);
                }
            }
        }
    }

    if (lDIVector.size() == 0)
    {
        std::cout << "Device not found!" << std::endl;
        lResult.SetCode(PV_NOT_FOUND);
        return lResult;
    }

    lSelectedDI = lDIVector.front();

    *aConnectionID = lSelectedDI->GetConnectionID();

    lResult.SetCode(PV_OK);

    return lResult;
}

PvResult StreamManager::ConnectToDevice(const PvString &aConnectionID)
{
    PvResult lResult;

    // Connect to the GigE Vision or USB3 Vision device
    std::cout << "Connecting to device." << std::endl;
    lDevice = PvDevice::CreateAndConnect(aConnectionID, &lResult);
    if (lDevice == NULL)
    {
        std::cout << "Unable to connect to device." << std::endl;
    }

    return lResult;
}

PvResult StreamManager::OpenStream(const PvString &aConnectionID)
{
    PvResult lResult;

    if (lDevice == NULL)
    {
        return lResult;
    }

    // Open stream to the GigE Vision or USB3 Vision device
    std::cout << "Opening stream from device." << std::endl;
    lStream = PvStream::CreateAndOpen(aConnectionID, &lResult);

    if (lStream != NULL)
    {
        // If this is a GigE Vision device, configure GigE Vision specific streaming parameters
        PvDeviceGEV* lDeviceGEV = dynamic_cast<PvDeviceGEV *>(lDevice);
        if ( lDeviceGEV != NULL )
        {
            PvStreamGEV *lStreamGEV = static_cast<PvStreamGEV *>(lStream);

            // Negotiate packet size
            lDeviceGEV->NegotiatePacketSize();

            // Configure device streaming destination
            lDeviceGEV->SetStreamDestination( lStreamGEV->GetLocalIPAddress(), lStreamGEV->GetLocalPort() );
        }
    }
    else
    {
        std::cout << "Unable to stream from device." << std::endl;
    }  

    return lResult;
}

void StreamManager::CreateStreamBuffers()
{
    // Reading payload size from device
    uint32_t lSize = lDevice->GetPayloadSize();

    // Use BUFFER_COUNT or the maximum number of buffers, whichever is smaller
    uint32_t lBufferCount = (lStream->GetQueuedBufferMaximum() < BUFFER_COUNT) ?
        lStream->GetQueuedBufferMaximum() :
        BUFFER_COUNT;

    // Allocate buffers
    for (uint32_t i = 0; i < lBufferCount; i++)
    {
        // Create new buffer object
        PvBuffer *lBuffer = new PvBuffer;

        // Have the new buffer object allocate payload memory
        lBuffer->Alloc(static_cast<uint32_t>(lSize));
        
        // Add to external list - used to eventually release the buffers
        lBufferList.push_back(lBuffer);
    }
    
    // Queue all buffers in the stream
    BufferList::iterator lIt = lBufferList.begin();
    while (lIt != lBufferList.end())
    {
        lStream->QueueBuffer( *lIt );
        lIt++;
    }
}

void StreamManager::AcquireImages(ImageProcessor processor)
{
    if (lDevice == NULL || lStream == NULL)
    {
        return;
    }

    CreateStreamBuffers();

    // Get device parameters need to control streaming
    PvGenParameterArray *lDeviceParams = lDevice->GetParameters();

    // Map the GenICam AcquisitionStart and AcquisitionStop commands
    PvGenCommand *lStart = dynamic_cast<PvGenCommand *>(lDeviceParams->Get("AcquisitionStart"));
    PvGenCommand *lStop = dynamic_cast<PvGenCommand *>(lDeviceParams->Get("AcquisitionStop"));

    // Get stream parameters
    PvGenParameterArray *lStreamParams = lStream->GetParameters();

    // Map a few GenICam stream stats counters
    PvGenFloat *lFrameRate = dynamic_cast<PvGenFloat *>(lStreamParams->Get("AcquisitionRate"));
    PvGenFloat *lBandwidth = dynamic_cast<PvGenFloat *>(lStreamParams->Get("Bandwidth"));

    // Enable streaming and send the AcquisitionStart command
    std::cout << "Enabling streaming and sending AcquisitionStart command." << std::endl;
    lDevice->StreamEnable();
    lStart->Execute();

    double lFrameRateVal = 0.0;
    double lBandwidthVal = 0.0;

    // Acquire images until the user instructs us to stop.
    while (true)
    {
        PvBuffer *lBuffer = NULL;
        PvResult lOperationResult;

        // Retrieve next buffer
        PvResult lResult = lStream->RetrieveBuffer(&lBuffer, &lOperationResult, 1000);
        if (lResult.IsOK())
        {
            if (lOperationResult.IsOK())
            {
                PvPayloadType lType;

                //
                // We now have a valid buffer. This is where you would typically process the buffer.
                // -----------------------------------------------------------------------------------------
                // ...

                lFrameRate->GetValue(lFrameRateVal);
                lBandwidth->GetValue(lBandwidthVal);

                // If the buffer contains an image, display width and height.
                uint32_t lWidth = 0, lHeight = 0;
                lType = lBuffer->GetPayloadType();

                if (lType == PvPayloadTypeImage)
                {
                    // Get image specific buffer interface.
                    PvImage *lImage = lBuffer->GetImage();

                    // Read width, height.
                    lWidth = lImage->GetWidth();
                    lHeight = lImage->GetHeight();

                    // Process Image
                    processor.ProcessImage(lWidth, lHeight, lImage->GetDataPointer());

                    std::cout << "  W: " << std::dec << lWidth << " H: " << lHeight;
                }
                else 
                {
                    std::cout << " (buffer does not contain image)";
                }
                std::cout << "  " << lFrameRateVal << " FPS  " << ( lBandwidthVal / 1000000.0 ) << " Mb/s   \r";
            }
            else
            {
                std::cout << lOperationResult.GetCodeString().GetAscii() << "\r";
            }

            // Re-queue the buffer in the stream object
            lStream->QueueBuffer(lBuffer);
        }
        else
        {
            // Retrieve buffer failure
            std::cout << lResult.GetCodeString().GetAscii() << "\r";
        }
    }

    // Tell the device to stop sending images.
    std::cout << "Sending AcquisitionStop command to the device" << std::endl;
    lStop->Execute();

    // Disable streaming on the device
    std::cout << "Disable streaming on the controller." << std::endl;
    lDevice->StreamDisable();

    // Abort all buffers from the stream and dequeue
    std::cout << "Aborting buffers still in stream" << std::endl;
    lStream->AbortQueuedBuffers();
    while (lStream->GetQueuedBufferCount() > 0)
    {
        PvBuffer *lBuffer = NULL;
        PvResult lOperationResult;

        lStream->RetrieveBuffer(&lBuffer, &lOperationResult);
    }

    FreeStreamBuffers();
}

void StreamManager::FreeStreamBuffers()
{
    // Go through the buffer list
    BufferList::iterator lIt = lBufferList.begin();
    while (lIt != lBufferList.end())
    {
        delete *lIt;
        lIt++;
    }

    // Clear the buffer list 
    lBufferList.clear();
}

void StreamManager::Disconnect()
{
    if (lStream != NULL)
    {
        lStream->Close();
        PvStream::Free( lStream );
    }
    if (lDevice != NULL)
    {
        lDevice->Disconnect();
        PvDevice::Free(lDevice);
    }
}