//
// 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 <vector>
#include <list>
#include <map>
#include <string>
#include <cstdlib>

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

using namespace std;

#define TURN_CHECK_CNT          6

enum {
    START_CAR_NOT_DO,
    START_CAR_DOING,
    START_CAR_DONE
};

enum {
    STOP_CAR_NOT_DO,
    STOP_CAR_DOING,
    STOP_CAR_DONE
};

static const int INVALID_ROAD = -1;

static const int TURN_THRESHOLD = 1;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const int OVERTAKE_HOLD_TIME = D_SEC(3);                // 在超车道行驶的一段时间
const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离

static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh

static const int FIND_POSITION = -2;
static const int INVALID_POSITION = -1;

static bool occurCrashRedLine;
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static bool occurSecondBreak;
static int checkCrashGreenTimeout;
static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
static bool reportTurnSignalError;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint, startPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;

static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;
static bool startCarLeftTurnSignal, checkStartCarSignal;

static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
static struct drive_timer overTakeCmpTime;

static int gearErrorTime;
static int gearNSlideTime;

static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static int nextRoadId;
static PointF roadItemStartPoint;
static struct drive_timer roadItemStartTime;
static bool overtake = false;
static bool checkTurn = false;
static bool checkDoor = false;
static bool handBreakActive = false;
static bool reportRPMOver = false;

static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);

static bool turnError13Cold, turnError14Cold;
static struct car_on_lane {
    int road;
    int separate;
    int lane;
    int direct;         // 车道方向限制
    int type;           // 实线，虚线
} CurrentLane;
static bool laneChanging;
static int changeLaneDirect;

static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
static int odoCnt;

typedef struct {
    int road;
    int sep;
    int lane;
} lane_t;

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

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);

static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
static const int CRL_SEP_SOLID = 2;
static const int CRL_EDGE_DOTTED = 3;
static const int CRL_EDGE_SOLID = 4;

static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);

static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);

static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
static int CrashRoadLine(road_t &road, const car_model *car);
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
static bool LaneIsValid(struct car_on_lane lane);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
static bool AllCmp(road_exam_map &map);
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
                             double speed, int moveDirect, const struct RtkTime *rtkTime);

static PointF CalcProjectionWithRoadEdge(vector<edge_t> &edge, PointF point);
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

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

    crashGreenCmpTime.hour = -1;
    occurCrashRedLine = false;
    occurCrashGreenLine = false;
    occurOverSpeed = false;

    occurSecondBreak = false;
    checkCrashGreenTimeout = 0;
    carIntersectionOfGreenLine = 0;

    reportTurnSignalError = false;
    currTurnSignalStatus = OFF_LIGHT;
    prevMoveDirect = 0;

    reportStopCarOnRedArea = false;
    occurSlide = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    prevGearError = false;
    gearErrorTime = 0;
    prevGearNSlide = false;
    gearNSlideTime = 0;

    currExamMapIndex = FIND_POSITION;

    startCar = START_CAR_NOT_DO;
    stopCar = STOP_CAR_NOT_DO;

    currRoadItem = NULL;

    checkDoor = false;
    handBreakActive = false;
    reportRPMOver = false;

    checkStartCarSignal = startCarLeftTurnSignal = false;

    turnError13Cold = turnError14Cold = true;

    CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
    laneChanging = false;
    changeLaneDirect = 0;

    nextRoadId = -1;
    checkTurn = false;
    ClearAll(RoadMap);

    odoGraph = 0.0;
    odoCnt = 0;

    // 初始化考项
}

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

    AppTimer_delete(TurnSignalError13ColdTimer);
    AppTimer_delete(TurnSignalError14ColdTimer);
}

/*********************************************************************
 * 计算某点到道路边线的投影点
 * @param edge
 * @param road
 * @param point
 * @return
 */
static 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;
}

static 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();
    }

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

    return index;
}

/************************************************
 * 车轮压实线，前后轴交叉，前后轮迹交叉
 * @param mode
 * @param RoadMap
 * @param car
 * @param CarModelList
 * @return
 */
static bool CrashRedLine(int mode, 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;

    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();

        PointF p11, p12, p21, p22, p31, p32, p41, p42;

        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.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= 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) {
                        return true;
                    }

                    if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        return true;
                    }
                    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) {
                        return true;
                    }

                    if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                         IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                        return true;
                    }
                    p1 = p2;
                }
            }
        }

        for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
            // 一组车道
            for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
                // 多根分道线
                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 (character == LINE_SOLID && RoadMap.roads[n].separate[m].lines[l][a].points.size() >= 2) {
                        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) {
                                return true;
                            }
                            if (track &&
                                (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                                 IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                                return true;
                            }
                            p1 = p2;
                        }
                    }
                    // 比较最近的连续的n个左侧点和上一个右侧点
                    if (character == LINE_HALF_SOLID_LEFT) {
                        // 不能从左移动到右

                    }
                }
            }
        }
    }

    return false;
}

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

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

    p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point);
    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;
                }
            }

            theLane.road = roadIndex;
            theLane.sep = i;
            theLane.lane = CLP.size();

            for (int x = 0; x < CLP.size(); ++x) {
                if (CLP[x].pos == 'L') {
                    theLane.lane = x;
                    break;
                }
            }

            return true;
        }
    }

    return false;
}

