//
// 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 <mutex>
#include <cstring>
#include <vector>
#include <list>
#include <numeric>
#include <algorithm>
#include <string>
#include <tuple>

#include "driver_test.h"
#include "defs.h"
#include "test_common/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/right_corner.h"
#include "test_items/driving_curve.h"
#include "test_items/uphill.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "utils/num.h"
#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"
#include "test_items/area_exam.h"
#include "test_common/odo_graph.h"
#include "map.h"
#include "common/semaphore.h"
#include "common/string_util.h"

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

using namespace std;

#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_TYPE_AREA = 2,
    TEST_TYPE_ROAD_DUMMY_LIGHT,
    TEST_TYPE_ROAD_TRUE_LIGHT,
    TEST_TYPE_ROAD_CALIBRATE
};

static const int DEFAULT_START_KEY_HOLD_TIME = D_SEC(2);
static const int DEFAULT_CURVE_PAUSE_TIME = D_SEC(2);
static const int DEFAULT_PARK_BOTTOM_PAUSE_TIME = D_SEC(2);
static const int DEFAULT_PARK_BOTTOM_FINISH_TIME = D_SEC(210);
static const int DEFAULT_PART_EDGE_PAUSE_TIME = D_SEC(2);
static const int DEFAULT_PARK_EDGE_FINISH_TIME = D_SEC(90);
static const int DEFAULT_TURN_A90_PAUSE_TIME = D_SEC(2);
static const int DEFAULT_RAMP_FINISH_TIME = D_SEC(30);
static const double DEFAULT_RAMP_STOPPOINT_RED_DISTANCE = 0.5;
static const double DEFAULT_RAMP_EDGE_YELLOW_DISTANCE = 0.3;
static const double DEFAULT_RAMP_EDGE_RED_DISTANCE = 0.5;
static const double DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE = 0.1;
static const double DEFAULT_RAMP_SLIDE_RED_DISTANCE = 0.3;

static const double DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE = 0.1;
static const double DEFAULT_ROAD_SLIDE_RED_DISTANCE = 0.3;
static const int DEFAULT_ROAD_MAX_DISTANCE = 3000;
static const int DEFAULT_ROAD_MAX_SPEED = 60;
static const int DEFAULT_GEAR_N_TIME = D_SEC(5);
static const int DEFAULT_SAME_GEAR_HOLD_TIME = D_SEC(5);
static const int DEFAULT_GEAR_SPEED_ERROR_MAX_TIME = D_SEC(15);
static const int DEFAULT_ROAD_PAUSE_TIME = D_SEC(2);
static const int DEFAULT_CHANGE_LANE_MIN_TIME = D_SEC(10);
static const int DEFAULT_CRASH_DOTTED_LINE_MAX_TIME = D_SEC(10);
static const int DEFAULT_TURN_SIGNAL_MIN_ADVANCE = D_SEC(3);
static const int DEFAULT_START_CAR_MAX_RMP = 2500;
static const int DEFAULT_START_CAR_DISTANCE = 10;
static const double DEFAULT_START_CAR_OPEN_DOOR_DISTANCE = 1.0;
static const char DEFAULT_PREPARE_TTS[] = "请开始上车准备";
static const char DEFAULT_TOUCH_LEFT_FRONT[] = "学员通过左前方";
static const char DEFAULT_TOUCH_LEFT_REAR[] = "学员通过左后方";
static const char DEFAULT_TOUCH_RIGHT_FRONT[] = "学员通过右前方";
static const char DEFAULT_TOUCH_RIGHT_REAR[] = "学员通过右后方";
static const char DEFAULT_START_ENGINE[] = "请启动发动机";
static const char DEFAULT_START_CAR_BEGIN_TTS[] = "请起步，继续完成考试";
static const char DEFAULT_START_CAR_END_TTS[] = "起步完成";
static const int CHANGE_LANE_MAX_DISTANCE = 100;
static const char DEFAULT_CHANGE_LANE_BEGIN_TTS[] = "前方请变更车道";
static const char DEFAULT_CHANGE_LANE_END_TTS[] = "变道完成";
static const int DEFAULT_SHIFT_MAX_DISTANCE = 120;
static const int DEFAULT_SHIFT_HOLD_TIME = D_SEC(3);
static const char DEFAULT_SHIFT_BEGIN_TTS[] = "请进行加减挡位操作";
static const char DEFAULT_SHIFT_END_TTS[] = "加减挡位完成";
static const char DEFAULT_STRAIGHT_BEGIN_TTS[] = "请保持直线行驶";
static const char DEFAULT_STRAIGHT_END_TTS[] = "直线行驶完成";
static const int DEFAULT_STRAIGHT_MAX_DISTANCE = 100;
static const double DEFAULT_STRAIGHT_MAX_OFFSET = 0.3;
static const int DEFAULT_OVERTAKE_MAX_DISTANCE = 150;
static const char DEFAULT_OVERTAKE_BEGIN_TTS[] = "请超越前方车辆";
static const char DEFAULT_OVERTAKE_END_TTS[] = "超车完成";
static const int DEFAULT_STOP_CAR_MAX_DISTANCE = 150;
static const int DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME = D_SEC(15);
static const double DEFAULT_STOP_CAR_EDGE_RED_DISTANCE = 0.5;
static const double DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE = 0.5;
static const char DEFAULT_STOP_CAR_BEGIN_TTS[] = "请靠边停车";
static const char DEFAULT_STOP_CAR_END_TTS[] = "靠边停车完成";
static const double DEFAULT_CROSSING_STOP_VALID_DISTANCE = 3.0;
static const int DEFAULT_CROSS_SCHOOL_MAX_SPEED = 30;
static const int DEFAULT_CROSS_BREAK_VALID_DISTANCE = 30;

