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

#include "stop_car.h"
#include "../driver_test.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "road_exam.h"
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../test_common/car_sensor.h"
#include "../defs.h"
#include "../test_common/odo_graph.h"

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

static bool BreakHandbreakReleaseSametime, OpenDoor;

static const int ENGINE_MIN_ROTATE = 100;

static double beginOdo;
static int setup;

static void TtsBack(int seq)
{
    setup = 1;
}

void StartStopCarExam(void) {
    DEBUG("靠边停车");
    BreakHandbreakReleaseSametime = false;
    setup = 0;
    OpenDoor = false;
    PlayTTS(examParam.stop_car_begin_tts, TtsBack);
}

bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) {
    static struct RtkTime time;

    if (setup == 0)
        return true;

    if (roadIndex < 0) {
        DEBUG("停车距离超标，靠边停车结束");
        AddExamFault(33, rtkTime);
        return false;
    }

    if (setup == 1) {
        beginOdo = ReadOdo();
        setup = 2;
    } else if (setup == 2) {
        if (moveDirect == 0) {
            time = *rtkTime;
            setup = 3;
        }
    } else if (setup == 3) {
        if (moveDirect != 0) {
            setup = 2;
        } else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) {
            // 停车超2秒，开始判断
            DEBUG("检测和路边的距离");

            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]);
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]);

            if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance ||
                    DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_red_distance) {
                DEBUG("停车超出路边0.5米");
                // 停车距离超过50厘米，不合格
                AddExamFault(36, rtkTime);
                return false;
            } else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance ||
                       DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > examParam.stop_car_edge_yellow_distance) {
                DEBUG("停车超出路边0.3米");
                // 停车距离超过30厘米，扣10分
                AddExamFault(37, rtkTime);
            }

            setup = 4;
        }
    } else if (setup == 4) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        car_sensor_value_t hbrk = ReadCarSensorValue(HAND_BREAK);
        car_sensor_value_t door = ReadCarSensorValue(DOOR);
        car_sensor_value_t rpm = ReadCarSensorValue(ENGINE_RPM);

        if (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) {
            // 拉手刹前，松脚刹，扣10分
            DEBUG("拉手刹前，松脚刹");
            AddExamFault(39, rtkTime);
            BreakHandbreakReleaseSametime = true;
        }

        if (door.value == DOOR_OPEN) {
            if (!OpenDoor) {
                if (rpm.value > ENGINE_MIN_ROTATE) {
                    // 下车前，不熄火，扣5分
                    DEBUG("下车前，不熄火");
                    AddExamFault(40, rtkTime);
                }
                if (hbrk.value == BREAK_INACTIVE) {
                    // 开门前，未拉手刹， 扣10分
                    DEBUG("开门前，未拉手刹");
                    AddExamFault(38, rtkTime);
                }
                time = *rtkTime;
                OpenDoor = true;
            }

            if (TimeGetDiff(rtkTime, &time) > examParam.stop_car_open_door_allow_time) {
                // 开门时间超过15秒，不合格
                DEBUG("开门时间超过15秒");
                AddExamFault(35, rtkTime);
                return false;
            }
        }

        if (OpenDoor && door.value == DOOR_CLOSE) {
            DEBUG("完成停车");
            PlayTTS(examParam.stop_car_end_tts, NULL);
            return false;
        }
    }

    if (ReadOdo() - beginOdo > examParam.stop_car_limit_distance) {
        // 150米内未停车，不合格
        DEBUG("停车距离超标，靠边停车结束");
        AddExamFault(33, rtkTime);
        return false;
    }

    return true;
}