#include "Sub3Judge02Qbxx.h"
#include "HFactory.h"

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

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

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

    m_itemv = TSub3Item02Qbxx();
    //10s内未起步只评判第一次^第一次起步以打左方向灯开始计时^
    const std::vector<std::string>& s502 = TableSysSet->asArray502();
    std::string str = s502.size() > 7 ? s502[7] : "";
    const std::vector<std::string> ss = Tools::split(str, "^");
    m_itemv.sys502_8_1 = ss.size() > 0 && ss[0] != "" ? std::atoi(ss[0].c_str()) : 0;
    m_itemv.sys502_8_2 = ss.size() > 1 && ss[1] != "" ? std::atoi(ss[1].c_str()) : 0; //StrToIntDef(GetDotStr(2, tempstr + '^', '^'), 0);

    //下面是视觉相关的，先把参数值拿到，暂时不实现
    const std::vector<std::string>& s531 = TableSysSet->asArray531();
    if(s531.size() > 0 && s531[0] == "1")
    {
        m_itemv.is_radar = true;
    }

    m_itemv.Sys502_11 = s502.size() > 10 && s502[10] != "" ? std::atoi(s502[10].c_str()) : 0; //StrToIntDef(GetDotStr(11, SysSet[502], ','), 0);
    const std::vector<std::string>& s524 = TableSysSet->asArray524();
    m_itemv.Sys524_4 = s524.size() > 3 && s524[3] != "" ? std::atoi(s524[3].c_str()) : 0; //StrToIntDef(GetDotStr(4, SysSet[524], ','), 0);

    const TChuanGan* cg = m_car->historyChuanGan();
    m_itemv.Temp_JL_CM = cg->ai_ljjl_cm;

    if(m_car->disForward() > 10*100) //10米
    {
        //20240730 在济南修改 yhy
        //根据程军华口述修改过滤，在下面这种情况下：
        //如果在做模拟灯光的时候，还没起步之前差分丢了，车子开起来一段距离后差分又恢复了，当进入路口直行项目时，如果这个时候正好在路口等红灯停车了，会触发第一次起步
        //路口直行是不需要打灯的，而程序又第一次触发了起步项目，又要打左灯，考生怎么操作都会扣分，
        //所以这里过滤下，如果起步的时候前进的距离超过10米了就不算第一次起步了，就不判首次起步打左灯了
        m_Pub_First_QiBu_Flag = true;
    }

    m_itemv.State = 1; //车辆停车状态到前进距离小于1米的阶段

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