static void TurnSignalError13ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError13ColdTimer);

    turnError13Cold = true;
}

static void TurnSignalError14ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError14ColdTimer);

    turnError14Cold = true;
}

static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
{
    if (err == 13 && turnError13Cold) {
        DEBUG("起步不开转向灯");
        AddExamFault(13, rtkTime);

        turnError13Cold = false;
        AppTimer_delete(TurnSignalError13ColdTimer);
        AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
    } else if (err == 14 && turnError14Cold) {
        DEBUG("起步转向灯不足3秒");
        AddExamFault(14, rtkTime);

        turnError14Cold = false;
        AppTimer_delete(TurnSignalError14ColdTimer);
        AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
    }
}

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;

    if (startCar == START_CAR_NOT_DO) {
        startPoint = car->basePoint;
        reportRPMOver = false;
        startCar = START_CAR_DOING;

        PlayTTS("请起步");
    } else if (startCar == START_CAR_DOING) {
        moveDistance = DistanceOf(startPoint, car->basePoint);
        DEBUG("起步行驶距离 %f", moveDistance);

        if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
            startCarLeftTurnSignal = true;
            Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
        }

        if (!checkStartCarSignal && moveDirect == 1) {
            checkStartCarSignal = true;
            if (!startCarLeftTurnSignal) {
                ReportTurnSignalError(13, rtkTime);
            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                   startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                ReportTurnSignalError(14, rtkTime);
            }
        }

        if (moveDistance > START_CAR_MOVE_DISTANCE) {
            if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                DEBUG("Handbreak active move over 10m");
                // 手刹拉起状态下，行驶了10米以上，不合格
                AddExamFault(25, rtkTime);
            } else if (handBreakActive) {
                // 手刹拉起状态下，行驶了1米以上，扣10分
                DEBUG("Handbreak active move over 1M");
                AddExamFault(26, rtkTime);
            }

            PlayTTS("完成起步");
            DEBUG("############# 完成起步 ############");
            startCar = START_CAR_DONE;
        } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
            if (!checkDoor) {
                checkDoor = true;

                if (ReadCarStatus(DOOR) == DOOR_OPEN) {
                    // 车门未完全关闭，不合格
                    DEBUG("车门未关闭");
                    AddExamFault(23, rtkTime);
                }

                if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                    handBreakActive = true;
                }
            }
        }

        if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
            // 转速超标，不合格
            DEBUG("转速超标");
            AddExamFault(29, rtkTime);
            reportRPMOver = true;
        }
    } else {

    }

    return startCar;
}

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    uint32_t cts = AppTimer_GetTickCount();
    int ri = CalcRoadIndex(-1, RoadMap, car);
    bool crash = CrashRedLine(0, RoadMap, car, CarModelList);
    lane_t laneInfo;
    double redist = -1;

    laneInfo.road = -1;
    laneInfo.sep = -1;
    laneInfo.lane = -1;

    if (ri >= 0) {
        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);

        int m = RoadMap.roads[ri].rightEdge.size();
        int n = RoadMap.roads[ri].rightEdge[m-1].points.size();

        PointF base;

        base.X = 428922.2985; base.Y = 3292119.5457;

        redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
                              RoadMap.roads[ri].rightEdge);
    }

    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);

    // 行驶距离，不包含倒车
    if (odoCnt == 0 && moveDirect == 1) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        Rtk2DriveTimer(odoTimer, rtkTime);
    } else if (odoCnt == 1) {
        uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                  odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
        if (tm >= D_SEC(1)) {
            odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
            if (moveDirect == 1) {
                Rtk2DriveTimer(odoTimer, rtkTime);
                odoPrevSpeed = speed;
            } else {
                odoCnt = 0;
            }
        }
    }

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            DEBUG("超速 %f", speed);
            AddExamFault(10, rtkTime);
        }
    } else {
        occurOverSpeed = false;
    }

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

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

    switch (ReadCarStatus(GEAR)) {
        case GEAR_N:
            if (moveDirect != 0) {
                // 空档滑行
                currGearNSlide = true;
            }
            break;
        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                currGearError = true;
            }
            break;
        case GEAR_2:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                currGearError = true;
            }
            break;
        case GEAR_3:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                currGearError = true;
            }
            break;
        case GEAR_4:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                currGearError = true;
            }
            break;
        case GEAR_5:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                currGearError = true;
            }
            break;
        default:break;
    }
    // 空档滑行超时
    if (currGearNSlide && prevGearNSlide) {
        gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                      gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
    }
    if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
        // 空档滑行超5秒，不合格
        DEBUG("挡位滑行，超过5秒");
        AddExamFault(8, rtkTime);
        gearNSlideTime = 0;
    }

    prevGearNSlide = currGearNSlide;
    if (prevGearNSlide) {
        Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
    } else {
        gearNSlideTime = 0;
    }
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                     gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

    prevGearError = currGearError;
    if (prevGearError) {
        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
    }

    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarOnRedArea = 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) {
        // 持续停车
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        /*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            reportStopCarOnRedArea = true;
        }*/
    } else if (moveDirect == -1) {
        // 持续后滑
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

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

    }

    // 转向灯读取
    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
        case LEFT_TURN_LIGHT:
            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                currTurnSignalStatus = LEFT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        case RIGHT_TURN_LIGHT:
            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                currTurnSignalStatus = RIGHT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        default:
            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
            break;
    }

    // 检测通过路口、人行道等区域时，释放刹车或减速
    CheckBreakActive(RoadMap, car, CarModelList,
                                 speed, moveDirect, rtkTime);

    // 检测离开此路段，全车需不在范围内
    if (currExamMapIndex >= 0) {
        Polygon area;
        int n = 0;

        area.num = 0;

        for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
            if (j > 0) {
                area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size() - 1;
            } else {
                area.num += RoadMap.roads[currExamMapIndex].leftEdge[j].points.size();
            }
        }
        for (int j = 0; j < RoadMap.roads[currExamMapIndex].rightEdge.size(); ++j) {
            if (j > 0) {
                area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1;
            } else {
                area.num += RoadMap.roads[currExamMapIndex].rightEdge[j].points.size();
            }
        }

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

        for (int j = 0; j < RoadMap.roads[currExamMapIndex].leftEdge.size(); ++j) {
            for (int k = (j>0?1:0); k < RoadMap.roads[currExamMapIndex].leftEdge[j].points.size(); ++k) {
                area.point[n++] = RoadMap.roads[currExamMapIndex].leftEdge[j].points[k];
            }
        }

        for (int j = RoadMap.roads[currExamMapIndex].rightEdge.size() - 1; j >= 0; --j) {
            if (j == RoadMap.roads[currExamMapIndex].rightEdge.size() - 1) {
                for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 1; k >= 0; --k) {
                    area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
                }
            } else {
                for (int k = RoadMap.roads[currExamMapIndex].rightEdge[j].points.size() - 2; k >= 0; --k) {
                    area.point[n++] = RoadMap.roads[currExamMapIndex].rightEdge[j].points[k];
                }
            }
        }

        // 全车都需不在地图中
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &area) == GM_None) {
            DEBUG("离开路段 id = %d", RoadMap.roads[currExamMapIndex].id);

            RoadMap.roads[currExamMapIndex].arrivedTail = false;

            currExamMapIndex = FIND_POSITION;
        }

        free(carBody.point);
        free(area.point);
    }
    if (currExamMapIndex == FIND_POSITION) {
        DEBUG("搜索道路");
        for (int i = 0; i < RoadMap.roads.size(); ++i) {
            Polygon area;
            int n = 0;
            
            area.num = 0;

            for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
                if (j > 0) {
                    area.num += RoadMap.roads[i].leftEdge[j].points.size() - 1;
                } else {
                    area.num += RoadMap.roads[i].leftEdge[j].points.size();
                }
            }
            for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
                if (j > 0) {
                    area.num += RoadMap.roads[i].rightEdge[j].points.size() - 1;
                } else {
                    area.num += RoadMap.roads[i].rightEdge[j].points.size();
                }
            }

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

            for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
                for (int k = (j>0?1:0); k < RoadMap.roads[i].leftEdge[j].points.size(); ++k) {
                    area.point[n++] = RoadMap.roads[i].leftEdge[j].points[k];
                }
            }

            for (int j = RoadMap.roads[i].rightEdge.size() - 1; j >= 0; --j) {
                if (j == RoadMap.roads[i].rightEdge.size() - 1) {
                    for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 1; k >= 0; --k) {
                        area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
                    }
                } else {
                    for (int k = RoadMap.roads[i].rightEdge[j].points.size() - 2; k >= 0; --k) {
                        area.point[n++] = RoadMap.roads[i].rightEdge[j].points[k];
                    }
                }
            }

            if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) {
                currExamMapIndex = i;
                DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
                if (nextRoadId >= 0 && RoadMap.roads[i].id != nextRoadId) {
                    DEBUG("不按规矩行驶，进入错误路段");
                    AddExamFault(3, rtkTime);
                }
                nextRoadId = -1;
                checkTurn = false;
                break;
            }

            free(area.point);
        }
        if (currExamMapIndex < 0) {
            currExamMapIndex = FIND_POSITION;//INVALID_POSITION;
            DEBUG("搜寻未果");
        }
    } else if (currExamMapIndex == INVALID_POSITION) {
        for (int i = 0; i < RoadMap.roads.size(); ++i) {
            if (CrashTheLine(RoadMap.roads[i].startLine, car, CarModelList)) {
                currExamMapIndex = i;
                DEBUG("进入道路 id = %d", RoadMap.roads[i].id);
                break;
            }
        }
    }

    // 检测压线状态
    bool crashRedLineNow = false;
    bool crashGreenLineNow = false;

    if (currExamMapIndex >= 0) {
        int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);

        if (crl == CRL_NONE) {
//            DEBUG("什么都没压");
        } else if (crl == CRL_SEP_DOTTED) {
            DEBUG("压分道虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_SEP_SOLID) {
            DEBUG("压分道实线");
            crashRedLineNow = true;
        } else if (crl == CRL_EDGE_DOTTED) {
            DEBUG("压边沿虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_EDGE_SOLID) {
            DEBUG("压边沿实线");
            crashRedLineNow = true;
        }

        // 检查当前车道
        struct car_on_lane lane;

        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane == CurrentLane.lane) {
                laneChanging = false;
            }

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane != CurrentLane.lane) {
                if (crl == CRL_SEP_DOTTED) {
                    // 变道进行中
                    if (!laneChanging) {
                        laneChanging = true;
                        DEBUG("发生变道");
                        // 比较上次跨线时间
                        if (crashGreenCmpTime.hour >= 0) {
                            uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                        rtkTime->mss * 10,
                                                        crashGreenCmpTime.hour,
                                                        crashGreenCmpTime.min,
                                                        crashGreenCmpTime.sec,
                                                        crashGreenCmpTime.msec * 10);
                            int laneDirect = 0;

                            if (CurrentLane.lane > lane.lane) {
                                laneDirect = -1;
                            } else {
                                laneDirect = 1;
                            }

                            if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) {
                                DEBUG("===================== 连续变道 %d -> %d ============", CurrentLane.lane, lane.lane);
                                // 连续变道，不合格
                                AddExamFault(15, rtkTime);
                            }
                        }

                        // 检查变道前，是否提前转向灯
                        if (lane.lane < CurrentLane.lane) {
                            // 向左侧变道
                            DEBUG("向左侧变道");
                            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                ReportTurnSignalError(13, rtkTime);
                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                   rtkTime->mss * 10,
                                                   turnSignalChangeTime.hour,
                                                   turnSignalChangeTime.min,
                                                   turnSignalChangeTime.sec,
                                                   turnSignalChangeTime.msec * 10) <
                                       TURN_SIGNAL_LAMP_ADVANCE) {
                                if (!reportTurnSignalError) {
                                    DEBUG("转向灯时间不足");
                                    // 不足3秒，不合格
                                    reportTurnSignalError = true;
                                    ReportTurnSignalError(14, rtkTime);
                                }
                            }
                        } else {
                            // 向右侧变道
                            DEBUG("向右侧变道");
                            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                ReportTurnSignalError(13, rtkTime);
                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                   rtkTime->mss * 10,
                                                   turnSignalChangeTime.hour,
                                                   turnSignalChangeTime.min,
                                                   turnSignalChangeTime.sec,
                                                   turnSignalChangeTime.msec * 10) <
                                       TURN_SIGNAL_LAMP_ADVANCE) {
                                if (!reportTurnSignalError) {
                                    DEBUG("转向灯时间不足");
                                    // 不足3秒，不合格
                                    reportTurnSignalError = true;
                                    ReportTurnSignalError(14, rtkTime);
                                }
                            }
                        }
                    }
                } else {
                    // 变道完成
                    DEBUG("变道完成 %d -> %d", CurrentLane.lane, lane.lane);

                    if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
                        DEBUG("变更车道项目完成");
                        currRoadItem = NULL;
                        PlayTTS("完成变道");
                    } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
                        if (CurrentLane.lane > lane.lane) {
                            DEBUG("超车变道完成");
                            overtake = true;
                            Rtk2DriveTimer(overTakeCmpTime, rtkTime);
                            PlayTTS("完成超车");
                        } else {
                            DEBUG("右道超车，错误");
                            AddExamFault(3, rtkTime);
                            currRoadItem = NULL;
                        }
                    }

                    if (CurrentLane.lane > lane.lane) {
                        changeLaneDirect = -1;
                    } else {
                        changeLaneDirect = 1;
                    }

                    CurrentLane = lane;
                    laneChanging = false;
                    // 记录本次变道时间点
                    Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
                }
            }

            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
                // 路或段变更，撤销变道跟踪
                DEBUG("============== 路或段变更 CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
                      CurrentLane.separate, CurrentLane.direct,
                      lane.road, lane.separate, lane.direct);

                CurrentLane = lane;
                laneChanging = false;
            }

            if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) {
                if (!RoadMap.roads[currExamMapIndex].errorLane) {
                    RoadMap.roads[currExamMapIndex].errorLane = true;
                    DEBUG("不按规定车道标线方向行驶 <%d> %d %d", RoadMap.roads[currExamMapIndex].id, CurrentLane.direct, RoadMap.roads[currExamMapIndex].active);
                    AddExamFault(9, rtkTime);
                }
            }
        }

        ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList);

        if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
            DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
            nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
            checkTurn = true;
        }

        if (checkTurn) {
            // 检查是否持续转向
            char turnDirect = CheckCarTurn(CarModelList);
            if (turnDirect == 'L') {
//        PlayTTS("左1");
                if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                    if (!reportTurnSignalError) {
                        DEBUG("没打左转灯");
                        // 没打左转灯，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                       turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                    if (!reportTurnSignalError) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            } else if (turnDirect == 'R') {
//        PlayTTS("右1");
                if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                    if (!reportTurnSignalError) {
                        DEBUG("没打右转灯");
                        // 没打右转灯，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                       turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                    if (!reportTurnSignalError) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            } else {
                reportTurnSignalError = false;
            }
        }
    }

    // 撞红线
    if (crashRedLineNow) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
            DEBUG("撞道路边缘线");
            AddExamFault(11, rtkTime);
            occurCrashRedLine = true;
        }
    } else {
        occurCrashRedLine = false;
    }

    // 压虚线
    if (crashGreenLineNow) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                Rtk2DriveTimer(crashGreenRunTime, rtkTime);         // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                            crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);

                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }

        // 检测当前车辆于虚线的位置，做变道检测;
        // 检测是否3秒前有开启对应之转向灯
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
        }
    } else {
        // 不再压虚线
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }

    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    if (startCar != START_CAR_DONE)
        return;

    if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
        // 在合适条件下停车结束考试
        StartStopCarExam("请靠边停车");
        stopCar = STOP_CAR_DOING;
    } else if (stopCar == STOP_CAR_DOING) {
        if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
            stopCar = STOP_CAR_DONE;
    }


    // 执行某个项目
    if (currRoadItem != NULL) {
        if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
            if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
                DEBUG("变道距离超标");
                AddExamFault(3, rtkTime);
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
            if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) {
                DEBUG("超车距离超标");
                AddExamFault(3, rtkTime);
                currRoadItem = NULL;
            } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                               rtkTime->mss * 10,
                                               overTakeCmpTime.hour, overTakeCmpTime.min,
                                               overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) {
                DEBUG("回原车道");
                PlayTTS("请返回原车道");
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
            if (ExecuteOperateGearExam(rtkTime) < 0) {
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
            if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
                currRoadItem = NULL;
            }
        }
    } // 检测由触发线控制的项目
    else if (currExamMapIndex >= 0) {
        trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);

        if (new_item != NULL && !new_item->cmp) {
            currRoadItem = new_item;
            if (!currRoadItem->tts.empty())
                PlayTTS(currRoadItem->tts.c_str());

            // 初始时间和距离限制
            roadItemStartPoint = car->basePoint;
            Rtk2DriveTimer(roadItemStartTime, rtkTime);

            if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
                overtake = false;
            } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
                StartOperateGearExam(rtkTime);
            } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
                StartDriveStraightExam(currRoadItem->tts);
            }
        }
    }
}

