#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 << "Sub ControlMsg:"
            << "\n  电磁刹: " << static_cast<int>(msg->brake)
            << "\n  挡位: " << static_cast<int>(msg->gear)
            << "\n  转速: " << static_cast<int>(msg->rpm)
            << "\n  轮端转向角度: " << msg->angle
            << "\n  转向角速度: " << msg->angle_speed
            << "\n  滚刷&吸风电机方向: " << static_cast<int>(msg->roll_brush_suction_direction)
            << "\n  滚刷&吸风电机: " << static_cast<int>(msg->roll_brush_suction)
            << "\n  边刷电机: " << static_cast<int>(msg->side_brush)
            << "\n  振尘电机: " << static_cast<int>(msg->dust_shake)
            << "\n  刷盘推杆电机: " << static_cast<int>(msg->brush_deck_pusher)
            << "\n  吸扒推杆电机: " << static_cast<int>(msg->suction_squeeqee_pusher)
            << "\n  喷水电机: " << static_cast<int>(msg->water_spray)
            << 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;
}
