#include "gimbal.hpp"

#include "src/component/crc.hpp"
#include "src/component/logger.hpp"
#include "src/component/math_tools.hpp"
#include "src/component/yaml.hpp"

namespace device
{
Gimbal::Gimbal(const std::string & config_path)
{
  auto yaml = component::load(config_path);
  auto com_port = component::read<std::string>(yaml, "com_port");
  auto baudrate = component::read<int>(yaml, "baudrate");

  try {
    serial_.setPort(com_port);
    serial_.setBaudrate(baudrate);
    serial_.setTimeout(serial::Timeout::max(), 100, 0, 100, 0);
    serial_.open();
    component::logger()->info("[Gimbal] Serial port {} opened at {} baud", com_port, baudrate);
  } catch (const std::exception & e) {
    component::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
    exit(1);
  }

  thread_ = std::thread(&Gimbal::read_thread, this);

  queue_.pop();
  component::logger()->info("[Gimbal] First q received.");
}

Gimbal::~Gimbal()
{
  quit_ = true;
  if (thread_.joinable()) thread_.join();
  serial_.close();
}

GimbalMode Gimbal::mode() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return mode_;
}

GimbalState Gimbal::state() const
{
  std::lock_guard<std::mutex> lock(mutex_);
  return state_;
}

std::string Gimbal::str(GimbalMode mode) const
{
  switch (mode) {
    case GimbalMode::IDLE:
      return "IDLE";
    case GimbalMode::AUTO_AIM:
      return "AUTO_AIM";
    case GimbalMode::SMALL_BUFF:
      return "SMALL_BUFF";
    case GimbalMode::BIG_BUFF:
      return "BIG_BUFF";
    default:
      return "INVALID";
  }
}

Eigen::Quaterniond Gimbal::q(std::chrono::steady_clock::time_point t)
{
  while (true) {
    auto [q_a, t_a] = queue_.pop();
    auto [q_b, t_b] = queue_.front();
    auto t_ab = component::delta_time(t_a, t_b);
    auto t_ac = component::delta_time(t_a, t);
    auto k = t_ac / t_ab;
    Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
    if (t < t_a) return q_c;
    if (!(t_a < t && t <= t_b)) continue;

    return q_c;
  }
}

void Gimbal::send(device::VisionToGimbal VisionToGimbal)
{
  tx_data_.mode = VisionToGimbal.mode;
  tx_data_.yaw = VisionToGimbal.yaw;
  tx_data_.yaw_vel = VisionToGimbal.yaw_vel;
  tx_data_.yaw_acc = VisionToGimbal.yaw_acc;
  tx_data_.pitch = VisionToGimbal.pitch;
  tx_data_.pitch_vel = VisionToGimbal.pitch_vel;
  tx_data_.pitch_acc = VisionToGimbal.pitch_acc;
  tx_data_.crc16 = component::get_crc16(
    reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));

  try {
    serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
  } catch (const std::exception & e) {
    component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
  }
}

void Gimbal::send(
  bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
  float pitch_acc)
{
  tx_data_.mode = control ? (fire ? 2 : 1) : 0;
  tx_data_.yaw = yaw;
  tx_data_.yaw_vel = yaw_vel;
  tx_data_.yaw_acc = yaw_acc;
  tx_data_.pitch = pitch;
  tx_data_.pitch_vel = pitch_vel;
  tx_data_.pitch_acc = pitch_acc;
  tx_data_.crc16 = component::get_crc16(
    reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));

  try {
    serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
  } catch (const std::exception & e) {
    component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
  }
}

bool Gimbal::read(uint8_t * buffer, size_t size)
{
  try {
    return serial_.read(buffer, size) == size;
  } catch (const std::exception & e) {
    // component::logger()->warn("[Gimbal] Failed to read serial: {}", e.what());
    return false;
  }
}