/*************************************************************
 * 检测当前车道，以车头中点为基准
 * @param road
 * @param car
 */
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
{
    bool leftExt = false, rightExt = false;
    Line leftExtLine, rightExtLine;
    struct car_on_lane lane;

    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                leftExt = true;
                MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto LEFT_EXT_CMP;
            }
            p1 = p2;
        }
    }
LEFT_EXT_CMP:
    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                rightExt = true;
                MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto RIGHT_EXT_CMP;
            } else {
//                DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
            }
            p1 = p2;
        }
    }
RIGHT_EXT_CMP:

    if (!leftExt || !rightExt) {
        DEBUG("左右边界不匹配");
        return false;
    }

    bool orthogonalInSegment = false;

    for (int i = 0; i < road.separate.size(); ++i) {        // 段
        PointF p1, p2;
        Line sep;

        map<int, vector<int>> orthogonal;

        // 一段分道组内，有一条正交，就必须保证同组的全都正交，否则直接退出
        for (int j = 0; j < road.separate[i].lines.size(); ++j) {   // 线组
            bool intersection = false;

            for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) {   // 节

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

                    if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
                        vector<int> stor(4);

                        stor[0] = 1;
                        stor[1] = road.separate[i].lines[j][k].character;
                        stor[2] = road.separate[i].lines[j][k].left_lane_direct;
                        stor[3] = road.separate[i].lines[j][k].right_lane_direct;

                        orthogonal.insert(pair<int, vector<int>>(j, stor));
                        orthogonalInSegment = true;
                        intersection = true;

                        break;
                    } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                        vector<int> stor(4);

                        stor[0] = 2;
                        stor[1] = road.separate[i].lines[j][k].character;
                        stor[2] = road.separate[i].lines[j][k].left_lane_direct;
                        stor[3] = road.separate[i].lines[j][k].right_lane_direct;

                        orthogonal.insert(pair<int, vector<int>>(j, stor));
                        orthogonalInSegment = true;
                        intersection = true;

                        break;
                    }
                    p1 = p2;
                }
            }
        }
