// Created by Chengfu Zou
// Copyright (C) FYT Vision Group. All rights reserved.

// std
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <future>
#include <memory>
#include <opencv2/calib3d.hpp>
#include <rclcpp/executors.hpp>
#include <thread>
// ros2
#include <tf2_ros/transform_broadcaster.h>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node_options.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// project
#include "rm_interfaces/msg/serial_receive_data.hpp"
#include "rm_interfaces/srv/set_mode.hpp"
#include "rm_utils/heartbeat.hpp"
#include "rm_utils/logger/log.hpp"
#include "rm_utils/math/utils.hpp"

namespace fyt::serial_driver {
class VirtualSerialNode : public rclcpp::Node {
  struct SetModeClient {
    SetModeClient(rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr p) : ptr(p) {}
    std::atomic<bool> on_waiting = false;
    std::atomic<int> mode = 0;
    rclcpp::Client<rm_interfaces::srv::SetMode>::SharedPtr ptr;
  };

public:
  explicit VirtualSerialNode(const rclcpp::NodeOptions &options) : Node("serial_driver", options) {
    FYT_REGISTER_LOGGER("serial_driver", "~/fyt2024-log", INFO);
    FYT_INFO("serial_driver", "Starting VirtualSerialNode!");

    // 设置关闭标志
    is_running_.store(true);

    serial_receive_data_pub_ =
      this->create_publisher<rm_interfaces::msg::SerialReceiveData>("serial/receive", 10);

    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    this->declare_parameter("vision_mode", static_cast<int>(0));

    has_rune_ = this->declare_parameter("has_rune", true);

    serial_receive_data_msg_.header.frame_id = "odom";
    serial_receive_data_msg_.bullet_speed = this->declare_parameter("bullet_speed", 25.0);
    transform_stamped_.header.frame_id = "odom";
    transform_stamped_.child_frame_id = "yaw_link";
    serial_receive_data_msg_.mode = 0;
    serial_receive_data_msg_.roll = this->declare_parameter("roll", 0.0);
    serial_receive_data_msg_.pitch = this->declare_parameter("pitch", 0.0);
    serial_receive_data_msg_.yaw = this->declare_parameter("yaw", 0.0);
    serial_receive_data_msg_.yaw_to_pitch_angle =
      this->declare_parameter("yaw_to_pitch_angle", 0.0);

    // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
    yaw_to_pitch_offset_ =
      this->declare_parameter<std::vector<double>>("yaw_to_pitch_offset", {0.0, 0.0, 0.0});
    yaw_to_pitch_radius_ = this->declare_parameter(
      "yaw_to_pitch_radius", std::hypot(yaw_to_pitch_offset_[0], yaw_to_pitch_offset_[2]));
    use_yaw_to_pitch_angle_ = this->declare_parameter("use_yaw_to_pitch_angle", true);

    // Heartbeat
    heartbeat_ = HeartBeatPublisher::create(this);

    // Param client
    auto autoaim_set_mode_client_1 =
      this->create_client<rm_interfaces::srv::SetMode>("armor_detector/set_mode");
    set_mode_clients_.emplace(autoaim_set_mode_client_1->get_service_name(),
                              autoaim_set_mode_client_1);
    auto autoaim_set_mode_client_2 =
      this->create_client<rm_interfaces::srv::SetMode>("armor_solver/set_mode");
    set_mode_clients_.emplace(autoaim_set_mode_client_2->get_service_name(),
                              autoaim_set_mode_client_2);
    if (has_rune_) {
      auto client1 = this->create_client<rm_interfaces::srv::SetMode>("rune_detector/set_mode");
      set_mode_clients_.emplace(client1->get_service_name(), client1);
      auto client2 = this->create_client<rm_interfaces::srv::SetMode>("rune_solver/set_mode");
      set_mode_clients_.emplace(client2->get_service_name(), client2);
    }

    timer_ = this->create_wall_timer(std::chrono::milliseconds(1), [this]() {
      serial_receive_data_msg_.header.stamp = this->now();
      int mode = this->get_parameter("vision_mode").as_int();
      [[maybe_unused]] double roll = this->get_parameter("roll").as_double();
      double pitch = this->get_parameter("pitch").as_double();
      double yaw = this->get_parameter("yaw").as_double();
      double yaw_to_pitch_angle = this->get_parameter("yaw_to_pitch_angle").as_double();
      double bullet_speed = this->get_parameter("bullet_speed").as_double();
      serial_receive_data_msg_.mode = mode;
      serial_receive_data_msg_.pitch = pitch;
      serial_receive_data_msg_.yaw = yaw;
      serial_receive_data_msg_.yaw_to_pitch_angle = yaw_to_pitch_angle;
      serial_receive_data_msg_.bullet_speed = bullet_speed;

      auto stamp = this->now();

      // 发布 odom -> yaw_link 变换 (只有yaw旋转)
      tf2::Quaternion q_yaw;
      q_yaw.setRPY(0, 0, yaw * M_PI / 180.0);
      transform_stamped_.transform.rotation = tf2::toMsg(q_yaw);
      transform_stamped_.transform.translation.x = 0;
      transform_stamped_.transform.translation.y = 0;
      transform_stamped_.transform.translation.z = 0;
      transform_stamped_.header.frame_id = "odom";
      transform_stamped_.child_frame_id = "yaw_link";
      transform_stamped_.header.stamp = stamp;
      tf_broadcaster_->sendTransform(transform_stamped_);

      // 发布 yaw_link -> gimbal_link 变换 (只有pitch旋转 + yaw-pitch轴偏移)
      tf2::Quaternion q_pitch;
      q_pitch.setRPY(0, -pitch * M_PI / 180.0, 0);  // 只有pitch
      transform_stamped_.transform.rotation = tf2::toMsg(q_pitch);
      if (use_yaw_to_pitch_angle_ && yaw_to_pitch_radius_ > 0.0) {
        double yaw_to_pitch_angle_rad = yaw_to_pitch_angle * M_PI / 180.0;
        transform_stamped_.transform.translation.x =
          -yaw_to_pitch_radius_ * std::sin(yaw_to_pitch_angle_rad);
        transform_stamped_.transform.translation.y = yaw_to_pitch_offset_[1];
        transform_stamped_.transform.translation.z =
          yaw_to_pitch_radius_ * std::cos(yaw_to_pitch_angle_rad);
      } else {
        transform_stamped_.transform.translation.x = yaw_to_pitch_offset_[0];
        transform_stamped_.transform.translation.y = yaw_to_pitch_offset_[1];
        transform_stamped_.transform.translation.z = yaw_to_pitch_offset_[2];
      }
      transform_stamped_.header.frame_id = "yaw_link";
      transform_stamped_.child_frame_id = "pitch_link";
      transform_stamped_.header.stamp = stamp;
      tf_broadcaster_->sendTransform(transform_stamped_);

      // serial_receive_data_msg.mode = mode;
      serial_receive_data_pub_->publish(serial_receive_data_msg_);

      for (auto &[service_name, client] : set_mode_clients_) {
        if (client.mode.load() != mode && !client.on_waiting.load()) {
          setMode(client, mode);
        }
      }
    });
  }

