#include "dehy_test.h"
#include "mw_soft_timer.h"

#define DEHY_SPEED_MIN              (0u)
#define DEHY_SPEED_MAX              (800u)
#define DEHY_SPEED_UPDATE_MS        (1000u)

/* 外部调用 速度设置值 */
static uint16_t speed_set_val = 0u;

typedef enum
{
    DEHY_NEW_STATE_ACCEL = 0u,
    DEHY_NEW_STATE_HOLD_CURRENT,
    DEHY_NEW_STATE_HOLD_TARGET
} dehy_new_state_e;

static uint8_t dehy_new_inited = 0u;
static dehy_new_state_e dehy_new_state = DEHY_NEW_STATE_ACCEL;
static uint16_t dehy_new_set_speed = 0u;
static uint16_t dehy_new_hold_speed = 0u;
static uint16_t dehy_new_last_target = 0u;
static uint32_t dehy_new_last_tick = 0u;

static uint16_t dehy_clamp_speed(uint16_t speed_val)
{
    if (speed_val > DEHY_SPEED_MAX)
    {
        return DEHY_SPEED_MAX;
    }
    return speed_val;
}

void dehy_speed_set(uint16_t init_cmd, uint16_t acc_val, uint16_t current_speed, uint16_t target_speed)
{

    static uint32_t timetick = 0;   // 时间戳
    static uint16_t add_speed_state = 0;    // 本函数的状态机
    static uint16_t result_speed = 0;   // 预设速度
    static uint16_t target_speed_cache = 0; // 缓存目标速度
    // static uint16_t current_speed_cache = 0; // 缓存当前速度
    if(init_cmd == 0x5AA5)
    {
        add_speed_state = 0;
    }

    switch(add_speed_state)
    {
        // init
        case 0:
            // 时间戳 初始化
            timetick = get_systick_ms();
            // 预设速度 初始化
            result_speed = current_speed;
            // 目标速度缓存 初始化
            target_speed_cache = target_speed;
            // // 刷新 当前速度缓存
            // current_speed_cache = current_speed;
            // 进入下一阶段
            add_speed_state = 1;
            break;
        // 加速
        case 1:
            // 当前 触发超震
            if(target_speed == 0)
            {
                add_speed_state = 3;
                result_speed = 0;
            }
            // 周期到
            else if(get_systick_ms() - timetick > 1000u)
            {

                timetick = get_systick_ms();
                
                // 目标速度 小于 目标缓存速度，说明外面超震大 目标速度变小了 高4-> 高3
                if(target_speed < target_speed_cache)
                {
                    // 若当前速度 小于 现在的目标速度 则没事 更新一下目标速度缓存 当前就不加速了
                    if(current_speed < target_speed)
                    {
                        // // 刷新 目标速度缓存
                        // target_speed_cache = target_speed;
                        // // 刷新 当前速度缓存
                        // current_speed_cache = current_speed;
                    }
                    else
                    {
                        // 否则保持当前缓存的速度 稳住
                        // result_speed = current_speed_cache;
                        // 记录当前速度
                        result_speed = current_speed;
                        // 切到速度保持
                        add_speed_state = 2;
                    }
                }
                else
                {
                    // // 刷新 目标速度缓存
                    // target_speed_cache = target_speed;
                    // // 刷新 当前速度缓存
                    // current_speed_cache = current_speed;
                    // 更新 预设速度
                    result_speed += acc_val;
                    // 进下一阶段判断
                    if(result_speed >= target_speed)
                    {
                        result_speed = target_speed;
                        add_speed_state = 2;
                    }

                }
                // 刷新 目标速度缓存
                target_speed_cache = target_speed;
                /* 设置速度 */
                speed_set_val = result_speed;
            }
            break;
        // 速度保持
        case 2:
            // 超震停机
            if(target_speed == 0)
            {
                result_speed = 0;
                add_speed_state = 3;
            }
            // 目标速度变好了 外面超震小了
            else if(target_speed > target_speed_cache)
            {
                target_speed_cache = target_speed;
                // // 刷新 当前速度缓存
                // current_speed_cache = current_speed;
                // 回第一步
                add_speed_state = 1;
            }
            
            /* 设置速度 */
            speed_set_val = result_speed;
            break;
        // 超震停机
        case 3:
            result_speed = 0;
            /* 设置速度 */
            speed_set_val = result_speed;
            break;
        default:
            break;
    }
}

void dehy_speed_set_new_init(uint16_t curr_speed)
{
    uint16_t speed_init = dehy_clamp_speed(curr_speed);

    dehy_new_inited = 1u;
    dehy_new_state = DEHY_NEW_STATE_ACCEL;
    dehy_new_set_speed = speed_init;
    dehy_new_hold_speed = speed_init;
    dehy_new_last_target = speed_init;
    dehy_new_last_tick = get_systick_ms();

    speed_set_val = dehy_new_set_speed;
}