//        DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());

        if (orthogonal.size() > 0) {
            if (orthogonal.size() == road.separate[i].lines.size()) {
                // 得到当前在第几个车道
                int x = 0;
                int left_char = LINE_SOLID;               // 道路左边线
                int right_direct = 0;

                for (x = 0; x < orthogonal.size(); ++x) {
                    auto itx = orthogonal.find(x);
                    if (itx != orthogonal.end()) {
                        if (itx->second[0] != 1) {          // 在车辆右侧
                            lane.road = road.id;
                            lane.separate = i;
                            lane.lane = itx->first;

//                            DEBUG("left_char %d second %d", left_char, itx->second[1]);

                            if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
                                    (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT))    // 车道左右均是实线
                                lane.direct = itx->second[2];
                            else
                                lane.direct = 0;
//                            DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                            break;
                        } else {
                            right_direct = itx->second[3];
                            left_char = itx->second[2];
                        }
                    }
                }
                if (x >= orthogonal.size()) {
                    lane.road = road.id;
                    lane.separate = i;
                    lane.lane = orthogonal.size();

//                    DEBUG("left_char %d right_direct %d", left_char, right_direct);

                    // 最后车道的右侧限定
                    if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
                        lane.direct = right_direct;
                    else
                        lane.direct = 0;
//                    DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                }
                out = lane;
                return true;
            } else {
                // 不完全正交，直接退出
            }
            DEBUG("分道线长短不同，退出");
            return false;
        }
    }
    DEBUG("段匹配失败");
    return false;
}