static const char DEFAULT_CROSSING_GO_STRAIGHT_TTS[] = "前方路口直行";
static const char DEFAULT_CROSSING_TURN_LEFT_TTS[] = "前方路口左转";
static const char DEFAULT_CROSSING_TURN_RIGHT_TTS[] = "前方路口右转";
static const char DEFAULT_CROSSING_TURN_BACK_TTS[] = "前方选择合适地点掉头";
static const char DEFAULT_CROSSING_TURN_UNKNOWN_TTS[] = "前方路口听从教练指示";

static int ExamType;
static bool reportSeatbeltEject;

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

ilovers::semaphore sem(0);

static int exam_dummy_light;

static car_model_t CarModel;

//static LIST_CAR_MODEL CarModelList;             // 一段时间的车辆轨迹集合

static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
static bool engineRuning = false;
const int ENGINE_MIN_ROTATE = 200;
static bool engineStart = false;

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

static std::list<rtk_info_t> RtkInfoList;

// 存入前后2组建模数据
static modeling_t realtimeBodyModeling[2];
static motion_t realtimeMotionStatus;

static prime_t prime;

static void SetExamParamDefault(void);
static void EngineStartHold(apptimer_var_t val);
static motion_t CalcMotionState(std::list<rtk_info_t> &s);
static void ExecuteExam(prime_t &prime);

static void ReadDriverExamPrimerTimeout(apptimer_var_t val);
static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk);
static void work_thread(void);
static void UploadModeling(motion_t &motion, modeling_t &modeling);

prime_t& GetPrime(void)
{
    return prime;
}

void DriverTestInit(void)
{
    prime.examing = false;
    prime.pMotion = &realtimeMotionStatus;
    prime.pModeling = realtimeBodyModeling;
    prime.pModel = &CarModel;

    SetExamParamDefault();

//    CarModelList.clear();

//    AreaMapList.clear();

    CarSensorInit();

    DummyLightContentSize = 0;
    DummyLightContent = NULL;

    std::thread(work_thread).detach();
}

