#include "Sub3Judge04Bgcd.h"
#include "HFactory.h"

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

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

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

    const TChuanGan* cg = m_car->historyChuanGan();
    const TRTKResult& RTKKM3 = cg->RTKKM3;
    const std::string& ksdd = TableSysSet->get211();

    if(RTKKM3.BaseLaneCount == 0) return false;
    if(RTKKM3.BasePointInLaneNo == 0) return false;

    m_itemv = TSub3Item04Bgcd();
    m_car->setPUB_JDCC_ZT(0); //借道超车

    if(ksdd != siteof::nj)
    {
        //ToDo:变更车道语音
    }
    //ToDo2:生成进项目事件
    m_itemv.Temp_LJJL_1 = cg->ai_ljjl;
    //1、变更车道项目距离
    const std::vector<std::string>& s504 = TableSysSet->asArray504();
    m_itemv.Temp_XMJL = s504.size() > 0 && s504[0] != "" ? std::atoi(s504[0].c_str()) : 150;
    //2、是否不需要变道，只要判断距离就可以了
    m_itemv.Temp_No_BGCD = s504.size() > 7 && s504[7] != "" ? std::atoi(s504[7].c_str()) : 0;
    //3、不需要实际变道模式下，车辆离右边线距离变化阀值
    m_itemv.Temp_No_BGCD_YuZhi = s504.size() > 8 && s504[8] != "" ? std::atoi(s504[8].c_str()) : 0;
    //4、进入项目时，车身右侧中心点离右边线距离
    if(m_itemv.Temp_No_BGCD == 1)
    {
        m_itemv.Temp_Right_JL = (RTKKM3.Body_RF_ToRightEdge + RTKKM3.Body_RB_ToRightEdge) / 2; //div
    }

    m_itemv.Temp_BasePointInLaneNo = RTKKM3.BasePointInLaneNo;
    m_itemv.Temp_BaseLaneCount = RTKKM3.BaseLaneCount;

    m_exam->TestPro = ItemProFlagJudge;
    return true;
}