/*************************************************************************
 * 碰触车道线型
 * @param road
 * @param car
 */
static int CrashRoadLine(road_t &road, const car_model *car)
{
    Line frontAxle, rearAxle;

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

    // 检查道路边缘
    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路左边线
                if (road.leftEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路右边线
                if (road.rightEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    // 检查分道线
    for (int i = 0; i < road.separate.size(); ++i) {
        // 段
        PointF p1, p2;
        Line sep;

        for (int j = 0; j < road.separate[i].lines.size(); ++j) {
            // 线
            for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
                // 节
                p1 = road.separate[i].lines[j][k].points[0];

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

                    if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
                        IntersectionOf(sep, rearAxle) == GM_Intersection) {
                        // 检查道路分隔线类型
                        if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
                            // 压虚线
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
                            // 压实线
                            return CRL_SEP_SOLID;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        }
                    }

                    p1 = p2;
                }
            }
        }
    }

    return CRL_NONE;
}

static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2)
{
    if (lane1.road == lane2.road && lane1.separate == lane2.separate && lane1.lane == lane2.lane) {
        return true;
    }
    return false;
}

static bool LaneIsValid(struct car_on_lane lane)
{
    if (lane.road >= 0 && lane.separate >= 0 && lane.lane >= 0) {
        return true;
    }
    return false;
}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
    tm.hour = rtkTime->hh;
    tm.min = rtkTime->mm;
    tm.sec = rtkTime->ss;
    tm.msec = rtkTime->mss;
}

