/*
 * Copyright (c) 2018-2020, NVIDIA CORPORATION.  All rights reserved.
 *
 * NVIDIA CORPORATION and its licensors retain all intellectual property
 * and proprietary rights in and to this software, related documentation
 * and any modifications thereto.  Any use, reproduction, disclosure or
 * distribution of this software and related documentation without an express
 * license agreement from NVIDIA CORPORATION is strictly prohibited.
 */

/* STL Headers */
#include <unistd.h>
#include <cstring>
#include <iostream>

#include "NvSIPLCamera.hpp" // Camera
#include "NvSIPLPipelineMgr.hpp" // Pipeline manager
#include "CNvSIPLMaster.hpp" // Master base class
#include "CUtils.hpp"

#include "nvscibuf.h"
#include "nvscisync.h"
#include "nvscistream.h"
#include "nvmedia_image_nvscibuf.h"

#ifndef CNVSIPLMASTERNVSCI_HPP
#define CNVSIPLMASTERNVSCI_HPP

#define MAX_SENSORS 16U
#define MAX_OUTPUTS_PER_SENSOR 3U
#define STREAMING_TIMEOUT 100000

using namespace std;
using namespace nvsipl;

/** NvSIPL Master class */
class CNvSIPLMasterNvSci final : public CNvSIPLMaster
{
 public:
    SIPLStatus Setup(PlatformCfg *pPlatformCfg, NvSciBufModule *bufModule, NvSciSyncModule *syncModule)
    {
        if ((bufModule == nullptr) || (syncModule == nullptr)) {
            LOG_ERR("Setup: Received unexpected nullptr\n");
            return NVSIPL_STATUS_BAD_ARGUMENT;
        }

        if(CLogger::GetInstance().GetLogLevel() >= CLogger::LEVEL_INFORMATION) {
            // Display platform configuration
            DisplayPlatformCfg(pPlatformCfg);
        }

        // Camera Master setup
        m_pCamera = INvSIPLCamera::GetInstance();
        CHK_PTR_AND_RETURN(m_pCamera, "INvSIPLCamera::GetInstance()");

        auto status = m_pCamera->SetPlatformCfg(pPlatformCfg);
        CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::SetPlatformCfg()");

        m_platCfg = *pPlatformCfg;

        auto sciErr = NvSciBufModuleOpen(&m_sciBufModule);
        CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciBufModuleOpen");

        auto nvmStatus = NvMediaImageNvSciBufInit();
        CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageNvSciBufInit");

        sciErr = NvSciSyncModuleOpen(&m_sciSyncModule);
        CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciSyncModuleOpen");

        *bufModule = m_sciBufModule;
        *syncModule = m_sciSyncModule;

        return status;
    }

    SIPLStatus Start(void)
    {
        m_bRunning = true;
        for (ProducerStream *pStream : m_usedStreams) {
            // Spawn a thread to collect and release used buffers
            pStream->numBuffersWithConsumer = 0U;
            pStream->producerThread = std::thread(&CNvSIPLMasterNvSci::ThreadFunc, this, pStream);
            LOG_INFO("Created producer thread: ID:%u\n", pStream->producerThread.get_id());
        }

        const SIPLStatus status = m_pCamera->Start();
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("Failed to start SIPL\n");
            return status;
        }

