#include "system/includes.h"
#include "app_mode.h"
#include "bsp_hw.h"

#define LOG_TAG_CONST APP
#define LOG_TAG       "[APP_MODE]"
#define LOG_INFO_ENABLE
#include "debug.h"

#define MAGIC_BOX_HIGH_PERCENT              100
#define MAGIC_BOX_MEDIUM_LOW_PERCENT         45
#define MAGIC_BOX_MODE1_FORWARD_MS          585
#define MAGIC_BOX_MODE1_BACKWARD_MS         600
#define MAGIC_BOX_MODE2_FORWARD_MS          260
#define MAGIC_BOX_MODE2_BACKWARD_MS         200
#define MAGIC_BOX_MODE3_FORWARD_MS          585
#define MAGIC_BOX_MODE3_BACKWARD_MS         600
#define MAGIC_BOX_MODE3_STOP_MS            2000
#define MAGIC_BOX_MODE3_LOWER_SEGMENTS       10
#define MAGIC_BOX_MODE4_FORWARD_MS          390
#define MAGIC_BOX_MODE4_BACKWARD_MS         300
#define MAGIC_BOX_MODE4_STOP_MS            3000
#define MAGIC_BOX_MODE5_BACKWARD_MS         600
#define MAGIC_BOX_MODE5_UPPER_STOP_MS      2000
#define MAGIC_BOX_MODE5_CYCLE_MS          20000

typedef struct {
    uint8_t initialized;
    uint8_t active;
    uint8_t output_valid;
    uint8_t upper_only;          /* 1=只输出上电机，下电机恒停 */
    uint8_t upper_phase;
    uint8_t lower_pattern;
    uint8_t lower_segment;
    int8_t upper_direction;
    int8_t lower_direction;
    uint8_t upper_speed;
    uint16_t upper_duration_ms;
    uint16_t lower_duration_ms;
    uint32_t state_started_ms;
    uint32_t upper_started_ms;
    uint32_t lower_started_ms;
    int8_t output_upper_direction;
    int8_t output_lower_direction;
    uint8_t output_upper_speed;
    uint8_t output_lower_speed;
} AppModeCtx_t;

static AppModeCtx_t s_mode;

static void app_mode_outputs_set(int8_t upper_direction, uint8_t upper_speed,
                                 int8_t lower_direction, uint8_t lower_speed)
{
    if (s_mode.output_valid &&
        upper_direction == s_mode.output_upper_direction &&
        upper_speed == s_mode.output_upper_speed &&
        lower_direction == s_mode.output_lower_direction &&
        lower_speed == s_mode.output_lower_speed) {
        return;
    }

    bsp_drive_set((BspMotorDir_t)upper_direction, (uint16_t)upper_speed * 100,
                  (BspMotorDir_t)lower_direction, (uint16_t)lower_speed * 100);
    s_mode.output_upper_direction = upper_direction;
    s_mode.output_upper_speed = upper_speed;
    s_mode.output_lower_direction = lower_direction;
    s_mode.output_lower_speed = lower_speed;
    s_mode.output_valid = 1;
}

static uint16_t app_mode4_random_step_ms(void)
{
    static const uint16_t step_ms[] = {80, 100, 150, 200};

    return step_ms[rand32() % ARRAY_SIZE(step_ms)];
}

static uint16_t app_mode5_random_forward_ms(void)
{
    static const uint16_t forward_ms[] = {555, 780, 1140};

    return forward_ms[rand32() % ARRAY_SIZE(forward_ms)];
}

static void app_mode5_select_lower_pattern(uint32_t now_ms)
{
    s_mode.lower_pattern = rand32() % 4;
    s_mode.lower_segment = 0;
    s_mode.lower_started_ms = now_ms;

    switch (s_mode.lower_pattern) {
    case 0:
        s_mode.lower_direction = BSP_MOTOR_FORWARD;
        s_mode.lower_duration_ms = 2000;
        break;
    case 1:
        s_mode.lower_direction = BSP_MOTOR_BACKWARD;
        s_mode.lower_duration_ms = 2000;
        break;
    case 2:
        s_mode.lower_direction = (rand32() & 1) ?
                                 BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
        s_mode.lower_duration_ms = 100;
        break;
    default:
        s_mode.lower_direction = (rand32() & 1) ?
                                 BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
        s_mode.lower_duration_ms = 200;
        break;
    }
}

static void app_mode5_start_cycle(uint32_t now_ms)
{
    s_mode.state_started_ms = now_ms;
    s_mode.upper_phase = 0;
    s_mode.upper_started_ms = now_ms;
    s_mode.upper_duration_ms = app_mode5_random_forward_ms();
    s_mode.upper_direction = BSP_MOTOR_FORWARD;
    s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
    app_mode5_select_lower_pattern(now_ms);
}

