#include "FilterTrainStepOneEngine.h"
#include "myutils.h"
#include <string>

using namespace ai_matrix;

namespace
{
    //按照x坐标排列
    bool CompareX(const PostSubData &a, const PostSubData &b)
    {
        return a.step1Location.fLTX < b.step1Location.fLTX;
    }
}

extern std::atomic_uint64_t g_i64ReRunTimeStamp;
extern std::atomic_uint32_t g_iReRunOrigFrameId;
extern std::atomic_uint32_t g_iReRunFrameId;
extern std::atomic_uint32_t g_iReRunOrigChkDateFid;
extern std::atomic_uint32_t g_iReRunOrigContainerFid;

FilterTrainStepOneEngine::FilterTrainStepOneEngine() {}

FilterTrainStepOneEngine::~FilterTrainStepOneEngine() {}

APP_ERROR FilterTrainStepOneEngine::Init()
{
    strPort0_ = engineName_ + "_" + std::to_string(engineId_) + "_0";
    strResultPath_ = MyYaml::GetIns()->GetPathValue("gc_result_path");
    iChkStopPX_ = MyYaml::GetIns()->GetIntValue("gc_chkstop_px");
    iChkStopCount_ = MyYaml::GetIns()->GetIntValue("gc_chkstop_count");
    iPartitionFrameNum_ = MyYaml::GetIns()->GetIntValue("partition_frame_span");
    iPlitFrameSpanPX_ = MyYaml::GetIns()->GetIntValue("gc_split_frame_span_px");
    iPushDirection_ = MyYaml::GetIns()->GetIntValue("gc_push_direction");

    //获取主摄像头信息
    mainCfg_ = MyYaml::GetIns()->GetDataSourceConfigById(0);

    std::map<int, ai_matrix::DataSourceConfig> mapUseDataSouceCfg = MyYaml::GetIns()->GetUseDataSourceConfig();
    for (auto iter = mapUseDataSouceCfg.begin(); iter != mapUseDataSouceCfg.end(); iter++)
    {
        this->rightFirst_ = iter->second.iRightFirst;
        this->leftFirst_ = iter->second.iLeftFirst;
        if (iter->second.strTarget.find("NUM") != std::string::npos)
        {
            LogDebug << "DataSource:" << iter->first << " deal NUM";
            mapHeadContinueCnt_[iter->first] = 0;
            mapProContinueCnt_[iter->first] = 0;
            mapNumContinueCnt_[iter->first] = 0;
            mapSpaceContinueCnt_[iter->first] = 0;
            mapTrainSpaceContinueCnt_[iter->first] = 0;
            mapDataSourceIsEnd_[iter->first] = false;

            PostData postDataTemp;
            mapPostDataFrist_[iter->first] = postDataTemp;
            std::map<int, std::vector<Step1Location>> mapStep1InfoTemp; //[key-目标框id, 目标框坐标集合]
            mapMapStep1Info_[iter->first] = mapStep1InfoTemp;
        }
    }

    mapTargetStr_.insert(std::make_pair(NUM, "NUM"));
    mapTargetStr_.insert(std::make_pair(PRO, "PRO"));
    mapTargetStr_.insert(std::make_pair(HEAD, "HEAD"));
    mapTargetStr_.insert(std::make_pair(SPACE, "SPACE"));//SPACE
    mapTargetStr_.insert(std::make_pair(TRAINSPACE, "SPACE"));//SPACE

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

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


/**
* 参数初始化(列车结束时需调用)
* inParam : N/A
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::InitParam()
{
    mapProcessDataPre_.clear();
    for (auto iter = mapHeadContinueCnt_.begin(); iter != mapHeadContinueCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapProContinueCnt_.begin(); iter != mapProContinueCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapNumContinueCnt_.begin(); iter != mapNumContinueCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapSpaceContinueCnt_.begin(); iter != mapSpaceContinueCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapTrainSpaceContinueCnt_.begin(); iter != mapTrainSpaceContinueCnt_.end(); iter++)
    {
        iter->second = 0;
    }
    for (auto iter = mapDataSourceIsEnd_.begin(); iter != mapDataSourceIsEnd_.end(); iter++)
    {
        iter->second = false;
    }
    for (auto iter = mapPostDataFrist_.begin(); iter != mapPostDataFrist_.end(); iter++)
    {
        iter->second.vecPostSubData.clear();
    }
    for (auto iter = mapMapStep1Info_.begin(); iter != mapMapStep1Info_.end(); iter++)
    {
        iter->second.clear();
    }
    iDirection_ = DIRECTION_UNKNOWN;
    iNotChgCount_ = 0;

    while (!stackBackInfo_.empty())
    {
        stackBackInfo_.pop();
    }
    while (!quePostData_.empty())
    {
        quePostData_.pop();
    }
    iTrainStatus_ = TRAINSTATUS_RUN;

    mapCalDirection_.clear();
}

/**
* push数据到队列，队列满时则休眠一段时间再push
* inParam : const std::string strPort                           push的端口
          : const std::shared_ptr<ProcessData> &pProcessData    push的数据
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::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;
    }
}

void FilterTrainStepOneEngine::AddBackInfo(std::shared_ptr<ProcessData> pProcessData)
{
    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    std::sort(pPostData->vecPostSubData.begin(), pPostData->vecPostSubData.end(), CompareX);

    std::string strAllClassType;
    for (size_t i = 0; i < pPostData->vecPostSubData.size(); i++)
    {
        if (strAllClassType.find(mapTargetStr_[pPostData->vecPostSubData[i].iTargetType]) != std::string::npos)
        {
            continue;
        }
        strAllClassType += mapTargetStr_[pPostData->vecPostSubData[i].iTargetType];
    }

    if (strAllClassType.empty())
    {
        return;
    }
    LogDebug << "frameId:" << pProcessData->iFrameId << " addbackinfo strAllClassType:" << strAllClassType;

    TrainBackInfo trainBackInfo;
    trainBackInfo.processData = pProcessData;
    trainBackInfo.strAllClassType = strAllClassType;
    if (stackBackInfo_.empty())
    {
        stackBackInfo_.push(trainBackInfo);
        LogDebug << "frameId:" << pProcessData->iFrameId << " push strAllClassType:" << strAllClassType
                 << " stacksize:" << stackBackInfo_.size();
    }
    else
    {
        TrainBackInfo trainBackInfoTop = stackBackInfo_.top();

        if (trainBackInfoTop.strAllClassType != trainBackInfo.strAllClassType)
        {
            if (((iDirection_ == DIRECTION_RIGHT &&
                  this->rightFirst_ == RIGHT_RUN_AND_PRO_FIRST) ||
                 (iDirection_ == DIRECTION_LEFT &&
                  this->leftFirst_ == LEFT_RUN_AND_PRO_FIRST)) &&
                ((trainBackInfo.strAllClassType == "SPACE" &&
                  (trainBackInfoTop.strAllClassType == "PROSPACE" ||
                   trainBackInfoTop.strAllClassType == "SPACEPRO")) ||
                 (trainBackInfo.strAllClassType == "NUM" &&
                  (trainBackInfoTop.strAllClassType == "NUMSPACE" ||
                   trainBackInfoTop.strAllClassType == "SPACENUM")) ||
                 ((trainBackInfo.strAllClassType == "PROSPACE" ||
                   trainBackInfo.strAllClassType == "SPACEPRO")
                  && trainBackInfoTop.strAllClassType == "PRO")))
            {
                return;
            }

            if (((iDirection_ == DIRECTION_RIGHT &&
                  this->rightFirst_ == RIGHT_RUN_AND_NUM_FIRST) ||
                 (iDirection_ == DIRECTION_LEFT &&
                  this->leftFirst_ == LEFT_RUN_AND_NUM_FIRST)) &&
                ((trainBackInfo.strAllClassType == "SPACE" &&
                  (trainBackInfoTop.strAllClassType == "NUMSPACE" ||
                   trainBackInfoTop.strAllClassType == "SPACENUM")) ||
                 (trainBackInfo.strAllClassType == "PRO" &&
                  (trainBackInfoTop.strAllClassType == "PROSPACE" ||
                   trainBackInfoTop.strAllClassType == "SPACEPRO")) ||
                 ((trainBackInfo.strAllClassType == "NUMSPACE" ||
                   trainBackInfo.strAllClassType == "SPACENUM") &&
                  trainBackInfoTop.strAllClassType == "NUM")))
            {
                return;
            }
            stackBackInfo_.push(trainBackInfo);
            LogDebug << "frameId:" << pProcessData->iFrameId << " push strAllClassType:" << strAllClassType
                     << " stacksize:" << stackBackInfo_.size();
        }
    }
}

bool FilterTrainStepOneEngine::IsEndDealBackInfo(std::shared_ptr<ProcessData> pProcessData)
{
    if (stackBackInfo_.empty())
    {
        return true;
    }

    bool bPopFlag = false;
    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    if (pPostData->vecPostSubData.size() == 0) return false;

    /*
    处理倒车数据时，数据需设置为倒车，主要是保证这样的数据后面Engine不处理，防止切分车厢出错。

    类型不相等时，就pop，当pop后，还剩一个数据时，则表示已经回到了刚开始倒车的地方。（只剩一个数据的逻辑在上方）

    处理最后一个时，不能只判断下类型相同就弹出。需要控制下位置。(要么类型相同位置合适，要么类型不相同)
        正向为向左行驶，则当前数据的位置尽量小于等于栈中最后一个元素的位置。
        正向为向右行驶，则当前数据的位置尽量大于等于栈中最后一个元素的位置。
    */
    std::sort(pPostData->vecPostSubData.begin(), pPostData->vecPostSubData.end(), CompareX);

    std::string strAllClassType;
    for (size_t i = 0; i < pPostData->vecPostSubData.size(); i++)
    {
        if (strAllClassType.find(mapTargetStr_[pPostData->vecPostSubData[i].iTargetType]) != std::string::npos)
        {
            continue;
        }
        strAllClassType += mapTargetStr_[pPostData->vecPostSubData[i].iTargetType];
    }
    if (strAllClassType.empty())
    {
        return false;
    }

    if (stackBackInfo_.size() == 1)
    {
        TrainBackInfo trainBackInfoLast = stackBackInfo_.top();
        std::shared_ptr<PostData> pPostDataBack = std::static_pointer_cast<PostData>(trainBackInfoLast.processData->pVoidData);
        std::sort(pPostDataBack->vecPostSubData.begin(), pPostDataBack->vecPostSubData.end(), CompareX);

        for (size_t i = 0; i < pPostDataBack->vecPostSubData.size(); i++)
        {
            int bFlag = -1;
            for (size_t j = 0; j < pPostData->vecPostSubData.size(); j++)
            {
                if (pPostDataBack->vecPostSubData[i].iBigClassId == pPostData->vecPostSubData[j].iBigClassId)
                {
                    if (pPostData->vecPostSubData[j].step1Location.fLTX < 1 || pPostDataBack->vecPostSubData[i].step1Location.fLTX < 1)
                    {
                        LogDebug << "大框X坐标小于1，判定为异常大框。过滤！！";
                        break;
                    }
                    bFlag = (pPostDataBack->vecPostSubData[i].step1Location.fLTX <= pPostData->vecPostSubData[j].step1Location.fLTX) ? 1 : 0;
                    LogDebug << "帧:" << pProcessData->iFrameId << " 倒车前帧:" << pPostDataBack->iFrameId << " 恢复到原位:" << bFlag
                             << " 当前框位置:" << pPostData->vecPostSubData[i].step1Location.fLTX
                             << " 倒车前位置:" << pPostDataBack->vecPostSubData[i].step1Location.fLTX << "方向：" << iDirection_;
                }
            }

            if ((iDirection_ == DIRECTION_LEFT && bFlag == 0) ||
                (iDirection_ == DIRECTION_RIGHT && bFlag == 1))
            {
                bPopFlag = true;
                break;
            }
        }

        if (bPopFlag)
        {
            LogDebug << "frameId:" << pProcessData->iFrameId << " 恢复倒车前的位置:" << bPopFlag;
            stackBackInfo_.pop();
        }
    }
    else
    {
        TrainBackInfo trainBackInfoTop_bak = stackBackInfo_.top();
        stackBackInfo_.pop();
        TrainBackInfo trainBackInfoTop = stackBackInfo_.top();

        if (trainBackInfoTop.strAllClassType != strAllClassType)
        {
            stackBackInfo_.push(trainBackInfoTop_bak);
            LogDebug << "帧:" << pProcessData->iFrameId << " 倒车信息:" << stackBackInfo_.size()
                     << " 顶部倒车信息:" << trainBackInfoTop.strAllClassType << " 本次识别信息:" << strAllClassType;
        }
        else
        {
//            bPopFlag = true;
            LogDebug << "帧:" << pProcessData->iFrameId << " 倒车信息:" << stackBackInfo_.size()
                     << " 顶部倒车信息:" << trainBackInfoTop.strAllClassType << " 本次识别信息:" << strAllClassType
                     << " 删除倒车信息:" << trainBackInfoTop_bak.strAllClassType;
        }


//        if(bPopFlag)
//        {
//            stackBackInfo_.pop();
//            bPopFlag = false;
//        }
    }

    return stackBackInfo_.empty() ? true : false;
}

