//
// Created by YY on 2019/10/21.
// Units note: distance - metre
//             speed - metre per second
//             angle - DEGREES
//

#include <cstdlib>
#include <cmath>
#include <semaphore.h>
#include <pthread.h>
#include <cstring>
#include <vector>
#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
#include "common/apptimer.h"
#include "jni_log.h"
#include "test_items/park_edge.h"
#include "native-lib.h"
#include "test_items/park_bottom.h"
#include "test_items/park_edge.h"
#include "test_items/error_list.h"
#include "test_items/turn_a90.h"
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"

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

using namespace std;

#define MAP_TYPE_PARK_BUTTOM        1
#define MAP_TYPE_STOP_START         2
#define MAP_TYPE_PART_EDGE         3
#define MAP_TYPE_CURVE         4
#define MAP_TYPE_TURN_90         5

#define RTK_INVALID         0
#define RTK_SINGLE_POINT   1
#define RTK_DIFF            2
#define RTK_FIX            3
#define RTK_FLOAT            4

#define CAR_MODEL_POINT_NUM             32
#define MAP_LIST_SIZE               32

enum {
    TEST_NONE,
    TEST_PARK_EDGE,
    TEST_PARK_BOTTOM,
    TEST_TUNE_90,
    TEST_S_CURVE,
    TEST_SLOPE,
    TEST_SIMPLE,
    TEST_END
};

enum {
    TEST_TYPE_AREA = 2,
    TEST_TYPE_ROAD_DUMMY_LIGHT,
    TEST_TYPE_ROAD_TRUE_LIGHT
};

static bool ExamStart = false;
static int ExamType;
static bool reportSeatbeltEject;

static int CarInArea = 0;
int errs = 0;

vector<int> ErrorList;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;

static struct map_list {
    int id;
    int type;
    Polygon map;
    Polygon map2;
} MapList[MAP_LIST_SIZE];

static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;

static int CurrExamStatus = EXAM_AREA_NONE;      // 1 测试完成 0 测试中 -1 测试错误退出

static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
static bool engineRuning = false;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10

static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

static void DetectEnterOrExitMap(void);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
static int GetMapType(int index, const struct map_list *mapList, int mapNum);

void DriverTestInit(void)
{
    ExamStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

    CarSensorInit();

    DummyLightContentSize = 0;
    DummyLightContent = NULL;

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;

    CurrExamStatus = EXAM_AREA_NONE;
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
{
    AppTimer_delete(ReadDriverExamPrimerTimeout);
    AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
    ReadDriverExamPrimer();
}

void ReadDriverExamPrimer(void)
{
    MA_ReadMap();
    MA_ReadCar();
    MA_ReadSensor();
}

void ClearMap(void)
{
    if (ExamStart) return;

    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
            free(MapList[i].map.point);
        if (MapList[i].map2.point != NULL)
            free(MapList[i].map2.point);
    }
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
}

