#include <stdint.h>
#include "string.h"


typedef enum
{
    LOGIC_INIT = 0x5AA5,
    LOGIC_RUN = 0x1111
    // LOGIC_CLOSE = 0x2222
}LOGIC_STATE_ENUM_T;


typedef enum
{
    SUPERSHAKE_LEVEL_MID_DEHY,   // 超震等级 中脱
    SUPERSHAKE_LEVEL_HIGH1_DEHY,   // 超震等级 高脱1
    SUPERSHAKE_LEVEL_HIGH2_DEHY,   // 超震等级 高脱2
    SUPERSHAKE_LEVEL_HIGH3_DEHY,   // 超震等级 高脱3
    SUPERSHAKE_LEVEL_HIGH4_DEHY,   // 超震等级 高脱4

    SUPERSHAKE_LEVEL_NUM

}SUPERSHAKE_LEVEL_ENUM_T;


// 超震记录单元
typedef struct
{
    /* 基础参数 */
    uint16_t imu_set;     // 超震值设定
    uint16_t over_judge_time; // 超标判断时间
    uint16_t recover_judge_time;  // 恢复判断时间
    uint16_t speed_limit;   // 速度限制
    /* 状态计数 */
    uint16_t over_time_cnt;      // 恢复时间计数
    uint16_t recover_time_cnt;   // 恢复时间计数
}SUPERSHAKE_RECORD_STR_T;

// 超震限速枚举变量
typedef enum
{
    SUPERSHAKE_LIMIT_INIT,  // 初始化
    // SUPERSHAKE_LIMIT_IDLE,  // 空闲 不检测超震
    SUPERSHAKE_LIMIT_DETECT,  // 检测 


}SUPERSHAKE_LIMIT_STATE_ENUM_T;

static uint16_t rt_imu_data = 0;    // 实时imu数据

static SUPERSHAKE_RECORD_STR_T supershake_record[SUPERSHAKE_LEVEL_NUM] = {0};   // 超震变速记录

/* 中脱转速 */
static uint16_t get_set_mid_dehy_speed_value(void){
    return 400;
}
/* 高脱1转速 */
static uint16_t get_set_high_1_dehy_speed_value(void){
    return 500;
}
/* 高脱2转速 */
static uint16_t get_set_high_2_dehy_speed_value(void){
    return 600;
}
/* 高脱3转速 */
static uint16_t get_set_high_3_dehy_speed_value(void){
    return 700;
}
/* 高脱4转速 */
static uint16_t get_set_high_4_dehy_speed_value(void){
    return 800;
}


/* 中脱超震值 */
static uint16_t get_set_mid_dehy_supershake_value(void){
    return 500;
}
/* 高脱1超震值 */
static uint16_t get_set_high_1_dehy_supershake_value(void){
    return 400;
}
/* 高脱2超震值 */
static uint16_t get_set_high_2_dehy_supershake_value(void){
    return 300;
}
/* 高脱3超震值 */
static uint16_t get_set_high_3_dehy_supershake_value(void){
    return 200;
}
/* 高脱4超震值 */
static uint16_t get_set_high_4_dehy_supershake_value(void){
    return 100;
}

/* 获取 实时imu数据 */
static uint16_t get_imu_data(void)
{
    return rt_imu_data;
}

/*************************************************************************************
 * @brief    初始化 超震记录
 * 
 * @warning  
 * @note     
  *************************************************************************************/
static void supershake_record_init(void)
{
    memset((uint8_t *)supershake_record, 0x00, sizeof(supershake_record));
    /* 中脱初始化 */
    supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].imu_set = get_set_mid_dehy_supershake_value();
    supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].speed_limit = get_set_mid_dehy_speed_value();
    supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].over_judge_time = 5;
    supershake_record[SUPERSHAKE_LEVEL_MID_DEHY].recover_judge_time = 10;
    /* 高脱1初始化 */
    supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].imu_set = get_set_high_1_dehy_supershake_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].speed_limit = get_set_high_1_dehy_speed_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].over_judge_time = 5;
    supershake_record[SUPERSHAKE_LEVEL_HIGH1_DEHY].recover_judge_time = 10;
    /* 高脱2初始化 */
    supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].imu_set = get_set_high_2_dehy_supershake_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].speed_limit = get_set_high_2_dehy_speed_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].over_judge_time = 5;
    supershake_record[SUPERSHAKE_LEVEL_HIGH2_DEHY].recover_judge_time = 10;
    /* 高脱3初始化 */
    supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].imu_set = get_set_high_3_dehy_supershake_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].speed_limit = get_set_high_3_dehy_speed_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].over_judge_time = 5;
    supershake_record[SUPERSHAKE_LEVEL_HIGH3_DEHY].recover_judge_time = 10;
    /* 高脱4初始化 */
    supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].imu_set = get_set_high_4_dehy_supershake_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].speed_limit = get_set_high_4_dehy_speed_value();
    supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].over_judge_time = 5;
    supershake_record[SUPERSHAKE_LEVEL_HIGH4_DEHY].recover_judge_time = 10;
    
}


