#include "StepOneContainerEngine.h"
#include <opencv2/opencv.hpp>
#include "myutils.h"
#include "myqueue.h"

using namespace ai_matrix;

StepOneContainerEngine::StepOneContainerEngine() {}

StepOneContainerEngine::~StepOneContainerEngine() {}

APP_ERROR StepOneContainerEngine::Init()
{
    bUseEngine_ = MyUtils::getins()->ChkIsHaveTarget("CONTAINER");
    if (!bUseEngine_)
    {
        LogWarn << "engineId_:" << engineId_ << " not use engine";
        return APP_ERR_OK;
    }

    strPort0_ = engineName_ + "_" + std::to_string(engineId_) + "_0";
    modelConfig_ = MyYaml::GetIns()->GetModelConfig("StepOneContainerEngine");

    //读取模型信息
    APP_ERROR ret = ReadModelInfo();
    if (ret != APP_ERR_OK)
    {
        LogError << "Failed to read model info, ret = " << ret;
        return ret;
    }
    ret = InitModel();
    if (ret != APP_ERR_OK)
    {
        LogError << "Failed to read model info, ret = " << ret;
        return ret;
    }

    LogInfo << "AclStepOneContainerEngine Init ok";
    return APP_ERR_OK;
}

APP_ERROR StepOneContainerEngine::InitModel()
{
    modelinfo.yolov5ClearityModelParam.uiClassNum = class_num;
    modelinfo.yolov5ClearityModelParam.uiClearNum = clear_num;
    modelinfo.yolov5ClearityModelParam.uiDetSize = det_size;
    modelinfo.yolov5ClearityModelParam.fScoreThreshold = score_threshold;
    modelinfo.yolov5ClearityModelParam.fNmsThreshold = nms_threshold;

    modelinfo.modelCommonInfo.uiModelWidth = model_width;
    modelinfo.modelCommonInfo.uiModelHeight = model_height;
    modelinfo.modelCommonInfo.uiInputSize = input_size;
    modelinfo.modelCommonInfo.uiOutputSize = output_size;
    modelinfo.modelCommonInfo.uiChannel = INPUT_CHANNEL;
    modelinfo.modelCommonInfo.uiBatchSize = batch_size;
    modelinfo.modelCommonInfo.strInputBlobName = INPUT_BLOB_NAME;
    modelinfo.modelCommonInfo.strOutputBlobName = OUTPUT_BLOB_NAME;

    string strModelName = "";

    int nRet = yolov5model.YoloV5ClearityInferenceInit(&modelinfo, strModelName, modelConfig_.strOmPath);
    if (nRet != 0)
    {
        LogInfo << "YoloV5ClassifyInferenceInit nRet:" << nRet;
        return APP_ERR_COMM_READ_FAIL;
    }
    return APP_ERR_OK;
}

APP_ERROR StepOneContainerEngine::ReadModelInfo()
{
    char szAbsPath[PATH_MAX];
    // Get the absolute path of model file
    if (realpath(modelConfig_.strOmPath.c_str(), szAbsPath) == nullptr)
    {
        LogError << "Failed to get the real path of " << modelConfig_.strOmPath.c_str();
        return APP_ERR_COMM_NO_EXIST;
    }

    // Check the validity of model path
    int iFolderExist = access(szAbsPath, R_OK);
    if (iFolderExist == -1)
    {
        LogError << "ModelPath " << szAbsPath << " doesn't exist or read failed!";
        return APP_ERR_COMM_NO_EXIST;
    }

    //读取模型参数信息文件
    Json::Value jvModelInfo;
    if (!MyUtils::getins()->ReadJsonInfo(jvModelInfo, modelConfig_.strModelInfoPath))
    {
        LogError << "ModelInfoPath:" << modelConfig_.strModelInfoPath << " doesn't exist or read failed!";
        return APP_ERR_COMM_NO_EXIST;
    }

    model_width = jvModelInfo["model_width"].asInt();
    model_height = jvModelInfo["model_height"].asInt();
    clear_num = jvModelInfo["clear"].isArray() ? jvModelInfo["clear"].size() : 0;
    class_num = jvModelInfo["class"].isArray() ? jvModelInfo["class"].size() : 0;
    input_size = GET_INPUT_SIZE(model_width, model_height);
    output_size = GET_OUTPUT_SIZE(model_width, model_height, clear_num, class_num);
    det_size = clear_num + class_num + 5;
    score_threshold = modelConfig_.fScoreThreshold;
    nms_threshold = modelConfig_.fNMSTreshold;

    return APP_ERR_OK;
}

APP_ERROR StepOneContainerEngine::DeInit()
{
    if (!bUseEngine_)
    {
        LogWarn << "engineId_:" << engineId_ << " not use engine";
        return APP_ERR_OK;
    }

    yolov5model.YoloV5ClearityInferenceDeinit();
    LogInfo << "StepOneContainerEngine DeInit ok";
    return APP_ERR_OK;
}

/**
* push数据到队列，队列满时则休眠一段时间再push。
* inParam : const std::string strPort                           push的端口
          : const std::shared_ptr<ProcessData> &pProcessData    push的数据
* outParam: N/A
* return  : N/A
*/
void StepOneContainerEngine::PushData(const std::string &strPort, const std::shared_ptr<ProcessData> &pProcessData)
{
    while (true)
    {
        int iRet = outputQueMap_[strPort]->push(std::static_pointer_cast<void>(pProcessData));
        if (iRet != 0)
        {
            LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " push fail iRet:" << iRet;
            if (iRet == 2)
            {
                usleep(10000); // 10ms
                continue;
            }
        }
        break;
    }
}

