#include "servo_operation.h"

#include <stdbool.h>
#include <stdio.h>
#include <string.h>

static UART_HandleTypeDef* m_uart;

static uint8_t tx_buf[PROCESS_MAX_LEN];
static uint8_t rx_buf[PROCESS_MAX_LEN];
static uint8_t tx_send_len;

static uint8_t cmd_type;

static uint8_t check_sum;

// 我需要一个能携带寄存器信息的变量
static reg_info_t reg_info;



/**
 * @description: 舵机串口初始化
 * @param {UART_HandleTypeDef*} uart
 * @return {*}
 */
void servo_uart_init(UART_HandleTypeDef* uart) { m_uart = uart; }

/**
 * @description: 写操作
 * @param {servo_obj_t*} servo 舵机对象
 * @return {*}
 */
bool Write_operete(uint8_t id, uint8_t cmd, param_t* param) {
  reg_distinguish(param, &reg_info);

  if (cmd != SYC_W_DATA)
    cmd_type = non_syn;
  else
    cmd_type = syn;

  write_reg(id, cmd, param);
  return true;
}

void Ping_operete(uint8_t id) {
  cmd_basic_t* cmd_basic = (cmd_basic_t*)tx_buf;
  cmd_basic->head        = 0xffff;
  cmd_basic->id          = id;
  cmd_basic->valid_len   = 0x02;
  cmd_basic->cmd         = PING;

  check_sum = check_sum_calculate(&tx_buf[2], NULL);
  memcpy(&tx_buf[BASCI_LEN], &check_sum, 1);
  tx_send_len = BASCI_LEN + 1;  // 1 = check_sum
  packet_process(m_uart, tx_buf, rx_buf, tx_send_len);
}

// 初始方案无法确定发送参数的长度，用了一个数组但是导致指令包过大，所以改了个方法
/**
 * @description:
 * @param {uint8_t} id
 * @param {uint8_t} cmd
 * @param {param_t*} param
 * @return {*}
 */
void write_reg(uint8_t id, uint8_t cmd, param_t* param) {
  uint8_t      param_real_len = param->count * reg_info.data_len;  // 参数个数*每个参数长度 = 实际参数长度
  cmd_basic_t* cmd_basic      = (cmd_basic_t*)tx_buf;              // 解决发送数据不是数组问题
  uint8_t*     cmd_param      = &tx_buf[5];

  cmd_basic->head = 0xffff;
  cmd_basic->id   = id;

  if (cmd_type) {
    cmd_basic->valid_len = param_real_len + 3;  // cmd + id + check_sum = 3
    cmd_basic->cmd       = cmd;
    cmd_param[0]         = param->addr;
    // 判断参数数据长度
    if (reg_info.data_len == byte) {
      memcpy(&cmd_param[1], param->data, param->count);
    } else if (reg_info.data_len == word) {
      // 高低位交换，传入的寄存器地址不需要交换
      high_low_exchange(param, &cmd_param[1], &reg_info);
    }
  }
  // else 同步和非同步无法合并为一个函数
  check_sum = check_sum_calculate(&tx_buf[2], param);
  // 将checksum放入指令包末尾
  memcpy(&tx_buf[BASCI_LEN + param_real_len + 1], &check_sum, 1);  // tx_buff[5] == addr    1 = addr

  tx_send_len = BASCI_LEN + param_real_len + 2;  // 2 = addr + check_sum
  packet_process(m_uart, tx_buf,rx_buf, tx_send_len);
}