// Copyright (c) 2025
// 简单的运动模块：提供 ArmSpaceControl 动作(Action)服务端。
// Simple motion module that exposes an ArmSpaceControl action server.

#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

// #include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/arm.hpp"

using namespace std::chrono_literals;

// MotionNode 节点：
// - 启动一个 ArmSpaceControl 动作服务端（默认话题名称：/arm_space_control，可通过参数 action_name 修改）
// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
class MotionNode : public rclcpp::Node
{
public:
  // using ArmSpaceControl = interfaces::action::ArmSpaceControl;
  using Arm = interfaces::action::Arm;
  // using GoalHandleArmSpaceControl = rclcpp_action::ServerGoalHandle<ArmSpaceControl>;
  using GoalHandleArm = rclcpp_action::ServerGoalHandle<Arm>;

  // 构造函数：
  // - 声明参数 action_name（默认 "Arm"，统一使用首字母大写形式避免重复）
  // - 创建 Arm 动作服务端，并绑定目标处理、取消处理和接收回调
  MotionNode()
  : Node("motion_node")
  {
  // Declare a parameter for action name (default: "Arm")
  action_name_ = this->declare_parameter<std::string>("action_name", "ArmAction");

    using std::placeholders::_1;
    using std::placeholders::_2;

    server_ = rclcpp_action::create_server<Arm>(
      this->get_node_base_interface(),
      this->get_node_clock_interface(),
      this->get_node_logging_interface(),
      this->get_node_waitables_interface(),
      action_name_,
      std::bind(&MotionNode::handle_goal, this, _1, _2),
      std::bind(&MotionNode::handle_cancel, this, _1),
      std::bind(&MotionNode::handle_accepted, this, _1));

    RCLCPP_INFO(
      this->get_logger(), "Arm action server started on '%s'",
      action_name_.c_str());
  }

private:
  // 目标处理回调：
  // - 参数 uuid：目标的唯一标识
  // - 参数 goal：ArmSpaceControl 的目标数据（target pose）
  // - 返回值：是否接受目标，这里直接接受并执行
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Arm::Goal> goal)
  {
    (void)uuid;
    RCLCPP_INFO(
      this->get_logger(), "Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.1f, %.1f, %.1f]",
      goal->body_id, goal->data_type, goal->data_length, goal->command_id, goal->frame_time_stamp,
      goal->data_array[0], goal->data_array[1], goal->data_array[2]);
    // Accept all goals for now. You can validate mode/effort here.
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  // 取消处理回调：
  // - 当客户端请求取消时被调用，这里统一接受取消请求
  rclcpp_action::CancelResponse handle_cancel(
    const std::shared_ptr<GoalHandleArm>/*goal_handle*/)
  {
    RCLCPP_INFO(this->get_logger(), "Received request to cancel Arm goal");
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  // 接受目标回调：
  // - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
  void handle_accepted(const std::shared_ptr<GoalHandleArm> goal_handle)
  {
    // Execute goal in a new thread to avoid blocking executor thread
    std::thread{std::bind(&MotionNode::execute, this, std::placeholders::_1), goal_handle}.detach();
  }

  // 执行函数：
  // - 模拟执行流程，按 10% 递增发布反馈(progress)
  // - 如果检测到取消请求，则返回取消结果
  // - 正常完成后返回 success=true，并附带简单描述信息
  void execute(const std::shared_ptr<GoalHandleArm> goal_handle)
  {
    const auto goal = goal_handle->get_goal();

    auto feedback = std::make_shared<Arm::Feedback>();
    auto result = std::make_shared<Arm::Result>();

    // Simulate progress from 0 to 100 with small delay; react to cancel requests
    for (int i = 0; i <= 100; i += 10) {
      if (goal_handle->is_canceling()) {
        result->result = 1;
        // result->message = "Arm canceled";
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Arm goal canceled");
        return;
      }
      // feedback->progress = static_cast<float>(i);
      feedback->command_id = goal->command_id;
      feedback->int_lenth = i;
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Arm progress: %d", i);
      std::this_thread::sleep_for(100ms);
    }

    // Complete successfully
    result->result = 0;
    result->command_id = goal->command_id;
    // result->message = std::string("Arm moved to target in frame '") +
    //   goal->target.header.frame_id + "'";
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Arm goal succeeded: %s", "arm");
  }

private:
  std::string action_name_;
  rclcpp_action::Server<Arm>::SharedPtr server_;
};

// 主函数：
// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
// 测试：
// ros2 action send_goal --feedback /Arm interfaces/action/Arm "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<MotionNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
