//
// Created by YY on 2020/3/17.
//

#include "road_exam.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"
#include "drive_straight.h"
#include "stop_car.h"
#include "operate_gear.h"
#include "../test_common/odo_graph.h"
#include "car_start.h"
#include "overtake.h"
#include "change_lane.h"

#include <cmath>
#include <vector>
#include <list>
#include <map>
#include <string>
#include <cstdlib>
#include <algorithm>

#define DEBUG(fmt, args...)     LOGD("<road_exam> <%s>: " fmt, __func__, ##args)

using namespace std;


enum {
    CROSSING_NOT_HINT,
    CROSSING_HAS_HINT,
};

typedef struct {
    int road;
    int sep;
    int no;
    int total;
    int guide;
} lane_t;

typedef struct {
    int edgeIndex;
    int pointIndex;
    PointF point;
} projection_t;

typedef struct {
    int gain;
    struct RtkTime time;
} change_lane_t;

typedef struct {
    int road_index;
    int stop_line_index;
    int sep;
    double distance;
} road_end_point_t;

static const int INVALID_ROAD = -1;

static const int CROSSING_TURN_THRESHOLD = 35;
static const int TURN_THRESHOLD = 3;

static bool occurOverSpeed;
static bool occurSecondBreak;


static int prevMoveDirect;

static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
static int GearNSlideStatus = 0;

static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;

static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
static struct RtkTime gearChangeTimePoint;
static struct RtkTime gearErrorTimePoint;
static struct RtkTime gearNSlideTimePoint;

static int gearChange;

static car_sensor_value_t turnSignalStatus;

static int gearErrorTime;

static int currExamMapIndex;

static lane_t Lane;
static change_lane_t ChangeLane;

static int CrashLineType;

static map<int, car_sensor_value_t> CarSensorValue;
static map<int, int> CrossingHint;
static map<int, bool> ErrorLaneReport;

typedef struct {
    int road_id;
    double distance;
} trigger_detect_t;
static map<int, trigger_detect_t> TriggerDetect;           // 距离本路段各触发点的距离

#define ROAD_EXAM_READY_NEXT            0
#define ROAD_EXAM_FREE_RUN              1
#define ROAD_EXAM_ITEM_CAR_START            2
#define ROAD_EXAM_ITEM_STRAIGHT            3
#define ROAD_EXAM_ITEM_OP_GEAR            4
#define ROAD_EXAM_ITEM_CHANGE_LANE           5
#define ROAD_EXAM_ITEM_OVER_TAKE            6
#define ROAD_EXAM_ITEM_CAR_STOP            7

#define ROAD_EXAM_ITEM_NOT_EXEC             0
#define ROAD_EXAM_ITEM_EXECING             1
#define ROAD_EXAM_ITEM_EXECED             2


typedef struct {
    int name;
    int status;
} RoadExamItem_t;
static map<int, int> RoadExamItem;
static int RoadExamStatus;
static bool win;

static struct RtkTime beginTurnTime, prevDetectTurnTime;
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;

static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);

static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);

static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);

static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);

void InitRoadExam(road_exam_map &RoadMap)
{
    DEBUG("Start road_exam");

    occurOverSpeed = false;

    occurSecondBreak = false;

    prevMoveDirect = 0;

    StopCarOnRedArea = false;
    occurSlide = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    prevGearError = false;
    gearErrorTime = 0;
    GearNSlideStatus = 0;

    currExamMapIndex = -1;

    Lane.road = Lane.sep = Lane.no = -1;
    Lane.guide = 0;


    CrashLineType = -1;
    turnCnt = -1;

    ResetOdo();
    ResetTarget(RoadMap);
    // 初始化考项
    RoadExamItem.clear();
    RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC;
    RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;

    RoadExamStatus = ROAD_EXAM_READY_NEXT;

    TriggerDetect.clear();

    win = false;
}

void TerminateRoadExam(void)
{
    TerminateDummyLightExam();
//    TerminateStopCarExam();
//    TerminateOperateGearExam();
//    TerminateDriveStraightExam();
}

/*********************************************************************
 * 计算某点到道路边线的投影点
 * @param edge
 * @param road
 * @param point
 * @return
 */
PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    PointF proj;

    vector<PointF> projArray;

    Line line;

    // 计算垂点
    for (int i = 0; i < edge.size(); ++i) {
        p1 = edge[i].points[0];
        for (int j = 1; j < edge[i].points.size(); ++j) {
            p2 = edge[i].points[j];
            MakeLine(&line, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(point, line, vp)) {
                projArray.push_back(vp);
            }

            p1 = p2;
        }
    }

    if (projArray.size() == 0) {
        // 没有任何垂点，找到最近的点
        proj = edge[0].points[0];
        for (int i = 0; i < edge.size(); ++i) {
            for (int j = 1; j < edge[i].points.size(); ++j) {
                if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) {
                    proj = edge[i].points[j];
                }
            }
        }
    } else {
        // 最近的垂点
        proj = projArray[0];
        for (int i = 1; i < projArray.size(); ++i) {
            if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) {
                proj = projArray[i];
            }
        }
    }

    return proj;
}

static projection_t CalcProjectionWithRoadEdgeEx(vector<edge_t> &edge, PointF point)
{
    PointF p1, p2;
    Line line;
    projection_t theProj;

    vector<projection_t> projSet;

    // 计算垂点
    for (int i = 0; i < edge.size(); ++i) {
        p1 = edge[i].points[0];
        for (int j = 1; j < edge[i].points.size(); ++j) {
            p2 = edge[i].points[j];
            MakeLine(&line, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(point, line, vp)) {
                projection_t proj;
                proj.edgeIndex = i;
                proj.pointIndex = j - 1;
                proj.point = vp;
                projSet.push_back(proj);
            }

            p1 = p2;
        }
    }

    if (projSet.size() == 0) {
        // 没有任何垂点，找到最近的点
        theProj.edgeIndex = 0;
        theProj.pointIndex = 0;
        theProj.point = edge[0].points[0];

        for (int i = 0; i < edge.size(); ++i) {
            for (int j = 1; j < edge[i].points.size(); ++j) {
                if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) {
                    theProj.edgeIndex = i;
                    theProj.pointIndex = j - 1;
                    theProj.point = edge[i].points[j];
                }
            }
        }
    } else {
        // 最近的垂点
        theProj = projSet[0];
        for (int i = 1; i < projSet.size(); ++i) {
            if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) {
                theProj = projSet[i];
            }
        }
    }

    return theProj;
}