static void SetExamParamDefault(void)
{
    prime.examParam.hold_start_key_limit_time = DEFAULT_START_KEY_HOLD_TIME;
    prime.examParam.curve_pause_criteria = DEFAULT_CURVE_PAUSE_TIME;
    prime.examParam.park_bottom_pause_criteria = DEFAULT_PARK_BOTTOM_PAUSE_TIME;
    prime.examParam.park_bottom_limit_time = DEFAULT_PARK_BOTTOM_FINISH_TIME;
    prime.examParam.park_edge_pause_criteria = DEFAULT_PART_EDGE_PAUSE_TIME;
    prime.examParam.park_edge_limit_time = DEFAULT_PARK_EDGE_FINISH_TIME;
    prime.examParam.turn_a90_pause_criteria = DEFAULT_TURN_A90_PAUSE_TIME;

    prime.examParam.ramp_stoppoint_red_distance = DEFAULT_RAMP_STOPPOINT_RED_DISTANCE;
    prime.examParam.ramp_edge_yellow_distance = DEFAULT_RAMP_EDGE_YELLOW_DISTANCE;
    prime.examParam.ramp_edge_red_distance = DEFAULT_RAMP_EDGE_RED_DISTANCE;
    prime.examParam.ramp_slide_yellow_distance = DEFAULT_RAMP_SLIDE_YELLOW_DISTANCE;
    prime.examParam.ramp_slide_red_distance = DEFAULT_RAMP_SLIDE_RED_DISTANCE;
    prime.examParam.ramp_start_car_limit_time = DEFAULT_RAMP_FINISH_TIME;

    prime.examParam.road_slide_yellow_distance = DEFAULT_ROAD_SLIDE_YELLOW_DISTANCE;
    prime.examParam.road_slide_red_distance = DEFAULT_ROAD_SLIDE_RED_DISTANCE;
    prime.examParam.road_total_distance = DEFAULT_ROAD_MAX_DISTANCE;
    prime.examParam.road_max_speed = DEFAULT_ROAD_MAX_SPEED;

    prime.examParam.gear_speed_table.resize(6);
    for (int i = 0; i < prime.examParam.gear_speed_table.size(); ++i) {
        prime.examParam.gear_speed_table[i].resize(2);
    }
    prime.examParam.gear_speed_table[0][0] = 0;
    prime.examParam.gear_speed_table[0][1] = 20;
    prime.examParam.gear_speed_table[1][0] = 5;
    prime.examParam.gear_speed_table[1][1] = 30;
    prime.examParam.gear_speed_table[2][0] = 15;
    prime.examParam.gear_speed_table[2][1] = 40;
    prime.examParam.gear_speed_table[3][0] = 25;
    prime.examParam.gear_speed_table[3][1] = 10000;
    prime.examParam.gear_speed_table[4][0] = 35;
    prime.examParam.gear_speed_table[4][1] = 10000;
    prime.examParam.gear_speed_table[5][0] = 45;
    prime.examParam.gear_speed_table[5][1] = 10000;

    prime.examParam.gear_n_allow_time = DEFAULT_GEAR_N_TIME;
    prime.examParam.same_gear_min_time = DEFAULT_SAME_GEAR_HOLD_TIME;            // x秒内，不允许N->X->N->X置同一挡位
    prime.examParam.gear_speed_error_cumulative_time = DEFAULT_GEAR_SPEED_ERROR_MAX_TIME;
    prime.examParam.road_pause_criteria = DEFAULT_ROAD_PAUSE_TIME;
    prime.examParam.continuous_change_lane_min_time = DEFAULT_CHANGE_LANE_MIN_TIME;
    prime.examParam.crash_dotted_line_cumulative_time = DEFAULT_CRASH_DOTTED_LINE_MAX_TIME;
    prime.examParam.turn_signal_min_advance = DEFAULT_TURN_SIGNAL_MIN_ADVANCE;
    prime.examParam.start_car_max_rpm = DEFAULT_START_CAR_MAX_RMP;
    prime.examParam.start_car_limit_distance = DEFAULT_START_CAR_DISTANCE;
    prime.examParam.open_door_drive_allow_distance = DEFAULT_START_CAR_OPEN_DOOR_DISTANCE;

    prime.examParam.prepare_tts = DEFAULT_PREPARE_TTS;
    prime.examParam.touch_leftfront_tts = DEFAULT_TOUCH_LEFT_FRONT;
    prime.examParam.touch_leftrear_tts = DEFAULT_TOUCH_LEFT_REAR;
    prime.examParam.touch_rightfront_tts = DEFAULT_TOUCH_RIGHT_FRONT;
    prime.examParam.touch_rightrear_tts = DEFAULT_TOUCH_RIGHT_REAR;
    prime.examParam.start_engine_tts = DEFAULT_START_ENGINE;

    prime.examParam.start_car_begin_tts = DEFAULT_START_CAR_BEGIN_TTS;
    prime.examParam.start_car_end_tts = DEFAULT_START_CAR_END_TTS;
    prime.examParam.change_lane_limit_distance = CHANGE_LANE_MAX_DISTANCE;
    prime.examParam.change_lane_begin_tts = DEFAULT_CHANGE_LANE_BEGIN_TTS;
    prime.examParam.change_lane_end_tts = DEFAULT_CHANGE_LANE_END_TTS;
    prime.examParam.shift_limit_distance = DEFAULT_SHIFT_MAX_DISTANCE;
    prime.examParam.shift_hold_time = DEFAULT_SHIFT_HOLD_TIME;
    prime.examParam.shift_begin_tts = DEFAULT_SHIFT_BEGIN_TTS;
    prime.examParam.shift_end_tts = DEFAULT_SHIFT_END_TTS;
    prime.examParam.shift_up_tts = "";
    prime.examParam.shift_down_tts = "";

    prime.examParam.straight_begin_tts = DEFAULT_STRAIGHT_BEGIN_TTS;
    prime.examParam.straight_end_tts = DEFAULT_STRAIGHT_END_TTS;
    prime.examParam.straight_limit_distance = DEFAULT_STRAIGHT_MAX_DISTANCE;
    prime.examParam.straight_max_offset = DEFAULT_STRAIGHT_MAX_OFFSET;

    prime.examParam.overtake_limit_distance = DEFAULT_OVERTAKE_MAX_DISTANCE;
    prime.examParam.overtake_begin_tts = DEFAULT_OVERTAKE_BEGIN_TTS;
    prime.examParam.overtake_end_tts = DEFAULT_OVERTAKE_END_TTS;

    prime.examParam.stop_car_limit_distance = DEFAULT_STOP_CAR_MAX_DISTANCE;
    prime.examParam.stop_car_open_door_allow_time = DEFAULT_STOP_CAR_OPEN_DOOR_MAX_TIME;
    prime.examParam.stop_car_edge_red_distance = DEFAULT_STOP_CAR_EDGE_RED_DISTANCE;
    prime.examParam.stop_car_edge_yellow_distance = DEFAULT_STOP_CAR_EDGE_YELLOW_DISTANCE;
    prime.examParam.stop_car_begin_tts = DEFAULT_STOP_CAR_BEGIN_TTS;
    prime.examParam.stop_car_end_tts = DEFAULT_STOP_CAR_END_TTS;
    prime.examParam.crossing_stop_valid_distance = DEFAULT_CROSSING_STOP_VALID_DISTANCE;
    prime.examParam.cross_school_max_speed = DEFAULT_CROSS_SCHOOL_MAX_SPEED;
    prime.examParam.crossing_break_valid_distance = DEFAULT_CROSS_BREAK_VALID_DISTANCE;

    prime.examParam.crossing_go_straight_tts = DEFAULT_CROSSING_GO_STRAIGHT_TTS;
    prime.examParam.crossing_turn_left_tts = DEFAULT_CROSSING_TURN_LEFT_TTS;
    prime.examParam.crossing_turn_right_tts = DEFAULT_CROSSING_TURN_RIGHT_TTS;
    prime.examParam.crossing_turn_back_tts = DEFAULT_CROSSING_TURN_BACK_TTS;
    prime.examParam.crossing_turn_unknown_tts = DEFAULT_CROSSING_TURN_UNKNOWN_TTS;
}