void Sub3Judge02Qbxx::dealJudgeItem()
{
#if JUDGE_USE_INSPECT
    //return;  //屏蔽起步
#endif

    HELP_COST_TIME("");
    if(m_exam->TestPro != ItemProFlagJudge) return;
    if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1)) return;

    const TChuanGan* cg = m_car->historyChuanGan();
    const TSensorInfo& sor = cg->real.sensor;
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    const TSensorInfo& sor1 = his1->real.sensor;
    const TChuanGan* his2 = m_car->historyChuanGan(2);
    const TSensorInfo& sor2 = his2->real.sensor;
    const std::string& ksdd = TableSysSet->get211();

    if(m_itemv.State == 1)  //起步相关的，起步距离小于1米
    {
        //评判起步闯动
        bool findFlag = true;
        if(ksdd == siteof::kunming && cg->tkCnt < 60*SECOND)
        {
            findFlag = false;
        }

        if(findFlag == true)
        {
            const TChuanGan* his3 = m_car->historyChuanGan(3);
            const TSensorInfo& sor3 = his3->real.sensor;
            const TChuanGan* his4 = m_car->historyChuanGan(4);
            const TSensorInfo& sor4 = his4->real.sensor;

            TCar* tcar = m_car->getTCar();

            int fdjds = tcar->FDJDS * tcar->fdjds_cdbl;
            //保持和windows一模一样 原先还叠加了fdjzs>10和前进状态条件
            if(  (sor.fdjzs > 0 && sor.fdjzs < fdjds)        // && cg->move == moveForward
                 && (sor1.fdjzs > 0 && sor1.fdjzs < fdjds)   // && his1->move == moveForward
                 && (sor2.fdjzs > 0 && sor2.fdjzs < fdjds)   // && his2->move == moveForward
                 && (sor3.fdjzs > 0 && sor3.fdjzs < fdjds)   // && his3->move == moveForward
                 && (sor4.fdjzs > 0 && sor4.fdjzs < fdjds)   // && his4->move == moveForward
                 && (m_itemv.Mark_ChuangDong == false) ) //闯动
            {
                m_itemv.Mark_ChuangDong = true;
                JUDGE_MARK_SUB3(2, "08", false);
            }
        }

        //起步时，加速踏板控制不当，致使发动机转速过高
        if(!m_itemv.Mark_2_09_Flag)
        {
            const TSensorInfo& sor3 = m_car->historySensor(3);
            const std::vector<std::string>& s502 = TableSysSet->asArray502();
            //发动机转速过高
            int ZSMax = s502.size() > 3 && s502[3] != "" ? std::atoi(s502[3].c_str()) : 2500; //StrToIntDef(GetDotStr(4, SysSet[502], ','), 2500);
            if(sor.fdjzs > ZSMax && sor1.fdjzs > ZSMax && sor2.fdjzs > ZSMax && sor3.fdjzs > ZSMax)
            {
                bool bb = true;
                int count  = m_car->historyCount();
                for(int i = 1; i <= 10; i++)
                {
                    if(i >= count) break;
                    const TSensorInfo& sori = m_car->historySensor(i);
                    if(sori.dh2 == SYES)
                    {
                        bb = false;
                        break;
                    }
                }
                if(IS_C2(m_carType))
                {
                    if(ksdd == siteof::shanghaikm3 || Tools::pos(siteof::guizhou, ksdd) == true) //Copy(Ctl.KSDD, 1, 7) = 'guizhou'
                    {
                        bb = false;
                    }
                }
                if(bb == true)
                {
                    m_itemv.Mark_2_09_Flag = true;
                    JUDGE_MARK_SUB3(2, "09", false);
                }
            }
        }

        //10秒未起步
        nogo_timeout();

        //溜车评判 20240730
        Judge_LiuChe();

        if(cg->ai_ljjl_cm - m_itemv.Temp_JL_CM >= 100)
        {
            if(cg->move == moveForward && his1->move == moveForward)
            {
                Judge_QiBu_FXD_LaBa(); //20240730
                Judge_QiBu_GuaCuoDang(); //20240730
                m_itemv.State = 2;
                return;
            }
        }
    }

    if(m_itemv.State == 2)  //起步10米问题
    {
        if(cg->move == moveForward && his1->move == moveForward)
        {
            if(cg->ai_ljjl_cm - m_itemv.Temp_JL_CM >= 10*100+10)  //放个10CM
            {
                if(sor.ssc == SYES)  //手刹错误，未及时纠正
                {
                    if(m_itemv.Error_SSC_Flag == true)
                    {
                        JUDGE_MARK_SUB3(2, "10", false, true);
                    }
                }
                m_exam->TestPro = ItemProFlagEnd;
                return;
            }
        }
    }

    if(cg->move == moveForward && his1->move == moveForward && his2->move == moveStop)
    {
        if(sor.ssc == SYES)
        {
            m_itemv.Error_SSC_Flag = true;
        }
    }

    if(m_itemv.Error_SSC_Flag == true)
    {
        if(cg->move == moveForward)
        {
            if(sor.ssc == SNOT)
            {
                m_itemv.Error_SSC_Flag = false;
                //不松驻车制动器起步，但能及时纠正
                JUDGE_MARK_SUB3(2, "06", false, true);
            }
        }
    }

    //状态提示
    char buf[128] = {0};
    SafeSprintf(buf, sizeof(buf), JUDGE_UTF8S("%s->State:%d,项目距离=%d厘米,最大后溜=%dCM"),
                m_itemv.State == 1 ? JUDGE_UTF8S("起步未到达1米...") : JUDGE_UTF8S("起步超过1米..."),
                m_itemv.State, cg->ai_ljjl_cm - m_itemv.Temp_JL_CM, m_itemv.PubMaxLCValue);

    showStatus(buf);

    NS3JudgeVision_40202();
}