double CalcDistanceReference(PointF point, PointF refPoint, vector<edge_t> &edge)
{
    double distance = 0;

    projection_t projection, refProjection;

    projection = CalcProjectionWithRoadEdgeEx(edge, point);
    refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint);

    if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) {
        bool ahead = false;
        if (projection.edgeIndex != refProjection.edgeIndex) {
            if (projection.edgeIndex > refProjection.edgeIndex) {
                projection_t temp;
                temp = projection;
                projection = refProjection;
                refProjection = temp;
                ahead = true;
            }
            int a = projection.edgeIndex;
            int b = projection.pointIndex + 1;

            if (b < edge[a].points.size()) {
                distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
            }

            for (; a < refProjection.edgeIndex; ++a) {
                for (; b < edge[a].points.size() - 1; ++b) {
                    distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
                }
                b = 0;
            }

            for (int i = 0; i < refProjection.pointIndex; i++) {
                distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]);
            }

            distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
        } else {
            if (projection.pointIndex > refProjection.pointIndex) {
                projection_t temp;
                temp = projection;
                projection = refProjection;
                refProjection = temp;
                ahead = true;
            }
            int a = projection.edgeIndex;
            int b = projection.pointIndex + 1;

            if (b < edge[a].points.size()) {
                distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]);
            }

            for (; b < refProjection.pointIndex; b++) {
                distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]);
            }

            distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]);
        }
        if (ahead) {
            distance *= -1.0;
        }
    } else {
        distance = DistanceOf(projection.point, refProjection.point);

        if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) >
                DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) {
            distance *= -1.0;
        }
    }

    return distance;
}

/***************************************************************
 * 车辆所在道路，根据车辆的中轴前点
 * @param currRoadIndex, 优先检测当前道路
 * @param RoadMap
 * @param car
 * @return
 */
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car)
{
    int n = 0;
    int index = currRoadIndex;

    if (index < 0) {
        index = 0;
    }

    while (n < RoadMap.roads.size()) {
        bool changeSegment = false;
        vector<PointF> roadOutLine;
        Polygon area;

        for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) {
            for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) {
                if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
                    continue;
                }
                changeSegment = false;
                roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]);
            }
            changeSegment = true;
        }

        for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) {
            for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) {
                if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) {
                    continue;
                }
                changeSegment = false;
                roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]);
            }
            changeSegment = true;
        }

        area.num = roadOutLine.size();
        area.point = (PointF *) malloc(area.num * sizeof(PointF));

        for (int i = 0; i < area.num; ++i) {
            area.point[i] = roadOutLine[i];
        }

        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
            free(area.point);
            break;
        }

        free(area.point);

        index = (index + 1) % RoadMap.roads.size();
        n++;
    }

    if (n >= RoadMap.roads.size()) {
        index = -1;
    }

    return index;
}

/************************************************
 * 车轮压实线，前后轴交叉，前后轮迹交叉
 * @param currCrashLineType
 * @param RoadMap
 * @param car
 * @param CarModelList
 * @return
 */
static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    Line frontTireAxle, rearTireAxle;
    Line frontLeftTireTrack, frontRightTireTrack;
    Line rearLeftTireTrack, rearRightTireTrack;
    bool track = false;
    int lineType = -1;
    PointF p11, p12, p21, p22, p31, p32, p41, p42;

    MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    if (CarModelList.size() >= 2) {
        list<car_model *>::iterator iter = CarModelList.begin();
        p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
        p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
        p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];

        struct RtkTime time1 = ((car_model *)(*iter))->tm;

        ++iter;

        p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
        p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
        p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]];

        struct RtkTime time2 = ((car_model *)(*iter))->tm;

        if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) {
            MakeLine(&frontLeftTireTrack, &p11, &p12);
            MakeLine(&frontRightTireTrack, &p21, &p22);
            MakeLine(&rearLeftTireTrack, &p31, &p32);
            MakeLine(&rearRightTireTrack, &p41, &p42);
            track = true;
        }
    }

    PointF p1, p2;
    Line redLine;

    // 左右路边实线
    for (int n = 0; n < RoadMap.roads.size(); ++n) {
        for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) {
            if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) {
                p1 = RoadMap.roads[n].leftEdge[m].points[0];
                for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) {
                    p2 = RoadMap.roads[n].leftEdge[m].points[l];

                    MakeLine(&redLine, &p1, &p2);

                    if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                        IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }

                    if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }
                    p1 = p2;
                }
            }
        }

        for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) {
            if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) {
                p1 = RoadMap.roads[n].rightEdge[m].points[0];
                for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) {
                    p2 = RoadMap.roads[n].rightEdge[m].points[l];

                    MakeLine(&redLine, &p1, &p2);

                    if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                        IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }

                    if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        lineType = LINE_SOLID;
                        goto CRASH_LINE_CONFIRM;
                    }
                    p1 = p2;
                }
            }
        }

        // 分道实线
//        DEBUG("%d 分道段 %d", n, RoadMap.roads[n].separate.size());

        for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
            // 一组车道
//            DEBUG("%d %d 车道数 %d", n, m, RoadMap.roads[n].separate[m].lines.size());

            for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
                // 多根分道线

