#include "can_driver/can_bridge.hpp"

#include <std_msgs/msg/header.hpp>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <net/if.h>
#include <unistd.h>
#include <cstring>
#include <iomanip>
#include <sstream>
#include <stdexcept>

CanBridgeNode::CanBridgeNode()
: rclcpp::Node("can_bridge")
{
  // 参数：默认 can0
  this->declare_parameter<std::string>("interface", "can0");
  this->get_parameter("interface", iface_);

  // 发布者：/can_msg
  pub_ = this->create_publisher<can_msgs::msg::Frame>("can_msg",
          rclcpp::QoS(100).reliable());

  // 打开 SocketCAN
  sock_ = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
  if (sock_ < 0) {
    RCLCPP_FATAL(this->get_logger(), "socket() failed: %s", std::strerror(errno));
    throw std::runtime_error("socket failed");
  }

  struct ifreq ifr {};
  std::snprintf(ifr.ifr_name, IFNAMSIZ, "%s", iface_.c_str());
  if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) {
    RCLCPP_FATAL(this->get_logger(), "ioctl(SIOCGIFINDEX) failed for %s: %s",
                 iface_.c_str(), std::strerror(errno));
    ::close(sock_);
    throw std::runtime_error("ioctl failed");
  }

  struct sockaddr_can addr {};
  addr.can_family  = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex;

  if (bind(sock_, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0) {
    RCLCPP_FATAL(this->get_logger(), "bind() failed: %s", std::strerror(errno));
    ::close(sock_);
    throw std::runtime_error("bind failed");
  }

  // ✅ 启动成功提示（仅一次）
  RCLCPP_INFO(this->get_logger(), "CAN interface '%s' bound successfully. Publishing to /can_msg", iface_.c_str());

  // 接收线程
  rx_thread_ = std::thread([this]() { this->rxLoop(); });
}

CanBridgeNode::~CanBridgeNode() {
  stop_ = true;
  if (sock_ >= 0) {
    ::shutdown(sock_, SHUT_RD);
  }
  if (rx_thread_.joinable()) {
    rx_thread_.join();
  }
  if (sock_ >= 0) {
    ::close(sock_);
    sock_ = -1;
  }
}

void CanBridgeNode::rxLoop() {
  while (!stop_) {
    struct can_frame frame {};
    ssize_t nbytes = ::recv(sock_, &frame, sizeof(frame), 0);
    if (nbytes < 0) {
      if (errno == EINTR) continue;
      if (stop_) break;
      continue;  // 不再打印错误日志
    }
    if (static_cast<size_t>(nbytes) < sizeof(struct can_frame)) {
      continue;  // 不打印短帧
    }

    can_msgs::msg::Frame msg;
    msg.header.stamp = this->now();

    const bool is_extended = (frame.can_id & CAN_EFF_FLAG) != 0;
    const bool is_rtr      = (frame.can_id & CAN_RTR_FLAG) != 0;
    const bool is_error    = (frame.can_id & CAN_ERR_FLAG) != 0;

    if (is_extended) {
      msg.id = frame.can_id & CAN_EFF_MASK;
    } else {
      msg.id = frame.can_id & CAN_SFF_MASK;
    }

    msg.is_extended = is_extended;
    msg.is_rtr      = is_rtr;
    msg.is_error    = is_error;
    msg.dlc         = frame.can_dlc;

    for (uint8_t i = 0; i < msg.dlc && i < 8; ++i) {
      msg.data[i] = frame.data[i];
    }

    // ✅ 仅发布消息，不打印帧内容
    pub_->publish(msg);
  }
}