static void ReadDriverExamPrimerTimeout(apptimer_var_t val)
{
    ReadDriverExamPrimer();
}

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

void ClearAreaMap(void)
{
    if (!prime.examing) {
        DEBUG("清除所有地图");
        vector<curve_map_t>().swap(std::get<MAP_TYPE_CURVE>(prime.maps));
        vector<park_button_map_t>().swap(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps));
        vector<park_edge_map_t>().swap(std::get<MAP_TYPE_PARK_EDGE>(prime.maps));
        vector<turn_a90_map_t>().swap(std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps));
        vector<uphill_map_t>().swap(std::get<MAP_TYPE_UPHILL>(prime.maps));
    }
}

void AddCurveMap(curve_map_t &map)
{
    DEBUG("添加曲线行驶地图");
    if (prime.examing) {
        return;
    }
    for (auto m: std::get<MAP_TYPE_CURVE>(prime.maps)) {
        if (m.id == map.id) {
            return;
        }
    }

    std::get<MAP_TYPE_CURVE>(prime.maps).push_back(map);
}

// 如果未采集车位口虚线内侧，则需要计算出来
void AddParkButtonMap(park_button_map_t &map)
{
    DEBUG("添加倒车入库地图");
    if (prime.examing) {
        return;
    }

    for (auto m: std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)) {
        if (m.id == map.id) {
            return;
        }
    }
    if (map.points.size() == 8) {
        PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
        PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
        map.points.push_back(p8);
        map.points.push_back(p9);
    }
    std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).push_back(map);
    DEBUG("倒库地图点数：%d", map.points.size());
    DEBUG("倒库地图点数 %d",std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[0].points.size());
}

