#include "Sub3Judge16Lkyz.h"
#include "HFactory.h"

Sub3Judge16Lkyz::Sub3Judge16Lkyz()
{
    m_exam = __NEW__(TKM3Item);
}

Sub3Judge16Lkyz::~Sub3Judge16Lkyz()
{
    __DELETE__(m_exam);
}

bool Sub3Judge16Lkyz::dealJudgeEnter()
{
    if(m_exam->TestPro != ItemProFlagInit) return false;
    // if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return false;

    const TChuanGan* cg = m_car->historyChuanGan();

    m_itemv = TSub3Item16Lkyz();

    m_itemv.Enter_TK = cg->tkCnt;
    m_itemv.LuK_Zx_Deg = m_exam->SetUp8 != "" ? std::atof(m_exam->SetUp8.c_str()) : 25;

    //m_itemv.ItemPosI = xi;
    m_itemv.Start_JL_CM = cg->ai_ljjl_cm;

    const std::vector<std::string>& s508 = TableSysSet->asArray508();

    //得到项目距离
    m_itemv.XMJL_Mi = s508.size() > 0 && s508[0] != "" ? std::atoi(s508[0].c_str()) : 250;
    //1、得到路口类型
    m_itemv.LuK_Type = m_exam->SetUp1.length() > 0 ? std::atoi(m_exam->SetUp1.c_str()) : 1;
    //2、得到减速类型和速度阈值
    const std::vector<std::vector<std::string>>& s522 = TableSysSet->asArray2_522();
    if(s522.size() > 0)
    {
        const std::vector<std::string>& ss522 = s522[0];
        m_itemv.JianSu_LeiXing = ss522.size() > 0 && ss522[0] != "" ? std::atoi(ss522[0].c_str()) : 0;
        m_itemv.JianSu_SuDu    = ss522.size() > 1 && ss522[1] != "" ? std::atoi(ss522[1].c_str()) : 30;
    }
    if(m_itemv.JianSu_LeiXing > 4)
    {
        m_itemv.JianSu_LeiXing = 0;
    }
    //3、得到行驶方向点和停车让行标志
    if(m_exam->SetUp3 != "")
    {
        std::vector<std::string> setup3 = Tools::split(m_exam->SetUp3, "-");
        m_itemv.LuK_Fx_PointNo = setup3.size() > 0 && setup3[0] != "" ? std::atoi(setup3[0].c_str()) : 0;
        //停车让行
        m_itemv.LuK_TcRx = (setup3.size() > 2 && setup3[2] == "1");
    }
    else
    {
        m_itemv.LuK_Fx_PointNo = 0;
    }

    //4、路口内禁止停车时间
    m_itemv.LuK_TCSJ = (m_exam->SetUp5 != "" ? std::atoi(m_exam->SetUp5.c_str()) : 10000);
    //5、越过停车线后X米后，不允许停车

    m_itemv.LuK_TCJL = (m_exam->SetUp6 != "" ? std::atof(m_exam->SetUp6.c_str()) : 10);

    m_exam->TestPro = ItemProFlagJudge;
    //ToDo1:播报项目语音
    //ToDo2:生成进项目事件
    return true;
}