void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
{
    DEBUG("加入地图信息 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);

    AppTimer_delete(ReadDriverExamPrimerTimeout);

    if (map == NULL || pointNum == 0 || ExamStart)
        return;

    MapList[MapNum].id = id;

    MapList[MapNum].type = type;

    MapList[MapNum].map.num = pointNum;
    if (pointNum > 0) {
        MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
        for (int i = 0; i < pointNum; ++i) {
            MapList[MapNum].map.point[i].X = map[i][0];
            MapList[MapNum].map.point[i].Y = map[i][1];
        }
    }

    MapList[MapNum].map2.num = pointNum2;
    if (pointNum2 > 0 && map2 != NULL) {
        MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
        for (int i = 0; i < pointNum2; ++i) {
            MapList[MapNum].map2.point[i].X = map2[i][0];
            MapList[MapNum].map2.point[i].Y = map2[i][1];
        }
    }

    MapNum++;

    DEBUG("AddMap num %d", MapNum);
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

    if (point == NULL || pointNum == 0 || ExamStart) return;

    if (CarModel != NULL) {
        if (CarModel->body != NULL)
            free(CarModel->body);
        if (CarModel->carDesc != NULL)
            free(CarModel->carDesc);
        if (CarModel->carXY != NULL)
            free(CarModel->carXY);
        free(CarModel);
        CarModel = NULL;
    }

    if (CarModelPrev != NULL) {
        if (CarModelPrev->body != NULL)
            free(CarModelPrev->body);
        if (CarModelPrev->carDesc != NULL)
            free(CarModelPrev->carDesc);
        if (CarModelPrev->carXY != NULL)
            free(CarModelPrev->carXY);
        free(CarModelPrev);
        CarModelPrev = NULL;
    }

    // 仅分配carXY, body和carDesc均没有
    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
    CarModelPrev->pointNum = 0;
    CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    CarModel = (car_model *)malloc(sizeof(car_model));
    CarModel->basePoint.X = basePoint[0];
    CarModel->basePoint.Y = basePoint[1];
    CarModel->axial[0] = axial[0];
    CarModel->axial[1] = axial[1];
    CarModel->left_front_tire[0] = left_front_tire[0];
    CarModel->left_front_tire[1] = left_front_tire[1];
    CarModel->right_front_tire[0] = right_front_tire[0];
    CarModel->right_front_tire[1] = right_front_tire[1];
    CarModel->left_rear_tire[0] = left_rear_tire[0];
    CarModel->left_rear_tire[1] = left_rear_tire[1];
    CarModel->right_rear_tire[0] = right_rear_tire[0];
    CarModel->right_rear_tire[1] = right_rear_tire[1];

    CarModel->bodyNum = bodyNum;
    if (bodyNum == 0 || body == NULL) {
        CarModel->bodyNum = 6;
        CarModel->body = (int *) malloc(sizeof(int) * 6);
        for (int i = 0; i < 6; ++i) {
            CarModel->body[i] = i;
        }
    } else {
        CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
        for (int i = 0; i < CarModel->bodyNum; ++i) {
            CarModel->body[i] = body[i];
        }
    }

    CarModel->antPitch = antPitch;

    CarModel->pointNum = pointNum;
    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
    CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    // 测量坐标转换为距离-角度形式
    double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
                 (point[0][1]-basePoint[1])*(point[0][1]-basePoint[1]);
    double C0 = sqrt(C02);

    CarModel->carDesc[0].distance = sqrt(C02);
    CarModel->carDesc[0].angle = 0.0;

    for (int i = 1; i < pointNum; ++i) {
        double dis2 = (point[i][0]-basePoint[0])*(point[i][0]-basePoint[0]) +
                      (point[i][1]-basePoint[1])*(point[i][1]-basePoint[1]);
        double dis = sqrt(dis2);

        CarModel->carDesc[i].distance = dis;

        CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
                                                   (point[i][1]-point[0][1])*(point[i][1]-point[0][1])))/(2*C0*dis)) / M_PI;

        if (i > axial[1])
            CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;

        DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
    }

//    CarModel->carDesc[0].distance = 0.2465;
//    CarModel->carDesc[0].angle = 0;
//
//    CarModel->carDesc[1].distance = 0.2635;
//    CarModel->carDesc[1].angle = 20.7;
//
//    CarModel->carDesc[2].distance = 0.14;
//    CarModel->carDesc[2].angle = 138.9;
//
//    CarModel->carDesc[3].distance = 0.1055;
//    CarModel->carDesc[3].angle = 180.0;
//
//    CarModel->carDesc[4].distance = 0.14;
//    CarModel->carDesc[4].angle = 221.1;
//
//    CarModel->carDesc[5].distance = 0.2635;
//    CarModel->carDesc[5].angle = 339.3;

    DEBUG("SetCarMeasurePoint Calc Over");
}

void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
{
    DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, ExamStart);

//    if (ExamStart) return;

    if (DummyLightContent != NULL) {
        free(DummyLightContent);
        DummyLightContent = NULL;
    }

    DummyLightContent = (struct dummy_light_exam *)malloc(n * sizeof(struct dummy_light_exam));
    DummyLightContentSize = n;

    for (int i = 0; i < n; i++) {
        DummyLightContent[i] = cfg[i];
    }

    StartDummyLightExam(DummyLightContent, DummyLightContentSize);
}