//                DEBUG("%d %d %d 线段树 %d", n, m, l, RoadMap.roads[n].separate[m].lines[l].size());
                for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
                    // 一根分道线中线型相同的
                    int character = RoadMap.roads[n].separate[m].lines[l][a].character;

                    if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2)
                        continue;

                    p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0];

                    for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) {
                        p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b];
                        MakeLine(&redLine, &p1, &p2);
                        if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                            IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                            lineType = character;
                            goto CRASH_LINE_CONFIRM;
                        }
                        if (track &&
                            (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                             IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                            lineType = character;
                            goto CRASH_LINE_CONFIRM;
                        }
                        p1 = p2;
                    }

                }
            }
        }
    }

    for (int n = 0; n < RoadMap.forbidLines.size(); ++n) {
        // 地图围栏
        if (RoadMap.forbidLines[n].type == LINE_BOUNDARY && track && RoadMap.forbidLines[n].points.size() >= 2) {
            p1 = RoadMap.forbidLines[n].points[0];
            for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
                p2 = RoadMap.forbidLines[n].points[m];
                MakeLine(&redLine, &p1, &p2);
                if (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                     IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection) {
                    lineType = LINE_BOUNDARY;
                    goto CRASH_LINE_CONFIRM;
                }
                p1 = p2;
            }
        }
        // 其它红线
        if (RoadMap.forbidLines[n].type == LINE_SOLID && RoadMap.forbidLines[n].points.size() >= 2) {
            p1 = RoadMap.forbidLines[n].points[0];
            for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
                p2 = RoadMap.forbidLines[n].points[m];
                MakeLine(&redLine, &p1, &p2);

                if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                    IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                    lineType = LINE_SOLID;
                    goto CRASH_LINE_CONFIRM;
                }

                if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                    lineType = LINE_SOLID;
                    goto CRASH_LINE_CONFIRM;
                }
                p1 = p2;
            }
        }
    }

CRASH_LINE_CONFIRM:
    if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
        if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
            // 非法跨线
            lineType += 100;
        }
    } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) {
        if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) {
            // 非法跨线
            lineType += 100;
        }
    }
    return lineType;
}

static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
    for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) {
        double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge);
        double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge);
        if (d1 < -1e-3 && d2 > 1e-3 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) {
            return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo];
        }
    }
    return 0;
}

/********************************************
 * 计算某点在哪个车道
 * @param point
 * @param RoadMap
 * @param roadIndex
 * @return
 */
static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex)
{
    Line leftProjLine, rightProjLine;
    Line sep;
    PointF p0, p1, p2;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return false;

    p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
    p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point);

    MakeLine(&leftProjLine, &point, &p1);
    MakeLine(&rightProjLine, &point, &p2);

    for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
        struct car_lane_pos {
            int lane_no;
            char pos;            // 点在车道的左右位置

            bool operator == (const int &x) {
                return (this->lane_no == x);
            }
        };

        vector<struct car_lane_pos> CLP;
        bool oneMoreIntersection = false;

        for (int j = 0;  j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {              // 每一根线分组
            bool intersection = false;
            for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {        // 同一线型的线分组

                p1 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0];
                for (int m = 1; m < RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size(); ++m) {
                    p2 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[m];
                    MakeLine(&sep, &p1, &p2);

                    if (IntersectionOf(sep, leftProjLine) == GM_Intersection) {
                        intersection = true;
                        struct car_lane_pos temp;

                        temp.lane_no = j;
                        temp.pos = 'R';
                        CLP.push_back(temp);
                        break;
                    }
                    if (IntersectionOf(sep, rightProjLine) == GM_Intersection) {
                        intersection = true;
                        struct car_lane_pos temp;

                        temp.lane_no = j;
                        temp.pos = 'L';
                        CLP.push_back(temp);
                        break;
                    }
                    p1 = p2;
                }
            }
            if (!intersection) {
                struct car_lane_pos temp;
                temp.lane_no = j;
                temp.pos = 0;
                CLP.push_back(temp);
            } else {
                oneMoreIntersection = true;
            }
        }

        if (oneMoreIntersection) {
            // 考虑到采集偏差可能每一根线无法对齐，只要其中一根相交，其它不相交的也参与
            for (vector<struct car_lane_pos>::iterator it = CLP.begin(); it != CLP.end(); ++it) {
                if ((*it).pos == 0) {
                    int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size();
                    int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size();

                    if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) >
                        DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) {
                        // 车辆接近车道尾部
                        MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]);
                    } else {
                        // 车辆接近车道头部
                        MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]);
                    }

                    struct car_lane_pos temp;

                    temp.lane_no = (*it).lane_no;

                    if (IntersectionOfLine(point, sep) == -1) {
                        temp.pos = 'R';
                    } else {
                        temp.pos = 'L';
                    }
                    (*it) = temp;
                }
            }

            lane.road = roadIndex;
            lane.sep = i;
            lane.no = CLP.size();
            lane.total = CLP.size() + 1;

            for (int x = 0; x < CLP.size(); ++x) {
                if (CLP[x].pos == 'L') {
                    lane.no = x;
                    break;
                }
            }
            lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no);
            return true;
        }
    }

    lane.road = roadIndex;
    lane.sep = 0;
    lane.no = 0;
    lane.total = 0;
    lane.guide = 0;

    return false;
}

static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime)
{
    car_sensor_value_t nv;

    nv.name = name;
    nv.value = value;
    nv.time = *rtkTime;

    auto it = CarSensorValue.find(name);

    if (it != CarSensorValue.end()) {
        if (it->second.value != value || name == ENGINE_RPM) {
            CarSensorValue[name] = nv;
        }
    } else {
        CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv));
    }
}