// 如果未采集车位口虚线内侧，则需要计算出来
void AddParkEdgeMap(park_edge_map_t &map)
{
    DEBUG("添加侧方停车地图");
    if (prime.examing) {
        return;
    }
    for (auto m: std::get<MAP_TYPE_PARK_EDGE>(prime.maps)) {
        if (m.id == map.id) {
            return;
        }
    }
    if (map.points.size() == 8) {
        PointF p8 = PointExtend(map.points[2], map.line_width, YawOf(map.points[2], map.points[3]));
        PointF p9 = PointExtend(map.points[5], map.line_width, YawOf(map.points[5], map.points[4]));
        map.points.push_back(p8);
        map.points.push_back(p9);
    }
    std::get<MAP_TYPE_PARK_EDGE>(prime.maps).push_back(map);
}

// 坡道起步地图要把左下角和左上角计算出来，方便场地的进入和退出判断
void AddUphillMap(uphill_map_t &map)
{
    DEBUG("添加上坡起步地图");
    if (prime.examing) {
        return;
    }
    for (auto m: std::get<MAP_TYPE_UPHILL>(prime.maps)) {
        if (m.id == map.id) {
            return;
        }
    }

    if (map.points.size() != 10)
        return;

    PointF bottom_left = Calc3Point(map.points[1], map.points[0], DistanceOf(map.points[1], map.points[2]), 'R');
    PointF top_left = Calc3Point(map.points[8], map.points[9], DistanceOf(map.points[8], map.points[7]), 'L');

    map.points.push_back(bottom_left);     // 作为10号点
    map.points.push_back(top_left);        // 作为11号点

    std::get<MAP_TYPE_UPHILL>(prime.maps).push_back(map);
}

void AddTurnA90Map(turn_a90_map_t &map)
{
    DEBUG("添加直角转弯地图");
    if (prime.examing) {
        return;
    }
    for (auto m: std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps)) {
        if (m.id == map.id) {
            return;
        }
    }
    std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).push_back(map);
}

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, double antHeight, double groundHeight)
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

    if (point == NULL || pointNum == 0 || prime.examing) return;

    vector<int>().swap(CarModel.body);
    vector<car_desc_t>().swap(CarModel.carDesc);

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

    if (bodyNum == 0 || body == NULL) {
        for (int i = 0; i < 6; ++i) {
            CarModel.body.push_back(i);
        }
    } else {
        for (int i = 0; i < bodyNum; ++i) {
            CarModel.body.push_back(body[i]);
        }
    }

    CarModel.antPitch = antPitch;
    CarModel.antHeight = antHeight;
    CarModel.groundHeight = groundHeight;

    CarModel.carDesc.resize(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 StartDriverExam(int start, int type)
{
    bool err = false;
    DEBUG("++++++++++++考试启动 start %d type %d+++++++++++++", start, type);

    if (start == 0) {
        DEBUG("结束考试");

        TerminateAreaExam();

        prime.examing = false;
        MA_SendExamStatus(0, 0);
        return;
    }

//    type = TEST_TYPE_ROAD_CALIBRATE;

    if (std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size() == 0 && type == TEST_TYPE_AREA) {
        DEBUG("没有场考地图");
        err = true;
        MA_SendExamStatus(0, -1);
    }
    if (prime.pModel->carDesc.size() == 0) {
        DEBUG("没有车模");
        err = true;
        MA_SendExamStatus(0, -2);
    }
    if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
        DEBUG("没有模拟灯光");
//        err = true;
//        MA_SendExamStatus(0, -3);
    }

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

            prime.examing = true;
            ExamType = type;
            reportSeatbeltEject = false;

            if (type == TEST_TYPE_AREA) {
                DEBUG("倒库地图数目：%d", std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps).size());
                DEBUG("侧方地图数目：%d", std::get<MAP_TYPE_PARK_EDGE>(prime.maps).size());
                DEBUG("曲线地图数目：%d", std::get<MAP_TYPE_CURVE>(prime.maps).size());
                DEBUG("坡道地图数目：%d", std::get<MAP_TYPE_UPHILL>(prime.maps).size());
                DEBUG("直角地图数目：%d", std::get<MAP_TYPE_RIGHT_CORNER>(prime.maps).size());

                InitAreaExam();
            }
        }
        MA_SendExamStatus(1, 0);
    }
}

