/* 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 -----------------------------------------------*/
void Motor_StartAll(void);
void Motor_StopAll(void);

/* 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, 85, 95};
/* 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;
}

/* 将两个电机都设置为指定占空比（使用 AT8236-MS H桥驱动） */
static void set_both_motors_percent(uint8_t percent) {
  uint32_t cmp = percent_to_compare(percent);

  /* 电机1: TIM4_CH1->AIN1, TIM4_CH2->AIN2
   * 正转：AIN1=PWM, AIN2=0
   * 反转：AIN1=0, AIN2=PWM
   * 这里用正转模式：CH1输出PWM, CH2为0 */
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, (uint16_t)cmp);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2, 0);

  /* 电机2: TIM4_CH3->BIN1, TIM4_CH4->BIN2
   * 正转：BIN1=PWM, BIN2=0
   * 这里用正转模式：CH3输出PWM, CH4为0 */
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, (uint16_t)cmp);
  __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, 0);
}
/* 启动时短暂提升到 100% 并保持 500ms，然后降到一档（档位1） */
void Motor_StartupBoost(void) {
  // 确保PWM通道已启动
  Motor_StartAll();
  set_both_motors_percent(100); /* 启动提升到 100% */
  elog_d("Motor", "启动冲击: 100%%");
  osDelay(1500);
  current_gear = 1; /* 启动后回到档位1（最低速） */
  elog_d("Motor", "设置档位: %d, PWM: %d%%", current_gear, gears_percent[current_gear - 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) {
    // 完全停止所有PWM输出
    Motor_Stop();
    elog_d("Motor", "Motor_SetGear: 档位0, 停止");
  } else {
    // 确保PWM通道已启动
    Motor_StartAll();
    elog_d("Motor", "Motor_SetGear: 档位%d, PWM: %d%%", current_gear, gears_percent[current_gear - 1]);
    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) {
    // 完全停止所有PWM输出
    Motor_Stop();
    elog_d("Motor", "Motor_NextGear: 档位0, 停止");
  } else {
    // 确保PWM通道已启动
    Motor_StartAll();
    elog_d("Motor", "Motor_NextGear: 档位%d, PWM: %d%%", current_gear, gears_percent[current_gear - 1]);
    set_both_motors_percent(gears_percent[current_gear - 1]);
  }
}

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

/* 向下减档（循环） */
void Motor_GearDown(void) {
  uint8_t old_gear = current_gear;

  if (current_gear == 0) {
    current_gear = 3;
  } else {
    current_gear--;
  }

  elog_d("Motor", "Motor_GearDown: %d -> %d", old_gear, current_gear);

  if (current_gear == 0) {
    // 完全停止所有PWM输出
    Motor_Stop();
    elog_d("Motor", "档位0, 停止");
  } else {
    // 确保PWM通道已启动
    Motor_StartAll();

    // 如果从2档或3档降到1档，需要短暂冲击保持电机转动
    if (current_gear == 1 && old_gear >= 2) {
      // 短暂提升到100%来维持转动，然后降到1档
      elog_d("Motor", "降档冲击: 100%%");
      set_both_motors_percent(100);
      osDelay(800);  // 800ms冲击，给电机更充裕的适应时间
    }

    elog_d("Motor", "最终档位%d, PWM: %d%%", current_gear, gears_percent[current_gear - 1]);
    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();


}

/**
 * @brief 完全停止电机并关闭所有PWM通道
 */
void Motor_StopAll(void) {
  // 设置所有通道的PWM为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);

  // 停止所有PWM通道
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_1);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_4);
}

/**
 * @brief 重新启动所有PWM通道（用于启动电机）
 */
void Motor_StartAll(void) {
  // 重新启动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);
}

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

  // 强制停止PWM通道，确保电机完全停止
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_1);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_2);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3);
  HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_4);
}