#include "work.h"
#include "board.h"
#define OUTPUT_INFORMATION_CYCLE_BASE 100
#define DISTANCE 1000
#define SPEED 80
#define TARGET_POS 500
#define TARGET_SPEED 20
work_t work;

/**
 * @brief 输出信息
 *
 * 该函数用于输出一些信息，具体输出内容未实现。
 */
static void output_information(void)
{
    char data[128];
    uint8_t len = 0;
    int32_t rst = 0;
    osel_memset((uint8_t *)data, 0, ARRAY_LEN(data));

    if (work.type == WORK_MOTOR_POS)
    {
        rst = snprintf(data, ARRAY_LEN(data), "%d,%d,%f\r\n", work.target_pos, work.encoder_cnt, work.pwm_percent);
    }
    else if (work.type == WORK_MOTOR_SPEED)
    {
        rst = snprintf(data, ARRAY_LEN(data), "%d,%d,%f\r\n", work.target_speed, work.encoder_show, work.pwm_percent);
    }
    else
    {
        return;
    }

    if (rst == 0 || rst > ARRAY_LEN(data))
    {
        return;
    }

    len = osel_mstrlen((uint8_t *)data);
    uart_send((uint8_t *)data, len);
}

static void pwm_map(void)
{
    set_motor_pwm(work.pwm_percent);
}

static void motor_pos(void)
{
    int32_t code = work.encoder_cnt;
    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_pos, code);
    set_motor_pwm(work.pwm_percent);
}

static void motor_speed(void)
{
    int32_t code = work.encoder_cnt;
    if (ABS(work.target_speed - work.encoder_show) > 20)
    {
        work.target_speed_filter = lpf_update(&work.target_speed_lpf, work.target_speed);
    }
    else
    {
        work.target_speed_filter = work.target_speed;
    }

    work.encoder_show = lpf_update(&work.encoder_lpf, work.encoder_cnt);
    work.encoder_cnt = 0;
    work.pwm_percent = work.pid.pid_u.fuzzy.execute(&work.pid.pid_u.fuzzy, work.target_speed_filter + 1, code); // 补偿1
    set_motor_pwm(work.pwm_percent);
}

static void pwm_map_key_handle(button_id_e id)
{
    int8_t min = -100, max = 100;
    int8_t step = 10;
    switch (id)
    {
    case KEY_ADD:
        if (work.pwm_percent < max)
        {
            work.pwm_percent += step;
        }
        break;
    case KEY_SUB:
        if (work.pwm_percent > min)
        {
            work.pwm_percent -= step;
        }
        break;
    case KEY_S:
        work.pwm_percent = 0;
        break;
    default:
        break;
    }
}

static void motot_pos_key_handle(button_id_e id)
{
    int16_t min = 0, max = DISTANCE;
    int16_t step = 100;
    switch (id)
    {
    case KEY_ADD:
        if (work.target_pos < max)
        {
            work.target_pos += step;
        }
        break;
    case KEY_SUB:
        if (work.target_pos > min)
        {
            work.target_pos -= step;
        }
        break;
    case KEY_S:
        work.target_pos = TARGET_POS;
        break;
    default:
        break;
    }
}

static void motor_speed_key_handle(button_id_e id)
{
    int16_t min = TARGET_SPEED, max = SPEED;
    int16_t step = 5;
    switch (id)
    {
    case KEY_ADD:
        if (work.target_speed < max)
        {
            work.target_speed += step;
        }
        break;
    case KEY_SUB:
        if (work.target_speed > min)
        {
            work.target_speed -= step;
        }
        break;
    case KEY_S:
        work.target_speed = TARGET_SPEED;
        break;
    default:
        break;
    }
}

void work_key_handle_cb(button_id_e id)
{

    switch (work.type)
    {
    case WORK_PWM_MAP:
        pwm_map_key_handle(id);
        break;
    case WORK_MOTOR_POS:
        motot_pos_key_handle(id);
        break;
    case WORK_MOTOR_SPEED:
        motor_speed_key_handle(id);
        break;
    default:
        DBG_ASSERT(FALSE __DBG_LINE);
        break;
    }
}

static void work_data_update(void)
{
    work.pid.pid_u.fuzzy.set_kp(&work.pid.pid_u.fuzzy, work.pid_params.kp);
    work.pid.pid_u.fuzzy.set_ki(&work.pid.pid_u.fuzzy, work.pid_params.ki);
    work.pid.pid_u.fuzzy.set_kd(&work.pid.pid_u.fuzzy, work.pid_params.kd);
}

void work_process(void)
{
    switch (work.type)
    {
    case WORK_PWM_MAP:
        pwm_map();
        break;
    case WORK_MOTOR_POS:
        motor_pos();
        break;
    case WORK_MOTOR_SPEED:
        motor_speed();
        break;
    default:
        break;
    }
    if (work.enter_cnt++ % (OUTPUT_INFORMATION_CYCLE_BASE / work.timer_cycle) == 0)
    {
        output_information();
    }
    work_data_update();
}

void work_encoder_exti(void)
{
    if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin) == 1)
    {
        work.encoder_cnt++;
    }
    else
    {
        work.encoder_cnt--;
    }
}

void work_init(void)
{
    osel_memset((uint8_t *)&work, 0, sizeof(work_t));
    PWM_START(PWM_TIM, PWM_CHANNEL);
    PWM_SET_DUTY(PWM_TIM, PWM_CHANNEL, 0);
    work.pwm_feq = PWM_GET_FREQ(PWM_TIM);
    work.pwm_arr = PWM_GET_ARR(PWM_TIM);
    work.type = WORK_MOTOR_SPEED;

    // PID初始化

    {
        work.pid.type = PID_TYPE_FUZZY;
        work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;

        work.pid_params.dead_zone = 1;

        if (work.type == WORK_MOTOR_POS)
        {
            work.pid_params.kp = 1;
            work.pid_params.ki = 0.01f;
            work.pid_params.kd = 10;
            work.pid.sub_type = PID_SUB_TYPE_POSITION;
        }
        else
        {
            work.pid_params.kp = 0.01;
            work.pid_params.ki = 0.1f;
            work.pid_params.kd = 0;
            work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
        }
        pid_constructor(&work.pid);

        if (work.type == WORK_MOTOR_POS)
        {
            work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
                                              work.pid_params.kd, work.pid_params.dead_zone, 0, -90, 90); // 电机输出不要超过90
            work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, TRUE);
            work.position = &work.pid.pid_u.fuzzy.pri_u.position;
        }
        else
        {
            work.pid.pid_u.fuzzy.set_ctrl_prm(&work.pid.pid_u.fuzzy, work.pid_params.kp, work.pid_params.ki,
                                              work.pid_params.kd, work.pid_params.dead_zone, 0, -1 * ((int32_t)SPEED), SPEED);
            work.pid.pid_u.fuzzy.set_kd_enable(&work.pid.pid_u.fuzzy, FALSE);
            work.increment = &work.pid.pid_u.fuzzy.pri_u.increment;
            work.encoder_lpf.alpha = work.pid_params.ki * 1.5f;
            lpf_init(&work.encoder_lpf);

            work.target_speed_lpf.alpha = work.pid_params.ki * 1.5f;
            lpf_init(&work.target_speed_lpf);

            // 改变定时器周期
            uint16_t arr = LL_TIM_GetAutoReload(WORK_TIM);
            LL_TIM_SetAutoReload(WORK_TIM, arr * 10 - 1);
        }
    }

    work.target_pos = TARGET_POS;
    work.target_speed = TARGET_SPEED;
    work.timer_cycle = TIM_CYCLE(WORK_TIM);

    return;
}