void Sub3Judge04Bgcd::dealJudgeItem()
{
    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 TRTKResult& RTKKM3 = cg->RTKKM3;
    const TGpsInfo& gps = cg->real.gps;
    const TSensorInfo& sor = cg->real.sensor;
    const TChuanGan* his1 = m_car->historyChuanGan(1);
    const TRTKResult& RTKKM3_1= his1->RTKKM3;
    //const TGpsInfo& gps1 = his1->real.gps;
    const TSensorInfo& sor1 = his1->real.sensor;
    const std::string& ksdd = TableSysSet->get211();

    if(RTKKM3.BaseLaneCount == 0) return;
    if(RTKKM3.BasePointInLaneNo == 0) return;

    if(ksdd == siteof::lyggy)
    {
        if(RTKKM3.BasePointInLaneNo == RTKKM3.BaseLaneCount)
        {
            m_itemv.Temp_Add_XMJL_Flag = true;
        }
    }
    //如果路口项目没有结束，项目距离加100
    if(ksdd == siteof::nj && m_exam->ItemNo == Sub3ItemType04Bgcd)  //yhy???
    {
        if(m_car->isCrossroadExaming() && !m_itemv.Temp_Add_XMJL_Flag)
        {
            m_itemv.Temp_XMJL += 100;
            m_itemv.Temp_Add_XMJL_Flag = true;
        }
    }

    //特殊地方需求，不可以停车
    if(ksdd == siteof::gdzjkm3)
    {
        if(m_car->historyMoveState(0) == moveStop &&
           m_car->historyMoveState(1) == moveStop &&
           m_car->historyMoveState(2) == moveStop &&
           m_car->historyMoveState(3) == moveStop)
        {
            JUDGE_MARK_SUB3(4, "41", false);
        }
    }

    //取消项目
    //SysSet[378]变更车道和超车:取消第1个项目  (0-不能 1-不变道也不开转向灯 2-开双跳)
    if(TableSysSet->get378() == "2" && sor.shtd == SYES)
    {
        if(m_exam->ItemEnterCancelID == false)
        {
            m_exam->TestPro = ItemProFlagIdle;
            m_exam->Item_Color = itemStateWk;
            m_exam->ItemEnterCancelID = true;
            m_car->setPUB_JDCC_ZT(0); //借道超车
            //ToDo:语音播报： 取消变更车道或者取消超车
            m_car->createEventCancelItem({m_exam->ItemNo, ""});
            return;
        }
    }

    //左方向灯赋值
    if(RTKKM3.TouchLineType != TRTKResult::TouchLineType0)
    {
        if(!m_itemv.JudgeFXD)
        {
            m_itemv.JudgeFXD = true;
            if(sor.zfxd == SYES && sor.yfxd == SNOT)
            {
                m_itemv.Temp_ZFXD = true;
                //跟踪3秒前
                bool OKFlag = true;
                for(int i = 1; i <= 14; i++)
                {
                    const TSensorInfo& sori = m_car->historySensor(i);
                    if(sori.zfxd == SNOT || sori.yfxd == SYES)
                    {
                        OKFlag = false;
                        break;
                    }
                }
                m_itemv.Temp_ZFXD3s = OKFlag;
            }

            if(sor.zfxd == SNOT && sor.yfxd == SYES)
            {
                m_itemv.Temp_YFXD = true;
                //跟踪3秒前
                bool OKFlag = true;
                for(int i = 1; i <= 14; i++)
                {
                    const TSensorInfo& sori = m_car->historySensor(i);
                    if(sori.zfxd == SYES || sori.yfxd == SNOT)
                    {
                        OKFlag = false;
                        break;
                    }
                }
                m_itemv.Temp_YFXD3s = OKFlag;
            }

        }
    }


    /*
    //左方向灯赋值
    if(sor.zfxd == SYES && sor.yfxd == SNOT)
    {
        m_itemv.Temp_ZFXD = true;
        if(m_itemv.Temp_ZFXD_TM == 0)
        {
            m_itemv.Temp_ZFXD_TM = gps.sj;
        }
        else
        {
            if(std::abs(gps.sj - m_itemv.Temp_ZFXD_TM) >= 3000)
            {
                m_itemv.Temp_ZFXD3s = true;
            }
        }
    }
    else
    {
        if(!m_itemv.Temp_ZFXD3s)
        {
            m_itemv.Temp_ZFXD_TM = 0;
        }
    }

    //右方向灯赋值
    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;
        }
    }
    */

    if(m_itemv.Temp_No_BGCD == 1)
    {
        int dis = (RTKKM3.Body_RF_ToRightEdge + RTKKM3.Body_RB_ToRightEdge) / 2; //div
        if(int(std::abs(dis - m_itemv.Temp_Right_JL)) >= m_itemv.Temp_No_BGCD_YuZhi)
        {
            m_itemv.Temp_No_BGCD_FinishFlag = true;
        }
    }

    //计算变道方向
    int BianDaoKind = 0; //0:未变道  1：往左侧变道 2:往右侧变道
    if(RTKKM3.BaseLaneCount == m_itemv.Temp_BaseLaneCount && RTKKM3.BasePointInLaneNo > m_itemv.Temp_BasePointInLaneNo)
    {
        BianDaoKind = 1;
    }
    //左侧变道2
    if(RTKKM3.BaseLaneCount < m_itemv.Temp_BaseLaneCount)
    {
        int offset = m_itemv.Temp_BaseLaneCount - RTKKM3.BaseLaneCount;
        if(offset == 1)
        {
            if(RTKKM3.BasePointInLaneNo <= m_itemv.Temp_BasePointInLaneNo)
            {
                BianDaoKind = 1;
            }
        }
        else
        {
            BianDaoKind = 1;
        }
    }
    //左侧变道3
    if(RTKKM3.BaseLaneCount > m_itemv.Temp_BaseLaneCount)
    {
        int offset = RTKKM3.BaseLaneCount - m_itemv.Temp_BaseLaneCount;
        if(RTKKM3.BasePointInLaneNo >= m_itemv.Temp_BasePointInLaneNo + offset + 1)
        {
            BianDaoKind = 1;
        }
    }

    //右侧变道1（因为我们测绘定义的缺陷，右侧变道暂不考虑车道数变多或者变少的情况)
    if(RTKKM3.BaseLaneCount == m_itemv.Temp_BaseLaneCount && RTKKM3.BasePointInLaneNo < m_itemv.Temp_BasePointInLaneNo)
    {
        BianDaoKind = 2;
    }

    //特殊情况1:借道
    //处理中心线虚线 左侧借道
    if(RTKKM3.BaseLaneCount == 1)
    {
        if(RTKKM3.TouchLineType == TRTKResult::TouchLineType6 && RTKKM3_1.TouchLineType == TRTKResult::TouchLineType6)
        {
            if(RTKKM3.Wheel_LF_ToBaseLine < RTKKM3.Wheel_RF_ToBaseLine && RTKKM3_1.Wheel_LF_ToBaseLine < RTKKM3_1.Wheel_RF_ToBaseLine)
            {
                BianDaoKind = 1; //左侧借道
            }
        }
    }

    if(BianDaoKind == 1)  //左侧变道
    {
        m_itemv.LeftBianDaoFlag = true;
    }
    else if(BianDaoKind == 2)  //右侧变道
    {
        m_itemv.RightBianDaoFlag = true;
    }

    if(m_itemv.LeftBianDaoFlag == true || m_itemv.RightBianDaoFlag == true)
    {
        if(m_itemv.Status <= 2)
        {
            //SysSet[414]:变道、超车以前后轮都过线（0-否 1-是）
            if(TableSysSet->get414() == "1")
            {
                if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
                {
                    m_itemv.Status = 3;
                    m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
                    return;
                }
                else
                {
                    m_itemv.Status = 13;
                    m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
                    return;
                }

            }
            else
            {
                if(ksdd != siteof::dingxi)
                {
                    if(ksdd == siteof::chongqingkm3)
                    {
                        if(m_exam->ItemNo == Sub3ItemType04Bgcd)
                        {
                            if(sor.dw < 2 && sor1.dw < 2 && IS_C1(m_carType))  //20171210   20171216
                            {
                                m_itemv.Status = 4;
                                JUDGE_MARK_SUB3(4, "41", false); //20160928
                                return;
                            }
                        }
                    }
                    m_itemv.Status = 3;
                    m_itemv.Temp_LJJL_2 = cg->ai_ljjl;
                    return;
                }
            }
        }
    }
    else
    {
        if(cg->ai_ljjl - m_itemv.Temp_LJJL_1 >= m_itemv.Temp_XMJL)  //出项目距离，结束
        {
            //到达这个距离，还没有完成项目，肯定要扣分的（除非你取消了项目）
            //SysSet[378]:变更车道和超车:取消第1个项目 (0-不能 1-不变道也不开转向灯 2-开双跳)
            if(TableSysSet->get378() == "1")  //如果你正常完成了，不可能直行到项目距离这里
            {
                if(m_exam->ItemEnterCancelID == false)
                {
                    m_exam->TestPro = ItemProFlagIdle;
                    m_exam->Item_Color = itemStateWk;
                    m_exam->ItemEnterCancelID = true;
                    m_car->setPUB_JDCC_ZT(0); //借道超车
                    //ToDo:播报语音：取消变更车道
                    m_car->createEventCancelItem({m_exam->ItemNo, ""});
                    return;
                }
                else
                {
                    Judge_XMJL_Finish();
                    return;
                }
            }
            else
            {
                Judge_XMJL_Finish();
                return;
            }
        }
    }

    if(m_itemv.Status == 13)
    {
        if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
        {
            //20181024
            if(m_itemv.Temp_BasePointInLaneNo == RTKKM3.BasePointInLaneNo && m_itemv.Temp_BaseLaneCount == RTKKM3.BaseLaneCount)
            {
                //不按规定变道
                //20180317
                JUDGE_MARK_SUB3(4, "41", false); //20160928
                m_itemv.Status = 4;
                return;
            }
            else
            {
                m_itemv.Status = 3;
                return;
            }
        }
        else
        {
            //20171207
            if(ksdd == siteof::chongqingkm3)
            {
                if(sor.dw < 2 && sor1.dw < 2 && IS_C1(m_carType))  //20171210   20171216
                {
                    m_itemv.Status = 4;
                    JUDGE_MARK_SUB3(4, "41", false); //20160928
                    return;
                }
            }
        }
    }

    if(m_itemv.Status == 3) //变道发生，延长一定距离后结束
    {

        if(cg->ai_ljjl - m_itemv.Temp_LJJL_2 >= 10) //延长距离，结束
        {
            if(TableSysSet->get414() == "1")
            {
                if(RTKKM3.TouchLineType == TRTKResult::TouchLineType0)
                {
                    m_itemv.Status = 4;
                    return;
                }
            }
            else
            {
                m_itemv.Status = 4;
                return;
            }
        }
    }

    if(m_itemv.Status == 4)
    {
        m_exam->TestPro = ItemProFlagEnd;
        flagEndJudge();
        return;
    }

    char buf[128] = {0};
    SafeSprintf(buf, sizeof(buf), XCharacter("Status=%d,车道数(初始,当前)-->%d,%d  车道号(初始,当前)-->%d,%d"),
                m_itemv.Status, m_itemv.Temp_BaseLaneCount, RTKKM3.BaseLaneCount,
                m_itemv.Temp_BasePointInLaneNo, RTKKM3.BasePointInLaneNo);
    showStatus(std::string(buf));
}