        return status;
    }

    SIPLStatus Stop(void)
    {
        const SIPLStatus status = m_pCamera->Stop();
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("Failed to stop SIPL\n");
            return status;
        }

        // Signal producer threads to stop
        m_bRunning = false;

        // Wait for the producer threads
        for (ProducerStream *pStream : m_usedStreams) {
            LOG_INFO("Waiting to join producer thread: ID:%u\n", pStream->producerThread.get_id());
            pStream->producerThread.join();
        }

        return status;
    }

    void Deinit(void)
    {
        auto status = m_pCamera->Deinit();
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("INvSIPLCamera::Deinit failed. status: %x\n", status);
        }

        for (auto i = 0u; i < MAX_SENSORS; i++) {
          for (auto j = 0u; j < MAX_OUTPUTS_PER_SENSOR; j++) {
            ProducerStream *pStream = &m_streams[i][j];
            if (pStream->bufAttrList != nullptr) {
              NvSciBufAttrListFree(pStream->bufAttrList);
            }
            if (pStream->signalerAttrList != nullptr) {
              NvSciSyncAttrListFree(pStream->signalerAttrList);
            }
            if (pStream->waiterAttrList != nullptr) {
              NvSciSyncAttrListFree(pStream->waiterAttrList);
            }
            for (NvSciBufObj bufObj : pStream->sciBufObjs) {
              if (bufObj != nullptr) {
                NvSciBufObjFree(bufObj);
              }
            }
            if (pStream->objFromConsumer != nullptr) {
              NvSciSyncObjFree(pStream->objFromConsumer);
            }
            if (pStream->producerSyncObj != nullptr) {
              NvSciSyncObjFree(pStream->producerSyncObj);
            }
            DeleteImageGroups(pStream->imageGroupList);
            DeleteImages(pStream->imageList);
            if (pStream->queue != 0U) {
              NvSciStreamBlockDelete(pStream->queue);
            }
            if (pStream->downstream != 0U) {
              NvSciStreamBlockDelete(pStream->downstream);
            }
            if (pStream->staticPool != 0U) {
              NvSciStreamBlockDelete(pStream->staticPool);
            }
            if (pStream->producer != 0U) {
              NvSciStreamBlockDelete(pStream->producer);
            }

            std::vector<NvSciBufObj>().swap(pStream->sciBufObjs);
            std::vector<NvMediaImageGroup*>().swap(pStream->imageGroupList);
            std::vector<NvMediaImage*>().swap(pStream->imageList);
          }
        }

        vector<ProducerStream*>().swap(m_usedStreams);

        NvMediaImageNvSciBufDeinit();

        if (m_sciBufModule != NULL) {
          NvSciBufModuleClose(m_sciBufModule);
        }

        if (m_sciSyncModule != NULL) {
          NvSciSyncModuleClose(m_sciSyncModule);
        }
    }

    SIPLStatus AllocateNvSciBuffers(ProducerStream *pStream)
    {
        pStream->sciBufObjs.resize(NUM_PACKETS);
        for(auto i = 0u; i < NUM_PACKETS; i++) {
           auto sciErr = NvSciBufObjAlloc(pStream->bufAttrList, &pStream->sciBufObjs[i]);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciBufObjAlloc");
        }
        return NVSIPL_STATUS_OK;
    }

    SIPLStatus AllocateBuffers(std::uint32_t uSensor, bool isp0Enabled, bool isp1Enabled)
    {
        SIPLStatus status;

        status = AllocateNvSciBuffers(&m_streams[uSensor][0]);
        CHK_STATUS_AND_RETURN(status, "ICP CNvSIPLMaster::AllocateNvSciBuffers");

        if (isp0Enabled) {
            status = AllocateNvSciBuffers(&m_streams[uSensor][1]);
            CHK_STATUS_AND_RETURN(status, "ISP0 CNvSIPLMaster::AllocateNvSciBuffers");
        }

        if (isp1Enabled) {
            status = AllocateNvSciBuffers(&m_streams[uSensor][2]);
            CHK_STATUS_AND_RETURN(status, "ISP1 CNvSIPLMaster::AllocateNvSciBuffers");
        }

        return NVSIPL_STATUS_OK;
    }

    SIPLStatus RegisterImageGroups(ProducerStream *pStream)
    {
        auto pDevice = m_upDevice.get();

        pStream->imageGroupList.resize(NUM_PACKETS);

        for(auto i = 0u; i < NUM_PACKETS; i++) {
            auto imgGrp =  new (std::nothrow) NvMediaImageGroup;
            CHK_PTR_AND_RETURN(imgGrp, "new NvMediaImageGroup");

            imgGrp->numImages = 1;
            auto nvmStatus = NvMediaImageCreateFromNvSciBuf(pDevice, pStream->sciBufObjs[i], &imgGrp->imageList[0]);
            CHK_NVMSTATUS_AND_RETURN(nvmStatus, "Producer NvMediaImageCreateFromNvSciBuf");
            nvmStatus = NvMediaImageSetTag(imgGrp->imageList[0], (void *)(&pStream->bufferInfo[i].cookie));
            CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageSetTag");

            pStream->imageGroupList[i] = imgGrp;
        }

        auto status = m_pCamera->RegisterImageGroups(pStream->uSensor,
                                                     pStream->imageGroupList);
        CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::RegisterImageGroups");

        return NVSIPL_STATUS_OK;
    }

    SIPLStatus RegisterImages(ProducerStream *pStream)
    {
        auto pDevice = m_upDevice.get();

        pStream->imageList.resize(NUM_PACKETS);

        for(auto i = 0u; i < NUM_PACKETS; i++) {
            auto nvmStatus = NvMediaImageCreateFromNvSciBuf(pDevice, pStream->sciBufObjs[i], &pStream->imageList[i]);
            CHK_NVMSTATUS_AND_RETURN(nvmStatus, "Producer NvMediaImageCreateFromNvSciBuf");
            nvmStatus = NvMediaImageSetTag(pStream->imageList[i], (void *)(&pStream->bufferInfo[i].cookie));
            CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageSetTag");
        }

        auto status = m_pCamera->RegisterImages(pStream->uSensor,
                                                pStream->outputType,
                                                pStream->imageList);
        CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::RegisterImages");

        return NVSIPL_STATUS_OK;
    }

    SIPLStatus RegisterBuffers(std::uint32_t pip, bool isp0Enabled, bool isp1Enabled)
    {
        SIPLStatus status;

        status = RegisterImageGroups(&m_streams[pip][0]);
        CHK_STATUS_AND_RETURN(status, "RegisterImageGroups");

        if (isp0Enabled) {
            status = RegisterImages(&m_streams[pip][1]);
            CHK_STATUS_AND_RETURN(status, "RegisterImages");
        }

        if (isp1Enabled) {
            status = RegisterImages(&m_streams[pip][2]);
            CHK_STATUS_AND_RETURN(status, "RegisterImages");
        }

        return NVSIPL_STATUS_OK;
    }


    SIPLStatus RegisterSource(std::uint32_t uSensor,
                              INvSIPLClient::ConsumerDesc::OutputType outputType,
                              bool isSimulatorMode,
                              bool streamingEnabled,
                              NvSciStreamBlock *consumer,
                              NvSciStreamBlock *consumerUpstream,
                              NvSciStreamBlock *queue)
    {
        NvSIPLImageAttr attr;
        NvSciStreamEvent event;
        NvSciBufAttrList reconciledAttrlist = nullptr;
        NvSciBufAttrList unreconciledAttrlist = nullptr;
        NvSciBufAttrList conflictlist = nullptr;
        ProducerStream *pStream = &m_streams[uSensor][int(outputType)];

        if (streamingEnabled) {
            if ((consumer == nullptr) || (consumerUpstream == nullptr) || (queue == nullptr)) {
                LOG_ERR("RegisterSource: Received unexpected nullptr\n");
                return NVSIPL_STATUS_BAD_ARGUMENT;
            }

            auto sciErr = NvSciStreamStaticPoolCreate(NUM_PACKETS, &pStream->staticPool);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciStreamStaticPoolCreate");

            sciErr = NvSciStreamProducerCreate(pStream->staticPool, &pStream->producer);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciStreamProducerCreate");

            pStream->downstream = *consumer;
            *consumerUpstream = pStream->producer;
            pStream->queue = *queue;

            sciErr = NvSciStreamBlockConnect(pStream->producer, pStream->downstream);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockConnect");

            sciErr = NvSciStreamBlockEventQuery(pStream->staticPool, STREAMING_TIMEOUT, &event);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Static Pool NvSciStreamBlockEventQuery");
            if (event.type != NvSciStreamEventType_Connected) {
                LOG_ERR("RegisterSource: Did not receive expected static pool Connected event\n");
                return NVSIPL_STATUS_ERROR;
            }

            sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockEventQuery");
            if (event.type != NvSciStreamEventType_Connected) {
                LOG_ERR("RegisterSource: Did not receive expected producer Connected event\n");
                return NVSIPL_STATUS_ERROR;
            }

            sciErr = NvSciStreamBlockEventQuery(pStream->queue, STREAMING_TIMEOUT, &event);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Queue NvSciStreamBlockEventQuery");
            if (event.type != NvSciStreamEventType_Connected) {
                LOG_ERR("RegisterSource: Did not receive expected queue Connected event\n");
                return NVSIPL_STATUS_ERROR;
            }

            sciErr = NvSciStreamBlockEventQuery(pStream->downstream, STREAMING_TIMEOUT, &event);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Consumer NvSciStreamBlockEventQuery");
            if (event.type != NvSciStreamEventType_Connected) {
                LOG_ERR("RegisterSource: Did not receive expected consumer Connected event\n");
                return NVSIPL_STATUS_ERROR;
            }
        }

        auto status = m_pCamera->GetImageAttributes(uSensor, outputType, attr);
        CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::GetImageAttributes");

        auto sciErr = NvSciBufAttrListCreate(m_sciBufModule, &unreconciledAttrlist);
        CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciBufAttrListCreate");

        // Need CPU Read permission for RAW->RGB conversion on compositor
        // Need CPU Write permission for FileReader
        // TODO: Determine the permission based on exact config instead of hardcoded value.
        NvSciBufAttrValAccessPerm accessPerm = NvSciBufAccessPerm_ReadWrite;
        NvSciBufAttrKeyValuePair attrKvp = { NvSciBufGeneralAttrKey_RequiredPerm,
                                             &accessPerm,
                                             sizeof(accessPerm)};

        sciErr = NvSciBufAttrListSetAttrs(unreconciledAttrlist, &attrKvp, 1);
        CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciBufAttrListSetAttrs");

        auto nvmStatus = NvMediaImageFillNvSciBufAttrs(m_upDevice.get(),
                                                       attr.surfaceType,
                                                       attr.surfaceAllocAttr.data(),
                                                       attr.surfaceAllocAttr.size(),
                                                       0,
                                                       unreconciledAttrlist);
        CHK_NVMSTATUS_AND_RETURN(nvmStatus, "Producer NvMediaImageFillNvSciBufAttrs");

        pStream->uSensor = uSensor;
        pStream->outputType = outputType;
        pStream->isSimulatorMode = isSimulatorMode;

        if (streamingEnabled) {
            pStream->bufAttrList = unreconciledAttrlist;
            m_usedStreams.push_back(pStream);
        } else {
            sciErr = NvSciBufAttrListReconcile(&unreconciledAttrlist, 1, &reconciledAttrlist, &conflictlist);
            CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciBufAttrListReconcile");
            pStream->bufAttrList = reconciledAttrlist;
            NvSciBufAttrListFree(unreconciledAttrlist);
            if (conflictlist != nullptr) {
                NvSciBufAttrListFree(conflictlist);
            }
        }

        return NVSIPL_STATUS_OK;
    }

    SIPLStatus Post(uint32_t uSensor, INvSIPLClient::ConsumerDesc::OutputType outputType, INvSIPLClient::INvSIPLNvMBuffer *pBuffer)
    {
        ProducerStream *pStream = &m_streams[uSensor][int(outputType)];
        NvSciSyncFence fence = NvSciSyncFenceInitializer;
        NvMediaImageGroup *imageGroup = nullptr;
        NvMediaImage *image = nullptr;
        void *imageTag = nullptr;
        NvSciStreamCookie cookie = -1U;

        CHK_PTR_AND_RETURN(pBuffer, "Post INvSIPLClient::INvSIPLNvMBuffer");
        CHK_PTR_AND_RETURN(pStream, "Post ProducerStream");

        if (!pStream->isSimulatorMode || (pStream->outputType != INvSIPLClient::ConsumerDesc::OutputType::ICP)) {
            auto status = pBuffer->GetEOFNvSciSyncFence(&fence);
            CHK_STATUS_AND_RETURN(status, "INvSIPLClient::INvSIPLNvMBuffer::GetEOFNvSciSyncFence");
        }

        if (outputType == INvSIPLClient::ConsumerDesc::OutputType::ICP) {
            imageGroup = pBuffer->GetImageGroup();
            CHK_PTR_AND_RETURN(imageGroup, "INvSIPLClient::INvSIPLNvMBuffer::GetImageGroup");
            image = imageGroup->imageList[0];
            CHK_PTR_AND_RETURN(image, "imageGroup->imageList[0]");
        } else if ((outputType == INvSIPLClient::ConsumerDesc::OutputType::ISP0)
                   || (outputType == INvSIPLClient::ConsumerDesc::OutputType::ISP1)) {
            image = pBuffer->GetImage();
            CHK_PTR_AND_RETURN(image, "INvSIPLClient::INvSIPLNvMBuffer::GetImage");
        } else {
            LOG_ERR("Received unexpected output type\n");
            return NVSIPL_STATUS_ERROR;
        }
        const NvMediaStatus nvmStatus = NvMediaImageGetTag(image, &imageTag);
        CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageGetTag");
        CHK_PTR_AND_RETURN(imageTag, "NvMediaImageGetTag");
        cookie = *((NvSciStreamCookie *)imageTag);

        // Subtract one from cookie to get index (since it was incremented on initialization)
        pStream->bufferInfo[cookie - 1U].buffer = pBuffer;
        pStream->numBuffersWithConsumer++;

        auto sciErr = NvSciStreamProducerPacketPresent(pStream->producer,
                                                       pStream->bufferInfo[cookie - 1U].packet,
                                                       &fence);
        if (sciErr != NvSciError_Success) {
            // Decrement count with consumer because send was unsuccessful
            pStream->numBuffersWithConsumer--;
            LOG_ERR("NvSciStreamProducerPacketPresent failed\n");
            return NVSIPL_STATUS_ERROR;
        }
        NvSciSyncFenceClear(&fence);

        pBuffer->AddRef();

        return NVSIPL_STATUS_OK;
    }

    SIPLStatus SetupStreaming(void)
    {
       NvSciStreamEvent event;

       // Wait for connection.
       // Get Camera waiter and signaler NvSciSync attributes and send waiter attributes to consumer.
       for(ProducerStream *pStream : m_usedStreams) {
          auto sciErr = NvSciSyncAttrListCreate(m_sciSyncModule, &pStream->signalerAttrList);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer signaler NvSciSyncAttrListCreate");

          sciErr = NvSciSyncAttrListCreate(m_sciSyncModule, &pStream->waiterAttrList);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer waiter NvSciSyncAttrListCreate");

          if (pStream->isSimulatorMode && (pStream->outputType == INvSIPLClient::ConsumerDesc::OutputType::ICP)) {
              /* CFileReader is the source for ICP, so fill CPU waiter and signaler attributes */
              NvSciSyncAttrKeyValuePair keyValue[2];
              bool cpuWaiter = true;
              keyValue[0].attrKey = NvSciSyncAttrKey_NeedCpuAccess;
              keyValue[0].value = (void *)&cpuWaiter;
              keyValue[0].len = sizeof(cpuWaiter);
              NvSciSyncAccessPerm cpuPerm = NvSciSyncAccessPerm_WaitOnly;
              keyValue[1].attrKey = NvSciSyncAttrKey_RequiredPerm;
              keyValue[1].value = (void*)&cpuPerm;
              keyValue[1].len = sizeof(cpuPerm);
              sciErr = NvSciSyncAttrListSetAttrs(pStream->waiterAttrList, keyValue, 2);
              CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer waiter NvSciSyncAttrListSetAttrs for simulator mode");

              cpuPerm = NvSciSyncAccessPerm_SignalOnly;
              keyValue[1].attrKey = NvSciSyncAttrKey_RequiredPerm;
              keyValue[1].value = (void*)&cpuPerm;
              keyValue[1].len = sizeof(cpuPerm);
              sciErr = NvSciSyncAttrListSetAttrs(pStream->signalerAttrList, keyValue, 2);
              CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer signaler NvSciSyncAttrListSetAttrs for simulator mode");
          } else {
              auto status = m_pCamera->FillNvSciSyncAttrList(pStream->uSensor,
                                                             pStream->outputType,
                                                             pStream->signalerAttrList,
                                                             NVMEDIA_SIGNALER);
              CHK_STATUS_AND_RETURN(status, "Signaler INvSIPLCamera::FillNvSciSyncAttrList");

              status = m_pCamera->FillNvSciSyncAttrList(pStream->uSensor,
                                                        pStream->outputType,
                                                        pStream->waiterAttrList,
                                                        NVMEDIA_WAITER);
              CHK_STATUS_AND_RETURN(status, "Waiter INvSIPLCamera::FillNvSciSyncAttrList");
          }

          sciErr = NvSciStreamBlockSyncRequirements(pStream->producer, false, pStream->waiterAttrList);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockSyncRequirements");
       }

       // Get consumer's NvSciSync attributes, reconcile with our own signaler attributes, create
       // producer NvSciSyncObjs with the reconciled attributes, register the objects with Camera
       // as EOF sync objs, set them as EOF sync objs, and send objects to the consumer.
       for(ProducerStream *pStream : m_usedStreams) {
          NvSciSyncAttrList unreconciledList[2];
          NvSciSyncAttrList reconciledList = NULL;
          NvSciSyncAttrList newConflictList = NULL;

          auto sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer SyncAttr NvSciStreamBlockEventQuery");

          if(event.type != NvSciStreamEventType_SyncAttr) {
               LOG_ERR("SetupStreaming: did not receive expected SyncAttr event\n");
               return NVSIPL_STATUS_ERROR;
          }

          unreconciledList[0] = pStream->signalerAttrList;
          unreconciledList[1] = event.syncAttrList;

          sciErr = NvSciSyncAttrListReconcile(unreconciledList,
                                              2,
                                              &reconciledList,
                                              &newConflictList);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciSyncAttrListReconcile");

          sciErr = NvSciSyncObjAlloc(reconciledList, &pStream->producerSyncObj);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciSyncObjAlloc");

          NvSciSyncAttrListFree(reconciledList);
          NvSciSyncAttrListFree(event.syncAttrList);
          if(newConflictList != nullptr) {
             NvSciSyncAttrListFree(newConflictList);
          }

          if (!pStream->isSimulatorMode || (pStream->outputType != INvSIPLClient::ConsumerDesc::OutputType::ICP)) {
              auto status = m_pCamera->RegisterNvSciSyncObj(pStream->uSensor,
                                                            pStream->outputType,
                                                            NVMEDIA_EOFSYNCOBJ,
                                                            pStream->producerSyncObj);
              CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::RegisterNvSciSyncObj");
          }

          sciErr = NvSciStreamBlockSyncObjCount(pStream->producer, 1);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockSyncObjCount");

          sciErr = NvSciStreamBlockSyncObject(pStream->producer, 0, pStream->producerSyncObj);
          CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockSyncObject");
       }

       // Get sync objs from consumer and register as presync objects.
       // Send SciBuf attributes to pool
       for(ProducerStream *pStream : m_usedStreams) {
           auto sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer SyncCount NvSciStreamBlockEventQuery");

           if(event.type != NvSciStreamEventType_SyncCount) {
               LOG_ERR("SetupStreaming: did not receive expected SyncCount event type\n");
               return NVSIPL_STATUS_ERROR;
           }
           if(event.count != 1U) {
               LOG_ERR("SetupStreaming: did not receive a single sync object as expected\n");
               return NVSIPL_STATUS_ERROR;
           }

           sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer SyncDesc NvSciStreamBlockEventQuery");

           if(event.type != NvSciStreamEventType_SyncDesc) {
               LOG_ERR("SetupStreaming: did not receive expected SyncDesc event type\n");
               return NVSIPL_STATUS_ERROR;
           }
           pStream->objFromConsumer = event.syncObj;

           if (!pStream->isSimulatorMode || (pStream->outputType != INvSIPLClient::ConsumerDesc::OutputType::ICP)) {
               auto status = m_pCamera->RegisterNvSciSyncObj(pStream->uSensor,
                                                             pStream->outputType,
                                                             NVMEDIA_PRESYNCOBJ,
                                                             pStream->objFromConsumer);
               CHK_STATUS_AND_RETURN(status, "INvSIPLCamera::RegisterNvSciSyncObj");
           }

           sciErr = NvSciStreamBlockPacketElementCount(pStream->producer, 1);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockPacketElementCount");

           sciErr = NvSciStreamBlockPacketAttr(pStream->producer, 0, 0,
                                               NvSciStreamElementMode_Asynchronous,
                                               pStream->bufAttrList);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockPacketAttr");
        }

       // As pool, get buffer attributes from producer and consumer, reconcile them, send to both.
       for(ProducerStream *pStream : m_usedStreams) {
           NvSciBufAttrList unreconciledAttrlist[2] {};
           NvSciBufAttrList reconciledAttrlist = nullptr;
           NvSciBufAttrList conflictlist = nullptr;
           bool fromConsumer = false, fromProducer = false;

           for(auto i = 0U; i < 4U; i++) {  // 2 counts, 2 attributes -- order uncertain
               auto sciErr = NvSciStreamBlockEventQuery(pStream->staticPool, STREAMING_TIMEOUT, &event);
               CHK_NVSCISTATUS_AND_RETURN(sciErr, "Pool NvSciStreamBlockEventQuery");

               switch(event.type) {
               case NvSciStreamEventType_PacketElementCountConsumer:
               case NvSciStreamEventType_PacketElementCountProducer:
                   if(event.count != 1U) {
                       LOG_ERR("SetupStreaming: pool did not receive single element count as expected\n");
                       return NVSIPL_STATUS_ERROR;
                   }
                   break;
               case NvSciStreamEventType_PacketAttrConsumer:
                   unreconciledAttrlist[0] = event.bufAttrList;
                   fromConsumer = true;
                   break;
               case NvSciStreamEventType_PacketAttrProducer:
                   unreconciledAttrlist[1] = event.bufAttrList;
                   fromProducer = true;
                   break;
               default:
                    LOG_ERR("SetupStreaming: did not receive expected event type\n");
                    return NVSIPL_STATUS_ERROR;
               }
           }

           if(!fromConsumer || !fromProducer) {
               LOG_ERR("SetupStreaming: attribute from consumer = %s, from producer = %s\n",
                        fromConsumer ? "true" : "false",
                        fromProducer ? "true" : "false");
               return NVSIPL_STATUS_ERROR;
           }

           auto sciErr = NvSciBufAttrListReconcile(unreconciledAttrlist, 2, &reconciledAttrlist, &conflictlist);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Pool NvSciBufAttrListReconcile");

           sciErr = NvSciStreamBlockPacketElementCount(pStream->staticPool, 1);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Pool NvSciStreamBlockPacketElementCount");

           sciErr = NvSciStreamBlockPacketAttr(pStream->staticPool, 0, 0, NvSciStreamElementMode_Asynchronous, reconciledAttrlist);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Pool NvSciStreamBlockPacketAttr");

           NvSciBufAttrListFree(unreconciledAttrlist[0]);
           NvSciBufAttrListFree(unreconciledAttrlist[1]);
           NvSciBufAttrListFree(reconciledAttrlist);
           if(conflictlist != nullptr) {
              NvSciBufAttrListFree(conflictlist);
           }
       }

       // Get reconciled buffer attributes from pool and replace our unreconciled list
       for(ProducerStream *pStream : m_usedStreams) {

           auto sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer PacketElementCount NvSciStreamBlockEventQuery");
           if(event.type != NvSciStreamEventType_PacketElementCount) {
               LOG_ERR("SetupStreaming: did not receive PacketElementCount event as expected\n");
               return NVSIPL_STATUS_ERROR;
           }
           if(event.count != 1U) {
               LOG_ERR("SetupStreaming: did not receive PacketElementCount==1 expected\n");
               return NVSIPL_STATUS_ERROR;
           }

           sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
           CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer PacketAttr NvSciStreamBlockEventQuery");
           if(event.type != NvSciStreamEventType_PacketAttr) {
               LOG_ERR("SetupStreaming: did not receive PacketAttr event as expected\n");
               return NVSIPL_STATUS_ERROR;
           }

           NvSciBufAttrListFree(pStream->bufAttrList);  // replace it
           pStream->bufAttrList = event.bufAttrList;
       }

       return NVSIPL_STATUS_OK;
    }

    SIPLStatus SetupBuffers(void)
    {
        NvSciStreamEvent event;
        NvSciStreamPacket packetHandle;
        NvSciStreamCookie cookie;
        NvSciSyncFence fence = NvSciSyncFenceInitializer;

        // Pool sends buffers to everyone
        for(ProducerStream *pStream : m_usedStreams) {
            for(auto i = 0U; i < NUM_PACKETS; i++) {

                auto sciErr = NvSciStreamPoolPacketCreate(pStream->staticPool, i+1, &packetHandle);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciStreamPoolPacketCreate");

                sciErr = NvSciStreamPoolPacketInsertBuffer(pStream->staticPool, packetHandle, 0,
                                                           pStream->sciBufObjs[i]);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "NvSciStreamPoolPacketInsertBuffer");
            }
        }

        // Producer replaces buffers
        for(ProducerStream *pStream : m_usedStreams) {
            for(auto i = 0U; i < NUM_PACKETS; i++) {
                auto sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer PacketCreate NvSciStreamBlockEventQuery");
                if(event.type != NvSciStreamEventType_PacketCreate) {
                    LOG_ERR("SetupBuffers: did not receive expected PacketCreate event type\n");
                    return NVSIPL_STATUS_ERROR;
                }

                // Set cookie to one more than the index
                pStream->bufferInfo[i].cookie = i+1;
                pStream->bufferInfo[i].packet = event.packetHandle;

                sciErr = NvSciStreamBlockPacketAccept(pStream->producer,
                                                      pStream->bufferInfo[i].packet,
                                                      pStream->bufferInfo[i].cookie,
                                                      NvSciError_Success);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockPacketAccept");

                sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer PacketElement NvSciStreamBlockEventQuery");
                if(event.type != NvSciStreamEventType_PacketElement) {
                    LOG_ERR("SetupBuffers: did not receive expected PacketElement event type\n");
                    return NVSIPL_STATUS_ERROR;
                }

                NvSciBufObjFree(pStream->sciBufObjs[i]);  // replace
                sciErr = NvSciBufObjDup(event.bufObj, &pStream->sciBufObjs[i]);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciBufObjDup");
                NvSciBufObjFree(event.bufObj);

                sciErr = NvSciStreamBlockElementAccept(pStream->producer,
                                                       pStream->bufferInfo[i].packet,
                                                       event.index,
                                                       NvSciError_Success);
                CHK_NVSCISTATUS_AND_RETURN(sciErr, "Producer NvSciStreamBlockElementAccept");
            }
        }

        // Pool reads back acceptance status from producer and consumer
        for(ProducerStream *pStream : m_usedStreams) {
            unsigned int producerPackets = 0, consumerPackets = 0;

            // for each packet we receive a PacketStatus and ElementStatus from producer and consumer
            for(auto i = 0u; i < (NUM_PACKETS * 2 * 2); i++) {
                 auto sciErr = NvSciStreamBlockEventQuery(pStream->staticPool, STREAMING_TIMEOUT, &event);
                 CHK_NVSCISTATUS_AND_RETURN(sciErr, "Pool Packet/ElementStatus NvSciStreamBlockEventQuery");

                 switch(event.type) {
                 case NvSciStreamEventType_PacketStatusProducer:
                     producerPackets++;
                     break;
                 case NvSciStreamEventType_PacketStatusConsumer:
                     consumerPackets++;
                     break;
                 case NvSciStreamEventType_ElementStatusProducer:
                 case NvSciStreamEventType_ElementStatusConsumer:
                     break;
                 default:
                     LOG_ERR("SetupBuffers: pool received unexpected event type\n");
                     return NVSIPL_STATUS_ERROR;
                 }
            }

            if((producerPackets != NUM_PACKETS) || (consumerPackets != NUM_PACKETS)) {
                 LOG_ERR("SetupBuffers: wrong number of packet acceptance events recieved by pool\n");
                 LOG_ERR("              consumer = %d, producer = %d.  Expected %d for both\n",
                                        consumerPackets, producerPackets, NUM_PACKETS);
                 return NVSIPL_STATUS_ERROR;
            }
        }

        // Producer takes initial ownership of packets
        for (ProducerStream *pStream : m_usedStreams) {
            for (uint32_t i = 0u; i < NUM_PACKETS; i++) {
                NvSciError sciErr = NvSciStreamBlockEventQuery(pStream->producer, STREAMING_TIMEOUT, &event);
                if (sciErr != NvSciError_Success) {
                    LOG_ERR("Failed to get initial ownership of packet\n");
                    return NVSIPL_STATUS_ERROR;
                }
                if (event.type != NvSciStreamEventType_PacketReady) {
                    LOG_ERR("Didn't receive expected PacketReady event\n");
                    return NVSIPL_STATUS_ERROR;
                }
                sciErr = NvSciStreamProducerPacketGet(pStream->producer, &cookie, &fence);
                if (sciErr != NvSciError_Success) {
                    LOG_ERR("NvSciStreamProducerPacketGet failed\n");
                    return NVSIPL_STATUS_ERROR;
                }
            }
        }

        return NVSIPL_STATUS_OK;
    }

 private:
    void ThreadFunc(ProducerStream *pStream)
    {
        NvSciStreamCookie cookie;
        NvSciSyncFence fence = NvSciSyncFenceInitializer;
        NvSciStreamEvent event;
        INvSIPLClient::INvSIPLNvMBuffer *pBuffer = nullptr;

        pthread_setname_np(pthread_self(), "NvSciStreamProducerPacketGet");

        while (m_bRunning || (pStream->numBuffersWithConsumer > 0U)) {
            pBuffer = nullptr;
            NvSciError sciErr = NvSciStreamBlockEventQuery(pStream->producer,
                                                           STREAMING_TIMEOUT,
                                                           &event);
            if (sciErr == NvSciError_Timeout) {
                continue;
            }
            if (sciErr != NvSciError_Success) {
                LOG_ERR("Failed to get returned packet\n");
                m_bRunning = false;
                return;
            }
            if (event.type != NvSciStreamEventType_PacketReady) {
                LOG_ERR("Didn't receive expected PacketReady event\n");
                m_bRunning = false;
                return;
            }
            sciErr = NvSciStreamProducerPacketGet(pStream->producer, &cookie, &fence);
            if (sciErr != NvSciError_Success) {
                LOG_ERR("NvSciStreamProducerPacketGet failed\n");
                m_bRunning = false;
                return;
            }

            pBuffer = pStream->bufferInfo[cookie - 1U].buffer;
            pStream->numBuffersWithConsumer--;

            if (pBuffer == nullptr) {
                LOG_ERR("Couldn't match cookie to buffer\n");
                m_bRunning = false;
                return;
            }

            const SIPLStatus status = pBuffer->AddNvSciSyncPrefence(fence);
            if (status != NVSIPL_STATUS_OK) {
                LOG_ERR("AddNvSciSyncPrefence failed\n");
                m_bRunning = false;
                return;
            }
            NvSciSyncFenceClear(&fence);

            pBuffer->Release();
        }
    }

    NvSciSyncModule m_sciSyncModule {NULL};
    ProducerStream m_streams[MAX_SENSORS][MAX_OUTPUTS_PER_SENSOR];
    std::vector<ProducerStream*> m_usedStreams;
    std::atomic<bool> m_bRunning; // Flag indicating if producer threads are running
};

#endif //CNVSIPLMASTERNVSCI_HPP
