#include "bsp_motor.h"
#include "tim.h"

/**
 * @file bsp_motor.c
 * @brief AT8236-MS 电机驱动实现 (PWM)
 * 
 * AT8236-MS 驱动逻辑参考：
 * - IN1 (PWM) / IN2 (L) -> 正转
 * - IN1 (L) / IN2 (PWM) -> 反转
 * - IN1 (L) / IN2 (L) -> 停止/刹车 (视具体芯片配置而定)
 */

/**
 * @brief 初始化电机相关外设
 */
void motor_init(void)
{
    /* 1. 开启 PWM 定时器硬件通道 (TIM1 用于 M1, M2) */
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

        /* --- 关键修改：强制使能 TIM1 的主输出 (仅高级定时器需要) --- */
    __HAL_TIM_MOE_ENABLE(&htim1); 

    /* 2. 开启 PWM 定时器硬件通道 (TIM2 用于 M3, M4) */
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
    
    /* 3. 初始状态设置为停止 */
    motor_stop(MOTOR_1);
    motor_stop(MOTOR_2);
    motor_stop(MOTOR_3);
    motor_stop(MOTOR_4);
}

/**
 * @brief 设置电机PWM速度和方向
 * @param motor_id 电机 ID (MOTOR_1 到 MOTOR_4)
 * @param speed 速度值 (-3599 到 3599，正数正转，负数反转)
 */
void motor_set_speed(motor_id_t motor_id, int16_t speed)
{
    TIM_HandleTypeDef* htim_w = NULL;
    uint32_t channel_w = 0;
    TIM_HandleTypeDef* htim_s = NULL;
    uint32_t channel_s = 0;

    /* 获取对应电机的 定时器句柄和通道 (根据 tim.c GPIO 复用定义) */
    switch (motor_id) {
        case MOTOR_1: // PE9->TIM1_CH1, PE11->TIM1_CH2
            htim_w = &htim1; channel_w = TIM_CHANNEL_1;
            htim_s = &htim1; channel_s = TIM_CHANNEL_2;
            break;
        case MOTOR_2: // PE13->TIM1_CH3, PE14->TIM1_CH4
            htim_w = &htim1; channel_w = TIM_CHANNEL_3;
            htim_s = &htim1; channel_s = TIM_CHANNEL_4;
            break;
        case MOTOR_3: // PA15->TIM2_CH1, PB3->TIM2_CH2
            htim_w = &htim2; channel_w = TIM_CHANNEL_1;
            htim_s = &htim2; channel_s = TIM_CHANNEL_2;
            break;
        case MOTOR_4: // PA2->TIM2_CH3, PA3->TIM2_CH4
            htim_w = &htim2; channel_w = TIM_CHANNEL_3;
            htim_s = &htim2; channel_s = TIM_CHANNEL_4;
            break;
        default: return;
    }

    /* 限制速度范围 (ARR=3599) */
    if (speed > 3599) speed = 3599;
    if (speed < -3599) speed = -3599;

    /* 计算绝对值速度 */
    uint16_t pwm_val = (speed < 0) ? (uint16_t)(-speed) : (uint16_t)speed;

    /* 根据速度判断方向 */
    if (speed > 0) {
        /* 正转：W 输出 PWM, S 输出低电平 (占空比 0) */
        __HAL_TIM_SET_COMPARE(htim_w, channel_w, pwm_val);
        __HAL_TIM_SET_COMPARE(htim_s, channel_s, 0);
    } else if (speed < 0) {
        /* 反转：W 输出低电平 (占空比 0), S 输出 PWM */
        __HAL_TIM_SET_COMPARE(htim_w, channel_w, 0);
        __HAL_TIM_SET_COMPARE(htim_s, channel_s, pwm_val);
    } else {
        motor_stop(motor_id);
    }
}

/**
 * @brief 停止电机
 * @param motor_id 电机 ID
 */
void motor_stop(motor_id_t motor_id)
{
    /* 设置对应通道占空比均为 0 以刹车/停止 */
    switch (motor_id) {
        case MOTOR_1:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
            break;
        case MOTOR_2:
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);
            break;
        case MOTOR_3:
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0);
            break;
        case MOTOR_4:
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
            __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
            break;
        default: break;
    }
}