void StartDriverExam(int start, int type)
{
    bool err = false;
    DEBUG("++++++++++++考试启动 start %d type %d+++++++++++++", start, type);

    if (start == 0) {
        DEBUG("结束考试");
//        CurrExamMapIndex = -1;
//        ExamStart = false;
//        CommTestStart(false);
//        MA_SendExamStatus(0, 0);
        return;
    }

    if (MapNum == 0) {
        err = true;
        MA_SendExamStatus(0, -1);
    }
    if (CarModel == NULL) {
        err = true;
        MA_SendExamStatus(0, -2);
    }
    if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
        err = true;
        MA_SendExamStatus(0, -3);
    }

    if (!err) {
        if (!ExamStart) {
            DEBUG("启动考试");
            ExamFaultList.clear();
            examFaultIndex = 0;

            ExamStart = true;
            ExamType = type;
            reportSeatbeltEject = false;

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                StartDummyLightExam(DummyLightContent, DummyLightContentSize);
            }
        }
        MA_SendExamStatus(1, 0);
    }
}

void StartMapExam(int map_id, int exam)
{
    DEBUG("测试该场地 %d: %d", map_id, exam);

    if (map_id >= 0 && exam == 0) {
        CurrExamMapIndex = map_id;
        CurrExamStatus = EXAM_AREA_START;
    }
}

void UpdateRTKInfo(const rtk_info *s)
{
    struct RtkTime rtkClock;

    rtkClock.YY = s->YY;
    rtkClock.MM = s->MM;
    rtkClock.DD = s->DD;
    rtkClock.hh = s->hh;
    rtkClock.mm = s->mm;
    rtkClock.ss = s->ss;
    rtkClock.mss = s->dss;

    if (ExamStart) {
        ExecuteExam(&rtkClock);     // 执行无需车辆定位的项目
    }

    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
            RtkBufferNum++;
    } else {
        return;
    }

    double speed;
    int move;
    int index;

    if (UpdateCarCoord(speed, move, index)) {
        struct carBrief brief;

        sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + RtkBuffer[index].YY,
                RtkBuffer[index].MM, RtkBuffer[index].DD, RtkBuffer[index].hh,
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
        brief.main_ant[0] = RtkBuffer[index].x;
        brief.main_ant[1] = RtkBuffer[index].y;

        brief.axial[0] = CarModel->axial[0];
        brief.axial[1] = CarModel->axial[1];
        brief.left_front_tire[0] = CarModel->left_front_tire[0];
        brief.left_front_tire[1] = CarModel->left_front_tire[1];
        brief.right_front_tire[0] = CarModel->right_front_tire[0];
        brief.right_front_tire[1] = CarModel->right_front_tire[1];
        brief.left_rear_tire[0] = CarModel->left_rear_tire[0];
        brief.left_rear_tire[1] = CarModel->left_rear_tire[1];
        brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
        brief.right_rear_tire[1] = CarModel->right_rear_tire[1];

        brief.bodyNum = CarModel->bodyNum;
        brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
        for (int i = 0; i < CarModel->bodyNum; ++i) {
            brief.body[i] = CarModel->body[i];
        }

        brief.pointNum = CarModel->pointNum;
        brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
        for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
            brief.point[j++] = CarModel->carXY[i].X;
            brief.point[j++] = CarModel->carXY[i].Y;
        }

        MA_SendCarPosition(&brief);

        free(brief.body);
        free(brief.point);

        struct RtkTime rtkTime;
        double azimuth = RtkBuffer[index].heading;

        rtkTime.YY = RtkBuffer[index].YY;
        rtkTime.MM = RtkBuffer[index].MM;
        rtkTime.DD = RtkBuffer[index].DD;
        rtkTime.hh = RtkBuffer[index].hh;
        rtkTime.mm = RtkBuffer[index].mm;
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        DetectEnterOrExitMap();

        if (ExamStart) {
            ExecuteExam(speed, move, azimuth, &rtkTime);
        }
    }
}

static void DetectEnterOrExitMap(void)
{
    if (ExamType == TEST_TYPE_AREA) {

    }
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);

                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                CurrEnterMapIndex = -1;
            }
        }
    }
}

static void ExecuteExam(const struct RtkTime* rtkTime)
{

}