  ~VirtualSerialNode() {
    // 设置关闭标志，让所有等待循环退出
    is_running_.store(false);
    // 取消定时器
    if (timer_) {
      timer_->cancel();
    }
    FYT_INFO("serial_driver", "VirtualSerialNode destroyed.");
  }

  void setMode(SetModeClient &client, const uint8_t mode) {
    std::string service_name = client.ptr->get_service_name();

    // Check if service is ready without blocking
    if (!client.ptr->service_is_ready()) {
      return;
    }

    // Send request
    auto req = std::make_shared<rm_interfaces::srv::SetMode::Request>();
    req->mode = mode;
    client.on_waiting.store(true);
    auto result = client.ptr->async_send_request(
      req, [mode, &client](rclcpp::Client<rm_interfaces::srv::SetMode>::SharedFuture result) {
        client.on_waiting.store(false);
        if (result.get()->success) {
          client.mode.store(mode);
        }
      });
  }

private:
  HeartBeatPublisher::SharedPtr heartbeat_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::Publisher<rm_interfaces::msg::SerialReceiveData>::SharedPtr serial_receive_data_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  rm_interfaces::msg::SerialReceiveData serial_receive_data_msg_;
  geometry_msgs::msg::TransformStamped transform_stamped_;

  bool has_rune_;

  // 运行状态标志，用于优雅关闭
  std::atomic<bool> is_running_{true};

  // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
  std::vector<double> yaw_to_pitch_offset_ = {0.0, 0.0, 0.0};
  double yaw_to_pitch_radius_ = 0.0;
  bool use_yaw_to_pitch_angle_ = true;

  std::unordered_map<std::string, SetModeClient> set_mode_clients_;
};
}  // namespace fyt::serial_driver

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(fyt::serial_driver::VirtualSerialNode)

