#include "by_servo.h"

#include "at32f415.h"
#include "lwprintf.h"

/**
 * @brief 设置云台摇臂
 *
 * @param angle 角度 (0-180)
 */
void by_servo_set_claw_arm(int16_t angle)
{
#define PULSE_WIDTH_MIN 0.5f
#define PULSE_WIDTH_MAX 2.5f

  uint32_t ch_val = 0;

  if (angle < 0) {
    angle = 0;
  } else if (angle > 270) {
    angle = 270;
  }

  ch_val = (uint32_t)(((float)angle / 270.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);

  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, ch_val);

#undef PULSE_WIDTH_MIN
#undef PULSE_WIDTH_MAX
}

/**
 * @brief 设置云台摇臂
 *
 * @param angle 角度 (0-180)
 */
void by_servo_set_claw(int16_t angle)
{
#define PULSE_WIDTH_MIN 0.5f
#define PULSE_WIDTH_MAX 2.5f

  uint32_t ch_val = 0;

  if (angle < 0) {
    angle = 0;
  } else if (angle > 180) {
    angle = 180;
  }

  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);

  tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, ch_val);

#undef PULSE_WIDTH_MIN
#undef PULSE_WIDTH_MAX
}

/**
 * @brief 设置云台摇臂
 *
 * @param angle 角度 (0-180)
 */
void by_servo_set_camera(int16_t angle)
{
#define PULSE_WIDTH_MIN 0.5f
#define PULSE_WIDTH_MAX 2.5f

  uint32_t ch_val = 0;

  if (angle < 0) {
    angle = 0;
  } else if (angle > 270) {
    angle = 270;
  }

  ch_val = (uint32_t)(((float)angle / 270.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);

  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_4, ch_val);

#undef PULSE_WIDTH_MIN
#undef PULSE_WIDTH_MAX
}

/**
 * @brief 设置顶端抓取机构
 *
 * @param angle 角度 (0-180)
 */
void by_servo_set_scoop(int16_t angle)
{
#define PULSE_WIDTH_MIN 0.5f
#define PULSE_WIDTH_MAX 2.5f

  uint32_t ch_val = 0;

  if (angle < 0) {
    angle = 0;
  } else if (angle > 180) {
    angle = 180;
  }

  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);

  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_3, ch_val);

#undef PULSE_WIDTH_MIN
#undef PULSE_WIDTH_MAX
}

/**
 * @brief 设置置物平台
 *
 * @param angle 角度 (0-180)
 */
void by_servo_set_storage(int16_t angle)
{
#define PULSE_WIDTH_MIN 0.5f
#define PULSE_WIDTH_MAX 2.5f

  uint32_t ch_val = 0;

  if (angle < 0) {
    angle = 0;
  } else if (angle > 180) {
    angle = 180;
  }

  ch_val = (uint32_t)(((float)angle / 180.0f * (PULSE_WIDTH_MAX - PULSE_WIDTH_MIN) + PULSE_WIDTH_MIN) / 20.0f * 9999.0f);

  tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_2, ch_val);

#undef PULSE_WIDTH_MIN
#undef PULSE_WIDTH_MAX
}

void by_servo_init(void)
{
  // 后续要不全初始化为空值，由主控按顺序初始化
  by_servo_set_claw_arm(220); // 36-220
  by_servo_set_claw(25);    // 25-90
  by_servo_set_camera(45);
  by_servo_set_scoop(0);
  by_servo_set_storage(0);
}

void by_servo_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
{

  int16_t angle = 0;

  angle = (int16_t)(data[0] | (data[1] << 8));

  switch (stdd_id) {
    case 0x009:
      by_servo_set_claw_arm(angle);
      break;
    case 0x00A:
      by_servo_set_claw(angle);
      break;
    case 0x00B:
      by_servo_set_camera(angle);
      break;
    case 0x00C:
      by_servo_set_scoop(angle);
      break;
    case 0x00D:
      by_servo_set_storage(angle);
      break;
    default:
      break;
  }
}