#include "motor_drive.h"

#include "driver/uart.h"
#include "esp_log.h"

#define MOTOR_DRIVE "MOTOR_DRIVE"

#define uart_num UART_NUM_2
#define tx_io_num 4
#define rx_io_num 5
#define buffer_size 128

void motor_drive_uart_init() {
  uart_config_t uart_config = {
      .baud_rate = 115200,
      .data_bits = UART_DATA_8_BITS,
      .parity = UART_PARITY_DISABLE,
      .stop_bits = UART_STOP_BITS_1,
      .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
      .source_clk = UART_SCLK_APB,
  };
  ESP_ERROR_CHECK(uart_driver_install(uart_num, buffer_size * 2, 0, 0, NULL, 0));

  ESP_ERROR_CHECK(uart_param_config(uart_num, &uart_config));

  ESP_ERROR_CHECK(uart_set_pin(uart_num, tx_io_num, rx_io_num, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
}

void motor_drive_turn(int direction, int speed_level, double position) {
  if ((direction > 2) || (direction < 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Direction out of range");
  }
  if ((speed_level > 9) || (speed_level < 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Speed level out of range");
  }
  if ((position > 360) || (position <= 0)) {
    ESP_LOGW(MOTOR_DRIVE, "Position out of range");
  }
  motor_drive_set_packages_ctr(position);
  uart_write_bytes(uart_num, "test", strlen("test"));
}

double motor_drive_read_encoder() { return 0.0; }

void motor_drive_set_packages() {}

void motor_drive_set_packages_ctr(double position) {
  int position_int = 0;
  uint8_t position_remainder = 0;
  uint8_t position_buffer_size = 5;  //从第五位开始(低位)
  uint8_t checksum = 0;
  uint8_t buffer[10] = {0x3E, 0XA7, 0X01, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00};
  uint8_t strbuffer[20] = {0};

  position_int = position * 100;

  if (position_int != 0) {
    while ((position_int / 256) > 0) {
      position_remainder = position_int % 256;
      buffer[position_buffer_size] = position_remainder;
      position_buffer_size += 1;
      position_int = position_int / 256;
      checksum += position_remainder;
    }
    buffer[position_buffer_size] = position_int;
    checksum += position_int;
    checksum %= 256;
    buffer[9] = checksum;
  }
  for (int i = 0; i < 10; i++) {
    ESP_LOGI(MOTOR_DRIVE, "%d", buffer[i]);
  }
  motor_drive_hex_to_str((char *)buffer, 10, (char *)strbuffer);
  ESP_LOGI(MOTOR_DRIVE, "%s", strbuffer);
}

void motor_drive_hex_to_str(char *hex, int hex_len, char *str) {
  int i, pos = 0;

  for (i = 0; i < hex_len; i++)

  {
    sprintf(str + pos, "%02x", hex[i]);

    pos += 2;
  }
}