/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file    motor_driver.c
 * @brief   电机驱动实现 - 双路AT8236-MS H桥驱动
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "motor_driver.h"
#include "elog.h"
#include "stm32f1xx_hal.h"
#include "stm32f1xx_hal_tim.h"
#include "cmsis_os.h"
#include "tim.h"

/* Private defines -----------------------------------------------------------*/
/* TIM4自动重装载值是99 (100-1)，所以PWM值范围是0-99 */


/* Private function prototypes -----------------------------------------------*/

/* PWM parameters */
#define PWM_ARR 99
#define PWM_STEPS (PWM_ARR + 1)

/* 三档占空比（百分比），索引为 0..2 对应 档位 1..3（档位0为停止） */
static const uint8_t gears_percent[3] = {75, 80, 85};
/* current_gear: 0 = 停止, 1 = 低, 2 = 中, 3 = 高 */
static uint8_t current_gear = 0; /* 0..3 */

static inline uint32_t percent_to_compare(uint8_t percent) {
  return ((uint32_t)percent * PWM_STEPS) / 100U;
}

/* 将两个电机都设置为指定占空比（只使用 TIM4_CH2, TIM4_CH4 输出，CH1/CH3 停止以保持单方向） */
static void set_both_motors_percent(uint8_t percent) {
  uint32_t cmp = percent_to_compare(percent);

  /* 停止并关闭 CH1、使用 CH2 输出 */
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_1);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, (uint16_t)cmp);

  /* 停止并关闭 CH3、使用 CH4 输出 */
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, (uint16_t)cmp);
}

/* 启动时短暂提升到 90% 并保持 500ms，然后降到一档（档位1） */
void Motor_StartupBoost(void) {
  set_both_motors_percent(90);
  osDelay(1500);
  current_gear = 1; /* 启动后回到档位1（最低速） */
  set_both_motors_percent(gears_percent[current_gear - 1]);
}

/* 设置挡位（0..3），0=停止 */
void Motor_SetGear(uint8_t gear) {
  if (gear > 3) gear = 3;
  current_gear = gear;
  if (current_gear == 0) {
    set_both_motors_percent(0);
  } else {
    set_both_motors_percent(gears_percent[current_gear - 1]);
  }
}

/* 切换到下一档（0->1->2->3->0 循环） */
void Motor_NextGear(void) {
  current_gear++;
  if (current_gear > 3) current_gear = 0;
  if (current_gear == 0) {
    set_both_motors_percent(0);
  } else {
    set_both_motors_percent(gears_percent[current_gear - 1]);
  }
}

/* 向上加档（与按键配合使用） */
void Motor_GearUp(void) {
  Motor_NextGear();
}

/* 向下减档（循环） */
void Motor_GearDown(void) {
  if (current_gear == 0) {
    current_gear = 3;
  } else {
    current_gear--;
  }
  if (current_gear == 0) {
    set_both_motors_percent(0);
  } else {
    set_both_motors_percent(gears_percent[current_gear - 1]);
  }
}

uint8_t Motor_GetGear(void) {
  return current_gear;
}


/* Exported functions --------------------------------------------------------*/

/**
 * @brief 电机驱动初始化
 */
void Motor_Init(void) {
  /* 启动TIM4 PWM输出 */
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);

  // 初始完全停止电机
  Motor_Stop();


}

void Motor_Stop(void) {
  // 所有引脚输出0 - 完全停止
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, 0);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, 0);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, 0);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, 0);
}