static void app_mode_start_current(uint32_t now_ms)
{
    bsp_drive_stop();
    s_mode.output_valid = 0;
    s_mode.state_started_ms = now_ms;
    s_mode.upper_phase = 0;
    s_mode.upper_started_ms = now_ms;
    s_mode.lower_started_ms = now_ms;

    if (s_mode.active == 4) {
        s_mode.lower_direction = (rand32() & 1) ?
                                 BSP_MOTOR_FORWARD : BSP_MOTOR_BACKWARD;
        s_mode.lower_duration_ms = app_mode4_random_step_ms();
    } else if (s_mode.active == 5) {
        app_mode5_start_cycle(now_ms);
    }
}

static void app_mode1_process(uint32_t now_ms)
{
    const uint16_t pair_ms = MAGIC_BOX_MODE1_FORWARD_MS +
                             MAGIC_BOX_MODE1_BACKWARD_MS;
    uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % pair_ms;

    if (elapsed_ms < MAGIC_BOX_MODE1_FORWARD_MS) {
        app_mode_outputs_set(BSP_MOTOR_FORWARD, MAGIC_BOX_HIGH_PERCENT,
                             BSP_MOTOR_STOP, 0);
    } else {
        app_mode_outputs_set(BSP_MOTOR_BACKWARD, MAGIC_BOX_HIGH_PERCENT,
                             BSP_MOTOR_STOP, 0);
    }
}

static void app_mode2_process(uint32_t now_ms)
{
    const uint16_t pair_ms = MAGIC_BOX_MODE2_FORWARD_MS +
                             MAGIC_BOX_MODE2_BACKWARD_MS;
    uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % pair_ms;

    if (elapsed_ms < MAGIC_BOX_MODE2_FORWARD_MS) {
        app_mode_outputs_set(BSP_MOTOR_FORWARD, MAGIC_BOX_HIGH_PERCENT,
                             BSP_MOTOR_STOP, 0);
    } else {
        app_mode_outputs_set(BSP_MOTOR_BACKWARD, MAGIC_BOX_HIGH_PERCENT,
                             BSP_MOTOR_STOP, 0);
    }
}

static void app_mode3_process(uint32_t now_ms)
{
    const uint16_t upper_move_ms = MAGIC_BOX_MODE3_FORWARD_MS +
                                   MAGIC_BOX_MODE3_BACKWARD_MS;
    const uint16_t cycle_ms = upper_move_ms + MAGIC_BOX_MODE3_STOP_MS;
    uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms) % cycle_ms;
    uint8_t lower_segment = ((uint32_t)elapsed_ms *
                             MAGIC_BOX_MODE3_LOWER_SEGMENTS) / cycle_ms;
    int8_t upper_direction;
    uint8_t upper_speed;
    int8_t lower_direction;

    if (elapsed_ms < MAGIC_BOX_MODE3_FORWARD_MS) {
        upper_direction = BSP_MOTOR_FORWARD;
        upper_speed = MAGIC_BOX_HIGH_PERCENT;
    } else if (elapsed_ms < upper_move_ms) {
        upper_direction = BSP_MOTOR_BACKWARD;
        upper_speed = MAGIC_BOX_HIGH_PERCENT;
    } else {
        upper_direction = BSP_MOTOR_STOP;
        upper_speed = 0;
    }

    lower_direction = (lower_segment & 1) ?
                      BSP_MOTOR_BACKWARD : BSP_MOTOR_FORWARD;
    app_mode_outputs_set(upper_direction, upper_speed,
                         lower_direction, MAGIC_BOX_HIGH_PERCENT);
}

static void app_mode4_process(uint32_t now_ms)
{
    const uint16_t upper_move_ms = MAGIC_BOX_MODE4_FORWARD_MS +
                                   MAGIC_BOX_MODE4_BACKWARD_MS;
    const uint16_t cycle_ms = upper_move_ms + MAGIC_BOX_MODE4_STOP_MS;
    uint16_t elapsed_ms = (uint32_t)(now_ms - s_mode.state_started_ms);
    int8_t upper_direction;
    uint8_t upper_speed;

    if (elapsed_ms >= cycle_ms) {
        s_mode.state_started_ms = now_ms;
        elapsed_ms = 0;
        s_mode.lower_started_ms = now_ms;
        s_mode.lower_direction = -s_mode.lower_direction;
        s_mode.lower_duration_ms = app_mode4_random_step_ms();
    }
    if ((uint32_t)(now_ms - s_mode.lower_started_ms) >=
        s_mode.lower_duration_ms) {
        s_mode.lower_started_ms = now_ms;
        s_mode.lower_direction = -s_mode.lower_direction;
        s_mode.lower_duration_ms = app_mode4_random_step_ms();
    }

    if (elapsed_ms < MAGIC_BOX_MODE4_FORWARD_MS) {
        upper_direction = BSP_MOTOR_FORWARD;
        upper_speed = MAGIC_BOX_HIGH_PERCENT;
    } else if (elapsed_ms < upper_move_ms) {
        upper_direction = BSP_MOTOR_BACKWARD;
        upper_speed = MAGIC_BOX_MEDIUM_LOW_PERCENT;
    } else {
        upper_direction = BSP_MOTOR_STOP;
        upper_speed = 0;
    }
    app_mode_outputs_set(upper_direction, upper_speed,
                         s_mode.lower_direction, MAGIC_BOX_HIGH_PERCENT);
}