void Sub3Judge04Bgcd::Judge_XMJL_Finish()
{
    if(m_itemv.Temp_No_BGCD == 1)
    {
        if(!m_itemv.Temp_No_BGCD_FinishFlag)
        {
            JUDGE_MARK_SUB3(4, "41", false);
        }
    }
    else
    {
        if(m_itemv.Status < 3)
        {
            JUDGE_MARK_SUB3(4, "41", false); //20160928
        }
    }

    if(m_itemv.Temp_ZFXD == false && m_itemv.Temp_YFXD == false)
    {
        if(!m_itemv.Mark_4_42_Flag && m_itemv.Status >= 3) //小于3就是还没变道，不判未打灯
        {
            m_itemv.Mark_4_42_Flag = true;
            JUDGE_MARK_SUB3(4, "42", false);
        }
    }
    else
    {
        if(m_itemv.Temp_ZFXD3s == false && m_itemv.Temp_YFXD3s == false)
        {
            if(!m_itemv.Mark_4_43_Flag)
            {
                m_itemv.Mark_4_43_Flag = true;
                JUDGE_MARK_SUB3(4, "43", false);
            }
        }
    }
    m_exam->TestPro = ItemProFlagEnd;
}


void Sub3Judge04Bgcd::flagEndJudge()
{
    //finally:
    if(m_exam->TestPro == ItemProFlagEnd)
    {
        //检查方向灯
        if(m_itemv.LeftBianDaoFlag == true)  //向左变道的
        {
            if(!m_itemv.Temp_ZFXD)
            {
                if(!m_itemv.Mark_4_42_Flag) // && m_itemv.Status >= 3 小于3就是还没变道，不判未打灯
                {
                    m_itemv.Mark_4_42_Flag = true;
                    JUDGE_MARK_SUB3(4, "42", false);
                }
            }
            else
            {
                if(!m_itemv.Temp_ZFXD3s)
                {
                    if(!m_itemv.Mark_4_43_Flag)
                    {
                        m_itemv.Mark_4_43_Flag = true;
                        JUDGE_MARK_SUB3(4, "43", false);
                    }
                }
            }
        }
        else if(m_itemv.RightBianDaoFlag == true)  //向右变道的
        {
            if(!m_itemv.Temp_YFXD)
            {
                if(!m_itemv.Mark_4_42_Flag) // && m_itemv.Status >= 3 小于3就是还没变道，不判未打灯
                {
                    m_itemv.Mark_4_42_Flag = true;
                    JUDGE_MARK_SUB3(4, "42", false);
                }
            }
            else
            {
                if(!m_itemv.Temp_YFXD3s)
                {
                    if(!m_itemv.Mark_4_43_Flag)
                    {
                        m_itemv.Mark_4_43_Flag = true;
                        JUDGE_MARK_SUB3(4, "43", false);
                    }
                }
            }
        }
    }
}