#include "app_line_seek.h"

#include "line_seek.h"
#include "main.h"
#include "motor.h"
#include "path_plan.h"
#include "stm32f1xx_hal_tim.h"
#include "syscalls.h"
#include "tim.h"

#define LOW_TUNE_SPEED -5
#define LOW_SPEED 35
#define MID_SPEED 40
#define HIGH_SPEED 45
#define MIN_TURN_TIME 300             // 最小转直角时间
#define MIN_CROSSING_TIME 200         // 最小走十字路口时间
#define MIN_CROSSING_STRAIGHT_TIME 50 // 最小走十字路口时间
#define MIN_TUNE_TIME 40              // 最小微调时间
#define MAX_TURN_TIME 4000            // 最大转弯时间

typedef enum
{
    MOVE_STRAIGHT, // 直行
    TURN_LEFT,     // 左转
    TURN_RIGHT,    // 右转
    TUNE_LEFT,     // 左微调
    TUNE_RIGHT,    // 右微调
    TURN_LEFT_BIG,
    TURN_RIGHT_BIG
} MoveState;

MoveState state, preState; // 运动状态
int turn_time;             // 转弯时间
int beep_time;             // 蜂鸣器鸣叫时间
int is_crossing;           // 是否正在走十字路口

void LINESEEK_HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{
    if (turn_time < INT16_MAX)
    {
        turn_time++; // 每1ms自增一
    }

    if (beep_time++ > 100)
    {
        HAL_GPIO_WritePin(FM_K2_POWERC_GPIO_Port, FM_K2_POWERC_Pin, GPIO_PIN_SET);
    }
}

void LineSeek_Init(void)
{
    // 注册定时器中断溢出回调
    HAL_TIM_RegisterCallback(&htim4, HAL_TIM_PERIOD_ELAPSED_CB_ID, LINESEEK_HAL_TIM_PeriodElapsedCallback);
    HAL_TIM_Base_Start_IT(&htim4);
}