static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    if (move != 0) {
        if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
            reportSeatbeltEject = true;
            AddExamFault(1, rtkTime);
        }
    }

    if (CurrExamMapIndex >= 0) {
        int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

        if (CurrExamStatus == EXAM_AREA_START) {
            DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    DEBUG("进入倒车入库场地");
                    MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                    StartParkBottom(move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地");
                    MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartSAS(move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地");
                    MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartParkEdge(move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地");
                    MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartDrivingCurve(move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地");
                    MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartTurnA90(move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
            }
        } else if (CurrExamStatus == EXAM_AREA_RUN) {
            int testing = 0;
            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                             CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
            }

            if (testing > 0) {
                CurrExamStatus = EXAM_AREA_RUN;
            } else {
                CurrExamStatus = EXAM_AREA_END;
            }
        }
        if (CurrExamStatus != EXAM_AREA_RUN) {
            // 某项结束
            //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            CurrExamStatus = EXAM_AREA_NONE;
            CurrExamMapIndex = -1;
        }
    }
}

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

    // 不及时松开启动开关，扣10分
    if (ReadSensor(SENSOR_ENGINE_START) == 1 && ExamStart) {
        if (ExamType == TEST_TYPE_AREA)
            AddExamFault(4, &rtkClock);
    }
}

void SensorChanged(int *sensorId, int *sensorValue, int num)
{
    const int ENGINE_MIN_ROTATE = 200;

    for (int i = 0; i < num; ++i) {
        switch (sensorId[i]) {
            case SENSOR_SEATBELT:
                break;
            case SENSOR_ENGINE_START:
                AppTimer_delete(EngineStartHold);

                if (sensorValue[i] == 1) {
                    if (ReadSensor(SENSOR_SHIFT_N) == 0) {
                        // 不是空挡点火，不合格
                        if (ExamType == TEST_TYPE_AREA)
                            AddExamFault(3, &rtkClock);
                        else
                            AddExamFault(4, &rtkClock);
                    }
                    AppTimer_add(EngineStartHold, D_SEC(2));
                }
                break;
            case SENSOR_ENGINE_RPM:
                if (sensorValue[i] < ENGINE_MIN_ROTATE) {
                    if (engineRuning) {
                        engineRuning = false;
                        if (ExamType == TEST_TYPE_AREA) {
                            // 熄火1次，扣10分
                            AddExamFault(5, &rtkClock);
                        } else {
                            AddExamFault(20, &rtkClock);
                        }
                    }
                } else {
                    engineRuning = true;
                }
                break;
            default:
                break;
        }
    }
}

/*************************************************
 * 2次采样相差的时间, a 最近的，b 先前的
 * @param a
 * @param b
 * @return ms
 */
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
    return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
}

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
{
    long tmDiff;

    if (CarModel == NULL)
        return false;

    if (RtkBufferNum < 2)
        return false;

    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 最近采样值
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 找到1秒前的采样值
    int pn = 0;

    for (pn = 1; pn < RtkBufferNum; ++pn) {
        p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
        if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
    }

    if (pn == RtkBufferNum)
        return false;

    // 如果一定的时间都没有有效定位，删除之前的值
    /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);

    DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);

    if (tmDiff > D_SEC(5)) {
        if (p1 != 0)
            RtkBuffer[0] = RtkBuffer[p1];
        RtkBufferIn = RtkBufferNum = 1;
        return false;
    }*/

    // 计算车辆轮廓点
    PointF main_ant_coord;
    main_ant_coord.X = RtkBuffer[p1].x;
    main_ant_coord.Y = RtkBuffer[p1].y;

    // 保存上一个车身轮廓点
    if (CarModelPrev->pointNum > 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }
    UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
    if (CarModelPrev->pointNum == 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }

    // 计算速度(米/秒)、前进后退
    double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
                (double)(tmDiff);
    int move = 0;
    double deg = 0.0;

    if (speed < 0.05) {
        // 停车
        move = 0;
    } else {
        // 判断前进还是后退
        if (fabs(RtkBuffer[p1].y - RtkBuffer[p2].y) <= GLB_EPSILON) {
            if (RtkBuffer[p1].x > RtkBuffer[p2].x) {
                deg = 90;
            } else {
                deg = 270;
            }
        } else if (fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) <= GLB_EPSILON) {
            if (RtkBuffer[p1].y > RtkBuffer[p2].y) {
                deg = 0;
            } else {
                deg = 180;
            }
        } else {
            deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
                       fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));

            deg = toDegree(deg);

            if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
                RtkBuffer[p1].y > RtkBuffer[p2].y) {

            } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
                       RtkBuffer[p1].y > RtkBuffer[p2].y) {
                deg = 360 - deg;
            } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
                       RtkBuffer[p1].y < RtkBuffer[p2].y) {
                deg = 180 + deg;
            } else if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
                       RtkBuffer[p1].y < RtkBuffer[p2].y) {
                deg = 180 - deg;
            }
        }

        deg = fabs(RtkBuffer[p1].heading - deg);
        if (deg > 180) {
            deg = 360 - deg;
        }
        if (deg < 90) {
            // 前进
            move = 1;
        } else {
            // 后退
            move = -1;
        }
    }

    spd = speed;
    mov = move;
    idx = p1;