/**
* 校验火车是否停止
* inParam : std::shared_ptr<ProcessData> pProcessData   :待处理数据
* outParam: N/A
* return  : true:停止; false:非停止   1(正常行驶)  2(停车)  3(倒车) 
*/
int FilterTrainStepOneEngine::GetTrainStatus(std::shared_ptr<ProcessData> pProcessData)
{
    if (iDirection_ == DIRECTION_UNKNOWN)
    {
        LogDebug << " frameId:" << pProcessData->iFrameId << " 未判断出行车方向，暂定认为火车正常行驶中";
        return TRAINSTATUS_RUN;
    }

    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    pPostData->iFrameId = pProcessData->iFrameId;

    // 1. 无框时，返回之前的列车状态
    if (pPostData->vecPostSubData.size() == 0)
    {
        return iTrainStatus_;
    }

    quePostData_.push(*pPostData.get());

    if (quePostData_.size() < 3)
    {
        return TRAINSTATUS_RUN;
    }

    PostData postDataFront = quePostData_.front();
    //iNotChgCount_大于0表示有可能停车，此时pop队列数据要多留存几个。用最开始的数据来判断是否真正停车，如果每次只用上上帧判断当列车超级慢时可能判断为停车。
    int iSizeTemp = iNotChgCount_ > 0 ? 10 : 2;
    while (quePostData_.size() > iSizeTemp)
    {
        quePostData_.pop();
    }
    // LogDebug << "frameId:" << pProcessData->iFrameId << " 判断运动状态队列 第一帧:" << postDataFront.iFrameId << " 队列size:" << quePostData_.size() << " iSizeTemp:" << iSizeTemp;

    bool bSameFlag = false;
    int iDiffValue = iChkStopPX_;
    for (size_t i = 0; i < pPostData->vecPostSubData.size(); i++)
    {
        PostSubData postSubDataBack = pPostData->vecPostSubData[i];
        for (size_t j = 0; j < postDataFront.vecPostSubData.size(); j++)
        {
            PostSubData postSubDataFront = postDataFront.vecPostSubData[j];
            /*
            使用iBigClassId，可能出现平车只有间隔大框，且间隔大框可以一会是平车间隔，一会是通用间隔。导致类别不一样
            使用iTargetType，可能出现平车只有间隔大框，且间隔大框可以一会是平车间隔，一会是通用间隔。导致像素差判断不准。
            */
            if (postSubDataFront.iTargetType != postSubDataBack.iTargetType)
            {
                LogDebug << "判断前后帧识别的是否一致 上一个：" << postSubDataFront.iTargetType << "  当前：" << postSubDataBack.iTargetType;
                continue;
            }

            // if (postSubDataFront.iTargetType == SPACE && postSubDataFront.iBigClassId != postSubDataBack.iBigClassId)
            // {
            //     iDiffValue = 50;
            // }

            bSameFlag = true;
            int iCenterBack = postSubDataBack.step1Location.fLTX + (postSubDataBack.step1Location.fRBX - postSubDataBack.step1Location.fLTX) / 2;
            int iCenterFront = postSubDataFront.step1Location.fLTX + (postSubDataFront.step1Location.fRBX - postSubDataFront.step1Location.fLTX) / 2;

            //位置比较大于10个像素，则表示有移动。再判断时正向移动，还是倒车
            // LogDebug << "frameId:" << pProcessData->iFrameId << " " << iCenterBack << "-" << iCenterFront
            //          << "=" << abs(iCenterBack - iCenterFront) << " 预期判定移动的差值为iDiffValue:" << iDiffValue;
            if (abs(iCenterBack - iCenterFront) > iDiffValue)
            {
                iNotChgCount_ = 0;
                /*
                iCenterBack > iCenterFront 表示向右行驶，且原方向为向左行驶
                iCenterBack < iCenterFront 表示向左行驶，且原方向为向右行驶
                以上2种表示倒车。
                */
                if ((iCenterBack > iCenterFront && iDirection_ == DIRECTION_LEFT) ||
                    (iCenterBack < iCenterFront && iDirection_ == DIRECTION_RIGHT))
                {
                    if (this->iPartitionFrameNum_ < (pProcessData->iFrameId - postDataFront.iFrameId)
                        && this->iPlitFrameSpanPX_ < abs(iCenterBack - iCenterFront))
                    {
                        return TRAINSTATUS_RUN;
                    }
                    LogDebug << "frameId:" << pProcessData->iFrameId << " 检测到火车倒车";
                    return TRAINSTATUS_BACK;
                }
                else
                {
                    LogDebug << "frameId:" << pProcessData->iFrameId << " 正常行驶";
                    return TRAINSTATUS_RUN;
                }
            }
                /*
                小于10个像素表示可能停车，累计未变化次数。
                    累计变化次数超过10次，返回停车
                    累计变化次数未超过10次，返回之前行驶状态
                */
            else
            {
                iNotChgCount_++;
                LogDebug << " frameId:" << pProcessData->iFrameId
                         << " 大框移动范围小("
                         << abs(iCenterBack - iCenterFront)
                         << ") 判断停车计数:" << iNotChgCount_ << "/" << iChkStopCount_;
                if (iNotChgCount_ > iChkStopCount_)
                {
                    LogDebug << "frameId:" << pProcessData->iFrameId << " 检测到火车停车";
                    return TRAINSTATUS_STOP;
                }
                else
                {
//                    LogDebug << "frameId:" << pProcessData->iFrameId << " iTrainStatus_:" << iTrainStatus_;
                    return iTrainStatus_;
                }
            }
        }
    }

    /*
    未找到相同的框，说明是老框消失掉了，新框出现了。
    按新框出现的位置判断是向左行驶，还是向右行驶。
    */
    LogDebug << "frameId:" << pProcessData->iFrameId << " bSameFlag:" << bSameFlag;
    if (!bSameFlag)
    {
        std::sort(pPostData->vecPostSubData.begin(), pPostData->vecPostSubData.end(), CompareX);
        Step1Location step1Location = pPostData->vecPostSubData.front().step1Location;
        if (iDirection_ == DIRECTION_LEFT)
        {
            step1Location = pPostData->vecPostSubData.back().step1Location;
        }
        LogDebug << "frameId:" << pProcessData->iFrameId << " fLTX:" << step1Location.fLTX << " fRBX:" << step1Location.fRBX;

        iNotChgCount_ = 0;
        int iCenter = step1Location.fLTX + (step1Location.fRBX - step1Location.fLTX) / 2;
        int iValue = pProcessData->iWidth / 2;
        if ((iCenter > iValue && iDirection_ == DIRECTION_RIGHT) ||
            (iCenter < iValue && iDirection_ == DIRECTION_LEFT))
        {
            /*
            针对有效帧较少时，和上上帧比较没有同类型大框，且当前帧已行驶到画面中心导致误判的情况，
            增加和上帧同类型大框的比较处理。
            */
            PostData postDataMiddle = quePostData_.front();
            for (size_t i = 0; i < pPostData->vecPostSubData.size(); i++)
            {
                PostSubData postSubDataBack = pPostData->vecPostSubData[i];
                for (size_t j = 0; j < postDataMiddle.vecPostSubData.size(); j++)
                {
                    PostSubData postSubDataMiddle = postDataMiddle.vecPostSubData[j];
                    if (postSubDataMiddle.iTargetType != postSubDataBack.iTargetType)
                    {
                        continue;
                    }
                    int iCenterBack = postSubDataBack.step1Location.fLTX + (postSubDataBack.step1Location.fRBX - postSubDataBack.step1Location.fLTX) / 2;
                    int iCenterMiddle = postSubDataMiddle.step1Location.fLTX + (postSubDataMiddle.step1Location.fRBX - postSubDataMiddle.step1Location.fLTX) / 2;

                    // 位置比较大于10个像素，则表示有移动。再判断时正向移动，还是倒车
                    LogDebug << "frameId:" << pProcessData->iFrameId << " " << iCenterBack << "-" << iCenterMiddle
                             << "=" << abs(iCenterBack - iCenterMiddle) << " 移动差值 iDiffValue:" << iDiffValue;
                    if (abs(iCenterBack - iCenterMiddle) > iDiffValue)
                    {
                        if ((iCenterBack > iCenterMiddle && iDirection_ == DIRECTION_LEFT) ||
                            (iCenterBack < iCenterMiddle && iDirection_ == DIRECTION_RIGHT))
                        {
                            LogDebug << "frameId:" << pProcessData->iFrameId << " 检测到火车倒车";
                            return TRAINSTATUS_BACK;
                        }
                        else
                        {
                            LogDebug << "frameId:" << pProcessData->iFrameId << " 正常行驶";
                            return TRAINSTATUS_RUN;
                        }
                    }
                }
            }

//            LogDebug << "frameId:" << pProcessData->iFrameId << " back2";
            return iTrainStatus_;
        }
    }
    // LogDebug << "frameId:" << pProcessData->iFrameId << " iNotChgCount_:" << iNotChgCount_ << " run run";
    return TRAINSTATUS_RUN;
}