car_sensor_value_t ReadCarSensorValue(int name)
{
    car_sensor_value_t nv = {.name = -1};

    auto it = CarSensorValue.find(name);

    if (it != CarSensorValue.end()) {
        nv = it->second;
    }

    return nv;
}

static void UpdateCarSensor(const struct RtkTime *rtkTime)
{
    WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime);
    WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime);
    WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime);
    WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime);
    WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
    WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
    WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}

static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
{
    static int checkCrashGreenTimeout = 0;
    static bool crashDottedLine = false;
    int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList);

    if (newLineType >= 100) {
        newLineType -= 100;
        DEBUG("非法跨越分道线");
        AddExamFault(11, rtkTime);
    }
    if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
        // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
        DEBUG("撞道路边缘线");
        AddExamFault(11, rtkTime);
    }
    if (newLineType == LINE_BOUNDARY && CrashLineType != LINE_BOUNDARY) {
        // 车辆越界，逃跑了，不合格
        DEBUG("车辆越界");
        AddExamFault(3, rtkTime);
    }

    if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
        if (!crashDottedLine) {
            checkCrashGreenTimeout = 0;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            crashGreenStartTime = *rtkTime;
            turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            DEBUG("开始压虚线 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
        }
        crashDottedLine = true;
    } else {
        crashDottedLine = false;
    }

    if (crashDottedLine) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                crashGreenRunTime = *rtkTime;           // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= examParam.crash_dotted_line_cumulative_time) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }
    } else {
        // 不再压虚线
        checkCrashGreenTimeout = 0;
    }

     CrashLineType = newLineType;
}

int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime)
{
    lane_t newLane;
    int gain = 0;

    if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) {
        // 排除跨线中
        return 0;
    }
    if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
        if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
            gain = newLane.no - Lane.no;
        } else {
            DEBUG("车道分段切换 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
            ChangeLane.gain = 0;
        }

        // 检查转向灯
        if (gain != 0) {
            DEBUG("变道 new lane %d, gain %d", newLane.no, gain);
            if (gain < 0) {
                if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
                    DEBUG("变调未打灯!!");
                    // 没打灯，不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                           examParam.turn_signal_min_advance) {
                    DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                          crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
                          crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
                          crashGreenStartTime.mss * 10,
                          turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
                          turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);

                    // 不足3秒，不合格
                    AddExamFault(14, rtkTime);
                }
            } else {
                if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
                    DEBUG("变调未打灯!!");
                    // 没打灯，不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                        examParam.turn_signal_min_advance) {
                    DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                          crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
                          crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
                          crashGreenStartTime.mss * 10,
                          turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
                          turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
                    // 不足3秒，不合格
                    AddExamFault(14, rtkTime);
                }
            }

            if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < examParam.continuous_change_lane_min_time) {
                DEBUG("连续变道");
                AddExamFault(15, rtkTime);
            }
            ChangeLane.gain = gain;
            ChangeLane.time = *rtkTime;
        }

        Lane = newLane;
    }
    return gain;
}

static void ChangeCrossingStatus(int roadIndex, int index, int status)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        CrossingHint.erase(it);
    }
    CrossingHint.insert(pair<int, int>(key, status));
}

static int GetCrossingStatus(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        return it->second;
    }
    return CROSSING_NOT_HINT;
}

/*****************************************************
 * 以正常通过路口或自由驾驶离开路段后
 * @param roadIndex
 */
static void ResetCrossingStatus(road_exam_map &RoadMap, int roadIndex)
{
    while (true) {
        bool w = false;
        for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
            if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
                ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
                CrossingChange(RoadMap.roads[roadIndex].id, it->first % 100, 0);
                w = true;
                break;
            }
        }
        if (!w)
            break;
    }
}

static void SetErrorLaneRpt(int roadIndex, int index, bool status)
{
    int key = roadIndex * 100 + index;

    auto it = ErrorLaneReport.find(key);

    if (it != ErrorLaneReport.end()) {
        ErrorLaneReport.erase(it);
    }
    ErrorLaneReport.insert(pair<int, bool>(key, status));
}

static bool GetErrorLaneRpt(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = ErrorLaneReport.find(key);

    if (it != ErrorLaneReport.end()) {
        return it->second;
    }
    return false;
}

static void ResetErrorLaneRpt(int roadIndex)
{
    while (true) {
        bool w = false;
        for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
            if (it->first / 100 == roadIndex && it->second == true) {
                SetErrorLaneRpt(it->first / 100, it->first % 100, false);
                w = true;
                break;
            }
        }
        if (!w)
            break;
    }
}

/*********************************************************
 * 计算当前所在分道线的剩余长度
 * @param RoadMap
 * @param lane
 * @param car
 * @return
 */
static double SeparateLength(road_exam_map &RoadMap, lane_t lane, const car_model *car)
{
    double distance = 0;

    if (lane.road < 0 || lane.total == 0 || lane.sep < 0)
        return distance;

    int n = RoadMap.roads[lane.road].separate[lane.sep].lines[0].size();
    int m = RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points.size();

    return CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]],
            RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points[m-1],
            RoadMap.roads[lane.road].rightEdge);
}

/***********************************************************
 * 前向最近的路口停止线或导向线起点，一定距离内做出路口提示，或项目自动触发
 * @param roadIndex
 * @param RoadMap
 * @param car
 */