/***************************************
 * 触发考试评判
 */
static void work_thread(void)
{
    while (true) {
        sem.wait();

        // 计算当前车速，前进后退或停止
        realtimeMotionStatus = CalcMotionState(RtkInfoList);
        // 累加里程
        UpdataOdo(realtimeMotionStatus);
        prime.odo = ReadOdo();
        // 计算当前车辆点坐标值
        if (CarModel.carDesc.size() > 0) {
            prime.prev_modeling_index = prime.curr_modeling_index;
            DEBUG("缓存坐标 %d", prime.prev_modeling_index);
            prime.curr_modeling_index = (prime.curr_modeling_index + 1) %
                                        (sizeof(realtimeBodyModeling) /
                                         sizeof(realtimeBodyModeling[0]));
            DEBUG("当前坐标 %d", prime.curr_modeling_index);
            CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel,
                             RtkInfoList.front());
            DEBUG("当前坐标 点数 %d", realtimeBodyModeling[prime.curr_modeling_index].points.size());
            // 向UI上报车辆点坐标
            UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
        }
        // 触发考试项目
        ExecuteExam(prime);
    }
}

static motion_t CalcMotionState(std::list<rtk_info_t> &s)
{
    motion_t motion;

    double speed;
    move_status_t move;

    if (s.size() < 2) {
        return motion;
    }

    auto curr = s.begin();

    motion.timestamp = curr->utc_time;

            // 查找1秒前的点,如果找不到则视为停车
    auto prev = s.begin();

    std::advance(prev, 1);

    for (; prev != s.end(); ++prev) {
        if (curr->utc_time - prev->utc_time > 3000) {
            return motion;
        }
        if (curr->utc_time - prev->utc_time >= 1000) {
            break;
        }
    }

    if (prev == s.end()) {
        return motion;
    }

    // 计算速度(米/秒)、前进后退
    speed = sqrt(pow(curr->x - prev->x, 2) + pow(curr->y - prev->y, 2)) * 1000 /
                   static_cast<double>(curr->utc_time - prev->utc_time);

    double deg = 0.0;

    if (speed < 0.05) {
        // 停车
        move = STOP;
    } else {
        // 判断前进还是后退
        if (fabs(curr->y - prev->y) <= GLB_EPSILON) {
            if (curr->x > prev->x) {
                deg = 90;
            } else {
                deg = 270;
            }
        } else if (fabs(curr->x - prev->x) <= GLB_EPSILON) {
            if (curr->y > prev->y) {
                deg = 0;
            } else {
                deg = 180;
            }
        } else {
            deg = toDegree(atan(fabs(curr->x - prev->x) /
                       fabs(curr->y - prev->y)));

            if (curr->x > prev->x &&
                    curr->y > prev->y) {

            } else if (curr->x < prev->x &&
                    curr->y > prev->y) {
                deg = 360 - deg;
            } else if (curr->x < prev->x &&
                    curr->y < prev->y) {
                deg = 180 + deg;
            } else if (curr->x > prev->x &&
                    curr->y < prev->y) {
                deg = 180 - deg;
            }
        }

        deg = fabs(curr->heading - deg);
        if (deg > 180) {
            deg = 360 - deg;
        }
        if (deg < 90) {
            // 前进
            move = FORWARD;
        } else {
            // 后退
            move = BACKWARD;
        }
    }

    motion.speed = speed;
    motion.move = move;

    return motion;
}