static char isTurn(int currYaw, int prevYaw, int &ang)
{
//    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);
    }

    ang = deltaAng;

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

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

    return 'N';
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
    // 最近2秒内，每0.5秒的角度差大于5度，且方向相同，连续4次；或突现超30度的转向；认为转向。
    if (CarModelList.size() < 1)
        return false;

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

    car_model *c1 = *iter, *c2;

    ++iter;

    char turn[TURN_CHECK_CNT] = {0};
    int checkCnt = 0;

//    DEBUG("CheckCarTurn.........");

    while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) {
        c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= TURN_CHECK_INTERVAL) {
            int ang = 0;
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
//            DEBUG("%c  角度比较 %02d:%02d:%02d.%03d  %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

            if (turn[checkCnt] == 'N') {
                break;
            } else if (ang >= 30) {
                DEBUG("左右转确认 %c", turn[checkCnt]);
                return turn[checkCnt];
            }

            c1 = c2;
            checkCnt++;
        }

        ++iter;
    }

    int i = 0;
    for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
        if (turn[i] != turn[i+1])
            break;
    }

    if (i == TURN_CHECK_CNT-1) {
        DEBUG("左右转确认 %c", turn[0]);
        return turn[0];
    }

    return 0;
}

/**********************************************************
 * 按整个车身是否覆盖计算
 * @param RoadMapList
 * @param car
 * @return
 */
