#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)
#define SPEEDUP_INIT_CMD            (0x5AA5)




/* 脱水速度设置 枚举变量 */
typedef enum
{
    DEHY_SPEEDSET_INIT,         // 初始化
    DEHY_SPEEDSET_SPEED_UP,     // 加速
    DEHY_SPEEDSET_SPEED_KEEP,   // 稳速
    DEHY_SPEEDSET_SPEED_STOP,   // 超震停机  

    DEHY_SPEEDSET_NUM
}SPEEDUP_ENUM_T;

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

/*************************************************************************************
 * @brief    下设速度
 * @param[in/out] val  
 * 
 * @warning  
 * @note     
  *************************************************************************************/
void motor_speed_set(uint16_t val)
{
    speed_set_val = val;
}


void dehy_speed_up(LOGIC_STATE_ENUM_T init_cmd, uint16_t acc_val, uint16_t current_speed, uint16_t target_speed)
{

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

    switch(add_speed_state)
    {
        // init
        case DEHY_SPEEDSET_INIT:
            // 时间戳 初始化
            timetick = get_systick_ms();
            // 预设速度 初始化
            result_speed = current_speed;
            // 目标速度缓存 初始化
            target_speed_cache = target_speed;
            // // 刷新 当前速度缓存
            // current_speed_cache = current_speed;
            // 进入下一阶段
            add_speed_state = DEHY_SPEEDSET_SPEED_UP;
            break;
        // 加速
        case DEHY_SPEEDSET_SPEED_UP:
            // 当前 触发超震
            if(target_speed == 0)
            {
                add_speed_state = DEHY_SPEEDSET_SPEED_STOP;
                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 = DEHY_SPEEDSET_SPEED_KEEP;
                    }
                }
                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 = DEHY_SPEEDSET_SPEED_KEEP;
                    }

                }
                // 刷新 目标速度缓存
                target_speed_cache = target_speed;
            }
            break;
        // 速度保持
        case DEHY_SPEEDSET_SPEED_KEEP:
            // 超震停机
            if(target_speed == 0)
            {
                result_speed = 0;
                add_speed_state = DEHY_SPEEDSET_SPEED_STOP;
            }
            // 目标速度变好了 外面超震小了
            else if(target_speed > target_speed_cache)
            {
                // 若预定速度 小于 新的目标速度，则回去加速
                if(result_speed < target_speed)
                {
                    // 回加速
                    add_speed_state = DEHY_SPEEDSET_SPEED_UP;
                }
                
                target_speed_cache = target_speed;
                // // 刷新 当前速度缓存
                // current_speed_cache = current_speed;
            }
            break;
        // 超震停机
        case DEHY_SPEEDSET_SPEED_STOP:
            result_speed = 0;
            break;
        default:
            break;
    }

    /* 设置速度 */
    motor_speed_set(result_speed);
}