void App_LineSeek(void)
{
    int LineL1 = 1, LineL2 = 1, LineR1 = 1, LineR2 = 1;

    // LineSeek_GetStatus(&LineL1, &LineL2, &LineR1, &LineR2); // 获取黑线检测状态

    LineSeek_GetStatusStr();
    // printf("%s %d \n", LineSeek_Status, turn_time);

    // return;

    if (turn_time > MIN_CROSSING_TIME)
    {
        is_crossing = 0;
    }

    if (is_crossing && GetCurrentDirection() == STRAIGHT)
    {
        LineSeek_Status[0] = '1';
        LineSeek_Status[3] = '1';
    }

    switch (state)
    {
        case MOVE_STRAIGHT: // 直行
            MOTOR_SetDuty(HIGH_SPEED, HIGH_SPEED);
            if (LineSeek_Equals("0000")) // 十字路口
            {
                Hanlde_Crossroad();
            }
            else if (LineSeek_Equals("1011")) // 左微调
            {
                state = TUNE_LEFT;
            }
            else if (LineSeek_Equals("1101")) // 右微调
            {
                state = TUNE_RIGHT;
            }
            else if (LineSeek_Equals("0001")) // 左直角
            {
                turn_time = 0;
                state = TURN_LEFT;
            }
            else if (LineSeek_Equals("1000")) // 右直角
            {
                turn_time = 0;
                state = TURN_RIGHT;
            }
            else if (LineSeek_Equals("0xx1")) // 左锐角
            {
                state = TURN_LEFT_BIG;
            }
            else if (LineSeek_Equals("1xx0")) // 右锐角
            {
                state = TURN_RIGHT_BIG;
            }
            else if (LineSeek_Equals("1111")) // 冲出去了，使用上一个转向
            {
                state = preState;
            }
            break;
        case TURN_LEFT: // 左直角
            MOTOR_SetDuty(-HIGH_SPEED, HIGH_SPEED);
            if (LineSeek_Equals("0000")) // 十字路口
            {
                Hanlde_Crossroad();
            }
            else if (LineSeek_Equals("1001") &&
                     ((preState != TUNE_LEFT && turn_time >= MIN_TURN_TIME - 1) ||
                      (preState == TUNE_LEFT && turn_time >= MIN_TUNE_TIME))) // 直行
            {
                state = MOVE_STRAIGHT;
                preState = TURN_LEFT;
                is_crossing = 0;
            }
            else if (LineSeek_Equals("xxx0") && !is_crossing) // 转过头了
            {
                turn_time = MIN_TURN_TIME;
                state = TURN_RIGHT;
                preState = TURN_LEFT;
            }
            break;
        case TURN_RIGHT: // 右直角
            MOTOR_SetDuty(HIGH_SPEED, -HIGH_SPEED);
            if (LineSeek_Equals("0000")) // 十字路口
            {
                Hanlde_Crossroad();
            }
            else if (LineSeek_Equals("1001") &&
                     ((preState != TUNE_RIGHT && turn_time >= MIN_TURN_TIME - 1) ||
                      (preState == TUNE_RIGHT && turn_time >= MIN_TUNE_TIME))) // 直行
            {
                state = MOVE_STRAIGHT;
                preState = TURN_RIGHT;
                is_crossing = 0;
            }
            else if (LineSeek_Equals("0xxx") && !is_crossing) // 转过头了
            {
                turn_time = MIN_TURN_TIME;
                state = TURN_LEFT;
                preState = TURN_LEFT;
            }
            break;
        case TURN_LEFT_BIG: // 左锐角
            MOTOR_SetDuty(-MID_SPEED, MID_SPEED);
            if (LineSeek_Equals("0000")) // 十字路口
            {
                Hanlde_Crossroad();
            }
            else if (LineSeek_Equals("0001")) // 左直角
            {
                turn_time = 0;
                state = TURN_LEFT;
                preState = TURN_LEFT_BIG;
            }
            else if (LineSeek_Equals("xxx0")) // 转过头了
            {
                state = TUNE_RIGHT;
                // preState = TURN_LEFT_BIG;
            }
            break;
        case TURN_RIGHT_BIG: // 右锐角
            MOTOR_SetDuty(MID_SPEED, -MID_SPEED);
            if (LineSeek_Equals("0000")) // 十字路口
            {
                Hanlde_Crossroad();
            }
            else if (LineSeek_Equals("1000")) // 右直角
            {
                turn_time = 0;
                state = TURN_RIGHT;
                preState = TURN_RIGHT_BIG;
            }
            else if (LineSeek_Equals("0xxx")) // 转过头了
            {
                state = TUNE_LEFT;
                preState = TURN_RIGHT_BIG;
            }
            break;
        case TUNE_LEFT: // 左微调
            MOTOR_SetDuty(LOW_TUNE_SPEED, HIGH_SPEED);
            if (LineSeek_Equals("x00x"))
            {
                state = MOVE_STRAIGHT;
            }
            else if (LineSeek_Equals("0xx1") && GetCurrentDirection() != STRAIGHT) // 左转
            {
                turn_time = 0;
                state = TURN_LEFT;
                preState = TUNE_LEFT;
            }
            else if (LineSeek_Equals("1xx0") && GetCurrentDirection() != STRAIGHT) // 右转
            {
                turn_time = 0;
                state = TURN_RIGHT;
                preState = TUNE_LEFT;
            }
            break;
        case TUNE_RIGHT: // 右微调
            MOTOR_SetDuty(HIGH_SPEED, LOW_TUNE_SPEED);
            if (LineSeek_Equals("x00x"))
            {
                state = MOVE_STRAIGHT;
            }
            else if (LineSeek_Equals("0xx1") && GetCurrentDirection() != STRAIGHT) // 左转
            {
                turn_time = 0;
                state = TURN_LEFT;
                preState = TUNE_RIGHT;
            }
            else if (LineSeek_Equals("1xx0") && GetCurrentDirection() != STRAIGHT) // 右转
            {
                turn_time = 0;
                state = TURN_RIGHT;
                preState = TUNE_RIGHT;
            }
            break;
        default:
            state = MOVE_STRAIGHT;
            MOTOR_SetDuty(MID_SPEED, MID_SPEED);
            break;
    }

    // if (turn_time >= MAX_TURN_TIME)
    // {
    //     // state = MOVE_STRAIGHT;
    //     MOTOR_SetDuty(0, 0);
    // }

    // printf("curr state: %d \n", state);
}

void Hanlde_Crossroad(void)
{
    if (is_crossing)
    {
        return;
    }

    beep_time = 0;
    HAL_GPIO_WritePin(FM_K2_POWERC_GPIO_Port, FM_K2_POWERC_Pin, GPIO_PIN_RESET);
    // state = MOVE_STRAIGHT;
    // buzzer(1);
    // HAL_Delay(50);
    // buzzer(0);

    // 根据规划的路径进行决断
    is_crossing = 1;
    turn_time = 0;
    int direction = GetNextDirection();
    switch (direction)
    {
        case STRAIGHT:
            state = MOVE_STRAIGHT;
            break;
        case LEFT:
            state = TURN_LEFT;
            break;
        case RIGHT:
            state = TURN_RIGHT;
            break;
        default:
            state = TURN_RIGHT;
            break;
    }
    // printf("state: %d, dir: %d \n", state, direction);
}