#include "StreamManager.hpp"
#include "FaultFinder.hpp"

StreamManager::StreamManager(QObject *parent) : QObject(parent)
{  
    // Connect Signals
    connect(((FaultFinder *)parent)->config_mgr, &ConfigManager::NewEmissivity, this, &StreamManager::SetEmissivity);
    connect(((FaultFinder *)parent)->config_mgr, &ConfigManager::NewFrameRate, this, &StreamManager::SetFrameRate);

    PvResult res;

    res = FindDevice();

    if (res.IsFailure())
        exit(res.GetCode());

    res = ConnectToDevice();

    if (res.IsFailure())
        exit(res.GetCode());

    res = OpenStream();

    if (res.IsFailure())
        exit(res.GetCode());

    ConfigureStream();

    CreatePipeline();

    StartStream();

    ImageThread = new StreamReader(Device, Stream, Pipeline);
    connect(ImageThread, &StreamReader::NewImage, this, &StreamManager::SendForProcessing, Qt::QueuedConnection);
    ImageThread->start(QThread::TimeCriticalPriority);
}

StreamManager::~StreamManager()
{
    ImageThread->requestInterruption();
    ImageThread->wait();
    StopStream();
    Disconnect();
}

void StreamManager::SendForProcessing(int width, int height, uint8_t *imgPointer)
{
    ((FaultFinder *)parent())->image_mgr->ProcessImage(width, height, imgPointer);
}

void StreamManager::SetEmissivity(double emissivity)
{
    if (lDeviceParams)
        lDeviceParams->SetFloatValue("ObjectEmissivity", emissivity);
}

void StreamManager::SetFrameRate(int frame_rate)
{
    if (lDeviceParams)
        lDeviceParams->SetFloatValue("PS0FrameRate", frame_rate);
}

bool StreamManager::GetReadyState()
{
    bool cold = false;
    if (lDeviceParams)
        lDeviceParams->GetBooleanValue("FPACold", cold);
    return cold;
}

PvResult StreamManager::FindDevice()
{
    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)
    {
        qCritical() << "Device not found!";
        lResult.SetCode(PV_NOT_FOUND);
        return lResult;
    }

    lSelectedDI = lDIVector.front();

    ConnectionID = lSelectedDI->GetConnectionID();

    lResult.SetCode(PV_OK);

    return lResult;
}

PvResult StreamManager::ConnectToDevice()
{
    PvResult lResult;

    // Connect to the GigE Vision or USB3 Vision device
    qInfo() << "Connecting to device.";
    Device = PvDevice::CreateAndConnect(ConnectionID, &lResult);
    if (Device == NULL)
    {
        qCritical() << "Unable to connect to device.";
    }

    return lResult;
}

PvResult StreamManager::OpenStream()
{
    PvResult lResult;

    // Open stream to the GigE Vision or USB3 Vision device
    qInfo() << "Opening stream from device.";
    Stream = PvStream::CreateAndOpen(ConnectionID, &lResult);

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

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

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

    return lResult;
}

void StreamManager::ConfigureStream()
{
    // Get device parameters need to control streaming
    lDeviceParams = Device->GetParameters();

    // Use Temperature Format Instead of Raw Counts
    lDeviceParams->SetEnumValue("IRFormat", 1);

    SetEmissivity(((FaultFinder *)parent())->config_mgr->GetEmissivity());
    SetFrameRate(((FaultFinder *)parent())->config_mgr->GetFrameRate());
}

void StreamManager::CreatePipeline()
{
    // Create the PvPipeline object
    Pipeline = new PvPipeline( Stream );

    if ( Pipeline != NULL )
    {        
        // Reading payload size from device
        uint32_t lSize = Device->GetPayloadSize();
    
        // Set the Buffer count and the Buffer size
        Pipeline->SetBufferCount( BUFFER_COUNT );
        Pipeline->SetBufferSize( lSize );
    }
}

void StreamManager::StartStream()
{
    // Map the GenICam AcquisitionStart and AcquisitionStop commands
    PvGenCommand *lStart = dynamic_cast<PvGenCommand *>(lDeviceParams->Get("AcquisitionStart"));

    qInfo() << "Starting pipeline";
    Pipeline->Start();

    // Get stream parameters
    PvGenParameterArray *lStreamParams = Stream->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
    qInfo() << "Enabling streaming and sending AcquisitionStart command.";
    Device->StreamEnable();
    lStart->Execute();
}

void StreamManager::StopStream()
{
    // Tell the device to stop sending images.
    qInfo() << "Sending AcquisitionStop command to the device";
    PvGenCommand *lStop = dynamic_cast<PvGenCommand *>(lDeviceParams->Get("AcquisitionStop"));
    lStop->Execute();

    // Disable streaming on the device
    qInfo() << "Disable streaming on the controller.";;
    Device->StreamDisable();

    // Stop the pipeline
    qInfo() << "Stop pipeline";
    Pipeline->Stop();
}

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