#include "FilterContainerStepOneEngine.h"
#include "myutils.h"

using namespace ai_matrix;

extern std::atomic_uint32_t g_iReRunOrigContainerFid;

FilterContainerStepOneEngine::FilterContainerStepOneEngine() {}

FilterContainerStepOneEngine::~FilterContainerStepOneEngine() {}

APP_ERROR FilterContainerStepOneEngine::Init()
{
    strPort0_ = engineName_ + "_" + std::to_string(engineId_) + "_0";

    //获取几个摄像头识别集装箱
    std::map<int, ai_matrix::DataSourceConfig> mapUseDataSouceCfg = MyYaml::GetIns()->GetUseDataSourceConfig();
    for (auto iter = mapUseDataSouceCfg.begin(); iter != mapUseDataSouceCfg.end(); iter++)
    {
        if (iter->second.strTarget.find("CONTAINER") != std::string::npos)
        {
            LogDebug << "DataSource:" << iter->first << " deal CONTAINER";
            mapProcessDataPre_[iter->first] = nullptr;
            mapContainerContCnt_[iter->first] = 0;
            mapTopSpaceContCnt_[iter->first] = 0;
            mapDataSourceIsEnd_[iter->first] = false;
        }
    }

    InitParam();
    LogInfo << "FilterContainerStepOneEngine Init ok";
    return APP_ERR_OK;
}

APP_ERROR FilterContainerStepOneEngine::DeInit()
{
    LogInfo << "FilterContainerStepOneEngine DeInit ok";
    return APP_ERR_OK;
}

/**
* 参数初始化(列车结束时需调用)
* inParam : N/A
* outParam: N/A
* return  : N/A
*/
void FilterContainerStepOneEngine::InitParam()
{
    for (auto iter = mapProcessDataPre_.begin(); iter != mapProcessDataPre_.end(); iter++)
    {
        iter->second = nullptr;
    }
    for (auto iter = mapContainerContCnt_.begin(); iter != mapContainerContCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapTopSpaceContCnt_.begin(); iter != mapTopSpaceContCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapDataSourceIsEnd_.begin(); iter != mapDataSourceIsEnd_.end(); iter++)
    {
        iter->second = false;
    }
    g_iReRunOrigContainerFid = 0;
}

/**
* push数据到队列，队列满时则休眠一段时间再push
* inParam : const std::string strPort                           push的端口
          : const std::shared_ptr<ProcessData> &pProcessData    push的数据
* outParam: N/A
* return  : N/A
*/
void FilterContainerStepOneEngine::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::shared_ptr<ProcessData> pProcessData   :当前帧数据
* outParam: N/A
* return  : N/A
*/
void FilterContainerStepOneEngine::DealProcessDataPre(std::shared_ptr<ProcessData> pProcessData)
{
    /*
    目标框是否是连续识别，只识别到一帧的目标框认为误识别，过滤掉。
    判断上一帧，当前帧 是否有框
        上一帧有框，当前帧有框，说明连续识别，正常处理。
        上一帧有框，当前帧无框，则非连续识别，过滤大框
        上一帧无框，当前帧有框，则连续识别个数置零。
        上一帧无框，当前帧无框，则连续识别个数置零。
    */
    auto iterProcessData = mapProcessDataPre_.find(pProcessData->iDataSource);
    if (nullptr == iterProcessData->second)
    {
        return;
    }

    auto iterContainerContCnt = mapContainerContCnt_.find(pProcessData->iDataSource);
    auto iterTopSpaceContCnt = mapTopSpaceContCnt_.find(pProcessData->iDataSource);

    //获取当前帧识别情况
    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    for (int i = 0; i < pPostData->vecPostSubData.size(); i++)
    {
        if (pPostData->vecPostSubData[i].iTargetType == CONTAINER)
        {
            iterContainerContCnt->second++;
        }
        else if (pPostData->vecPostSubData[i].iTargetType == TOPSPACE)
        {
            iterTopSpaceContCnt->second++;
        }
    }

    //获取上一帧识别情况
    std::shared_ptr<PostData> pPostDataPre = std::static_pointer_cast<PostData>(iterProcessData->second->pVoidData);
    bool bPreHaveContainer = false;
    bool bPreHaveTopSpace = false;
    for (int iPre = 0; iPre < pPostDataPre->vecPostSubData.size(); iPre++)
    {
        if (pPostDataPre->vecPostSubData[iPre].iTargetType == CONTAINER)
        {
            iterContainerContCnt->second++;
            bPreHaveContainer = true;
        }
        else if (pPostDataPre->vecPostSubData[iPre].iTargetType == TOPSPACE)
        {
            iterTopSpaceContCnt->second++;
            bPreHaveTopSpace = true;
        }
    }

    //前一帧无大框连续识别次数置零
    iterContainerContCnt->second = bPreHaveContainer ? iterContainerContCnt->second : 0;
    iterTopSpaceContCnt->second = bPreHaveTopSpace ? iterTopSpaceContCnt->second : 0;

    //非连续识别的情况，认为误识别，剔除误识别的大框信息
    for (std::vector<PostSubData>::iterator it = pPostDataPre->vecPostSubData.begin(); it != pPostDataPre->vecPostSubData.end();)
    {
        if (iterContainerContCnt->second < 2 && it->iTargetType == CONTAINER)
        {
            LogError << "sourceid:" << iterProcessData->second->iDataSource << " frameId:" << iterProcessData->second->iFrameId << " container wrong";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        if (iterTopSpaceContCnt->second < 2 && it->iTargetType == TOPSPACE)
        {
            LogError << "sourceid:" << iterProcessData->second->iDataSource << " frameId:" << iterProcessData->second->iFrameId << " TopSpace wrong";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        it++;
    }

    //上一帧，push端口0，第一步结果记录
    PushData(strPort0_, iterProcessData->second);
}


APP_ERROR FilterContainerStepOneEngine::Process()
{
    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);

        //停止后再次行驶，把队列中多余的数据丢弃。
        if (g_iReRunOrigContainerFid != 0 && pProcessData->iOrigFrameId > g_iReRunOrigContainerFid && !pProcessData->bIsEnd)
        {
            LogDebug << "reRunOrigContainerFid:" << g_iReRunOrigContainerFid << " origFrameId:" << pProcessData->iOrigFrameId;
            continue;
        }

        if (pProcessData->bIsEnd)
        {
            mapDataSourceIsEnd_[pProcessData->iDataSource] = pProcessData->bIsEnd;
        }

        //根据当前帧数据，处理上一帧数据 (按有多个摄像头处理)
        DealProcessDataPre(pProcessData);
        mapProcessDataPre_[pProcessData->iDataSource] = pProcessData;
        if (pProcessData->bIsEnd)
        {
            //结束帧，push端口0
            PushData(strPort0_, pProcessData);
        }

        // 3. 全部结束，初始化相关参数
        bool bAllEnd = true;
        for (auto iter = mapDataSourceIsEnd_.begin(); iter != mapDataSourceIsEnd_.end(); iter++)
        {
            bAllEnd = bAllEnd && iter->second;
        }
        if (bAllEnd)
        {
            InitParam();
        }
    }
    return APP_ERR_OK;
}