static road_end_point_t NearbyRoadEndPoint(int roadIndex, road_exam_map &RoadMap, const car_model *car)
{
    road_end_point_t rec;

    rec.road_index = roadIndex;
    rec.stop_line_index = 0;
    rec.distance = 0;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return rec;

    // 最近的路口距离
    int stop_line_index;
    PointF nearbyStopPoint;

    NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
    nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;

    double dis1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], nearbyStopPoint, RoadMap.roads[roadIndex].rightEdge);

    struct ep_ {
        int sep;
        double distance;
    } ep;

    vector<struct  ep_> dset;

    ep.sep = -1;
    ep.distance = dis1;
    dset.push_back(ep);

    // 最近的分道线距离，且不超过最近路口
    for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
        for (int j = 0;  j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {
            for (int k = 0; k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {
                if (RoadMap.roads[roadIndex].separate[i].lines[j][k].character != LINE_DOTTED &&
                        RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size() > 0) {
                    double dis2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0], RoadMap.roads[roadIndex].rightEdge);

                    if (dis2 < -1e-3) {
                        continue;
                    }else if (dis2 < dis1) {
                        // 找出最短的点
                        ep.sep = i;
                        ep.distance = dis2;
                        dset.push_back(ep);
                        continue;
                    }
                    goto FIND_END_POINT;
                }
            }
        }
    }

    FIND_END_POINT:
    sort(dset.begin(), dset.end(), [=](struct ep_ x, struct ep_ y)->bool {return x.distance < y.distance;});

    rec.stop_line_index = stop_line_index;
    rec.distance = dset[0].distance;
    rec.sep = dset[0].sep;

    return rec;
}

int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
{
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
        return -1;
    }
    for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
        if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
                RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
            return RoadMap.examScheme[0].crossingActive[i].active;
        }
    }
    return -1;
}

static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;

    if (distance > 5 && distance < 70) {
        // 提示路口怎么走
        if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
            // 项目标定模式下，这里提示标定本路口动作
            if (RoadMap.calibrate) {
                CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
            } else {
                int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);

                switch (act) {
                    case ROAD_ACTIVE_FORWARD:
                        DEBUG("路口提示 直行");
                        PlayTTS(examParam.crossing_go_straight_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_LEFT:
                        DEBUG("路口提示 左转");
                        PlayTTS(examParam.crossing_turn_left_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_RIGHT:
                        DEBUG("路口提示 右转");
                        PlayTTS(examParam.crossing_turn_right_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_BACKWARD:
                        DEBUG("路口提示 掉头");
                        PlayTTS(examParam.crossing_turn_back_tts, NULL);
                        break;
                    default:
                        DEBUG("路口提示 未配置");
                        PlayTTS(examParam.crossing_turn_unknown_tts, NULL);
                        break;
                }
            }
            ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
        }
    } else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
        ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);

        if (RoadMap.calibrate) {
            CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
        }
    }
}

static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
    int guide = 0, stopLine = 0;
    double distance;

    struct nearby_crossing {
        int stopLine;
        int guide;
        double distance;
    };

    vector<struct nearby_crossing> set;

    for (int i = 0; i < road.stopLine.size(); ++i) {
        PointF point;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point,
                                         road.rightEdge);

        if (distance > 1e-3) {
            struct nearby_crossing temp;

            temp.stopLine = i;
            temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
            temp.distance = distance;

            set.push_back(temp);
        }
    }

    if (set.size() > 0) {
        distance = set[0].distance;
        guide = set[0].guide;

        for (int i = 1; i < set.size(); ++i) {
            if (distance > set[i].distance) {
                distance = set[i].distance;
                guide = set[i].guide;
                stopLine = set[i].stopLine;
            }
        }
    }

    stopLineIndex = stopLine;
    return guide;
}

