/* Includes ----------------------------------------------------------------- */
#include "bsp\servo.h"

#include <main.h>
#include <tim.h>

/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static float range[BSP_SERVO_NUM];

/* Private function  -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_Servo_Init(BSP_Servo_Channel_t ch, float max_angle) {
  range[ch] = max_angle;

  return BSP_OK;
}

int8_t BSP_Servo_Start(BSP_Servo_Channel_t ch) {
  switch (ch) {
    case BSP_SERVO_A:
      HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
      break;

    case BSP_SERVO_B:
      HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
      break;

    case BSP_SERVO_C:
      HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
      break;

    case BSP_SERVO_D:
      HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
      break;

    case BSP_SERVO_E:
      HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
      break;

    case BSP_SERVO_F:
      HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
      break;

    case BSP_SERVO_G:
      HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);

    case BSP_SERVO_NUM:
      break;
  }
  return BSP_OK;
}

int8_t BSP_Servo_Set(BSP_Servo_Channel_t ch, uint8_t angle) {
  if (angle > 1.0f) return BSP_ERR;

  uint16_t pulse = angle * UINT16_MAX;

  switch (ch) {
    case BSP_SERVO_A:
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
      break;

    case BSP_SERVO_B:
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pulse);
      break;

    case BSP_SERVO_C:
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pulse);
      break;

    case BSP_SERVO_D:
      __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pulse);
      break;

    case BSP_SERVO_E:
      __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, pulse);
      break;

    case BSP_SERVO_F:
      __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, pulse);
      break;

    case BSP_SERVO_G:
      __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, pulse);

    case BSP_SERVO_NUM:
      break;
  }
  return BSP_OK;
}

int8_t BSP_Servo_Stop(BSP_Servo_Channel_t ch) {
  switch (ch) {
    case BSP_SERVO_A:
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
      break;

    case BSP_SERVO_B:
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
      break;

    case BSP_SERVO_C:
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
      break;

    case BSP_SERVO_D:
      HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4);
      break;

    case BSP_SERVO_E:
      HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
      break;

    case BSP_SERVO_F:
      HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_2);
      break;

    case BSP_SERVO_G:
      HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_3);

    case BSP_SERVO_NUM:
      break;
  }
  return BSP_OK;
}