//具备起步条件，10秒没有起步
void Sub3Judge02Qbxx::nogo_timeout()
{
    const TChuanGan* cg = m_car->historyChuanGan();
    const TSensorInfo& sor = cg->real.sensor;

    const TChuanGan* his1 = m_car->historyChuanGan(1);
    const TSensorInfo& sor1 = his1->real.sensor;

    // C2  N空挡是0  R倒车档9  P驻车档10  D前进挡11
    //档位或者手刹信号发生变化，且变化的结果是前进挡和松手刹状态，开始计时，10秒后扣分
    if(m_itemv.ZhunBeiQiBuTK == 0)
    {
        const TSensorInfo& sor2 = m_car->historySensor(2);

        if((sor.dw != 0 && sor1.dw != 0 && sor.dw != sor2.dw && sor.dw >= 1 && sor.dw != 9 && sor.dw != 10) ||
                (sor.ssc == SNOT && sor1.ssc == SYES))
        {
            if(sor.dw >= 1 && sor.dw != 9 && sor.dw != 10 && sor.ssc == SNOT)
            {
                m_itemv.ZhunBeiQiBuTK = cg->tkCnt;
            }
        }
    }
    else
    {
        if(sor.dw == 0 || sor.ssc == SYES)
        {
            m_itemv.ZhunBeiQiBuTK = 0;
        }
        if(m_itemv.ZhunBeiQiBuTK != 0 && cg->tkCnt - m_itemv.ZhunBeiQiBuTK > 10 * SECOND)  //10秒未起步
        {
            if(m_car->rtkEnabled() && m_car->rtkEnabled(1))
            {
                if(cg->move != moveForward)
                {
                    JUDGE_MARK_SUB3(2, "48", false);
                    m_itemv.ZhunBeiQiBuTK = 0; //赋值0就是动作变化才重新扣 如果是循环扣分赋值成cg->tkCnt
                }
            }
        }
    }

    /*
    //档位或者手刹信号发生变化，且变化的结果是前进挡和松手刹状态，开始计时，10秒后扣分
    if(m_itemv.ZhunBeiQiBuTK == 0)
    {
        if((sor.dw != 0 && sor1.dw != 0 && sor.dw != sor1.dw && sor.dw >= 1 && sor.dw != 9) ||
                (sor.ssc == SNOT && sor1.ssc == SYES))
        {
            m_itemv.ZhunBeiQiBuTK = cg->tkCnt;
        }
    }
    else
    {
        if(cg->tkCnt - m_itemv.ZhunBeiQiBuTK > 10 * SECOND)  //10秒未起步
        {
            if(gps.rtkEnabled == true && gps1.rtkEnabled == true)
            {
                if(cg->move != moveForward)
                {
                    JUDGE_MARK_SUB3(2, "48", false);
                    m_itemv.ZhunBeiQiBuTK = cg->tkCnt;
                }
            }
        }
    }
    */
}