static void RingBreak(void)
{
    static int oldValue = BREAK_INACTIVE;
    car_sensor_value_t brk = ReadCarSensorValue(BREAK);

    if (brk.name == BREAK) {
        if (brk.value == BREAK_ACTIVE && oldValue != BREAK_ACTIVE) {
            PlayRing();
        }
        oldValue = brk.value;
    }
}

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;
    int forward = 0;                    // 车辆相对当前道路的
    UpdateCarSensor(rtkTime);

    UpdataOdo(speed, moveDirect, rtkTime);

    if (!win) {
        win = true;
        turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
    }

    if (RoadMap.calibrate == 0) {
        // 刹车提示音
        RingBreak();

        // 超速检测
        if (ConvertMs2KMh(speed) > examParam.road_max_speed) {
            if (!occurOverSpeed) {
                occurOverSpeed = true;
                // 超速，不合格
                DEBUG("超速 %f", ConvertMs2KMh(speed));
                AddExamFault(10, rtkTime);
            }
        } else if (ConvertMs2KMh(speed) < examParam.road_max_speed - 5) {
            occurOverSpeed = false;
        }

        // 副刹车检测
        if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
            // 副刹车踩下，不合格
            if (!occurSecondBreak) {
                DEBUG("副刹车动作了");
                occurSecondBreak = true;
                AddExamFault(17, rtkTime);
            }
        } else {
            occurSecondBreak = false;
        }

        // 两次换挡不进
        static int prevGear = -1;

        int currGear = ReadCarStatus(GEAR);
        if (currGear != GEAR_N && prevGear == GEAR_N) {
            // 一次换挡
            if (gearChange == currGear && TimeGetDiff(rtkTime, &gearChangeTimePoint) < D_SEC(5)) {
                AddExamFault(7, rtkTime);
            }

            gearChange = currGear;
            gearChangeTimePoint = *rtkTime;
        }
        prevGear = currGear;

        // 挡位匹配检测
        bool currGearError = false;
        bool currGearNSlide = false;

        switch (ReadCarStatus(GEAR)) {
            case GEAR_N:
                if (moveDirect != 0) {
                    // 空档滑行
                    currGearNSlide = true;
                }
                break;
            case GEAR_1:
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[0][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[0][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_2:
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[1][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[1][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_3:
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[2][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[2][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_4:
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[3][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[3][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_5:
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[4][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[4][1]) {
                    currGearError = true;
                }
                break;
            default:
                break;
        }
        // 空档滑行超时
        if (currGearNSlide) {
            if (GearNSlideStatus == 0) {
                DEBUG("检测到空挡滑行");
                GearNSlideStatus = 1;
                gearNSlideTimePoint = *rtkTime;
            }
            if (GearNSlideStatus == 1 &&
                TimeGetDiff(rtkTime, &gearNSlideTimePoint) > examParam.gear_n_allow_time) {
                // 空档滑行超5秒，不合格
                DEBUG("挡位滑行，超过5秒");
                AddExamFault(8, rtkTime);
                GearNSlideStatus = 2;
            }
        } else if (GearNSlideStatus != 0) {
            GearNSlideStatus = 0;
            DEBUG("空挡滑行结束");
        }

        // 挡位不匹配超时
        if (currGearError && prevGearError) {
            DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
                  ReadCarStatus(GEAR), ConvertMs2KMh(speed));
            gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
        }
        if (gearErrorTime > examParam.gear_speed_error_cumulative_time) {
            // 累计15秒，挡位-车速不匹配，不合格
            DEBUG("挡位错误超过15秒");
            AddExamFault(6, rtkTime);
            gearErrorTime = 0;
        }

        prevGearError = currGearError;
        if (prevGearError) {
            gearErrorTimePoint = *rtkTime;
        }

        // 起步后滑
        if (moveDirect != prevMoveDirect) {
            if (moveDirect == 0) {
                stopTimepoint = *rtkTime;
                StopCarOnRedArea = false;

                DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
                      rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

                if (slideNormalDistance) {
                    // 后滑，扣10分
                    AddExamFault(18, rtkTime);
                    DEBUG("后滑超过10厘米, 但不超过30厘米");
                }

                slideNormalDistance = false;
                slideLongDistance = false;
                occurSlide = false;
            } else if (moveDirect == -1 && prevMoveDirect == 0) {
                DEBUG("开始后滑");
                stopPoint = car->basePoint;
                occurSlide = true;
            }
            prevMoveDirect = moveDirect;
        } else if (moveDirect == 0) {
            // 持续停车
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea &&
                StopOnRedArea(RoadMap, car)) {
                // 停车超2秒，停在红区，不合格
                AddExamFault(16, rtkTime);
                DEBUG("禁停区停车");
                StopCarOnRedArea = true;
            }
        } else if (moveDirect == -1) {
            // 持续后滑
            if (occurSlide) {
                double slideDistance = DistanceOf(stopPoint, car->basePoint);

                if (slideDistance > examParam.road_slide_yellow_distance) {
                    slideNormalDistance = true;
                }

                if (slideDistance > examParam.road_slide_red_distance && !slideLongDistance) {
                    // 后滑超过30厘米, 不合格
                    AddExamFault(5, rtkTime);
                    DEBUG("后滑超过30厘米");
                    slideLongDistance = true;
                    slideNormalDistance = false;
                    occurSlide = false;
                }
            }
        } else {
            // 前进
        }
    }

    // 检测离开此路段，全车需不在范围内
    int oldid = currExamMapIndex;
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
    if (oldid != currExamMapIndex) {
        DEBUG("道路ID切换 %d ======> %d", oldid, currExamMapIndex);

        if (oldid >= 0) {
            ResetCrossingStatus(RoadMap, oldid);
            ResetErrorLaneRpt(oldid);
        }

        if (RoadMap.calibrate) {
            if (currExamMapIndex < 0 && oldid >= 0) {
                RoadChange(RoadMap.roads[oldid].id, 0);
            } else if (currExamMapIndex >= 0) {
                RoadChange(RoadMap.roads[currExamMapIndex].id, 1);
            }
        }
        Lane.guide = 0;
    }

    if (currExamMapIndex >= 0) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        // 检测通过路口、人行道等区域时，释放刹车或减速
        TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
        // 检查
        projection_t projection;

        uint32_t tm1 = AppTimer_GetTickCount();

        projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);

        if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) {
            if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) {
                double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1],
                      RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]);

                int deltaAng;

                if (ABS((int)roadYaw - (int)car->yaw) > 180) {
                    deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw);
                } else {
                    deltaAng = ABS((int)roadYaw - (int)car->yaw);
                }

                if (deltaAng < 90) {
                    forward = 1;
                } else {
                    forward = -1;
                }
//                DEBUG("路段方向 %f 车辆方向 %f 差值 %d 耗时 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1);
            }
        }
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    if (RoadMap.calibrate == 0) {
        oldid = CrashLineType;
        // 检测压线状态
        DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

        if (oldid != CrashLineType) {
            DEBUG("压线类型切换 %d", CrashLineType);
        }

        oldid = Lane.guide;
        DetectLane(RoadMap, car, currExamMapIndex, rtkTime);

//    DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
        if (Lane.guide > 0 && currExamMapIndex >= 0) {
            int stop_line_index;
            int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
                                          RoadMap.roads[currExamMapIndex], car);

            if (act != 0 && !(act & Lane.guide)) {
                if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                    DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex,
                          stop_line_index, act, Lane.guide);
                    AddExamFault(9, rtkTime);
                    SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
                }
            }
        }

        if (Lane.guide != oldid) {
            DEBUG("导向类型切换 %d", Lane.guide);
        }
    }

    if (currExamMapIndex >= 0 && Lane.guide == 0) {
        BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);

        road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
        // 提示路口怎么走
        HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);

        double freeSepDis = SeparateLength(RoadMap, Lane, car);

        // 剩余距离就是导向线起点的距离
        if (Lane.sep >= 0 && Lane.sep == ep.sep) {
            freeSepDis = ep.distance;
        }

        RoadCrossingFree = freeSepDis;

//        DEBUG("车速 %f, 累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f 特殊区域距离 %f", ConvertMs2KMh(speed), ReadOdo(), BigStraightRoadFree, RoadCrossingFree, TargetFree);
    }

    // 额外的转向检测

    if (RoadMap.calibrate == 0) {
        DetectTurn(car, moveDirect, rtkTime);

        ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
                 BigStraightRoadFree,
                 TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
        ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect);
    }
}