void UpdateSensor(int id, int value)
{
    switch (id) {
        case OBD_SPEED:
            prime.sensor.speed = value;
            break;
        case ENGINE_RPM:
            prime.sensor.rpm = value;
            break;
        case GEAR:
            prime.sensor.gear = value;
            break;
        case TURN_SIGNAL_LAMP:
            prime.sensor.turn_signal_lamp = value;
            break;
        case DIPPED_BEAM_LAMP:
            prime.sensor.dipped_beam_lamp = value;
            break;
        case FOG_LAMP:
            prime.sensor.fog_lamp = value;
            break;
        case CLEARANCE_LAMP:
            prime.sensor.clearance_lamp = value;
            break;
        case FLASH_BEAM_LAMP:
            prime.sensor.flash_beam_lamp = value;
            break;
        case MAIN_BEAM_LAMP:
            prime.sensor.main_beam_lamp = value;
            break;
        case SEATBELT:
            prime.sensor.seatbelt = value;
            break;
        case ENGINE_START:
            prime.sensor.engine_start = value;
            break;
        case BRAKE:
            prime.sensor.brake = value;
            break;
        case HAND_BRAKE:
            prime.sensor.hand_brake = value;
            break;
        case SECOND_BRAKE:
            prime.sensor.second_brake = value;
            break;
        case DOOR:
            prime.sensor.door = value;
            break;
        case SURROUND_CAR_1:
            prime.sensor.surround_car_1 = value;
            break;
        case SURROUND_CAR_2:
            prime.sensor.surround_car_2 = value;
            break;
        case SURROUND_CAR_3:
            prime.sensor.surround_car_3 = value;
            break;
        case SURROUND_CAR_4:
            prime.sensor.surround_car_4 = value;
            break;
        default:break;
    }
}

void UpdateRTKInfo(const rtk_info_t *s)
{
    RtkInfoList.push_front(*s);

    while (RtkInfoList.size() > 100) {
        RtkInfoList.pop_back();
    }
    sem.signal();
}

static void UploadModeling(motion_t &motion, modeling_t &modeling)
{
    struct carBrief brief;

    struct TimeStructure ts;

    TimeBreakdown(modeling.utc_time / 1000, &ts);

    sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", ts.Year, ts.Month, ts.Day,
            ts.Hour, ts.Minute, ts.Second, (modeling.utc_time % 1000) / 10);

        brief.qf = 3;
        brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = motion.move;
        brief.speed = ConvertMs2KMh(motion.speed);
        brief.heading = modeling.yaw;
        brief.main_ant[0] = modeling.base_point.X;
        brief.main_ant[1] = modeling.base_point.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.body.assign(CarModel.body.begin(), CarModel.body.end());

        for (auto po: modeling.points) {
            brief.point.push_back({round(po.X, 4), round(po.Y, 4)});
        }

        MA_SendCarPosition(&brief);
}