//6、起步方向灯、喇叭
void Sub3Judge02Qbxx::Judge_QiBu_FXD_LaBa()
{
    const TChuanGan* cg = m_car->historyChuanGan();
    const TSensorInfo& sor = cg->real.sensor;
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    //const TChuanGan* his2 = m_car->historyChuanGan(2);

    //1、检查起步前有没有打转向灯
    //if(cg->move == moveForward && his1->move == moveForward && his2->move == moveStop)
    if(cg->move == moveForward && his1->move == moveForward)
    {
        //可以多次触发起步，但是起步方向灯，全程只评判一次
        if(m_Pub_First_QiBu_Flag == true) return;
        m_Pub_First_QiBu_Flag = true;

        TCar* tcar = m_car->getTCar();
        if(tcar->Night_ID == true)
        {
            //大车夜考起步时未开启前照灯 yhy 2025-02-13
            bool FindFlag = (sor.ygd + sor.jgd != SNOT);
            if(!FindFlag)
            {
                for(int i = 1; i < 50; i++) //前进前10秒看看打没打过前照灯
                {
                    const TChuanGan* hisi = m_car->historyChuanGan(i);
                    if(hisi == nullptr) break;
                    const TSensorInfo& sori = hisi->real.sensor;
                    if(hisi->move == moveStop)
                    {
                        if(sori.ygd + sori.jgd != SNOT)
                        {
                            FindFlag = true;
                            break;
                        }
                    }
                }
            }
            if(!FindFlag)
            {
                JUDGE_MARK_SUB3(13, "01", true);
            }
        }

        bool FindFlag = (sor.zfxd == SYES && sor.yfxd == SNOT);
        if(!FindFlag)
        {
            for(int i = 1; i < 50; i++)
            {
                const TChuanGan* hisi = m_car->historyChuanGan(i);
                if(hisi == nullptr) break;
                const TSensorInfo& sori = hisi->real.sensor;
                if(hisi->move == moveStop)
                {
                    if(sori.zfxd == SYES && sori.yfxd == SNOT)
                    {
                        FindFlag = true;
                        break;
                    }
                }
            }
        }
        if(!FindFlag)
        {
            JUDGE_MARK_SUB3(2, "42", false, true); //JUDGE_MARK_SUB3(20, "73", false, true);
        }
        else
        {
            //看方向灯是否小于3秒
            int OKNum = 0;
            for(int i = 1; i < 50; i++)
            {
                const TChuanGan* hisi = m_car->historyChuanGan(i);
                if(hisi == nullptr) break;
                const TSensorInfo& sori = hisi->real.sensor;
                if(hisi->move == moveStop)
                {
                    if(sori.zfxd == SYES && sori.yfxd == SNOT)
                    {
                        OKNum++;
                    }
                }
            }
            if(OKNum < 3*FRAMES_PER_SECOND)
            {
                //JUDGE_MARK_SUB3(20, "224", false, true);
                JUDGE_MARK_SUB3(2, "43", false, true);
            }
        }

        //起步喇叭评判
        FindFlag = (sor.lb == SYES);
        if(!FindFlag)
        {
            int count = m_car->historyCount();
            for(int i = 1; i < 100; i++)
            {
                if(i >= count) break;
                const TChuanGan* hisi = m_car->historyChuanGan(i);
                const TSensorInfo& sori = hisi->real.sensor;
                if(hisi->move == moveStop)
                {
                    if(sori.lb == SYES)
                    {
                        FindFlag = true;
                        break;
                    }
                }
            }
        }
        if(!FindFlag)
        {
            JUDGE_MARK_SUB3(2, "07", false, true);
        }
    }
}

//22、起步挂错挡
void Sub3Judge02Qbxx::Judge_QiBu_GuaCuoDang()
{
    if(m_car->historyCount() < 2) return;
    const TChuanGan* cg = m_car->historyChuanGan();
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    //const TChuanGan* his5 = m_car->historyChuanGan(5);
    //const TChuanGan* his10 = m_car->historyChuanGan(10);
    //if(cg->move == moveForward && his1->move == moveStop && his5->move == moveStop && his10->move == moveStop)
    if(cg->move == moveForward && his1->move == moveForward)
    {
        const TSensorInfo& sor = cg->real.sensor;
        //小车1档起步, 大车1、2档起步
        //static const int gear = TableSysParm->findQiBuGear(m_car->carCode());
        static const int gear = m_car->getTCar()->StartSpeed;
#if JUDGE_USE_INSPECT
        if(sor.dw_plc > gear)  //无锡所用原始档位
        {
            JUDGE_MARK_SUB3(2, "44", false);
        }
#else
        if(sor.dw > gear)
        {
            JUDGE_MARK_SUB3(2, "44", false);
        }
#endif
    }
}

