/*
 * 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 <mutex>

#include "NvSIPLCamera.hpp" // Camera
#include "NvSIPLPipelineMgr.hpp" // Pipeline manager
#include "CUtils.hpp"

#include "nvscibuf.h"
#include "nvmedia_image_nvscibuf.h"

extern "C" {
#include "nveeprom.h"
}

#ifndef CNVSIPLMASTER_HPP
#define CNVSIPLMASTER_HPP

#define NUM_PACKETS 6U
#define MAX_PARAM_SIZE (sizeof(float)*4*10*10)

using namespace std;
using namespace nvsipl;

/** NvSIPL Master class */
class CNvSIPLMaster
{
 protected:
    typedef struct {
        INvSIPLClient::INvSIPLNvMBuffer *buffer = nullptr;
        NvSciStreamPacket packet;
        NvSciStreamCookie cookie;
    } BufferInfo;
    typedef struct {
        uint32_t uSensor;
        INvSIPLClient::ConsumerDesc::OutputType outputType;
        bool isSimulatorMode;
        NvSciStreamBlock staticPool;
        NvSciStreamBlock producer;
        NvSciStreamBlock queue;
        NvSciStreamBlock downstream;
        NvSciSyncObj producerSyncObj;
        NvSciSyncObj objFromConsumer;
        NvSciBufAttrList bufAttrList;
        NvSciSyncAttrList signalerAttrList;
        NvSciSyncAttrList waiterAttrList;
        std::vector<NvSciBufObj> sciBufObjs;            // one per packet
        std::vector<NvMediaImageGroup*> imageGroupList; // one per packet (ICP only)
        std::vector<NvMediaImage*> imageList;           // one per packet
        std::thread producerThread;
        BufferInfo bufferInfo[NUM_PACKETS];
        std::atomic<std::uint32_t> numBuffersWithConsumer;
    } ProducerStream;

 public:
    virtual SIPLStatus Setup(PlatformCfg* pPlatformCfg,
                             NvSciBufModule *bufModule,
                             NvSciSyncModule *syncModule)
    {
        if(CLogger::GetInstance().GetLogLevel() >= CLogger::LEVEL_INFORMATION) {
            // Display platform configuration
            DisplayPlatformCfg(pPlatformCfg);
        }

        // Camera Master setup
        m_pCamera = INvSIPLCamera::GetInstance();
        if (m_pCamera == nullptr) {
            LOG_ERR("INvSIPLCamera::GetInstance() failed!\n");
            return NVSIPL_STATUS_ERROR;
        }

        auto status = m_pCamera->SetPlatformCfg(pPlatformCfg);
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("INvSIPLCamera::SetPlatformCfg() failed! %x\n", status);
            return status;
        }
        m_platCfg = *pPlatformCfg;

        NvSciError err = NvSciBufModuleOpen(&m_sciBufModule);
        if (err != NvSciError_Success) {
            cout << "NvSciBufModuleOpen failed\n";
            return NVSIPL_STATUS_ERROR;
        }

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

