// Created by Chengfu Zou on 2023.7.1
// Copyright (C) FYT Vision Group. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "rm_serial_driver/serial_driver_node.hpp"

#include <tf2/LinearMath/Matrix3x3.h>
// std
#include <chrono>
#include <cmath>
#include <cstdint>
#include <memory>
#include <thread>
// ros2
#include <Eigen/Geometry>
#include <rclcpp/rclcpp.hpp>
// project
#include "rm_serial_driver/uart_transporter.hpp"
#include "rm_utils/logger/log.hpp"
#include "rm_utils/math/utils.hpp"

namespace fyt::serial_driver {
SerialDriverNode::SerialDriverNode(const rclcpp::NodeOptions &options)
: Node("serial_driver", options) {
  FYT_REGISTER_LOGGER("serial_driver", "~/fyt2024-log", INFO);

  // Task thread
  listen_thread_ = std::make_unique<std::thread>(&SerialDriverNode::listenLoop, this);
}

void SerialDriverNode::init() {
  FYT_INFO("serial_driver", "Initializing SerialDriverNode!");
  // Init
  target_frame_ = this->declare_parameter("target_frame", "odom");
  std::string port_name = this->declare_parameter("port_name", "/dev/ttyUSB0");
  std::string protocol_type = this->declare_parameter("protocol", "infantry");
  bool enable_data_print = this->declare_parameter("enable_data_print", false);
  bool enable_send_print = this->declare_parameter("enable_send_print", false);
  // Create Protocol
  protocol_ =
    ProtocolFactory::createProtocol(protocol_type, port_name, enable_data_print, enable_send_print);
  if (protocol_ == nullptr) {
    FYT_FATAL("serial_driver", "Failed to create protocol with type: {}", protocol_type);
    rclcpp::shutdown();
    return;
  }
  FYT_INFO(
    "serial_driver", "Protocol has been created with type: {}, port: {}", protocol_type, port_name);

  // Subscriptions
  subscriptions_ = protocol_->getSubscriptions(this->shared_from_this());
  for (auto sub : subscriptions_) {
    FYT_INFO("serial_driver", "Subscribe to topic: {}", sub->get_topic_name());
  }
  // Publisher
  serial_receive_data_pub_ = this->create_publisher<rm_interfaces::msg::SerialReceiveData>(
    "serial/receive", rclcpp::SensorDataQoS());

  // TF broadcaster
  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
  
  // yaw轴到pitch轴的偏移量 [x, y, z] (单位: 米)
  // x: 前后方向 (正值表示pitch轴在yaw轴前方)
  // y: 左右方向 (正值表示pitch轴在yaw轴左方)
  // z: 上下方向 (正值表示pitch轴在yaw轴上方)
  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);
  protocol_->setRxPacketLength(use_yaw_to_pitch_angle_ ? 20 : 16);

  // Param client
  for (auto client : protocol_->getClients(this->shared_from_this())) {
    std::string name = client->get_service_name();
    set_mode_clients_.emplace(name, client);
    FYT_INFO("serial_driver", "Create client for service: {}", name);
  }

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

  FYT_INFO("serial_driver", "SerialDriverNode has been initialized!");
}

SerialDriverNode::~SerialDriverNode() {
  FYT_INFO("serial_driver", "Destroy SerialDriverNode!");
  rclcpp::shutdown();
  if (listen_thread_ != nullptr) {
    listen_thread_->join();
  }
}

void SerialDriverNode::listenLoop() {
  if (protocol_ == nullptr) {
    // Lazy init because shared_from_this() is not available in constructor
    init();
  }

  rm_interfaces::msg::SerialReceiveData receive_data;
  while (rclcpp::ok()) {
    if (protocol_->receive(receive_data)) {
      receive_data.header.stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
      receive_data.header.frame_id = target_frame_;
      serial_receive_data_pub_->publish(receive_data);

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

      geometry_msgs::msg::TransformStamped t;
      timestamp_offset_ = this->get_parameter("timestamp_offset").as_double();
      auto stamp = this->now() + rclcpp::Duration::from_seconds(timestamp_offset_);
      
      auto pitch = -receive_data.pitch * M_PI / 180.0;
      auto yaw = receive_data.yaw * M_PI / 180.0;
      
      // 发布 odom -> yaw_link 变换 (只有yaw旋转)
      t.header.stamp = stamp;
      t.header.frame_id = target_frame_;
      t.child_frame_id = "yaw_link";
      tf2::Quaternion q_yaw;
      q_yaw.setRPY(0, 0, yaw);
      t.transform.rotation = tf2::toMsg(q_yaw);
      t.transform.translation.x = 0;
      t.transform.translation.y = 0;
      t.transform.translation.z = 0;
      tf_broadcaster_->sendTransform(t);
      
      // 发布 yaw_link -> gimbal_link 变换 (只有pitch旋转 + yaw-pitch轴偏移)
      // yaw_to_pitch_offset: yaw轴到pitch轴的偏移量 [x, y, z]
      // 需要在参数文件中配置: yaw_to_pitch_offset: [0.0, 0.0, 0.05]
      t.header.stamp = stamp;
      t.header.frame_id = "yaw_link";
      t.child_frame_id = "pitch_link";
      tf2::Quaternion q_pitch;
      q_pitch.setRPY(0, pitch, 0);  // 只有pitch
      t.transform.rotation = tf2::toMsg(q_pitch);
      // 从参数获取yaw到pitch轴的偏移
      if (use_yaw_to_pitch_angle_ && yaw_to_pitch_radius_ > 0.0 &&
          std::isfinite(receive_data.yaw_to_pitch_angle)) {
        double yaw_to_pitch_angle = receive_data.yaw_to_pitch_angle * M_PI / 180.0;
        t.transform.translation.x = -yaw_to_pitch_radius_ * std::sin(yaw_to_pitch_angle);
        t.transform.translation.y = yaw_to_pitch_offset_[1];
        t.transform.translation.z = yaw_to_pitch_radius_ * std::cos(yaw_to_pitch_angle);
      } else {
        // Fallback to fixed offset when angle data is unavailable.
        t.transform.translation.x = yaw_to_pitch_offset_[0];
        t.transform.translation.y = yaw_to_pitch_offset_[1];
        t.transform.translation.z = yaw_to_pitch_offset_[2];
      }
      tf_broadcaster_->sendTransform(t);
    } else {
      auto error_message = protocol_->getErrorMessage();
      error_message = error_message.empty() ? "unknown" : error_message;
      FYT_WARN("serial_driver", "Failed to reveive packet! error message :{}", error_message);
      std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
  }
}

void SerialDriverNode::setMode(SetModeClient &client, const uint8_t mode) {
  using namespace std::chrono_literals;

  std::string service_name = client.ptr->get_service_name();
  // Wait for service
  while (!client.ptr->wait_for_service(1s)) {
    if (!rclcpp::ok()) {
      FYT_ERROR(
        "serial_driver", "Interrupted while waiting for the service {}. Exiting.", service_name);
      return;
    }
    FYT_INFO("serial_driver", "Service {} not available, waiting again...", service_name);
  }
  if (!client.ptr->service_is_ready()) {
    FYT_WARN("serial_driver", "Service: {} is not available!", service_name);
    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);
      }
    });
}

}  // 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::SerialDriverNode)