/**
* 过滤无效信息
* inParam : std::vector<stDetection> &vecRet            :识别结果数据
          : std::shared_ptr<ProcessData> pProcessData   :帧信息数据
* outParam: N/A
* return  : N/A
*/
void StepOneContainerEngine::FilterInvalidInfo(std::vector<stDetection> &vecRet, std::shared_ptr<ProcessData> &pProcessData)
{
    ai_matrix::DataSourceConfig dataSourceCfg = MyYaml::GetIns()->GetDataSourceConfigById(pProcessData->iDataSource);
    for (auto it = vecRet.begin(); it != vecRet.end();)
    {
        LogDebug << "sourceid:" << pProcessData->iDataSource << " frameId:" << pProcessData->iFrameId
                 << " bigclassid:" << it->class_id << " ltx:" << it->bbox[0] << " lty:" << it->bbox[1]
                 << " rbx:" << it->bbox[2] << " rby:" << it->bbox[3];

        if (!(it->bbox[0] >= dataSourceCfg.fIdentifyAreasLTX &&
              it->bbox[1] >= dataSourceCfg.fIdentifyAreasLTY &&
              it->bbox[2] <= dataSourceCfg.fIdentifyAreasRBX &&
              it->bbox[3] <= dataSourceCfg.fIdentifyAreasRBY))
        {
            LogWarn << "sourceid:" << pProcessData->iDataSource << " frameId:" << pProcessData->iFrameId
                    << " bigclassid:" << it->class_id << " container invalid areas";
            it = vecRet.erase(it);
            continue;
        }
        ++it;
    }
}

APP_ERROR StepOneContainerEngine::Process()
{
    if (!bUseEngine_)
    {
        LogWarn << "engineId_:" << engineId_ << " not use engine";
        return APP_ERR_OK;
    }
    int iRet = APP_ERR_OK;

    while (!isStop_)
    {
        std::shared_ptr<void> pVoidData0 = nullptr;
        inputQueMap_[strPort0_]->pop(pVoidData0);
        if (nullptr == pVoidData0)
        {
            usleep(1000); //1ms
            continue;
        }

        std::shared_ptr<ProcessData> pProcessData = std::static_pointer_cast<ProcessData>(pVoidData0);
        //组织输出数据
        std::shared_ptr<PostData> pPostData = std::make_shared<PostData>();
        pPostData->iModelType = MODELTYPE_CONTAINER;

        //获取图片
        if (pProcessData->iStatus == TRAINSTATUS_RUN || pProcessData->bIsEnd)
        {
            if (pProcessData->pData != nullptr && pProcessData->iSize != 0)
            {            
                cv::Mat img(pProcessData->iHeight, pProcessData->iWidth, CV_8UC3, static_cast<uint8_t *>(pProcessData->pData.get())); //RGB

                //进行推理
                std::vector<stDetection> res;
                auto start = std::chrono::system_clock::now(); // 计时开始
                yolov5model.YoloV5ClearityInferenceModel(img, res);
                auto end = std::chrono::system_clock::now();
                LogInfo << "cont1 inference time: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << "ms";

                //过滤无效信息
                FilterInvalidInfo(res, pProcessData);

                //整理推理结果
                //根据非极大值抑制的结果标注相关信息(画框,文字信息等)
                //res.size()为每张图片上的识别到的对象数目
                for (size_t j = 0; j < res.size(); j++)
                {
                    /*
                    非集装箱大框不处理
                    【0:集装箱; 1:反向集装箱; 2:顶部敞车间隔; 3:顶部平车间隔; 4:顶部罐车间隔; 5:顶部棚车间隔; 6:顶部混合车间隔】
                    */
                    if (res[j].class_id < 0 || res[j].class_id > 6)
                    {
                        continue;
                    }

                    SingleData singledata;
                    // singledata.iLine = -1;
                    singledata.iClassId = res[j].class_id;
                    singledata.fScore = res[j].class_conf;
                    // singledata.iAnchorId = -1;
                    singledata.fLTX = res[j].bbox[0];
                    singledata.fLTY = res[j].bbox[1];
                    singledata.fRBX = res[j].bbox[2];
                    singledata.fRBY = res[j].bbox[3];
                    singledata.fClear = res[j].clear_id;

                    PostSubData postSubData;
                    postSubData.iBigClassId = res[j].class_id;
                    postSubData.vecSingleData.emplace_back(singledata);
                    postSubData.step1Location.fLTX = res[j].bbox[0];
                    postSubData.step1Location.fLTY = res[j].bbox[1];
                    postSubData.step1Location.fRBX = res[j].bbox[2];
                    postSubData.step1Location.fRBY = res[j].bbox[3];

                    if (postSubData.iBigClassId == 0 || postSubData.iBigClassId == 1)
                    {
                        postSubData.iTargetType = CONTAINER;
                    }
                    else if (postSubData.iBigClassId >= 2 && postSubData.iBigClassId <= 6)
                    {
                        postSubData.iTargetType = TOPSPACE;
                    }
                    pPostData->vecPostSubData.emplace_back(postSubData);

                    LogDebug << "sourceid:" << pProcessData->iDataSource << " frameId:" << pProcessData->iFrameId
                             << " --iClassId:" << singledata.iClassId << " iLine:" << singledata.iLine << " confidence=" << singledata.fScore
                             << " lx=" << singledata.fLTX << " ly=" << singledata.fLTY << " rx=" << singledata.fRBX << " ry=" << singledata.fRBY
                             << " clear:" << singledata.fClear;
                }
            }
        }

        //及时释放内存
        if (pProcessData->pData != nullptr)
        {
            pProcessData->pData = nullptr;
            pProcessData->iSize = 0;
        }

        //push端口0，第1步推理
        pProcessData->pVoidData = std::static_pointer_cast<void>(pPostData);
        PushData(strPort0_, pProcessData);
    }
    return APP_ERR_OK;
}