void Gimbal::read_thread()
{
  component::logger()->info("[Gimbal] read_thread started.");
  int error_count = 0;
  uint8_t byte;
  int total_bytes_read = 0;
  int valid_packets = 0;

  while (!quit_) {
    if (error_count > 5000) {
      error_count = 0;
      component::logger()->warn("[Gimbal] Too many errors (read {} bytes, {} valid packets), attempting to reconnect...",
                                total_bytes_read, valid_packets);
      reconnect();
      continue;
    }

    // 逐字节查找包头第一个字节 'M'
    if (!read(&byte, 1)) {
      error_count++;
      continue;
    }

    // 读取成功，重置错误计数
    error_count = 0;
    total_bytes_read++;

    if (byte != 'M') continue;

    // 读取第二个字节检查是否为 'R'
    if (!read(&byte, 1)) {
      error_count++;
      continue;
    }

    total_bytes_read++;

    if (byte != 'R') {
      if (valid_packets < 3) {
        component::logger()->debug("[Gimbal] Found 'M' but next byte is 0x{:02X}, not 'R'", byte);
      }
      continue;
    }

    // 找到包头，记录时间戳
    rx_data_.head[0] = 'M';
    rx_data_.head[1] = 'R';
    auto t = std::chrono::steady_clock::now();

    // 读取剩余数据
    if (!read(
          reinterpret_cast<uint8_t *>(&rx_data_) + sizeof(rx_data_.head),
          sizeof(rx_data_) - sizeof(rx_data_.head))) {
      error_count++;
      component::logger()->warn("[Gimbal] Failed to read packet body");
      continue;
    }

    // 验证数据合理性
    if (rx_data_.mode > 3) {
      // mode 应该在 0-3 范围内
      if (valid_packets < 10) {
        component::logger()->warn("[Gimbal] Invalid mode {}, skipping packet (possible misalignment)", rx_data_.mode);
      }
      continue;
    }

    // 验证四元数范数是否接近1
    float q_norm = rx_data_.q[0] * rx_data_.q[0] +
                   rx_data_.q[1] * rx_data_.q[1] +
                   rx_data_.q[2] * rx_data_.q[2] +
                   rx_data_.q[3] * rx_data_.q[3];
    if (q_norm < 0.9f || q_norm > 1.1f) {
      if (valid_packets < 10) {
        component::logger()->warn("[Gimbal] Invalid quaternion norm {:.3f}, skipping packet", q_norm);
      }
      continue;
    }

    total_bytes_read += sizeof(rx_data_) - sizeof(rx_data_.head);
    valid_packets++;

    if (valid_packets <= 5) {
      component::logger()->info("[Gimbal] Packet #{}: mode={}, q=[{:.3f},{:.3f},{:.3f},{:.3f}], yaw={:.3f}",
                                valid_packets, (int)rx_data_.mode,
                                (float)rx_data_.q[0], (float)rx_data_.q[1], (float)rx_data_.q[2], (float)rx_data_.q[3],
                                (float)rx_data_.yaw);
    } else if (valid_packets % 100 == 0) {
      // 每100个包打印一次状态
      component::logger()->info("[Gimbal] Received {} packets, total {} bytes", valid_packets, total_bytes_read);
    }

    // if (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
    //   component::logger()->debug("[Gimbal] CRC16 check failed.");
    //   continue;
    // }

    error_count = 0;
    Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);
    queue_.push({q, t});

    std::lock_guard<std::mutex> lock(mutex_);

    state_.yaw = rx_data_.yaw;
    state_.yaw_vel = rx_data_.yaw_vel;
    state_.pitch = rx_data_.pitch;
    state_.pitch_vel = rx_data_.pitch_vel;
    state_.bullet_speed = rx_data_.bullet_speed;
    state_.bullet_count = rx_data_.bullet_count;

    switch (rx_data_.mode) {
      case 0:
        mode_ = GimbalMode::IDLE;
        break;
      case 1:
        mode_ = GimbalMode::AUTO_AIM;
        break;
      case 2:
        mode_ = GimbalMode::SMALL_BUFF;
        break;
      case 3:
        mode_ = GimbalMode::BIG_BUFF;
        break;
      default:
        mode_ = GimbalMode::IDLE;
        component::logger()->warn("[Gimbal] Invalid mode: {}", rx_data_.mode);
        break;
    }
  }

  component::logger()->info("[Gimbal] read_thread stopped.");
}

void Gimbal::reconnect()
{
  int max_retry_count = 10;
  for (int i = 0; i < max_retry_count && !quit_; ++i) {
    component::logger()->warn("[Gimbal] Reconnecting serial, attempt {}/{}...", i + 1, max_retry_count);
    try {
      serial_.close();
      std::this_thread::sleep_for(std::chrono::seconds(1));
    } catch (...) {
    }

    try {
      serial_.open();  // 尝试重新打开
      queue_.clear();
      component::logger()->info("[Gimbal] Reconnected serial successfully.");
      break;
    } catch (const std::exception & e) {
      component::logger()->warn("[Gimbal] Reconnect failed: {}", e.what());
      std::this_thread::sleep_for(std::chrono::seconds(1));
    }
  }
}

}  // namespace device