static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
{
    static double freeRunDistance;
    static double totalRunDistance;
    static double freeRunExceptDistance = 50.0;

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
        if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
            CarStartInit();
            totalRunDistance = ReadOdo();
            RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
        } else {
            bool not_complete = false;

            if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
//                if (straight > 170 && road_end > 170) {
//                    StartDriveStraightExam();
//                    RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
//                if (road_end > 170) {
//                    StartOperateGearExam();
//                    RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
//                if (road_end > 150 && Lane.total > 1) {
//                    StartChaneLaneExam(Lane.no);
//                    RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
//                if (road_end > 200 && Lane.total > 1) {
//                    if (Lane.no == 0) {
//                        // 已在最左车道，先变更车道？
//                        StartChaneLaneExam(Lane.no);
//                        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
//                    } else {
//                        StartOvertakeExam(Lane.no);
//                        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
//                    }
//                    return;
//                }
            }

            if (!not_complete) {
                if (road_end > 200 && ReadOdo() > examParam.road_total_distance) {
                    RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
                    StartStopCarExam();
                    return;
                }
            }
        }
    } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
        if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
            DEBUG("寻找下一个子项目");
            RoadExamStatus = ROAD_EXAM_READY_NEXT;
        }
    } else {
        bool testing = false;

        switch (RoadExamStatus) {
            case ROAD_EXAM_ITEM_CAR_START:
                testing = TestCarStart(car, speed, moveDirect, rtkTime);
                break;
            case ROAD_EXAM_ITEM_OP_GEAR:
                testing = TestOperateGear(rtkTime);
                break;
            case ROAD_EXAM_ITEM_CHANGE_LANE:
                testing = TestChangeLane(Lane.no, rtkTime);
                break;
            case ROAD_EXAM_ITEM_OVER_TAKE:
                testing = TestOvertake(Lane.no, rtkTime);
                break;
            case ROAD_EXAM_ITEM_STRAIGHT:
                testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
                break;
            case ROAD_EXAM_ITEM_CAR_STOP:
                testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
                break;
            default:break;
        }

        if (!testing) {
            DEBUG("当前子项结束");
            RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
            if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
                freeRunExceptDistance = 60.0;
            } else if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_STOP) {
                // 考试结束
                MA_SendExamStatus(1, 1);
            } else {
                freeRunExceptDistance = 2;//250.0;
            }

            RoadExamStatus = ROAD_EXAM_FREE_RUN;
            freeRunDistance = ReadOdo();
        }
    }
}

static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
{
    int item = 0;

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
        item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
    }

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
        StartDriveStraightExam();
        RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
        StartOperateGearExam();
        RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
        StartChaneLaneExam(Lane.no);
        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
        StartOvertakeExam(Lane.no);
        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 5) {
        if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_EXECED &&
                RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_EXECED &&
                RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_EXECED &&
                RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_EXECED) {
            StartStopCarExam();
            RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
        }
    }
}

/*******************************************************************
 * 车辆越过停止线的方式通过路口
 * @param RoadMap
 * @param road
 * @param stop_line
 * @param active
 * @param car
 */
void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
{
    if (RoadMap.calibrate) {
        CrossingChange(RoadMap.roads[road].id, stop_line, 0);
    }
    SetErrorLaneRpt(road, stop_line, false);
    if (active != ROAD_ACTIVE_FORWARD) {
        ResetTurnDetect(car);
    }
}

static int isTurn(int currYaw, int prevYaw, int thres)
{
//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
    int deltaAng = 0;

    if (ABS(currYaw - prevYaw) > 180) {
        deltaAng = 360 - ABS(currYaw-prevYaw);
    } else {
        deltaAng = ABS(currYaw - prevYaw);
    }


//    DEBUG("角度差值 %d", deltaAng);

    if (deltaAng >= thres) {
        if((( currYaw + 360 - prevYaw) % 360) < 180) {
//            DEBUG("右转");
            return deltaAng;
        } else {
//            DEBUG("左转");
            return 0 - deltaAng;
        }
    }

    return 0;
}

static void ResetTurnDetect(const car_model *car)
{
    turnCnt = 0;
    turnTimeCnt = 0;
    prevDetectTurnTime = car->tm;
    prevYaw = car->yaw;
}

static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
{
    int angle;

    if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
        return;
    }

    angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);

    prevYaw = (int) car->yaw;
    prevDetectTurnTime = *rtkTime;

    if (turnCnt == 0) {
        if (angle != 0 && moveDirect != 0) {
            DEBUG("有转向迹象");
            turnCnt++;
            beginTurnTime = *rtkTime;
            startTurnYaw = (int) car->yaw;

            turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
        }
    } else if (turnCnt == 1) {
        if (angle != 0 && moveDirect != 0) {
            if (angle * prevTurnWise > 0) {
                DEBUG("确认转向");
                // 同向转动
                turnCnt++;
                turnTimeCnt = 0;
            } else {
                beginTurnTime = *rtkTime;
                startTurnYaw = (int) car->yaw;

                turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            }
        } else {
            turnCnt = 0;
        }
    } else if (turnCnt >= 2) {
        if (moveDirect == 0) {
            // 暂停
        } else {
            turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
            int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
            DEBUG("转动角度 %d", wise);
            if (ABS(wise) >= CROSSING_TURN_THRESHOLD) {
                // 确认转弯行为，检测开始刚转弯时转向灯情况
                turnCnt = -1;

                if (wise < 0) {
                    if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                            examParam.turn_signal_min_advance) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                } else {
                    if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                            examParam.turn_signal_min_advance) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                }
            } else if (turnTimeCnt > D_SEC(10)) {
                // 取消转向检测
                DEBUG("取消转向检测1");
                turnCnt = -1;
            }
        }
    }

    if (turnCnt < 2 && moveDirect != 0) {
        turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
        if (turnTimeCnt > D_SEC(15)) {
            // 取消转向检测
            DEBUG("取消转向检测2");
            turnCnt = -1;
        }
    }

    prevTurnWise = angle;
}

