#include "work.h"
#include "board.h"

work_t work;

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

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

static void motor_speed(void)
{
    work.pid.sub_type = PID_SUB_TYPE_INCREMENT;
}

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)
{
    int8_t min = -100, max = 100;
    int8_t step = 10;
    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 = 50;
        break;
    default:
        break;
    }

    if (work.target_pos == 0)
    {
        work.encoder_cnt = 0;
    }
}

static void motor_speed_key_handle(button_id_e id)
{
}

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;
    }
}

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;
    }
    return;
}

void work_encoder_exti(void)
{
    if (GPIO_READ(MOTOR_B_GPIO_Port, MOTOR_B_Pin))
    {
        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.type = WORK_MOTOR_POS;
    work.pid_params.kp = 1;
    work.pid_params.ki = 0.01f;
    work.pid_params.kd = 5;
    work.pid_params.dead_zone = 0.5f;
    // PID初始化

    {
        work.pid.type = PID_TYPE_FUZZY;
        work.pid.sub_type = PID_SUB_TYPE_POSITION;
        work.pid.pid_u.fuzzy.deadzone_dir = DEAD_ZONE_BOTH;
        pid_constructor(&work.pid);
        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.target_pos = 20;
    return;
}