#include "motor_drive.h"

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

#define MOTOR_DRIVE "MOTOR_DRIVE"

#define uart_num UART_NUM_1
#define tx_io_num 23
#define rx_io_num 22
#define buffer_size 128
#define uart_read_time_ms 200
#define MOTOR_ID 0X01

static motor_t *motor_init_structer;

void motor_init(motor_t *motor) {
  motor_init_structer = motor;
  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));
  return;
}
void motor_set_zero_point() { return; }

/**
 * @brief 返回当前电机所在位置0->360
 *
 * @return uint32_t
 */
uint32_t motor_get_position_degree() { return 0; }

/**
 * @brief 电机转到多少度
 */
void motor_run_to_postion(int potion) { return; }
void motor_reg_event_cb(motor_on_event_t onevent) { return; }

double motor_drive_read_encoder() {
  size_t encoder_buffer_size = 5;
  uint8_t buffer[5] = {0X3E, 0X90, MOTOR_ID, 0X00, 0XCF};
  uint16_t encoder_data = 0;

  // Send cmd
  uart_flush(uart_num);
  uart_write_bytes(uart_num, (const char *)buffer, encoder_buffer_size);

  encoder_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 5);

  // Wait receive
  encoder_buffer_size = uart_read_bytes(uart_num, buffer, 12, uart_read_time_ms / portTICK_RATE_MS);
  if (encoder_buffer_size != 12 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "encoder size:%d,buffer[0] = 0X%x", encoder_buffer_size, buffer[0]);

    return -1.0;
  }

  encoder_data = buffer[5] + (buffer[6] << 8);

  return ((double)encoder_data / 100.0);
}

uint8_t motor_drive_set_packages_ctr(double position, int direction) {
  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, MOTOR_ID, 0X04, 0XEA, 0X00, 0X00, 0X00, 0X00, 0X00};
//   char *notify_err = "set size error";

  position_int = position * 100;
  if (direction == 2) {
    position_int = 0 - position_int;
  }

  if (position_int != 0) {
    if (position_int > 0) {  // Positive number
      while ((position_int / 0X100) > 0) {
        position_remainder = position_int & 0XFF;
        buffer[position_buffer_size] = position_remainder;
        position_buffer_size += 1;
        position_int = position_int >> 8;
        checksum += position_remainder;
      }
      buffer[position_buffer_size] = position_int;
      checksum += position_int;
      checksum = checksum & 0XFF;
      buffer[9] = checksum;
    } else {  // Negative
      while ((position_int / 0X100) < 0) {
        position_remainder = position_int & 0XFF;
        buffer[position_buffer_size] = position_remainder;
        position_buffer_size += 1;
        position_int = position_int >> 8;
        checksum += position_remainder;
      }
      buffer[position_buffer_size] = position_int;
      position_buffer_size += 1;
      while (position_buffer_size != 9) {
        buffer[position_buffer_size] = 0XFF;
        position_buffer_size += 1;
        checksum += 0XFF;
      }
      checksum += position_int;
      checksum = checksum & 0XFF;
      buffer[9] = checksum;
    }
  }

  // Send cmd
  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 10);

  position_buffer_size = 0;
  memset(buffer, 0, sizeof(uint8_t) * 10);

  // Wait uart receive,if time out return error and output log
  position_buffer_size = uart_read_bytes(uart_num, buffer, 13, uart_read_time_ms / portTICK_RATE_MS);
  if (position_buffer_size != 13 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "set motor size error ,buffer_size:%d,buffer[0] = 0X%x", position_buffer_size, buffer[0]);
    // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return 1;
  }

  // Parse receive
  // motor_drive_buffer_cmd_parse(buffer);
  return 0;
}