//1、溜车评判
void Sub3Judge02Qbxx::Judge_LiuChe()
{
    if(!m_car->rtkEnabled() || !m_car->rtkEnabled(1) || !m_car->rtkEnabled(2)) return;

    const TChuanGan* cg = m_car->historyChuanGan();
    const TGpsInfo& gps = cg->real.gps;
    const TSensorInfo& sor = cg->real.sensor;
    const TChuanGan* his1 = m_car->historyChuanGan(1);

    static constexpr int dis30CM = 30; //32 后溜车距离
    static constexpr int dis10CM = 10; //12
    if(cg->move == moveForward && his1->move == moveForward)  //前进的时候要评判溜车小于30厘米的
    {
        if(m_itemv.PubMaxLCValue >= dis10CM && m_itemv.PubMaxLCValue <= dis30CM)
        {
            //通用或起步项目
            JUDGE_MARK_SUB3(2, "47", false);
        }

        m_itemv.PubLCTCPtt.x = 0;
        m_itemv.PubLCTCPtt.y = 0;
        m_itemv.PubMaxLCValue = 0;
        m_itemv.PubCanMarkLC50Flag = true;
    }
    if(sor.dw != 9 && sor.dw != 6 && sor.ssc == SNOT)
    {
        //if(m_itemv.PubLCTCPtt.x == 0 && m_itemv.PubLCTCPtt.y == 0 && m_car->rtkEnabled())
        if(m_car->rtkEnabled())
        {
            const TChuanGan* his2 = m_car->historyChuanGan(2);
            if(cg->move == moveStop && his1->move == moveStop && his2->move == moveStop)
            {
                m_itemv.PubLCTCPtt.x = gps.ai_gps.x;
                m_itemv.PubLCTCPtt.y = gps.ai_gps.y;
                //m_itemv.PubMaxLCValue = 0;
            }
        }
        ExamSensor* sensor = m_car->sensor();
        if(m_itemv.PubLCTCPtt.x != 0 && m_itemv.PubLCTCPtt.y != 0 &&
                gps.ai_gps.x != 0 && gps.ai_gps.y != 0 &&
                sensor->calcMoveState() != moveForward)
        {
            int x = m_itemv.PubLCTCPtt.x - gps.ai_gps.x;
            int y = m_itemv.PubLCTCPtt.y - gps.ai_gps.y;
            int a = std::round(std::sqrt(x*x + y*y)); //cm
            if(m_itemv.PubMaxLCValue < a)
            {
                m_itemv.PubMaxLCValue = a;
            }
            //溜车大于30了，直接扣分
            if(m_itemv.PubCanMarkLC50Flag == true && m_itemv.PubMaxLCValue > dis30CM)
            {
                JUDGE_MARK_SUB3(2, "46", false);
                m_itemv.PubCanMarkLC50Flag = false;
            }
        }
    }
}

void Sub3Judge02Qbxx::NS3JudgeVision_40202()
{
    //起步项目中，车辆状态从停止切换前进前10s内，头部姿态没有大于左后侧角度【23度】
    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.leftBack)
    {
        m_itemv.obsTime = nowTime;
    }

    const TChuanGan* cg = m_car->historyChuanGan();
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    const TChuanGan* his2 = m_car->historyChuanGan(2);
    if(cg->move == moveForward && his1->move == moveForward && his2->move == moveStop)
    {
        if(nowTime - m_itemv.obsTime > 10*SECOND)
        {
            m_itemv.obsTime = nowTime;
            JUDGE_MARK_NSUB3(2, "03", false);
            //NS3video
        }
    }
}