/*************************************************************************************
 * @brief    超震速度限制逻辑
 * @return uint16_t    超震限制的速度
 * 
 * @warning  
 * @note     
  *************************************************************************************/
uint16_t supershake_speed_limit(LOGIC_STATE_ENUM_T cmd)
{
    static SUPERSHAKE_LIMIT_STATE_ENUM_T supershake_limit_speed_state = SUPERSHAKE_LIMIT_INIT;  // 超震限速逻辑状态机
    static uint32_t timetick = 0;   // 时间戳
    uint16_t realtime_imu_val = 0;  // 超震实时值
    SUPERSHAKE_LEVEL_ENUM_T index_i = (SUPERSHAKE_LEVEL_ENUM_T)0;
    uint16_t supershake_recover_speed = 0;   // 恢复速度
    static uint16_t ret_limit_speed = 0;       // 对外输出的超震限制速度值

    /* 外部开关 */
    if(cmd == LOGIC_INIT)
    {
        supershake_limit_speed_state = SUPERSHAKE_LIMIT_INIT;
    }


    switch(supershake_limit_speed_state)
    {
        case SUPERSHAKE_LIMIT_INIT:
            supershake_record_init();
            supershake_limit_speed_state = SUPERSHAKE_LIMIT_DETECT;
            ret_limit_speed = get_set_high_4_dehy_speed_value();
            break;
        case SUPERSHAKE_LIMIT_DETECT:
            realtime_imu_val = get_imu_data();
            /* 检测 可指定周期 */
            for(index_i = (SUPERSHAKE_LEVEL_ENUM_T)0; index_i < SUPERSHAKE_LEVEL_NUM; ++index_i)
            {
                if(realtime_imu_val <= supershake_record[index_i].imu_set)
                {
                    supershake_record[index_i].over_time_cnt = 0;
                }
                else
                {
                    supershake_record[index_i].recover_time_cnt = 0;
                }
            }
            // 每秒处理事情
            if(get_systick_ms() - timetick >= 1000u)
            {
                timetick = get_systick_ms();

                for(index_i = (SUPERSHAKE_LEVEL_ENUM_T)0; index_i < SUPERSHAKE_LEVEL_NUM; ++index_i)
                {
                    // 更新 超震cnt 恢复cnt
                    if(supershake_record[index_i].over_time_cnt < 60)
                    {
                        (supershake_record[index_i].over_time_cnt)++;
                    }
                    if(supershake_record[index_i].recover_time_cnt < 60)
                    {
                        (supershake_record[index_i].recover_time_cnt)++;
                    }
                    /* 平稳 恢复速度 */
                    if(supershake_record[index_i].recover_time_cnt >= supershake_record[index_i].recover_judge_time)
                    {
                        if(ret_limit_speed < supershake_record[index_i].speed_limit)
                        {
                            ret_limit_speed = supershake_record[index_i].speed_limit;
                        }
                    }
                    /* 超震 限速 */
                    if(supershake_record[index_i].over_time_cnt >= supershake_record[index_i].over_judge_time)
                    {
                        if(index_i == (SUPERSHAKE_LEVEL_ENUM_T)0)
                        {
                            ret_limit_speed = 0;
                        }
                        else
                        {
                            if(ret_limit_speed > supershake_record[index_i-1].speed_limit)
                            {
                                ret_limit_speed = supershake_record[index_i-1].speed_limit;
                            }
                            // ret_limit_speed = supershake_record[index_i-1].speed_limit;
                        }
                    }
                }
            }
            break;
    }

    return ret_limit_speed;
}