/**
* 设置行车方向
* inParam : std::vector<Step1Location> &vecLocation     :大框坐标集合
          : std::shared_ptr<ProcessData> pProcessData   :待处理数据
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::SetDirection(std::vector<Step1Location> &vecLocation, std::shared_ptr<ProcessData> pProcessData)
{
    if (vecLocation.size() < 1)
    {
        return;
    }
    Step1Location slFront = vecLocation.front();
    Step1Location slBack = vecLocation.back();
    /*
    移动距离小于50个像素，则判断最后一个框的出现的位置
    */
    LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " slFront:" << slFront.fLTX << " slBack:" << slBack.fLTX;
    if (fabs(slBack.fLTX - slFront.fLTX) < 50)
    {
        float fTempX1 = pProcessData->iWidth / 5;      //5分之1的X坐标
        float fTempX2 = pProcessData->iWidth / 5 * 4;  //5分之4的X坐标

        if (slBack.fLTX < fTempX1)
        {
            iDirection_ = DIRECTION_LEFT;
        }
        else if (slBack.fRBX > fTempX2)
        {
            iDirection_ = DIRECTION_RIGHT;
        }
    }
    else
    {
        //行驶方向 左
        if ((slBack.fLTX - slFront.fLTX) < 0)
        {
            iDirection_ = DIRECTION_LEFT;
        }
            //行驶方向 右
        else if ((slBack.fLTX - slFront.fLTX) > 0)
        {
            iDirection_ = DIRECTION_RIGHT;
        }
    }

    //未判断出移动方向，清除数后重新计算。
    if (iDirection_ == DIRECTION_UNKNOWN)
    {
        auto iterMapStep1Info = mapMapStep1Info_.find(pProcessData->iDataSource);
        iterMapStep1Info->second.clear();
    }
    else
    {
        if (MyYaml::GetIns()->GetDataSourceConfigById(pProcessData->iDataSource).iLeftFirst != mainCfg_.iLeftFirst)
        {
            LogDebug << "before sourceid:" << pProcessData->iDataSource << "frameid:" << pProcessData->iFrameId << " direction:" << iDirection_;
            iDirection_ = (iDirection_ == DIRECTION_LEFT) ? DIRECTION_RIGHT : DIRECTION_LEFT;
            LogDebug << "after sourceid:" << pProcessData->iDataSource << "frameid:" << pProcessData->iFrameId << " direction:" << iDirection_;
        }
    }
}