static void app_mode5_process(uint32_t now_ms)
{
    if ((uint32_t)(now_ms - s_mode.state_started_ms) >=
        MAGIC_BOX_MODE5_CYCLE_MS) {
        app_mode5_start_cycle(now_ms);
    }

    if ((uint32_t)(now_ms - s_mode.upper_started_ms) >=
        s_mode.upper_duration_ms) {
        s_mode.upper_started_ms = now_ms;
        s_mode.upper_phase++;
        if (s_mode.upper_phase == 1) {
            s_mode.upper_direction = BSP_MOTOR_BACKWARD;
            s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
            s_mode.upper_duration_ms = MAGIC_BOX_MODE5_BACKWARD_MS;
        } else if (s_mode.upper_phase == 2) {
            s_mode.upper_direction = BSP_MOTOR_STOP;
            s_mode.upper_speed = 0;
            s_mode.upper_duration_ms = MAGIC_BOX_MODE5_UPPER_STOP_MS;
        } else {
            s_mode.upper_phase = 0;
            s_mode.upper_direction = BSP_MOTOR_FORWARD;
            s_mode.upper_speed = MAGIC_BOX_HIGH_PERCENT;
            s_mode.upper_duration_ms = app_mode5_random_forward_ms();
        }
    }

    if ((uint32_t)(now_ms - s_mode.lower_started_ms) >=
        s_mode.lower_duration_ms) {
        if (s_mode.lower_pattern >= 2 && ++s_mode.lower_segment < 8) {
            s_mode.lower_started_ms = now_ms;
            s_mode.lower_direction = -s_mode.lower_direction;
        } else {
            app_mode5_select_lower_pattern(now_ms);
        }
    }
        app_mode_outputs_set(s_mode.upper_direction, s_mode.upper_speed,
                         s_mode.upper_only ? BSP_MOTOR_STOP : s_mode.lower_direction,
                         s_mode.upper_only ? 0 : MAGIC_BOX_HIGH_PERCENT);

}

AppMode_Ret_t app_mode_init(void)
{
    memset(&s_mode, 0, sizeof(s_mode));
    s_mode.initialized = 1;
    bsp_drive_stop();
    return APP_MODE_OK;
}

AppMode_Ret_t app_mode_get_status(void)
{
    return s_mode.initialized ? APP_MODE_OK : APP_MODE_ERR_NOT_INIT;
}

uint8_t app_mode_get_active(void)
{
    return s_mode.active;
}

AppMode_Ret_t app_mode_start(uint8_t mode)
{
    if (!s_mode.initialized) {
        return APP_MODE_ERR_NOT_INIT;
    }
    if (mode < 1 || mode > APP_MODE_UPPER_ONLY) {   /* 放宽到 6 */
        app_mode_stop();
        return APP_MODE_ERR_PARAM;
    }

    s_mode.upper_only = (mode == APP_MODE_UPPER_ONLY);
    s_mode.active = s_mode.upper_only ? 5 : mode;  /* 内部仍按模式 5 跑 */
    app_mode_start_current(timer_get_ms());
    log_info("magic box mode %d start\n", mode);
    return APP_MODE_OK;
}

void app_mode_stop(void)
{
    s_mode.active = 0;
    s_mode.upper_only = 0;  
    s_mode.output_valid = 0;
    bsp_drive_stop();
}

void app_mode_run(void)
{
    uint32_t now_ms;

    bsp_motor_switch_tick();
    if (!s_mode.initialized || !s_mode.active) {
        return;
    }
    now_ms = timer_get_ms();
    switch (s_mode.active) {
    case 1:
        app_mode1_process(now_ms);
        break;
    case 2:
        app_mode2_process(now_ms);
        break;
    case 3:
        app_mode3_process(now_ms);
        break;
    case 4:
        app_mode4_process(now_ms);
        break;
    case 5:
        app_mode5_process(now_ms);
        break;
    default:
        app_mode_stop();
        break;
    }
}