uint16_t dehy_speed_set_new_get(void)
{
    return dehy_new_set_speed;
}

uint16_t dehy_speed_set_new(uint16_t curr_speed, uint16_t target_speed, uint16_t acc_val)
{
    uint32_t now_ms;
    uint32_t elapsed_ms;
    uint32_t step_count;
    uint32_t next_speed;
    uint8_t target_decreased;
    uint8_t target_increased;

    curr_speed = dehy_clamp_speed(curr_speed);
    target_speed = dehy_clamp_speed(target_speed);

    if (dehy_new_inited == 0u)
    {
        dehy_speed_set_new_init(curr_speed);
    }

    now_ms = get_systick_ms();
    target_decreased = (target_speed < dehy_new_last_target) ? 1u : 0u;
    target_increased = (target_speed > dehy_new_last_target) ? 1u : 0u;

    /* Requirement 4: target == 0 must force set_speed to 0 immediately. */
    if (target_speed == DEHY_SPEED_MIN)
    {
        dehy_new_set_speed = 0u;
        dehy_new_hold_speed = 0u;
        dehy_new_last_target = 0u;
        dehy_new_state = DEHY_NEW_STATE_ACCEL;
        dehy_new_last_tick = now_ms;
        speed_set_val = dehy_new_set_speed;
        return dehy_new_set_speed;
    }

    /* Requirement 2: target decreases and current speed is above it -> hold current speed. */
    if ((target_decreased > 0u) && (curr_speed > target_speed))
    {
        dehy_new_hold_speed = curr_speed;
        dehy_new_set_speed = dehy_new_hold_speed;
        dehy_new_state = DEHY_NEW_STATE_HOLD_CURRENT;
        dehy_new_last_target = target_speed;
        dehy_new_last_tick = now_ms;
        speed_set_val = dehy_new_set_speed;
        return dehy_new_set_speed;
    }

    /* In hold-current state, leave hold when current speed is no longer above target. */
    if (dehy_new_state == DEHY_NEW_STATE_HOLD_CURRENT)
    {
        if (curr_speed > target_speed)
        {
            dehy_new_set_speed = dehy_new_hold_speed;
            dehy_new_last_target = target_speed;
            speed_set_val = dehy_new_set_speed;
            return dehy_new_set_speed;
        }

        dehy_new_state = DEHY_NEW_STATE_ACCEL;
        dehy_new_last_tick = now_ms;
    }

    /*
     * Requirement 3: target increases -> continue accelerating.
     * Also for requirement 2 (curr <= new target), keep running normal ramp logic.
     */
    if ((target_increased > 0u) && (dehy_new_state == DEHY_NEW_STATE_HOLD_TARGET))
    {
        dehy_new_state = DEHY_NEW_STATE_ACCEL;
        dehy_new_last_tick = now_ms;
    }

    dehy_new_last_target = target_speed;

    switch (dehy_new_state)
    {
        case DEHY_NEW_STATE_ACCEL:
            if (dehy_new_set_speed >= target_speed)
            {
                dehy_new_set_speed = target_speed;
                dehy_new_state = DEHY_NEW_STATE_HOLD_TARGET;
                dehy_new_last_tick = now_ms;
                break;
            }

            elapsed_ms = (uint32_t)(now_ms - dehy_new_last_tick);
            if (elapsed_ms < DEHY_SPEED_UPDATE_MS)
            {
                break;
            }

            step_count = elapsed_ms / DEHY_SPEED_UPDATE_MS;
            dehy_new_last_tick += (step_count * DEHY_SPEED_UPDATE_MS);

            next_speed = (uint32_t)dehy_new_set_speed + ((uint32_t)acc_val * step_count);
            if (next_speed > (uint32_t)target_speed)
            {
                next_speed = (uint32_t)target_speed;
            }
            if (next_speed > DEHY_SPEED_MAX)
            {
                next_speed = DEHY_SPEED_MAX;
            }

            dehy_new_set_speed = (uint16_t)next_speed;
            if (dehy_new_set_speed >= target_speed)
            {
                dehy_new_set_speed = target_speed;
                dehy_new_state = DEHY_NEW_STATE_HOLD_TARGET;
            }
            break;

        case DEHY_NEW_STATE_HOLD_CURRENT:
            /* Guard path, usually returned above. */
            dehy_new_set_speed = dehy_new_hold_speed;
            break;

        case DEHY_NEW_STATE_HOLD_TARGET:
            dehy_new_set_speed = target_speed;
            break;

        default:
            dehy_new_state = DEHY_NEW_STATE_ACCEL;
            dehy_new_last_tick = now_ms;
            break;
    }

    speed_set_val = dehy_new_set_speed;
    return dehy_new_set_speed;
}