/**
* 计算行车方向新
* inParam : std::shared_ptr<ProcessData> pProcessData   :待处理数据
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::CalculateDirectionNew(std::shared_ptr<ProcessData> pProcessData)
{
    /*
    连续3帧同目标识别框信息
        判断位置差异是否超过10px(判停车参数)，且两两之间都是线性。如果符合则计算方向。
        上述条件不符合则剔除第一个元素，再次累计连续3帧处理。
    */
    auto iterMap = mapCalDirection_.find(pProcessData->iDataSource);
    if (iterMap == mapCalDirection_.end())
    {
        std::map<int, std::vector<CalculateInfo>> mapTemp;
        mapCalDirection_.insert(std::make_pair(pProcessData->iDataSource, mapTemp));
        iterMap = mapCalDirection_.find(pProcessData->iDataSource);
    }

    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    for (auto iter = pPostData->vecPostSubData.begin();
         iter != pPostData->vecPostSubData.end(); iter++) {
        CalculateInfo calInfo;
        calInfo.iFrameId = pProcessData->iFrameId;
        calInfo.iBigClassId = iter->iBigClassId;
        calInfo.fCenterX = iter->step1Location.fLTX + (iter->step1Location.fRBX - iter->step1Location.fLTX) / 2;
        calInfo.fTargetWidth = iter->step1Location.fRBX - iter->step1Location.fLTX;

        auto iterSubMap = iterMap->second.find(iter->iBigClassId);
        if (iterSubMap == iterMap->second.end())
        {
            std::vector<CalculateInfo> vecTemp;
            iterMap->second.insert(std::make_pair(iter->iBigClassId, vecTemp));
            iterSubMap = iterMap->second.find(iter->iBigClassId);
        }
        iterSubMap->second.emplace_back(calInfo);

        if (iterSubMap->second.size() > 2)
        {
            LogDebug << "souceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId
                     << " last:" << iterSubMap->second.at(2).iFrameId << " " << iterSubMap->second.at(2).fCenterX
                     << " mid:" << iterSubMap->second.at(1).iFrameId << " " << iterSubMap->second.at(1).fCenterX
                     << " pre:" << iterSubMap->second.at(0).iFrameId << " " << iterSubMap->second.at(0).fCenterX;
            //如果帧号连续，且移动位置大于15px，则计算方向
            if (iterSubMap->second.at(2).iFrameId - iterSubMap->second.at(1).iFrameId != mainCfg_.iSkipInterval ||
                iterSubMap->second.at(1).iFrameId - iterSubMap->second.at(0).iFrameId != mainCfg_.iSkipInterval)
            {
                iterSubMap->second.erase(iterSubMap->second.begin());
                continue;
            }
            if (abs(iterSubMap->second.at(0).fTargetWidth -
                    iterSubMap->second.at(2).fTargetWidth) >= 1.5*iChkStopPX_) {
                iterSubMap->second.erase(iterSubMap->second.begin());
                continue;
            }

            int iLast = iterSubMap->second.at(2).fCenterX;
            int iMid = iterSubMap->second.at(1).fCenterX;
            int iPre = iterSubMap->second.at(0).fCenterX;
            if (abs(iPre - iLast) <= iChkStopPX_)
            {
                iterSubMap->second.erase(iterSubMap->second.begin());
                continue;
            }
            if (iPre <= iMid && iMid <= iLast)
            {
                iDirection_ = DIRECTION_RIGHT;
            }
            else if (iPre >= iMid && iMid >= iLast)
            {
                iDirection_ = DIRECTION_LEFT;
            }
            else
            {
                iterSubMap->second.erase(iterSubMap->second.begin());
                continue;
            }

            LogDebug << "souceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " iDirection_:" << iDirection_;
        }
    }

    //主相机的对侧相机需反向设置方向
    if (iDirection_ != DIRECTION_UNKNOWN && MyYaml::GetIns()->GetDataSourceConfigById(pProcessData->iDataSource).iLeftFirst != mainCfg_.iLeftFirst)
    {
        LogDebug << "before sourceid:" << pProcessData->iDataSource << "frameid:" << pProcessData->iFrameId << " direction:" << iDirection_;
        iDirection_ = (iDirection_ == DIRECTION_LEFT) ? DIRECTION_RIGHT : DIRECTION_LEFT;
        LogDebug << "after sourceid:" << pProcessData->iDataSource << "frameid:" << pProcessData->iFrameId << " direction:" << iDirection_;
    }
}