void Sub3Judge16Lkyz::dealJudgeItem()
{
    HELP_COST_TIME("");
    if(m_exam->TestPro != ItemProFlagJudge) return;
    if(!judgeAllowable()) return;

    //setup1: 路口类型 1-平交 2-环道 3-三叉   缺省 1－平交
    //setup2: 限速值，通过路口停车线时的车速  缺省 35
    //setup3: 正确行驶方向点-方向-停车让行
    //setup4: *语音提示方向，保存语音文件名  缺省 空
    //setup5: 路口内禁止停车时间  缺省 10000秒 标识不限制
    //setup6: 越过停车线后X米后，不允许停车超过setup5秒    缺省 10米
    const TChuanGan* cg = m_car->historyChuanGan();
    const TRTKResult& RTKKM3 = cg->RTKKM3;
    const TGpsInfo& gps = cg->real.gps;
    const TSensorInfo& sor = cg->real.sensor;
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    const std::string& ksdd = TableSysSet->get211();

    if(m_car->isExamMode())  //正式考试
    {
        if(!m_car->isQualified())
        {
            m_exam->TestPro = ItemProFlagEnd;
            JudgeFlagEnd();
            return;
        }
    }

    double MaxAngleChange = 0;
    if(m_exam->Gps_Itemno1 == 3) //穿越3点才判断角度 20250409
    {
        for(int i = 1; i < 500; i++)
        {
            const TChuanGan* hisi = m_car->historyChuanGan(i);
            if(hisi == nullptr) break;

            const TGpsInfo& gpsi = hisi->real.gps;

            if(!gpsi.rtkEnabled) break;
            if (hisi->tkCnt <= m_itemv.Enter_TK) break;
            double a = gps.hxj - gpsi.hxj;
            //处理越界问题
            if(a > 300)
            {
                a = a - 360;
            }
            else
            {
                if(a < -300)
                {
                    a = a + 360;
                }
            }

            a = std::abs(a);
            if(Tools::greater(a, MaxAngleChange))
            {
                MaxAngleChange = a;
            }

            if(a > m_itemv.LuK_Zx_Deg && a < 210)  //20180830 -150
            {
                m_itemv.LuK_FXD_AngleOKFlag = true;
                break;
            }
        }
    }

    //(*2024-03-08*)
    if(Tools::pos("+", cg->MapPoint_Road_Code) == true && Tools::pos("+", his1->MapPoint_Road_Code) == false)  //包含路口端点的路口项目
    {
        if(m_exam->Gps_Itemno1 < 3)
        {
            m_exam->Gps_Itemno1 = 3;
        }
    }

    if(m_exam->Gps_Itemno1 == 3 || m_itemv.Cross_TZX_Flag == true)
    {
        if(m_itemv.Step3Flag == false)
        {
            m_itemv.Step3Flag = true;
            std::string roadData = cg->MapPoint_Road_Code;
            if(roadData == "")
            {
                //这里要稍微讲解一下(有些地方，可能驾校在路段中间，这种情况下，初始读不到路段，这种情况比较少)
                roadData = RTKKM3.MapRoad_Name;
            }
            roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
            m_itemv.Step3RoadData = roadData;
        }
    }

    //读到下一个路段标志
    if(m_itemv.Step3Flag == true)
    {
        std::string roadData = cg->MapPoint_Road_Code;
        if(roadData == "")
        {
            roadData = RTKKM3.MapRoad_Name;
        }
        roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
        if(m_itemv.Step3RoadData != roadData)
        {
            m_itemv.ReadNextLuDuanFlag = true;
        }

        //做完项目后，有没有关闭灯光(综合评判会用到，这里先赋值)
        if(sor.zfxd == SNOT && sor.yfxd == SNOT)
        {
            TTestCtl* ctl = m_car->getTTestCtl();
            ctl->ZXD_GuanBi = true;
        }
    }

    //方向灯评判
    //方向灯标志
    if(sor.yfxd == SYES && sor.zfxd == SNOT)
    {
        m_itemv.Temp_YFXD = true;
        if(m_itemv.Temp_YFXD_TM == 0)
        {
            m_itemv.Temp_YFXD_TM = gps.sj;
        }
        else
        {
            if(std::abs(gps.sj - m_itemv.Temp_YFXD_TM) >= 3000)
            {
                m_itemv.Temp_YFXD3s = true;
            }
        }
    }
    else
    {
        if(!m_itemv.Temp_YFXD3s)
        {
            m_itemv.Temp_YFXD_TM = 0;
        }
    }

    //方向灯错误评判
    //508:路口项目参数:xmjl,Fxdpp,
    //	Xmjl:a项目距离
    //Fxdpp:方向灯评判时机（0：打方向角度，1：停止线（默认））
    //fxdcc：方向灯错误评判，如左转开右方向灯立即扣分
    //大车右转必停时长（毫秒，默认为0不用停车）
    //方向灯必须持续到路口停止线
    if(!m_itemv.Step3Flag)
    {
        const std::vector<std::string>& s508 = TableSysSet->asArray508();
        if(s508.size() > 2 && std::atoi(s508[2].c_str()) == 1)
        {
            const TSensorInfo& sor1 = m_car->historySensor(1);
            const TSensorInfo& sor2 = m_car->historySensor(2);
            if(sor.zfxd == SYES && sor1.zfxd == SYES && sor2.zfxd == SYES)
            {
                if(!m_itemv.Mark_20_73_Flag)
                {
                    m_itemv.Mark_20_73_Flag = true;
                    JUDGE_MARK_SUB3(20, "73", false);
                }
            }
        }
    }

    std::string msg = "";
    if(m_itemv.Cross_TZX_Flag == true)
    {
        if(m_itemv.StopLine_JLCM != 0)
        {
            //环岛业务逻辑翻译（一般很少有地方是环岛） //路口类型   1-平交  2-环岛
            int disMI = m_itemv.LuK_Type == 2 ? 190 : 100;
            int dis1 = cg->ai_ljjl_cm - m_itemv.StopLine_JLCM;
            int dis2 = disMI * 100;
            msg = JUDGE_UTF8S(",过停止线距离cm=") + std::to_string(dis1) + JUDGE_UTF8S(",结束=") + std::to_string(dis2);
            if(cg->ai_ljjl_cm - m_itemv.StopLine_JLCM >= dis2)
            {
                m_exam->TestPro = ItemProFlagEnd;
                JudgeFlagEnd();
                return;
            }
        }
    }

    // (*2024-03-07*)
    //增加南京地区特殊判断
    if(ksdd == siteof::nj && m_exam->Gps_Itemno1 == 3 && TableSysSet->get346() == "-1")
    {
        if(!m_itemv.Cross_TZX_Flag)
        {
            m_itemv.Cross_TZX_Flag = true;
            //0-未越线 1-停车线 2-中心点右侧线   3-等待控制线 20150119
            m_itemv.StopLine_JLCM = cg->ai_ljjl_cm; //停止线时刻的距离
        }
    }

    //判断是否到达停止线
    //0-未越线 1-停车线 2-中心点右侧线   3-等待控制线 20150119
    if(m_itemv.Cross_TZX_Flag == false)
    {
        if(RTKKM3.CrossLineAttr == CrossLineAttr_1)
        {
            m_itemv.Cross_TZX_Flag = true;

            if(m_exam->Gps_Itemno1 < 3)
            {
                m_exam->Gps_Itemno1 = 3;
            }
            //0-未越线 1-停车线 2-中心点右侧线   3-等待控制线 20150119
            m_itemv.StopLine_JLCM = cg->ai_ljjl_cm; //停止线时刻的距离
            //路段状况:基准天线所在车道方向: 1-右转 2-直行 3-左转 4-掉头
            //1000 0100 0010 0001
            //评判方向灯
            NS3JudgeVision_40903(true);
        }
    }

    if(m_itemv.Cross_TZX_Flag == true || m_itemv.LuK_FXD_AngleOKFlag == true)
    {
        if(m_itemv.Judge_TCX == false)
        {
            m_itemv.Judge_TCX = true;
            //方向灯评判
            JudgeFXD();
        }
    }

    //如果没测绘停止线，就不判停车让行和减速
    if(m_itemv.Cross_TZX_Flag == true)
    {
        if(!m_itemv.Judge_TCRX_JianSu)
        {
            m_itemv.Judge_TCRX_JianSu = true;
            //停车让行评判（掉头不存在停车让行)
            if(m_itemv.LuK_TcRx == true)
            {
                if(CheckJL_TingChe(30) == false) //20170924
                {
                    JUDGE_MARK_SUB3(16, "03", true);
                }
            }
            //(*XLG_Modify 2024-03-15*)
            Judge_LuKou_TZX_JianSu(itemNo(), m_itemv.JianSu_LeiXing, m_itemv.JianSu_SuDu);
            Judge_LuKou_TZX_Night(itemNo()); //大车夜考通过路口时使用远光灯评判 yhy 2025-02-13
        }
    }

    //检查车道是否正确
    if(RTKKM3.CrossLineAttr == CrossLineAttr_1 || m_exam->Gps_Itemno1 == 2 || m_exam->Gps_Itemno1 == 3)
    {
        //if(m_itemv.Check_CheDao == false)
        {
            //m_itemv.Check_CheDao = true;
            //路段状况:基准天线所在车道方向: 1-右转 2-直行 3-左转 4-掉头
            //1000 0100 0010 0001
            if(forbidInLane(RTKKM3.BasePointInLaneDir, TURN_R))
            {
                JUDGE_MARK_SUB3(16, "45", true);
            }
        }
    }

    //(5,04):遇有路口交通阻塞时进入路口，将车辆停在路口内等候
    //路口内停车判断
    //RTK_PP穿越停车线赋值
    if(cg->move != moveStop)
    {
        m_itemv.StopLine_TCTM = 0;
    }
    if(cg->move == moveStop && m_itemv.Cross_TZX_Flag == true)
    {
        if(cg->ai_ljjl_cm - m_itemv.StopLine_JLCM > m_itemv.LuK_TCJL * 100)
        {
            if(m_itemv.StopLine_TCTM == 0)
            {
                m_itemv.StopLine_TCTM = gps.sj;
            }
            if(std::abs(gps.sj - m_itemv.StopLine_TCTM) > m_itemv.LuK_TCSJ * SECOND)
            {
                if(!m_itemv.LKTCKF)
                {
                    JUDGE_MARK_SUB3(16, "04", false);
                    m_itemv.LKTCKF = true;
                }
            }
        }
    }

    //停车让行评判

    //不按规定考试（比如：走错车道了

    //方向灯标志

    if(m_itemv.Cross_TZX_Flag == false)
    {
        if(cg->ai_ljjl_cm - m_itemv.Start_JL_CM > m_itemv.XMJL_Mi * 100)
        {
            m_exam->TestPro = ItemProFlagEnd;
            JudgeFlagEnd();
            return;
        }
    }
    if(m_itemv.Step3Flag == true)
    {
        //1、左转方向扣分（标配）
        //{
        //if not Itmv16.ZhongXinDian3MarkFlag then
        //{
        //  if(RTKKM3.CrossLineAttr = 2) or (RTKKM3.CrossLineAttr = 4) then
        //  {
        //    JUDGE_MARK_SUB3(16, '05', False);
        //    Itmv16.ZhongXinDian3MarkFlag := True;
        //  }
        //}
        //}
        ////////////////////////////////////////////////////////////////////////
        //插入穿越点
        int ptNo = m_car->lastCrossPtNo();
        if(ptNo != 0)
        {
            bool findFlag = false;
            int len = m_itemv.Step3PtArr.size();
            for(int i = 0; i < len; i++)
            {
                if(m_itemv.Step3PtArr[i] == ptNo)
                {
                    findFlag = true;
                }
            }
            if(!findFlag)
            {
                m_itemv.Step3PtArr.push_back(ptNo);
            }

            findFlag = false;
            len = m_itemv.Step3PtArr.size();
            for(int i = 0; i < len; i++)
            {
                if(m_itemv.Step3PtArr[i] == m_itemv.LuK_Fx_PointNo)
                {
                    findFlag = true;
                }
            }
            m_itemv.OKFangXiangPt_SuccessFlag = findFlag;
        }

        std::string roadData = cg->MapPoint_Road_Code;
        if(roadData == "")
        {
            roadData = RTKKM3.MapRoad_Name;
        }
        roadData = Tools::replace(roadData, "+", ""); // [rfReplaceAll]
        if(roadData != "")
        {
            //如果路段发生变化了(如果持续3帧不同，可以认为路口项目结束了
            if(roadData != m_itemv.Step3RoadData)
            {
                bool OKFlag = true;
                for(int i = 1; i <= 2; i++)
                {
                    TChuanGan* cgi = m_car->historyChuanGan(i);
                    roadData = cgi->MapPoint_Road_Code;
                    if(roadData == "")
                    {
                        roadData = cgi->RTKKM3.MapRoad_Name;
                    }
                    roadData = Tools::replace(roadData, "+", ""); //, [rfReplaceAll]
                    if(roadData == m_itemv.Step3RoadData)
                    {
                        OKFlag = false;
                        break;
                    }
                }

                m_itemv.ReadNextLuDuanFlag = OKFlag;
            }
        }

        if(m_itemv.LuK_Fx_PointNo > 0)
        {
            //如果设置了方向点，路段发生变化,并且读到方向点
            if(m_itemv.ReadNextLuDuanFlag && m_itemv.OKFangXiangPt_SuccessFlag)
            {
                m_exam->TestPro = ItemProFlagEnd;
                JudgeFlagEnd();
                return;
            }
        }
        else
        {
            //如果没设置方向点,路段发生变化，并且读到路段点
            if(m_itemv.ReadNextLuDuanFlag && m_itemv.Step3PtArr.size() > 0)
            {
                m_exam->TestPro = ItemProFlagEnd;
                JudgeFlagEnd();
                return;
            }
        }
    }
    //ToDo:贵州地区的信号灯评判
    if(ksdd == siteof::guizhoubj)
    {
        //ToDo:贵州地区的红绿灯通信数据
    }
    //ToDo: 等待控制线，需要结合红绿灯信号 （暂时不实现)
    //0-未越线 1-停车线 2-中心点右侧线   3-等待控制线 20150119
    //RTKKM3.CrossLineAttr = 3 。
    ////////////////////////////////////////////////////////////////////////////
    //ToDo:暂时屏蔽观察类算法，后期实现
    //except
    //}

    //状态提示
    std::string str = "";
    std::vector<std::string> xhd = Tools::split(sor.xhd, ",");
    if(ksdd == siteof::nmgcfkm3 && xhd.size() > 0 && xhd[0] == m_exam->ItemSNO)
    {
        char buf[128] = {0};
        SafeSprintf(buf, sizeof(buf), JUDGE_UTF8S("%s;St=%d;Cs=%0.2f,角度变化=%0.1f,xhd=%s"),
                    m_exam->ItemSNO.c_str(), m_exam->Gps_Itemno1, gps.sd, MaxAngleChange, sor.xhd.c_str());
        str += buf;
    }
    else
    {
        char buf[128] = {0};
        SafeSprintf(buf, sizeof(buf), JUDGE_UTF8S("%s;St=%d;Cs=%0.2f,角度变化=%0.1f"),
                    m_exam->ItemSNO.c_str(), m_exam->Gps_Itemno1, gps.sd, MaxAngleChange);
        str += buf;
    }


    str += msg;
    showStatus(str);
    NS3JudgeVision_30107(m_itemv.green, TVisionTarg::XD_R);
    NS3JudgeVision_40903(false);
}

