#include "motor_drive.h"

#include <math.h>

#include "driver/uart.h"
#include "esp_log.h"
#include "port.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

#define angular_error 1.0  //角度误差

static motor_t *motor_init_structer;
static motor_on_event_t s_onevent;

static double motor_position;
static bool motor_cmd_ctr_turn_flag = false;

/**
 * @brief 电机初始化
 *
 * @param motor 初始化的结构体(暂时写死，所以结构体为空)
 */
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;
}

static double motor_drive_read_encoder();
// static uint8_t motor_drive_set_packages_ctr(double position, int direction);

/**
 * @brief 设置电机零点位置，需电机转动前设置。
 *
 */
void motor_set_zero_point() {
  size_t set_current_buffer_size = 5;
  uint8_t buffer[5] = {0X3E, 0X19, MOTOR_ID, 0X00, 0X58};
  //   char *notify_err = "Set current size err";
  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, 5);

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

  // Wait receive
  set_current_buffer_size = uart_read_bytes(uart_num, buffer, 5, uart_read_time_ms / portTICK_RATE_MS);
  if (set_current_buffer_size != 5 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "Set current size:%d,buffer[0] = 0X%x", set_current_buffer_size, buffer[0]);
    // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
  }

  return;
}

/**
 * @brief 返回当前电机所在位置0->360
 *
 * @return uint32_t
 */
double motor_get_position_degree() {
  uint16_t turn_angle_data = 0;
  size_t turn_angle_buffer_size = 5;
  uint8_t buffer[8] = {0X3E, 0X94, MOTOR_ID, 0X00, 0XD3};
  // char *notify_err = "Turn angle size err";

  uart_flush(uart_num);
  uart_write_bytes(uart_num, buffer, turn_angle_buffer_size);

  turn_angle_buffer_size = 0;
  memset(buffer, 0, sizeof(buffer));

  // Wait receive
  turn_angle_buffer_size = uart_read_bytes(uart_num, buffer, 8, uart_read_time_ms / portTICK_RATE_MS);
  if (turn_angle_buffer_size != 8 || buffer[0] != 0X3E) {
    ESP_LOGW(MOTOR_DRIVE, "Turn angle size:%d,buffer[0] = 0X%x", turn_angle_buffer_size, buffer[0]);
    // bluetooth_active_notify((uint8_t *)notify_err, strlen(notify_err));
    return -1;
  }

  // Parse receive
  // motor_drive_buffer_cmd_parse(buffer);

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

  return ((double)turn_angle_data / 100.0);
}

/**
 * @brief 电机转到多少度
 */
uint8_t motor_run_to_postion(int rotation_direction, double position, int speed_level) {
  int position_int = 0;
  uint8_t position_remainder = 0;
  uint8_t position_buffer_size = 6;  //从第五位开始(低位)
  uint8_t checksum = 0;
  uint32_t limit_speed = 0;
  uint8_t buffer[14] = {0x3E, 0XA6, MOTOR_ID, 0X08, 0XED, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00, 0X00};
  //   char *notify_err = "set size error";

  // forward
  if ((rotation_direction == 1)) {
    buffer[5] = 0;
    checksum += 0;
  }

  // reverse
  if ((rotation_direction == 2)) {
    buffer[5] = 1;
    checksum += 1;
  }

  position_int = position * 100;
  position_int = position_int % 36000;

  if (position_int != 0) {
    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;
  }

  if (speed_level <= 9) {
    limit_speed = speed_level * 360;
    position_buffer_size = 9;
  }

  if (limit_speed != 0) {
    while ((limit_speed / 0X100) > 0) {
      position_remainder = limit_speed & 0XFF;
      buffer[position_buffer_size] = position_remainder;
      position_buffer_size += 1;
      limit_speed = limit_speed >> 8;
      checksum += position_remainder;
    }
    buffer[position_buffer_size] = limit_speed;
    checksum += limit_speed;
  }

  checksum = checksum & 0XFF;
  buffer[13] = checksum;

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

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

  // 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;
  }

  motor_position = position;
  motor_cmd_ctr_turn_flag = true;

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

/**
 * @brief 注册回调
 *
 * @param onevent 注册的回调函数
 */
void motor_reg_event_cb(motor_on_event_t onevent) {
  s_onevent = onevent;
  return;
}

/**
 * @brief 读取编码器
 *
 * @return double 编码器当前位置值返回
 */
static double motor_drive_read_encoder() {
  size_t encoder_buffer_size = 5;
  uint8_t buffer[12] = {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(buffer));

  // 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);
}

/**
 * @brief 控制电机转多少度
 *
 * @param position 角度
 * @param direction 方向
 * @return uint8_t
 */
// static 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[13] = {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(buffer));

//   // 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;
// }

bool motor_stop() {
  static uint32_t time;
  static double motor_befor_encoder;
  static double motor_now_encoder;
  static uint8_t motor_read_encoder_count = 0;
  if (motor_cmd_ctr_turn_flag && port_haspassedms(time) > 100) {
    motor_now_encoder = motor_drive_read_encoder();

    if (motor_now_encoder == motor_befor_encoder) {
      motor_read_encoder_count = motor_read_encoder_count + 1;
    } else {
      motor_befor_encoder = motor_now_encoder;
      motor_read_encoder_count = 0;
    }

    if (motor_read_encoder_count == 3) {
      motor_read_encoder_count = 0;
      motor_cmd_ctr_turn_flag = false;
      return true;
    }

    time = port_get_ticket();
  }
  return false;
}

/**
 * @brief 电机模块调度(验证电机是否到达指定位置，然后主动上报数据)
 *
 */
void motor_module_schedule() {
  if (motor_stop()) {
    s_onevent(kRunToPosition);  //组包，主动上报
  }

  return;
}