/**
* 计算行车方向
* inParam : std::shared_ptr<ProcessData> pProcessData   :待处理数据
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::CalculateDirection(std::shared_ptr<ProcessData> pProcessData)
{
    auto iterPostDataFrist = mapPostDataFrist_.find(pProcessData->iDataSource);
    auto iterMapStep1Info = mapMapStep1Info_.find(pProcessData->iDataSource);
    /*
    1帧暂时最多识别3个大框。[(车头、间隔、车号); (车号、间隔、属性)]
    因间隔大框不是很准确，暂时把间隔大框剔除后，计算方向。
    剔除间隔后，大框的可能情况为(车头；  车号；  属性；  车头&车号；  车号&属性)
    */
    std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);
    PostData postDataTemp = *pPostData;
    for (auto iter = postDataTemp.vecPostSubData.begin(); iter != postDataTemp.vecPostSubData.end();)
    {
        if (iter->iTargetType == SPACE)
        {
            iter = postDataTemp.vecPostSubData.erase(iter);
            continue;
        }
        auto iterMap = iterMapStep1Info->second.find(iter->iTargetType);
        if (iterMap != iterMapStep1Info->second.end())
        {
            iterMap->second.emplace_back(iter->step1Location);
        }
        else
        {
            std::vector<Step1Location> vecTemp;
            vecTemp.emplace_back(iter->step1Location);
            iterMapStep1Info->second.insert(std::pair<int, std::vector<Step1Location>>(iter->iTargetType, vecTemp));
        }

        iter++;
    }

    //记录第一次识别到大框的信息
    if (iterPostDataFrist->second.vecPostSubData.size() == 0)
    {
        iterPostDataFrist->second = postDataTemp;
        LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId
                 << " size:" << iterPostDataFrist->second.vecPostSubData.size();
    }

    //无框时，不计算方向
    if (iterPostDataFrist->second.vecPostSubData.size() == 0)
    {
        LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " no find data";
        return;
    }
    //框的个数和类型没有变化时，不计算方向
    if (iterPostDataFrist->second.vecPostSubData.size() - postDataTemp.vecPostSubData.size() == 0)
    {
        std::sort(iterPostDataFrist->second.vecPostSubData.begin(), iterPostDataFrist->second.vecPostSubData.end(), CompareX);
        std::sort(postDataTemp.vecPostSubData.begin(), postDataTemp.vecPostSubData.end(), CompareX);
        bool bFlag = true;
        for (int i = 0; i < iterPostDataFrist->second.vecPostSubData.size(); i++)
        {
            if (iterPostDataFrist->second.vecPostSubData[i].iTargetType != postDataTemp.vecPostSubData[i].iTargetType)
            {
                bFlag = false;
                break;
            }
        }
        if (bFlag)
        {
            LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " datasize and datatype equal";
            return;
        }
    }

    auto iter = iterMapStep1Info->second.begin();
    int iMaxSize = iter->second.size();
    int iMaxSizeKey = iter->first;
    int iMinSize = iter->second.size();
    int iMinSizeKey = iter->first;
    while (iter != iterMapStep1Info->second.end())
    {
        if (iter->second.size() > iMaxSize)
        {
            iMaxSize = iter->second.size();
            iMaxSizeKey = iter->first;
        }
        if (iter->second.size() < iMinSize)
        {
            iMinSize = iter->second.size();
            iMinSizeKey = iter->first;
        }
        iter++;
    }
    LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId
             << " iMaxSize:" << iMaxSize << " iMaxSizeKey:" << iMaxSizeKey
             << " iMinSize:" << iMinSize << " iMinSizeKey:" << iMinSizeKey;


    /*
    第一次识别为1个框，后续变化为无框或2个框，使用框个数多的数据计算方向
    第一次识别为2个框，后续变化为1个框，使用个数多的数据计算方向。
                      后续变化为0个框，使用个数少的数据计算方向。
    */
    if (iterPostDataFrist->second.vecPostSubData.size() == 1)
    {
        SetDirection(iterMapStep1Info->second[iMaxSizeKey], pProcessData);
    }
    else if (iterPostDataFrist->second.vecPostSubData.size() == 2)
    {
        if (postDataTemp.vecPostSubData.size() == 1)
        {
            SetDirection(iterMapStep1Info->second[iMinSizeKey], pProcessData);
        }
        else if (postDataTemp.vecPostSubData.size() == 0)
        {
            SetDirection(iterMapStep1Info->second[iMaxSizeKey], pProcessData);
        }
    }
    LogDebug<< "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " 行车方向:" << iDirection_;
}