void Sub3Judge16Lkyz::JudgeFXD()
{
    const std::string& ksdd = TableSysSet->get211();
    const TSensorInfo& cur = m_car->historySensor(0);
    const TSensorInfo& his = m_car->historySensor(1);
    //右转
    if(!m_itemv.JudgeFXD_Flag)
    {
        m_itemv.JudgeFXD_Flag = true;
        if(ksdd == siteof::zs)
        {
            if(m_itemv.Temp_YFXD == false || (cur.yfxd == SNOT && his.yfxd == SNOT))
            {
                JUDGE_MARK_SUB3(16, "42", false);
            }
            else
            {
                if(m_itemv.Temp_YFXD3s == false)
                {
                    JUDGE_MARK_SUB3(16, "43", false);
                }
            }
        }
        else
        {
            if(m_itemv.Temp_YFXD == false)
            {
                JUDGE_MARK_SUB3(16, "42", false);
            }
            else
            {
                if(m_itemv.Temp_YFXD3s == false)
                {
                    JUDGE_MARK_SUB3(16, "43", false);
                }
            }
        }
    }
}

void Sub3Judge16Lkyz::JudgeFlagEnd()
{
    if(m_exam->TestPro == ItemProFlagEnd)
    {
        if(m_itemv.LuK_Fx_PointNo > 0)
        {
            if(m_itemv.OKFangXiangPt_SuccessFlag == false && m_itemv.ReadNextLuDuanFlag == true)
            {
                JUDGE_MARK_SUB3(16, "41", true);
            }
        }
    }
}

void Sub3Judge16Lkyz::NS3JudgeVision_40903(bool opportunity)
{
    //不观察左、右方交通情况，转弯通过路口时，未观察侧前方交通情况,
    //通过停止线前10s内，头部姿态没有大于左侧角度【18度】或大于右侧角度【20度】
    if(!NS3UsingAndData(NS3UsingPose)) return;

    int64 nowTime = m_car->GetCurrentTime2();
    const TNS3PoseSysset& poseSys = m_nsub3->getPoseSysset();
    const TVisionPose* pose0 = m_nsub3->hisPose(0);
    if(pose0->zy < -poseSys.left || pose0->zy > poseSys.right)
    {
        m_itemv.observe.obsTime = nowTime;
    }

    if(opportunity && !m_itemv.observe.isJudge)
    {
        m_itemv.observe.isJudge = true;
        if(nowTime - m_itemv.observe.obsTime > 10*SECOND)
        {
            JUDGE_MARK_NSUB3(16, "02", false);
            //NS3video
        }
    }
}