#include "app_bus.h"

#include "app_line_seek.h"
#include "block_detect.h"
#include "buzzer.h"
#include "led.h"
#include "line_seek.h"
#include "motor.h"
#include "path_plan.h"
#include "syn6288.h"

typedef enum
{
    RUNNING,        // 按路线运营
    READY_TO_PARK,  // 准备进站停车（或者准备左转）
    PARKING,        // 进站停车
    WAIT_PASSENGER, // 等候乘客
    EXIT_STATION,   // 出站
    STOP            // 停车
} BusState;

#define MIN_CROSS_TIME 10
static BusState state;
static cross_time;

static int current_station;
char* stations[] = {"太平园站", "西南交大站", "新业北街站", "终点站"};

void App_Bus(void)
{
    // printf("bus_state:%d\n", state);

    LineSeek_GetStatusStr();

    switch (state)
    {
        case RUNNING: // 按路线运营
        {
            if (Has_Block(FRONT)) // 前方有障碍物，停车
            {
                state = STOP;
            }
            else if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物，准备停车或者左转避障
            {
                state = READY_TO_PARK;
                // state = WAIT_PASSENGER;
            }
            else if (Has_Block(LEFT_FRONT)) // 左前方有障碍物，右转避障
            {
                Direction nextDirs[] = {RIGHT, RIGHT};
                ReplacePathStep(nextDirs, 2);
            }
            break;
        }
        case READY_TO_PARK: // 准备进站停车，或者准备左转避障
        {
            if (LineSeek_Equals("0xxx")) // 左方有路线，左转
            {
                Direction nextDirs[] = {LEFT, LEFT};
                ReplacePathStep(nextDirs, 2);
                state = RUNNING;
            }
            else if (LineSeek_Equals("xxx0")) // 右方有路线，要确保左方没路线，才右转进站
            {
                Direction nextDirs[] = {RIGHT};
                ReplacePathStep(nextDirs, 1);
                cross_time = 0;
                state = PARKING;
            }
            break;
        }
        case PARKING: // 进站停车
        {
            if (LineSeek_Equals("0000")) // 左方有路线，左转
            {
                Direction nextDirs[] = {LEFT, LEFT};
                ReplacePathStep(nextDirs, 2);
                state = RUNNING;
            }

             if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物，停车
            {
                state = WAIT_PASSENGER;
            }
            break;
        }
        case WAIT_PASSENGER: // 等候乘客
        {
            // TODO: 添加乘客等待逻辑
            LineSeek_Stop();
            SYN_FrameInfo(0, stations[current_station]);
            MOTOR_Stop();
            LED_StartNTimes(5, 400, 200, 0, 255, 255);
            HAL_Delay(2000);
            current_station++;
            state = EXIT_STATION;
            break;
        }
        case EXIT_STATION: // 出站
        {
            LineSeek_Start();
            if (LineSeek_Equals("xxx0")) // 右方有路线，右转出站
            {
                Direction nextDirs[] = {RIGHT};
                ReplacePathStep(nextDirs, 1);
                state = RUNNING;
            }
            break;
        }
        case STOP: // 停车
        {
            LineSeek_Stop();
            BUZZER_StartNTimes(10, 1000, 500);
            LED_StartNTimes(10, 1000, 500, 255, 0, 0);
            break;
        }
        default:
            break;
    }
    App_LineSeek();
}