/**********************************************************
 * 按整个车身是否覆盖计算
 * @param RoadMap
 * @param car
 * @return
 */
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car)
{
    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        Polygon redArea;
        redArea.num = 0;

        if (RoadMap.specialAreas[i].type == GRID_AREA && RoadMap.specialAreas[i].area.size() == 4) {
            redArea.num = 4;
            redArea.point = (PointF *)malloc(4 * sizeof(PointF));

            redArea.point[0] = RoadMap.specialAreas[i].area[0];
            redArea.point[1] = RoadMap.specialAreas[i].area[1];
            redArea.point[2] = RoadMap.specialAreas[i].area[2];
            redArea.point[3] = RoadMap.specialAreas[i].area[3];
        } else if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING) {
            int j = 0;
            for (; j < RoadMap.roads.size(); ++j) {
                if (RoadMap.specialAreas[i].road == RoadMap.roads[j].id) {
                    break;
                }
            }

            if (j < RoadMap.roads.size()) {

                PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
                                                       RoadMap.specialAreas[i].area[0]);
                PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
                                                       RoadMap.specialAreas[i].area[1]);

                redArea.num = 4;
                redArea.point = (PointF *) malloc(4 * sizeof(PointF));

                redArea.point[0] = RoadMap.specialAreas[i].area[0];
                redArea.point[1] = RoadMap.specialAreas[i].area[1];
                redArea.point[2] = p2;
                redArea.point[3] = p1;
            }
        }

        if (redArea.num > 0) {
            for (int i = 0; i < car->bodyNum; ++i) {
                if (IntersectionOf(car->carXY[car->body[i]], &redArea) == GM_Containment) {
                    free(redArea.point);
                    return true;
                }
            }
            free(redArea.point);
        }
    }

    return false;
}

bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    if (CarModelList.size() < 2)
        return false;

    list<car_model *>::iterator iter = CarModelList.begin();

    Line trace;
    PointF p1, p2;

    p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    ++iter;
    p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    MakeLine(&trace, &p1, &p2);

    if (IntersectionOf(trace, line) == GM_Intersection &&
            IntersectionOfLine(p1, line) == -1) {
        return true;
    }
    return false;
}

static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
{
    if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2)
        return -1;

    list<car_model *>::iterator iter = CarModelList.begin();

    Line trace;
    PointF p1, p2;

    p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    ++iter;
    p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
    MakeLine(&trace, &p1, &p2);

    double trace_length = DistanceOf(p1, p2);

    if (TriggerDetect.size() > 0) {
        auto it = TriggerDetect.begin();

        if (it->second.road_id != RoadMap.roads[index].id) {
            TriggerDetect.clear();
        }
    }

    for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
        if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {


            /*Line triggerLine;

            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);

            MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);

            if (CrashTheLine(triggerLine, car, CarModelList)) {
                DEBUG("触发项目 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
                return RoadMap.triggerLines[i].active;
            }*/

           /* vector<double> vec;

            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   car->carXY[car->axial[AXIAL_FRONT]]);
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   RoadMap.examScheme[0].triggerLines[j].points[0]);

            vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                                     RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));

            sort(vec.begin(), vec.end());

            if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
                DEBUG("触发项目 %d", RoadMap.examScheme[0].triggerLines[j].active);
                return RoadMap.examScheme[0].triggerLines[j].active;
            }*/

//            double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw);
//            auto it = TriggerDetect.find(j);
//
//            if (it != TriggerDetect.end()) {
//                if (fabs(angleOf - it->second.angleOf) > 2.0) {
//                    bool trigger = false;
//
//                    trigger_detect_t up;
//
//                    if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) {
//                        trigger = true;
//                    }
//                    DEBUG("观察子<%d> 更新 %f -> %f", j, it->second.angleOf, angleOf);
//
//                    up.angleOf = angleOf;
//                    up.road_id = RoadMap.roads[index].id;
//
//                    TriggerDetect[j] = up;
//
//
//
//                    if (trigger) {
//                        DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
//                        return RoadMap.examScheme[0].triggerLines[j].active;
//                    }
//                }
//            } else {
//                trigger_detect_t up;
//
//                up.road_id = RoadMap.roads[index].id;
//                up.angleOf = angleOf;
//
//                TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
//
//                DEBUG("观察子<%d> 加入 %f", j, angleOf);
//            }

//            double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
//                       RoadMap.examScheme[0].triggerLines[j].points[0]);
            auto it = TriggerDetect.find(j);

            if (it != TriggerDetect.end()) {
                PointF verticalPoint;

                if (moveDirect == 1 &&
                    forward == 1 &&
                    trace_length > 0.01 &&
                    VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) &&
                    DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) {
                    DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active);

                    DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f",
                          RoadMap.examScheme[0].triggerLines[j].points[0].X,
                          RoadMap.examScheme[0].triggerLines[j].points[0].Y,
                          trace.X1,
                          trace.Y1,
                          trace.X2,
                          trace.Y2,
                          verticalPoint.X,
                          verticalPoint.Y,
                          trace_length);

                    return RoadMap.examScheme[0].triggerLines[j].active;
                }
            } else {
                PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
                PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);

                double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]);
                double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]);

                trigger_detect_t up;

                up.road_id = RoadMap.roads[index].id;
                up.distance = MAX(dist1, dist2);

                TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));

                DEBUG("观察子<%d> 加入 %f", j, MAX(dist1, dist2));
            }
        }
    }

    return -1;
}

static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
{
    double distance = 0;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return 0;

    for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
        for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
            PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
            distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].leftEdge);
            if (distance > 1e-3) {
                return distance;
            }
        }
    }

    return 0;
}