/*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].type == GENERAL_MAP) {

            // 每条红区都检测
            for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
                if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
                    ret = true;
                }
            }
            break;
        }
    }

    free(carBody.point);

    return ret;
}*/

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;
}


/***************************************************
 * 接近路口时，提示下一步怎么走
 * @param road
 * @param car
 * @param CarModelList
 */
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    // 计算车前进轨迹延长线
    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);

    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
    Line extLine;

    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

    if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
        IntersectionOfLine(extPoint, road.stopLine) == -1) {
            if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
                // 接近路口后，要检查车辆是否进入错误车道
                DEBUG("接近路口");
                road.arrivedTail = true;
                if (!road.tts.empty())
                    PlayTTS(road.tts.c_str());
            }
    } else if (road.arrivedTail) {
        road.arrivedTail = false;
    }
}

static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
        if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
            Line triggerLine;

            if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
                RoadMap.triggerLines[i].leftPoints.clear();

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
                    RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
                }

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
                    DEBUG("触发线补齐 road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active,
                          RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y);
                }
            }

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

            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];
            }
        }
    }

    return NULL;
}

static bool AllCmp(road_exam_map &map)
{
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        if (!map.triggerLines[i].cmp)
            return false;
    }
    return true;
}

/************************************************************************
 * 开始新的考试后，清除地图所有的刹车、停车记录
 * @param map
 */
static void ClearAll(road_exam_map &map)
{
    for (int i = 0; i < map.roads.size(); ++i) {
        map.roads[i].activeStop = map.roads[i].activeBreak = false;
        map.roads[i].arrivedTail = false;
        map.roads[i].errorLane = false;
    }
    for (int i = 0; i < map.specialAreas.size(); i++) {
        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
    }
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        map.triggerLines[i].cmp = false;
    }
}

static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
                             double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    int BreakDone = ReadCarStatus(BREAK);

    // 计算车前进轨迹延长线
    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);

    Line extLine;
    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

    // 路口刹车点
    for (int i = 0; i < map.roads.size(); ++i) {
        // 车头和路口距离不足30米
        if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
            IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
            DEBUG("进入减速区");
            if (BreakDone == BREAK_ACTIVE) {
                map.roads[i].activeBreak = true;
            }
            if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                           map.roads[i].stopLine) < DISTANCE_STOP_CAR_TO_STOP_LINE && moveDirect == 0) {
                map.roads[i].activeStop = true;
            }
        }
        // 跨线后，检查刹车动作
        if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
            if (map.roads[i].stopFlag != 0 && !map.roads[i].activeStop) {
                // 不停车瞭望，不合格
                DEBUG("不停车瞭望");
                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
                    AddExamFault(42, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
                    AddExamFault(44, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
                    AddExamFault(47, rtkTime);
                }
            }
            if (!map.roads[i].activeBreak) {
                // 不按规定减速，不合格
                DEBUG("不按规定减速");
                if (map.roads[i].active == ROAD_ACTIVE_FORWARD) {
                    AddExamFault(41, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) {
                    AddExamFault(43, rtkTime);
                } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) {
                    AddExamFault(46, rtkTime);
                }
            }
        }
    }
    // 人行道、公交站刹车点；学校限速区
//    DEBUG("补全特殊区域 size = %d", map.specialAreas.size());
    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == GRID_AREA)
            continue;

        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
            }

            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
//            DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);

            map.specialAreas[i].leftPoints.push_back(vPoint);

            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
//            DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
            map.specialAreas[i].leftPoints.push_back(vPoint);
        }

        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
//            DEBUG("斑马线");
            Line startLine;

            MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区 %d", map.specialAreas[i].type);
                if (BreakDone == BREAK_ACTIVE) {
                    map.specialAreas[i].activeBreak = true;
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                if (!map.specialAreas[i].activeBreak) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");
                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
                        AddExamFault(48, rtkTime);
                    } else {
                        AddExamFault(50, rtkTime);
                    }
                } else {
                    DEBUG("按规定减速");
                }

            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
            Polygon school;
//            DEBUG("学校");
            school.num = 4;
            school.point = (PointF *) malloc(school.num * sizeof(PointF));

            school.point[0] = map.specialAreas[i].area[0];
            school.point[1] = map.specialAreas[i].area[1];
            school.point[2] = map.specialAreas[i].leftPoints[1];
            school.point[3] = map.specialAreas[i].leftPoints[0];

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
                    if (!map.specialAreas[i].overSpeed) {
                        DEBUG("通过学校区域超速");
                        AddExamFault(49, rtkTime);
                        map.specialAreas[i].overSpeed = true;
                    }
                }
            }

            free(school.point);
        }
    }