void FilterTrainStepOneEngine::sendComeTrain(const std::string strTrainDate, const std::string strTrainName, const int iDirection) {
    std::string message = "{\"cometime\":\"" + strTrainDate + " " + strTrainName + "\",\"type\":\"1\",\"direction\":" + to_string(iDirection == iPushDirection_ ? 1:-1) + "}";
    LogWarn << message;
    outputQueMap_[engineName_ + "_" + std::to_string(engineId_) + "_1"]->push(std::static_pointer_cast<void>(std::make_shared<std::string>(message)));
}

/**
* 根据当前帧数据，处理上一帧数据
* inParam : std::shared_ptr<ProcessData> pProcessData   :当前帧数据
* outParam: N/A
* return  : N/A
*/
void FilterTrainStepOneEngine::DealProcessDataPre(std::shared_ptr<ProcessData> pProcessData)
{
    /*
    目标框是否是连续识别，只识别到一帧的目标框认为误识别，过滤掉。
    判断上一帧，当前帧 是否有框
        上一帧有框，当前帧有框，说明连续识别，正常处理。
        上一帧有框，当前帧无框，则非连续识别，过滤大框
        上一帧无框，当前帧有框，则连续识别个数置零。
        上一帧无框，当前帧无框，则连续识别个数置零。
    */
    auto iterProcessData = mapProcessDataPre_.find(pProcessData->iDataSource);
    if (iterProcessData == mapProcessDataPre_.end() || nullptr == iterProcessData->second)
    {
        return;
    }

    auto iterHeadContinueCnt = mapHeadContinueCnt_.find(pProcessData->iDataSource);
    auto iterProContinueCnt = mapProContinueCnt_.find(pProcessData->iDataSource);
    auto iterNumContinueCnt = mapNumContinueCnt_.find(pProcessData->iDataSource);
    auto iterSpaceContinueCnt = mapSpaceContinueCnt_.find(pProcessData->iDataSource);
    auto iterTranSpaceContinueCnt = mapTrainSpaceContinueCnt_.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 == HEAD)
        {
            iterHeadContinueCnt->second++;
        }
        else if (pPostData->vecPostSubData[i].iTargetType == PRO)
        {
            iterProContinueCnt->second++;
        }
        else if (pPostData->vecPostSubData[i].iTargetType == NUM)
        {
            iterNumContinueCnt->second++;
        }
        else if (pPostData->vecPostSubData[i].iTargetType == SPACE)
        {
            iterSpaceContinueCnt->second++;
        }
        else if (pPostData->vecPostSubData[i].iTargetType == TRAINSPACE)
        {
            iterTranSpaceContinueCnt->second++;
        }
    }
    //LogDebug << "engineId:" << engineId_ << " frameId:" << pProcessData->iFrameId << " " << iProContinueCnt_ << " " << iNumContinueCnt_;

    //获取上一帧识别情况
    std::shared_ptr<PostData> pPostDataPre = std::static_pointer_cast<PostData>(iterProcessData->second->pVoidData);
    bool bPreHaveHead = false;
    bool bPreHavePro = false;
    bool bPreHaveNum = false;
    bool bPreHaveSpace = false;
    bool bPreHaveTrainSpace = false;
    for (int iPre = 0; iPre < pPostDataPre->vecPostSubData.size(); iPre++)
    {
        if (pPostDataPre->vecPostSubData[iPre].iTargetType == HEAD)
        {
            iterHeadContinueCnt->second++;
            bPreHaveHead = true;
        }
        else if (pPostDataPre->vecPostSubData[iPre].iTargetType == PRO)
        {
            iterProContinueCnt->second++;
            bPreHavePro = true;
        }
        else if (pPostDataPre->vecPostSubData[iPre].iTargetType == NUM)
        {
            iterNumContinueCnt->second++;
            bPreHaveNum = true;
        }
        else if (pPostDataPre->vecPostSubData[iPre].iTargetType == SPACE)
        {
            iterSpaceContinueCnt->second++;
            bPreHaveSpace = true;
        }
        else if (pPostDataPre->vecPostSubData[iPre].iTargetType == TRAINSPACE)
        {
            iterTranSpaceContinueCnt->second++;
            bPreHaveTrainSpace = true;
        }
    }

    //前一帧无大框连续识别次数置零
    iterHeadContinueCnt->second = bPreHaveHead ? iterHeadContinueCnt->second : 0;
    iterProContinueCnt->second = bPreHavePro ? iterProContinueCnt->second : 0;
    iterNumContinueCnt->second = bPreHaveNum ? iterNumContinueCnt->second : 0;
    iterSpaceContinueCnt->second = bPreHaveSpace ? iterSpaceContinueCnt->second : 0;
    iterTranSpaceContinueCnt->second = bPreHaveTrainSpace ? iterTranSpaceContinueCnt->second : 0;

    //非连续识别的情况，认为误识别，剔除误识别的大框信息
    for (std::vector<PostSubData>::iterator it = pPostDataPre->vecPostSubData.begin(); it != pPostDataPre->vecPostSubData.end();)
    {
        if (iterHeadContinueCnt->second < 2 && it->iTargetType == HEAD)
        {
            LogDebug << " frameId:" << iterProcessData->second->iFrameId << " Head 框因非连续识别而过滤";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        if (iterProContinueCnt->second < 2 && it->iTargetType == PRO)
        {
            LogDebug << " frameId:" << iterProcessData->second->iFrameId << " PRO 框因非连续识别而过滤";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        if (iterNumContinueCnt->second < 2 && it->iTargetType == NUM)
        {
            LogDebug << " frameId:" << iterProcessData->second->iFrameId << " NUM 框因非连续识别而过滤";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        if (iterSpaceContinueCnt->second < 2 && it->iTargetType == SPACE)
        {
            LogDebug << " frameId:" << iterProcessData->second->iFrameId << " SPACE 框因非连续识别而过滤";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        if (iterTranSpaceContinueCnt->second < 2 && it->iTargetType == TRAINSPACE)
        {
            LogDebug << " frameId:" << iterProcessData->second->iFrameId << " TRAINSPACE 框因非连续识别而过滤";
            it = pPostDataPre->vecPostSubData.erase(it);
            continue;
        }
        it++;
    }

    //判定行驶方向, 记录Direction文件信息
    if (iDirection_ == DIRECTION_UNKNOWN)
    {
        iDirection_ = iterProcessData->second->iDirection;

        LogInfo << "方向：" << (iDirection_ == DIRECTION_LEFT ? "左" : (iDirection_ == DIRECTION_RIGHT ? "右" : "未判断出来"));
        if (iDirection_ == DIRECTION_UNKNOWN)
        {
            //CalculateDirection(iterProcessData->second);
            CalculateDirectionNew(iterProcessData->second);
            if (iDirection_ != DIRECTION_UNKNOWN) this->sendComeTrain(pProcessData->strTrainDate, pProcessData->strTrainName, iDirection_);
        }

        if (iDirection_ != DIRECTION_UNKNOWN)
        {
            auto iterPostDataFrist = mapPostDataFrist_.find(pProcessData->iDataSource);
            Json::Value jvDirectionInfo;
            jvDirectionInfo["direction"] = iDirection_;
            jvDirectionInfo["firstStep1Cnt"] = iterPostDataFrist->second.vecPostSubData.size();
            jvDirectionInfo["frameid"] = iterProcessData->second->iFrameId;
            jvDirectionInfo["sourceid"] = iterProcessData->second->iDataSource;
            std::string strFilePath = strResultPath_ + pProcessData->strTrainDate + "/" + pProcessData->strTrainName + "/" + "direction.txt";
            MyUtils::getins()->WriteJsonInfo(jvDirectionInfo, strFilePath);

        }
    }

    //主摄像头校验是否停车
    int iTrainStatusTemp =  iTrainStatus_;
    if (iterProcessData->second->iDataSource == 0)
    {
        iTrainStatus_ = GetTrainStatus(iterProcessData->second);
        iTrainStatusTemp = iTrainStatus_;

        if (iTrainStatus_ == TRAINSTATUS_STOP)
        {
            //停车
        }
        else if (iTrainStatus_ == TRAINSTATUS_BACK)
        {
            //倒车
            AddBackInfo(iterProcessData->second);
            iTrainStatusTemp = TRAINSTATUS_STOP;
        }
        else if(iTrainStatus_ == TRAINSTATUS_RUN)
        {
            /*
            正向行驶需先把倒车产生的倒车数据处理完毕，即使车辆回到原倒车点，再开始识别行驶数据
            */
            if(!IsEndDealBackInfo(iterProcessData->second))
            {
                iTrainStatusTemp = TRAINSTATUS_STOP;
            }
        }
    }
    LogDebug << "数据源:" << iterProcessData->second->iDataSource << " 帧:" << iterProcessData->second->iFrameId
             << " 火车实时运行状态:" << iTrainStatus_ << "(0无车，1运行，2停车，3倒车) iTrainStatusTemp:" << iTrainStatusTemp;
    iterProcessData->second->iStatus = iTrainStatusTemp;

    // this->sendComeTrain(pProcessData->strTrainDate, pProcessData->strTrainName, iDirection_);

    //上一帧，push端口0
    PushData(strPort0_, iterProcessData->second);
}


APP_ERROR FilterTrainStepOneEngine::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);
        std::shared_ptr<PostData> pPostData = std::static_pointer_cast<PostData>(pProcessData->pVoidData);

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

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

        //1帧暂时最多识别4个大框。[(车头、车厢间隔、间隔、车号); (车号、车厢间隔、间隔、属性)]
        if (pPostData->vecPostSubData.size() > 4)
        {
            LogError << "sourceid:" << pProcessData->iDataSource << " frameId:" << pProcessData->iFrameId << " vecpostsubdata size:" << pPostData->vecPostSubData.size();
            pPostData->vecPostSubData.clear();
        }

        //按指定识别区域过滤误识别信息--过滤逻辑迁移至TrainStepOneEngine中。

        //根据当前帧数据，处理上一帧数据
        DealProcessDataPre(pProcessData);
        mapProcessDataPre_[pProcessData->iDataSource] = pProcessData;
        if (pProcessData->bIsEnd)
        {
            //结束帧，push端口0
            LogDebug << "sourceid:" << pProcessData->iDataSource << " frameid:" << pProcessData->iFrameId << " isEnd:" << pProcessData->bIsEnd;
            PushData(strPort0_, pProcessData);
        }

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