        return status;
    }

    SIPLStatus SetPipelineConfig(uint32_t uIndex, NvSIPLPipelineCfg &pipelineCfg)
    {
        return m_pCamera->SetPipelineCfg(uIndex, pipelineCfg);
    }

    SIPLStatus RegisterAutoControl(uint32_t uIndex, PluginType type, ISiplControlAuto* customPlugin, std::vector<std::uint8_t>& blob)
    {
        return m_pCamera->RegisterAutoControlPlugin(uIndex, type, customPlugin, blob);
    }

    SIPLStatus ToggleLED(uint32_t uIndex, bool enable)
    {
        return m_pCamera->ToggleLED(uIndex, enable);
    }

    SIPLStatus Init()
    {
        auto status = m_pCamera->Init();
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("NvSIPLCamera Init failed\n");
            return status;
        }

        m_upDevice.reset(NvMediaDeviceCreate());
        if (m_upDevice == NULL) {
            LOG_ERR("NvMediaDeviceCreate failed\n");
            return NVSIPL_STATUS_OUT_OF_MEMORY;
        }

        return NVSIPL_STATUS_OK;
    }

    virtual SIPLStatus Start()
    {
        return m_pCamera->Start();
    }

    virtual SIPLStatus Stop()
    {
        return m_pCamera->Stop();
    }

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

        for (auto& imageGrps : m_imageGroupList) {
            DeleteImageGroups(imageGrps);
        }
        for (auto& images : m_imagesList) {
            DeleteImages(images);
        }

        for (auto& sciBuf : m_sciBufObjs) {
            NvSciBufObjFree(sciBuf);
        }

        NvMediaImageNvSciBufDeinit();

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

    SIPLStatus RecoverLink(std::uint32_t index)
    {
        return m_pCamera->RecoverLink(index);
    }

    //! Allocates imagegroups to be used with ICP
    SIPLStatus AllocateImageGroups(NvMediaDevice* pDevice,
                                   NvMediaSurfaceType surfType,
                                   std::vector<NvMediaSurfAllocAttr> allocAttr,
                                   std::uint32_t numPlanes,
                                   std::vector<NvMediaImageGroup*>& imageGroups)
    {
        auto bufferPoolSize = 6U;

        NvSciBufAttrList reconciledAttrlist;
        NvSciBufAttrList unreconciledAttrlist = NULL;
        NvSciBufAttrList conflictlist = NULL;

        NvSciError err = NvSciBufAttrListCreate(m_sciBufModule, &unreconciledAttrlist);
        CHK_NVSCISTATUS_AND_RETURN(err, "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)};

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

        auto nvmStatus = NvMediaImageFillNvSciBufAttrs(pDevice,
                                                       surfType,
                                                       allocAttr.data(),
                                                       allocAttr.size(),
                                                       0,
                                                       unreconciledAttrlist);
        CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageFillNvSciBufAttrs");

        err = NvSciBufAttrListReconcile(&unreconciledAttrlist, 1, &reconciledAttrlist, &conflictlist);
        CHK_NVSCISTATUS_AND_RETURN(err, "NvSciBufAttrListReconcile");

        for (auto i = 0u; i < bufferPoolSize; i++) {
            // Allocate image group
            auto imgGrp =  new (std::nothrow) NvMediaImageGroup;
            if (imgGrp == nullptr) {
                LOG_ERR("Failed to allocate image Group\n");
                return NVSIPL_STATUS_OUT_OF_MEMORY;
            }

            imgGrp->numImages = numPlanes;
            for (auto p = 0U; p < numPlanes; p++) {
                NvSciBufObj imageGrpSciBufObj;
                err = NvSciBufObjAlloc(reconciledAttrlist, &imageGrpSciBufObj);
                CHK_NVSCISTATUS_AND_RETURN(err, "NvSciBufObjAlloc");

                NvMediaImageCreateFromNvSciBuf(pDevice,
                                               imageGrpSciBufObj,
                                               &imgGrp->imageList[p]);
                if (imgGrp->imageList[p] == NULL) {
                    LOG_ERR("NvMediaImageCreateFromNvSciBuf failed\n");
                    return NVSIPL_STATUS_OUT_OF_MEMORY;
                }

                m_sciBufObjs.push_back(imageGrpSciBufObj);
            }
            imageGroups.push_back(imgGrp);
        }

        if (conflictlist != NULL) {
            NvSciBufAttrListFree(conflictlist);
        }

        if (unreconciledAttrlist != NULL) {
            NvSciBufAttrListFree(unreconciledAttrlist);
        }

        if (reconciledAttrlist != NULL) {
            NvSciBufAttrListFree(reconciledAttrlist);
        }

        return NVSIPL_STATUS_OK;
    }
    //! Deletes imagegroups
    SIPLStatus DeleteImageGroups(std::vector<NvMediaImageGroup*>& imageGroups)
    {
        auto bufferPoolSize = imageGroups.size();
        for (auto i = 0u; i < bufferPoolSize; i++) {
            if (imageGroups[i] == NULL) {
                LOG_ERR("Attempt to destroy null image group\n");
                continue;
            }
            for (auto p = 0u; p < imageGroups[i]->numImages; p++) {
                if (imageGroups[i]->imageList[p] == NULL) {
                    LOG_ERR("Attempt to destroy null image\n");
                    continue;
                }
                NvMediaImageDestroy(imageGroups[i]->imageList[p]);
            }
            delete imageGroups[i];
        }
        return NVSIPL_STATUS_OK;
    }

    //! Allocates images to be used with ISP
    SIPLStatus AllocateImages(NvMediaDevice* pDevice,
                              NvMediaSurfaceType surfType,
                              std::vector<NvMediaSurfAllocAttr> allocAttr,
                              std::vector<NvMediaImage*>& images)
    {
        auto bufferPoolSize = 4U;
        images.resize(bufferPoolSize);

        NvSciBufAttrList reconciledAttrlist;
        NvSciBufAttrList unreconciledAttrlist = NULL;
        NvSciBufAttrList conflictlist = NULL;

        NvSciError err = NvSciBufAttrListCreate(m_sciBufModule, &unreconciledAttrlist);
        CHK_NVSCISTATUS_AND_RETURN(err, "NvSciBufAttrListCreate");

        NvSciBufAttrValAccessPerm accessPerm = NvSciBufAccessPerm_Readonly;
        NvSciBufAttrKeyValuePair attrKvp = { NvSciBufGeneralAttrKey_RequiredPerm,
                                             &accessPerm,
                                             sizeof(accessPerm)};

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

        auto nvmStatus = NvMediaImageFillNvSciBufAttrs(pDevice,
                                                       surfType,
                                                       allocAttr.data(),
                                                       allocAttr.size(),
                                                       0,
                                                       unreconciledAttrlist);
        CHK_NVMSTATUS_AND_RETURN(nvmStatus, "NvMediaImageFillNvSciBufAttrs");

        err = NvSciBufAttrListReconcile(&unreconciledAttrlist, 1, &reconciledAttrlist, &conflictlist);
        CHK_NVSCISTATUS_AND_RETURN(err, "NvSciBufAttrListReconcile");

        for (auto i = 0u; i < bufferPoolSize; i++) {
            NvSciBufObj imageGrpSciBufObj;
            err = NvSciBufObjAlloc(reconciledAttrlist, &imageGrpSciBufObj);
            CHK_NVSCISTATUS_AND_RETURN(err, "NvSciBufObjAlloc");

            NvMediaImageCreateFromNvSciBuf(pDevice,
                                           imageGrpSciBufObj,
                                           &images[i]);
            if (images[i] == NULL) {
                LOG_ERR("NvMediaImageCreateFromNvSciBuf failed\n");
                return NVSIPL_STATUS_OUT_OF_MEMORY;
            }

            m_sciBufObjs.push_back(imageGrpSciBufObj);
        }

        if (conflictlist != NULL) {
            NvSciBufAttrListFree(conflictlist);
        }

        if (unreconciledAttrlist != NULL) {
            NvSciBufAttrListFree(unreconciledAttrlist);
        }

        if (reconciledAttrlist != NULL) {
            NvSciBufAttrListFree(reconciledAttrlist);
        }

        return NVSIPL_STATUS_OK;
    }

    //! Allocates images to be used with ISP
    SIPLStatus DeleteImages(std::vector<NvMediaImage*>& images)
    {
        auto bufferPoolSize = images.size();
        for (auto i = 0u; i < bufferPoolSize; i++) {
            if (images[i] == NULL) {
                LOG_ERR("Attempt to destroy null image\n");
                continue;
            }
            NvMediaImageDestroy(images[i]);
        }
        return NVSIPL_STATUS_OK;
    }

    SIPLStatus ShowEEPROM(uint32_t pip)
    {
        SIPLStatus status = NVSIPL_STATUS_OK;
        std::uint16_t addr = 0x4a;
        std::uint32_t length = 4;
        std::uint8_t buf[length];

        memset(buf, 0, sizeof(buf));

        LOG_MSG("Performing example EEPROM register access\n");
        LOG_MSG("Reading %u byte(s) at 0x%X\n", length, addr);

        status = m_pCamera->ReadEEPROMData(pip, addr, length, buf);
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("Failed to read EEPROM data via SIPL API");
            goto done;
        }

        LOG_MSG("Read 0x%X 0x%X 0x%X 0x%X\n",
            buf[0], buf[1], buf[2], buf[3]);

    done:
        return status;
    }

    SIPLStatus AllocateAndRegisterBuffers(std::uint32_t pip,
                                          bool isp0Enabled,
                                          bool isp1Enabled)
    {
        // Get attributes
        auto pDevice = m_upDevice.get();
        NvSIPLImageAttr attr;
        auto status = m_pCamera->GetImageAttributes(pip, INvSIPLClient::ConsumerDesc::OutputType::ICP, attr);
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("GetImageAttributes failed\n");
            return status;
        }
        std::vector<NvMediaImageGroup*> imageGroups;
        status = AllocateImageGroups(pDevice, attr.surfaceType, attr.surfaceAllocAttr, 1U, imageGroups);
        if (status != NVSIPL_STATUS_OK) {
            LOG_ERR("AllocateImageGroups failed\n");
            return status;
        }
        status = m_pCamera->RegisterImageGroups(pip, imageGroups);
        if (status != NVSIPL_STATUS_OK) {
            LOG_WARN("RegisterImageGroups failed\n");
            DeleteImageGroups(imageGroups);
            return status;
        }
        m_imageGroupList.push_back(imageGroups);

        if (isp0Enabled) {
            // Get attributes
            NvSIPLImageAttr attr;
            status = m_pCamera->GetImageAttributes(pip, INvSIPLClient::ConsumerDesc::OutputType::ISP0, attr);
            if (status != NVSIPL_STATUS_OK) {
                LOG_ERR("GetImageAttributes failed\n");
                return status;
            }
            std::vector<NvMediaImage*> images;
            status = AllocateImages(pDevice, attr.surfaceType, attr.surfaceAllocAttr, images);
            if (status != NVSIPL_STATUS_OK) {
                LOG_ERR("AllocateImages failed\n");
                return status;
            }
            status = m_pCamera->RegisterImages(pip, INvSIPLClient::ConsumerDesc::OutputType::ISP0, images);
            if (status != NVSIPL_STATUS_OK) {
                LOG_WARN("RegisterImageGroups failed\n");
                DeleteImages(images);
                return status;
            }
            m_imagesList.push_back(images);
        }

        if (isp1Enabled) {
            // Get attributes
            NvSIPLImageAttr attr;
            status = m_pCamera->GetImageAttributes(pip, INvSIPLClient::ConsumerDesc::OutputType::ISP1, attr);
            if (status != NVSIPL_STATUS_OK) {
                LOG_ERR("GetImageAttributes failed\n");
                return status;
            }
            std::vector<NvMediaImage*> images;
            status = AllocateImages(pDevice, attr.surfaceType, attr.surfaceAllocAttr, images);
            if (status != NVSIPL_STATUS_OK) {
                LOG_ERR("AllocateImageGroups failed\n");
                return status;
            }
            status = m_pCamera->RegisterImages(pip, INvSIPLClient::ConsumerDesc::OutputType::ISP1, images);
            if (status != NVSIPL_STATUS_OK) {
                LOG_WARN("RegisterImageGroups failed\n");
                DeleteImages(images);
                return status;
            }
            m_imagesList.push_back(images);
        }

        return NVSIPL_STATUS_OK;
    }

    static void DisplayPlatformCfg(const PlatformCfg *pPlatformCfg)
    {
        cout << "Platform: " << pPlatformCfg->platform << endl;
        cout << "Platform configuration: " << pPlatformCfg->platformConfig << endl;
        cout << "Platform configuration description: " << pPlatformCfg->description << endl;
        cout << endl;
        for (auto d = 0u; d != pPlatformCfg->numDeviceBlocks; d++) {
            auto db = pPlatformCfg->deviceBlockList[d];
            if (db.isSlaveModeEnabled)
                cout << "\tSlaveMode Enabled\n";
            if (db.isSimulatorModeEnabled)
                cout << "\tSimulator Mode Enabled\n";
            cout << "\tCSI Port: " << db.csiPort << endl;
            cout << "\tI2C Device " << db.i2cDevice << endl;
            auto deser = db.deserInfo;
            cout << "\tDeserializer Name:" << deser.name << endl;
            cout << "\t             Description: " << deser.description << endl;
            cout << "\t             I2C Address: " << showbase << hex << (int)deser.i2cAddress << dec << endl;
            for (auto m=0u; m != db.numCameraModules; m++) {
                auto mod = db.cameraModuleInfoList[m];
                auto serInfo = mod.serInfo;
                cout << "\tCameraModule on link " << mod.linkIndex << endl;
                cout << "\t             Name: " << mod.name << endl;
                cout << "\t             Description: " << mod.description << endl;
                cout << "\t\tSerializer Name: " << serInfo.name << endl;
                cout << "\t\t           Address: " << showbase << hex << (int)serInfo.i2cAddress << dec << endl;
                auto sensor = mod.sensorInfo;
                cout << "\t\tSensor Name: " << sensor.name << endl;
                cout << "\t\t       Description: " << sensor.description << endl;
                cout << "\t\t       ID: " << sensor.id << endl;
                cout << "\t\t       Address: " << showbase << hex << (int)sensor.i2cAddress << dec << endl;
                if (mod.isEEPROMSupported) {
                    auto e2p = mod.eepromInfo;
                    cout << "\t\tEEPROM Name: " << e2p.name << endl;
                    cout << "\t\t       Address: " << showbase << hex << (int)e2p.i2cAddress << dec << endl;
                }
                cout << endl;
            } // cameraModule
            cout << endl;
        } // DeviceBlock
        cout << endl;
    }

    virtual SIPLStatus AllocateNvSciBuffers(ProducerStream *pStream)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus AllocateBuffers(std::uint32_t uSensor, bool isp0Enabled, bool isp1Enabled)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus RegisterImageGroups(ProducerStream *pStream)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus RegisterImages(ProducerStream *pStream)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus RegisterBuffers(std::uint32_t pip, bool isp0Enabled, bool isp1Enabled)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus RegisterSource(std::uint32_t uSensor,
                                      INvSIPLClient::ConsumerDesc::OutputType outputType,
                                      bool isSimulatorMode,
                                      bool streamingEnabled,
                                      NvSciStreamBlock *consumer,
                                      NvSciStreamBlock *consumerUpstream,
                                      NvSciStreamBlock *queue)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus Post(uint32_t uSensor,
                            INvSIPLClient::ConsumerDesc::OutputType outputType,
                            INvSIPLClient::INvSIPLNvMBuffer *pBuffer)
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus SetupStreaming()
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

    virtual SIPLStatus SetupBuffers()
    {
        return NVSIPL_STATUS_NOT_SUPPORTED;
    }

 protected:
    unique_ptr<INvSIPLCamera> m_pCamera;
    std::unique_ptr<NvMediaDevice, CloseNvMediaDevice> m_upDevice;
    NvSciBufModule m_sciBufModule {NULL};
    PlatformCfg m_platCfg;

 private:
    std::vector<std::vector<NvMediaImageGroup*>> m_imageGroupList;
    std::vector<std::vector<NvMediaImage*>> m_imagesList;
    std::vector<NvSciBufObj> m_sciBufObjs;
};

#endif //CNVSIPLMASTER_HPP