//    DEBUG("补全特殊区域 over");
}

#if 0

typedef struct {
    int road;
    int segment;
    int lane;
} CarOnTrackInfo_t;

static CarOnTrackInfo_t CarOnTrackInfo;

void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 检测车道变更
    CarOnTrackInfo_t newCarOnTrackInfo;

    if (newCarOnTrackInfo.road == CarOnTrackInfo.road &&
            newCarOnTrackInfo.segment == CarOnTrackInfo.segment &&
            newCarOnTrackInfo.lane != CarOnTrackInfo.lane) {

    }

    CarOnTrackInfo = newCarOnTrackInfo;


}

/**************************************************
 * 车辆当前所在路段，车头需越过起始线一定距离
 * @param currIndex
 * @param map
 * @param car
 */
static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car)
{
    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    Polygon roadArea;
    roadArea.num = 0;
    roadArea.point = NULL;

    for (int i = 0; i < map.roads.size(); ++i) {
        if (roadArea.point != NULL) {
            free(roadArea.point);
        }

        for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) {
            roadArea.num += map.roads[i].leftEdge[x].points.size();
        }
        for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) {
            roadArea.num += map.roads[i].rightEdge[x].points.size();
        }
        roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF));

        int n = 0;
        for (int j = 0; j < map.roads[i].leftEdge.size(); j++) {
            for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) {
                if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) &&
                             isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].leftEdge[j].points[x];
                }
            }
        }
        for (int j = map.roads[i].rightEdge.size(); j > 0; j--) {
            for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) {
                if (n > 0 && x == map.roads[i].rightEdge[j].points.size() &&
                            isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) &&
                            isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) {
                    // 第一个点已经存在于上一条线
                } else {
                    roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1];
                }
            }
        }
        roadArea.num = n;

        if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) {
            currIndex = i;
            goto CHECK_CAR_ON_ROAD_END;
        }
    }
    currIndex = INVALID_ROAD;

    CHECK_CAR_ON_ROAD_END:
    if (roadArea.point != NULL) {
        free(roadArea.point);
    }
    free(carBody.point);
}

/******************************************************
 * 全车都需离开这个区域
 * @param currIndex
 * @param map
 * @param car
 * @return
 */
static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    // 全车都需不在地图中
    bool ret = false;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return ret;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) {
        ret = true;
    }

    free(carBody.point);
    return ret;
}

static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    Line frontAxle, rearAxle;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return false;

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

    // 分段
    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
        // 分段中的每条线
        for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) {
            Line theLine;
            int p1 = 0;
            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) {
                MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]);
                if (IntersectionOf(theLine, frontAxle) == GM_Intersection ||
                    IntersectionOf(theLine, rearAxle) == GM_Intersection) {
                    return true;
                }
                p1 = p2;
            }
        }
    }
    return false;
}


static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car)
{
    int segment;
    int lane = -1;

    CarOnTrackInfo_t newInfo;

    if (currIndex == INVALID_ROAD || currIndex >= map.roads.size())
        return;

    // 遍历每一分段
    for (int i = 0; i < map.roads[currIndex].separate.size(); i++) {
        int separate_line_num = map.roads[currIndex].separate[i].lines.size();

        struct vrecord_ {
            int valid;
            int p1;
            int p2;
        };

        vector<struct vrecord_> vrecord;
        vrecord.clear();

        bool match1 = false;
        // 遍历当前分段的每一条线
        for (int j = 0; j < separate_line_num; ++j) {
            Line theLine;
            int p1 = 0;

            struct vrecord_ v;
            v.valid = 0;

            // 单独的一条虚线
            for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) {
                MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1],
                         &map.roads[currIndex].separate[i].lines[j].point[p2]);
                if (VerticalPointOnLine(car->basePoint, theLine)) {
                    v.valid = 1;
                    v.p1 = p1;
                    v.p2 = p2;
                    match1 = true;
                    break;
                }
                p1 = p2;
            }

            vrecord.push_back(v);

            lane = separate_line_num;//
        }



        if (match1) {
            for (int x = 0; x < vrecord.size(); ++x) {
                if (vrecord[i].valid == 0) {
                    // 首尾两段线的延申必有一个垂点
                    if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) <
                        DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) {
                        vrecord[x].p1 = 0;
                        vrecord[x].p2 = 1;
                    } else {
                        vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2;
                        vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1;
                    }
                }

                int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1],
                                             map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2],
                                             car->basePoint);
                if (rel != -1) {
                    newInfo.road = currIndex;
                    newInfo.segment = i;
                    newInfo.lane = x;
                    break;
                }
            }

            newInfo.road = currIndex;
            newInfo.segment = i;
            newInfo.lane = vrecord.size();

            break;
        }
    }
}



#endif