//    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);

    return true;
}

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
{
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
//        if (mapList[i].type == MAP_TYPE_STOP_START) {
//            // 构造虚拟的左上角点
//            double x9, y9, xo, yo;
//
//            bool enter = false;
//
//            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
//            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
//
//            x9 = 2*xo - mapList[i].map.point[8].X;
//            y9 = 2*yo - mapList[i].map.point[8].Y;
//
//            Polygon map;
//
//            map.num = 4;
//            map.point = (PointF *) malloc(map.num * sizeof(PointF));
//
//            map.point[0] = mapList[i].map.point[0];
//            map.point[1] = mapList[i].map.point[8];
//            map.point[2] = mapList[i].map.point[7];
//            map.point[3].X = x9;
//            map.point[3].Y = y9;
//
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
//                Line enterLine1;
//
//                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
//                    enter = true;
//            }
//
//            free(map.point);
//
//            if (enter) return i;
//        }
//        if (mapList[i].type == MAP_TYPE_CURVE) {
//
//        }
//        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_TURN_90) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
//                    return i;
//            }
//        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line startLine;
            Line carAxial;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
                    return i;
            }
        }
    }
    return -1;
}

static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
{
    bool ret = false;
    if (index < 0 || mapList == NULL || mapNum == 0) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
        mapList[index].type == MAP_TYPE_PART_EDGE ||
        mapList[index].type == MAP_TYPE_TURN_90) {
        // 全车都需不在地图中
        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, &mapList[index].map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }
    if (mapList[index].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            bool enter = false;

            xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
            yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[index].map.point[8].X;
            y9 = 2*yo - mapList[index].map.point[8].Y;

            Polygon map;

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

            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[8];
            map.point[2] = mapList[index].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;

            // 全车都需不在地图中
            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) == GM_None) {
                ret = true;
            }

            free(carBody.point);
            free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
    }

    return ret;
}

static int GetMapId(int index, const struct map_list *mapList, int mapNum)
{
    if (index < 0 || mapList == NULL || mapNum == 0)
        return -1;

    return mapList[index].id;
}

static int GetMapType(int index, const struct map_list *mapList, int mapNum)
{
    if (index < 0 || mapList == NULL || mapNum == 0)
        return -1;

    return mapList[index].type;
}

void AddExamFault(int wrong, const struct RtkTime *rtkTime)
{
    struct ExamFault fault;

    if (!ExamStart)
        return;

    fault.sn = examFaultIndex++;
    sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
            rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
    fault.wrong_id = wrong;

    DEBUG("考试发生错误 %d %s", wrong, fault.utc);

    ExamFaultList.push_back(fault);

    MA_SendExamWrong(ExamFaultList);

    ExamFaultList.clear();
}

car_model_cache_t *GetCarModelCache(int node)
{
    return NULL;
}

/*******************************************************************
 * @brief 由主天线坐标计算车身点坐标
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;

    pitch = pitch - carModel->antPitch;

    for (int i = 0; i < carModel->pointNum; ++i) {
        double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
        double qry =
                carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
                cos(toRadians(pitch));

        double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
        double projectAngle = toDegree(acos(qry / projectDistance));

        if (carModel->carDesc[i].angle > 180) {
            projectAngle = 360 - projectAngle;
        }

//        double tx = projectDistance*sin(toRadians(azimuth));
//        double ty = projectDistance*cos(toRadians(azimuth));

        carModel->carXY[i].X =
                projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                main_ant.X;
        carModel->carXY[i].Y =
                projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                main_ant.Y;
    }
}

void SystemShutdown(int event, int timeout)
{
    // 关机
    if (event == 1) {
        ShutdownInd(timeout);
    }
    // 重启
    if (event == 0) {

    }
}