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

#include "operate_gear.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../native-lib.h"
#include "../test_common/car_sensor.h"
#include "../jni_log.h"
#include "../test_common/odo_graph.h"
#include "road_exam.h"

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

static int expectGear;
static int upDownShift;
static int oldGear;

static int setup;

static double maxMoveDistance;

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

void StartOperateGearExam(void) {
    DEBUG("开始加减挡操作");

    setup = 0;

    PlayTTS(examParam.shift_begin_tts, TtsBack);
}

bool TestOperateGear(const struct RtkTime *rtkTime)
{
    static struct RtkTime shiftTime;

    car_sensor_value_t sensor = ReadCarSensorValue(GEAR);

    if (sensor.name != GEAR)
        return false;

    if (setup == 0) {
        return true;
    } else if (setup == 1) {
        if (sensor.value != GEAR_N) {
            oldGear = sensor.value;
            DEBUG("准备首次换挡 GEAR = %d", oldGear);
        }
        switch (sensor.value) {
            case GEAR_1: {
                expectGear = GEAR_2;
                upDownShift = 1;
                PlayTTS("请加到二挡", NULL);
                setup = 2;
                break;
            }
            case GEAR_2: {
                expectGear = GEAR_3;
                upDownShift = 1;
                PlayTTS("请加到三挡", NULL);
                setup = 2;
                break;
            }
            case GEAR_3: {
                expectGear = GEAR_2;
                upDownShift = -1;
                PlayTTS("请减到二挡", NULL);
                setup = 2;
                break;
            }
            case GEAR_4: {
                expectGear = GEAR_3;
                upDownShift = -1;
                PlayTTS("请减到三挡", NULL);
                setup = 2;
                break;
            }
            case GEAR_5: {
                upDownShift = -1;
                expectGear = GEAR_4;
                PlayTTS("请减到四挡", NULL);
                setup = 2;
                break;
            }
        }
    } else if (setup == 2) {
        if (sensor.value == GEAR_N || sensor.value == oldGear) {
            DEBUG("等待首次换挡");
        } else if (sensor.value != expectGear) {
            // 未按指令操作挡位，不合格
            DEBUG("首次换挡错误 GEAR %d  希望 %d", sensor.value, expectGear);
            AddExamFault(40401, rtkTime);
            return false;
        } else {
            // 在此挡位行驶一定距离，再执行下一个
            DEBUG("首次换挡成功，下一个...");
            oldGear = expectGear;
            shiftTime = *rtkTime;
            setup = 3;
        }
    } else if (setup == 3) {
        if (TimeGetDiff(rtkTime, &shiftTime) >= examParam.shift_hold_time) {
            setup = 4;
            char buff[128];
            expectGear += 0 - upDownShift;
            sprintf(buff, "请%s到%d挡", upDownShift > 0 ? "减": "加", expectGear - GEAR_N);
            PlayTTS(buff, NULL);

            DEBUG("%s", buff);
            DEBUG("准备二次换挡 希望 %d", expectGear);
        }
    } 
    else if (setup == 4) {
        if (sensor.value == GEAR_N || sensor.value == oldGear) {
            DEBUG("等待二次换挡");
        } else if (sensor.value != expectGear) {
            // 未按指令操作挡位，不合格
            DEBUG("二次换挡错误 GEAR %d  希望 %d", sensor.value, expectGear);
            AddExamFault(40401, rtkTime);
            return false;
        } else {
            // 在此挡位行驶一定距离，再执行下一个
            shiftTime = *rtkTime;
            setup = 5;
            DEBUG("二次换挡成功，下一个...");
        }
    } else if (setup == 5) {
        if (TimeGetDiff(rtkTime, &shiftTime) >= examParam.shift_hold_time) {
            PlayTTS(examParam.shift_end_tts, NULL);
            DEBUG("加减挡位操作结束");
            return false;
        }
    }

    if (ReadOdo() - maxMoveDistance > 120) {
        // 未按指令操作挡位，不合格
        DEBUG("未按指令操作挡位，超时");
        AddExamFault(40401, rtkTime);
        return false;
    }

    return true;
}