/*
    debug Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
#include "bsp/pwm.h"
#include "bsp/gpio.h"
#include <stdbool.h>
/* USER INCLUDE BEGIN */

/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
bool flag = false;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* USER PRIVATE CODE BEGIN */
/* 可根据舵机实际行程调整脉宽范围 */
#define SERVO_MIN_PULSE_MS 0.4f
#define SERVO_MAX_PULSE_MS 2.6f

static float Servo_AngleToDuty(float angle_deg) {
  if (angle_deg < 0.0f) {
    angle_deg = 0.0f;
  }
  if (angle_deg > 180.0f) {
    angle_deg = 180.0f;
  }

  const float period_ms = 20.0f; /* 50 Hz */
  float pulse_ms = SERVO_MIN_PULSE_MS +
                   (SERVO_MAX_PULSE_MS - SERVO_MIN_PULSE_MS) * (angle_deg / 180.0f);

  return pulse_ms / period_ms;
}

/* USER PRIVATE CODE END */
/* Exported functions ------------------------------------------------------- */
void Task_debug(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / DEBUG_FREQ;

  osDelay(DEBUG_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  BSP_GPIO_WritePin(BSP_GPIO_POWER_5V,true);
  BSP_PWM_SetFreq(BSP_PWM_TIM2_CH1, 50.0f);
  BSP_PWM_Start(BSP_PWM_TIM2_CH1);

  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
    static float angle_1 = 45.0f;
    static float angle_2 = 90.0f;
    // static float step = 1.0f;

    // BSP_PWM_SetComp(BSP_PWM_TIM2_CH1, Servo_AngleToDuty(angle));

    // angle += step;
    // if (angle >= 90.0f) {
    //   angle = 90.0f;
    //   step = -step;
    // } else if (angle <= 45.0f) {
    //   angle = 45.0f;
    //   step = -step;
    // }
    if (flag) {
      BSP_PWM_SetComp(BSP_PWM_TIM2_CH1, Servo_AngleToDuty(angle_1));
      flag = false;
    } else {
      BSP_PWM_SetComp(BSP_PWM_TIM2_CH1, Servo_AngleToDuty(angle_2));
      flag = true;
    }

    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}