#include "can_driver/can_analy.hpp"
#include <sstream>
#include <iomanip>

using namespace std::chrono_literals;

CanAnalyNode::CanAnalyNode()
: Node("can_analy")
{
  sub_ = this->create_subscription<can_msgs::msg::Frame>(
    "can_msg", 100,
    std::bind(&CanAnalyNode::canCallback, this, std::placeholders::_1)
  );

  pub_ = this->create_publisher<can_driver::msg::CanMine>("can_msg2mine", 10);

  RCLCPP_INFO(this->get_logger(), "can_analy node started, subscribing to /can_msg");
}

void CanAnalyNode::canCallback(const can_msgs::msg::Frame::SharedPtr msg)
{
  // 仅处理 ID 0x32 (十六进制)
  if (msg->id != 0x32) return;

  if (msg->dlc < 8) return;  // 帧长度不足

  // 解析数据
  uint16_t speed_raw = static_cast<uint16_t>(msg->data[0] | (msg->data[1] << 8));
  float speed_rpm = speed_raw * 0.1f;  // 单位 0.1RPM

  uint8_t state = msg->data[2];
  uint8_t water_status = msg->data[3];

  uint16_t fault_word_1 = static_cast<uint16_t>(msg->data[4] | (msg->data[5] << 8));
  uint16_t fault_word_2 = static_cast<uint16_t>(msg->data[6] | (msg->data[7] << 8));

  std::string fault_text = parseFaultWord(fault_word_1, fault_word_2);

  // 发布消息
  can_driver::msg::CanMine out_msg;
  out_msg.header.stamp = this->now();
  out_msg.speed_rpm = speed_rpm;
  out_msg.state = state;
  out_msg.water_status = water_status;
  out_msg.fault_word_1 = fault_word_1;
  out_msg.fault_word_2 = fault_word_2;
  out_msg.fault_text = fault_text;

  pub_->publish(out_msg);

  // 节流打印：每秒一次
  if ((this->now() - last_print_time_).seconds() >= 1.0) {
    last_print_time_ = this->now();

    RCLCPP_INFO(this->get_logger(),
      "解析结果 -> 转速: %.1f RPM | 状态: %u | 汲水: %u | 故障1: 0x%04X | 故障2: 0x%04X",
      speed_rpm, state, water_status, fault_word_1, fault_word_2);

    if (!fault_text.empty()) {
      RCLCPP_INFO(this->get_logger(), "故障详情: %s", fault_text.c_str());
    }
  }
}

std::string CanAnalyNode::parseFaultWord(uint16_t f1, uint16_t f2)
{
  std::ostringstream oss;
  // 故障字1
  if (f1 != 0) {
    oss << "[Fault1]";
    if (f1 & (1 << 0))  oss << " Motor1高温;";
    if (f1 & (1 << 1))  oss << " Motor1低温;";
    if (f1 & (1 << 2))  oss << " Motor1失效;";
    if (f1 & (1 << 3))  oss << " Motor2高温;";
    if (f1 & (1 << 4))  oss << " Motor2低温;";
    if (f1 & (1 << 5))  oss << " Motor2失效;";
    if (f1 & (1 << 6))  oss << " Tboard高温;";
    if (f1 & (1 << 7))  oss << " Tboard低温;";
    if (f1 & (1 << 8))  oss << " Tboard失效;";
    if (f1 & (1 << 9))  oss << " 进水;";
    if (f1 & (1 << 10)) oss << " 进水信号失效;";
    if (f1 & (1 << 11)) oss << " 螺旋桨出水;";
    if (f1 & (1 << 12)) oss << " 母线过压;";
    if (f1 & (1 << 13)) oss << " 母线欠压;";
    if (f1 & (1 << 14)) oss << " 母线掉线;";
    if (f1 & (1 << 15)) oss << " 瞬时过流;";
  }

  // 故障字2
  if (f2 != 0) {
    oss << " [Fault2]";
    if (f2 & (1 << 0))  oss << " 相电流瞬时过流;";
    if (f2 & (1 << 1))  oss << " 有效值持续过流;";
    if (f2 & (1 << 2))  oss << " 相电流持续过流;";
    if (f2 & (1 << 3))  oss << " 零漂过大;";
    if (f2 & (1 << 4))  oss << " 超速;";
    if (f2 & (1 << 5))  oss << " 转速波动;";
    if (f2 & (1 << 6))  oss << " CAN通讯故障;";
    if (f2 & (1 << 7))  oss << " CAN校验错误;";
    if (f2 & (1 << 8))  oss << " 堵转;";
  }

  return oss.str();
}