static void ExecuteExam(prime_t &prime)
{
    static bool rec = false;
    static bool handBreakActive = false, handBreakActive2 = false;
    static double startCarMoveDistance;
    static move_status_t prevMove = STOP;

    // 更新车辆传感器数据
    DEBUG("GEAR = %d", ReadCarStatus(GEAR));
    DEBUG("RPM = %d", ReadCarStatus(ENGINE_RPM));
    DEBUG("TURN_SIGNAL_LAMP = %d", ReadCarStatus(TURN_SIGNAL_LAMP));
    DEBUG("DIPPED_BEAM_LAMP = %d", ReadCarStatus(DIPPED_BEAM_LAMP));
    DEBUG("FOG_LAMP = %d", ReadCarStatus(FOG_LAMP));
    DEBUG("CLEARANCE_LAMP = %d", ReadCarStatus(CLEARANCE_LAMP));
    DEBUG("FLASH_BEAM_LAMP = %d", ReadCarStatus(FLASH_BEAM_LAMP));
    DEBUG("MAIN_BEAM_LAMP = %d", ReadCarStatus(MAIN_BEAM_LAMP));
    DEBUG("SEATBELT = %d", ReadCarStatus(SEATBELT));
    DEBUG("BRAKE = %d", ReadCarStatus(BRAKE));
    DEBUG("HAND_BRAKE = %d", ReadCarStatus(HAND_BRAKE));
    DEBUG("SECOND_BRAKE = %d", ReadCarStatus(SECOND_BRAKE));
    DEBUG("DOOR = %d", ReadCarStatus(DOOR));

    if (!prime.examing) {
        return;
    }

    if (prime.pMotion->move != STOP) {
        if (prime.sensor.seatbelt == EJECT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
            reportSeatbeltEject = true;
            AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101);
        }
        if (rec) {
            if (!handBreakActive2 && prime.odo - startCarMoveDistance >= prime.examParam.start_car_limit_distance) {
                handBreakActive2 = true;

                if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT) {
                    if (prime.sensor.hand_brake == ACTIVE) {
                        DEBUG("Handbreak active move over 10m");
                        // 手刹拉起状态下，行驶了10米以上，不合格
                        AddExamFault(40205);
                    } else if (handBreakActive) {
                        // 手刹拉起状态下，行驶了1米以上，扣10分
                        DEBUG("Handbreak active move over 1M");
                        AddExamFault(40206);
                    }
                }
            } else if (!handBreakActive && prime.odo - startCarMoveDistance >= prime.examParam.open_door_drive_allow_distance && prime.sensor.hand_brake == ACTIVE) {
                handBreakActive = true;

                if (ExamType == TEST_TYPE_AREA) {
                    DEBUG("Handbreak active move over 1M");
                    AddExamFault(10107);
                }
            }
        }
    } else if (!rec || prevMove != STOP) {          // 记录停车点
        rec = true;
        handBreakActive = handBreakActive2 = false;
        startCarMoveDistance = prime.odo;
    }

    prevMove = prime.pMotion->move;

    AreaExam(prime);
}

static void EngineStartHold(apptimer_var_t val) {
    DEBUG("点火超时");
    if (ReadCarStatus(ENGINE_START) == ACTIVE) {
        // 不及时松开启动开关，扣10分
        if (ExamType == TEST_TYPE_AREA) {
            AddExamFault(10201);
        } else if (ExamType != TEST_TYPE_ROAD_CALIBRATE) {
            AddExamFault(40207);
        }
    }
}

void AddExamFault(int wrong)
{
    struct ExamFault fault;

    fault.sn = examFaultIndex++;

    //strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
    strcpy(fault.utc, "20230413172712.20");

    fault.wrong_id = wrong;

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

    ExamFaultList.push_back(fault);

    MA_SendExamWrong(ExamFaultList);

    ExamFaultList.clear();
}

/***************************************************************
 * 车速判断需要1秒前后的对比，实测停车确认需要1.5秒左右，固减去这个时间
 * @param src
 * @return
 */
int CorrectPauseCriteria(int src)
{
    return src;
//    return (src > 500) ? src - 500 : 500;
}

/*******************************************************************
 * @brief 由主天线坐标计算车身点坐标
 * @param azimuth
 * @param coord
 */
static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk)
{
    car.utc_time = rtk.utc_time;
    car.yaw = rtk.heading;
    car.pitch = rtk.pitch;
    car.roll = rtk.roll;
// 俯仰角修正
    double pitch = rtk.pitch - carModel.antPitch;
    double azimuth = rtk.heading;
//    DEBUG("yaw = %f 修正俯仰角 %f", azimuth, pitch);

    // 主天线投影修正
    car.base_point.X = rtk.x + fabs(carModel.antHeight - carModel.groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
    car.base_point.Y = rtk.y + fabs(carModel.antHeight - carModel.groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));

    // 首次计算
    if (car.points.size() != carModel.carDesc.size()) {
        car.points.resize(carModel.carDesc.size());
    }

    for (int i = 0; i < carModel.carDesc.size(); ++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));

        car.points[i].X = projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                          projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                          car.base_point.X;
        car.points[i].Y = projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                          projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                          car.base_point.Y;
//        DEBUG("<%d>. 标距 %f 标角 %f X = %f Y = %f", i, carModel->carDesc[i].distance, carModel->carDesc[i].angle,
//              carModel->carXY[i].X, carModel->carXY[i].Y);
    }
}

void SystemShutdown(int event, int timeout)
{
    // 关机
    if (event == 1) {

    }
    // 重启
    if (event == 0) {

    }
}