#include "rclcpp/rclcpp.hpp"
#include "mc/get_config.h"
#include "mc/can_driver.h"
#include "mc/can_utils.hpp"
#include "mc/control_cache.hpp"
#include "mc/timer_tasks.hpp"
#include "sweeper_interfaces/msg/can_frame.hpp"
#include "sweeper_interfaces/msg/mc_ctrl.hpp"

#include <iostream>

namespace sweeperMsg = sweeper_interfaces::msg;

CANDriver canctl;

void mcCtrlCallback(const sweeperMsg::McCtrl::SharedPtr msg)
{
  // std::cout << "\n  刹车: " << (msg->brake ? "已刹车" : "未刹车")
  //           << "\n  挡位: ";
  // switch (msg->gear)
  // {
  // case 0:
  //   std::cout << "空挡";
  //   break;
  // case 2:
  //   std::cout << "前进挡";
  //   break;
  // case 1:
  //   std::cout << "后退挡";
  //   break;
  // default:
  //   std::cout << "未知挡位(" << static_cast<int>(msg->gear) << ")";
  //   break;
  // }
  // std::cout << "\n  行走电机转速: " << static_cast<int>(msg->rpm) << " RPM"
  //           << "\n  轮端转向角度: " << msg->angle << "°"
  //           << "\n  清扫状态: " << (msg->sweep ? "正在清扫" : "未清扫")
  //           << std::endl;

  control_cache.update(*msg);
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("can_driver_node");
  RCLCPP_INFO(node->get_logger(), "Starting mc package...");

  auto pub = node->create_publisher<sweeperMsg::CanFrame>("can_data", 10);
  auto sub = node->create_subscription<sweeperMsg::McCtrl>("mc_ctrl", 10, mcCtrlCallback);

  Config mc_config;
  load_config(mc_config);

  if (!canctl.open(mc_config.can_dev))
  {
    RCLCPP_ERROR(node->get_logger(), "Failed to open CAN interface: %s", mc_config.can_dev.c_str());
    return -1;
  }

  auto context = std::make_shared<CanHandlerContext>();
  context->node = node;
  context->publisher = pub;
  canctl.setReceiveCallback(receiveHandler, context.get());

  setupTimers(node, canctl);

  rclcpp::on_shutdown([&]()
                      { canctl.close(); });
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
