// leg_control_node.cpp
// 提供 LegControl.action 的简单示例 Action Server

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "interfaces/action/leg_control.hpp"
#include <cmath>

using interfaces::action::LegControl;

class LegControlServer : public rclcpp::Node {
public:
  using GoalHandleLeg = rclcpp_action::ServerGoalHandle<LegControl>;

  LegControlServer(): Node("leg_control_node") {
    using namespace std::placeholders;
    action_server_ = rclcpp_action::create_server<LegControl>(
      this,
      "leg_control",  // 话题/动作名称，与技能 snake_case 一致
      std::bind(&LegControlServer::handle_goal, this, _1, _2),
      std::bind(&LegControlServer::handle_cancel, this, _1),
      std::bind(&LegControlServer::handle_accepted, this, _1));
    RCLCPP_INFO(get_logger(), "LegControl action server ready on /leg_control");
  }

private:
  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,
                                          std::shared_ptr<const LegControl::Goal> goal) {
    (void)uuid;
    const auto & lin = goal->target.twist.linear;
    const auto & ang = goal->target.twist.angular;
    double lin_norm = std::sqrt(lin.x * lin.x + lin.y * lin.y + lin.z * lin.z);
    double ang_norm = std::sqrt(ang.x * ang.x + ang.y * ang.y + ang.z * ang.z);
    RCLCPP_INFO(get_logger(), "Received LegControl goal: frame=%s lin_norm=%.3f ang_norm=%.3f",
                goal->target.header.frame_id.c_str(), lin_norm, ang_norm);
    // 简单校验: 线速度和角速度不能同时为 0
    // if (lin_norm < 1e-6 && ang_norm < 1e-6) {
    //   RCLCPP_WARN(get_logger(), "Reject goal: zero twist");
    //   return rclcpp_action::GoalResponse::REJECT;
    // }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleLeg> gh) {
    RCLCPP_INFO(get_logger(), "Cancel request received");
    (void)gh;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handle_accepted(const std::shared_ptr<GoalHandleLeg> gh) {
    // 在新的线程里执行以免阻塞 executor
    std::thread{std::bind(&LegControlServer::execute, this, gh)}.detach();
  }

  void execute(const std::shared_ptr<GoalHandleLeg> gh) {
  const auto goal = gh->get_goal();
    auto feedback = std::make_shared<LegControl::Feedback>();
    auto result = std::make_shared<LegControl::Result>();

  const auto & lin = goal->target.twist.linear;
  const auto & ang = goal->target.twist.angular;
  RCLCPP_INFO(get_logger(), "Executing LegControl: lin(%.2f,%.2f,%.2f) ang(%.2f,%.2f,%.2f)",
        lin.x, lin.y, lin.z, ang.x, ang.y, ang.z);

    const int steps = 10;
    for (int i = 1; rclcpp::ok() && i <= steps; ++i) {
      if (gh->is_canceling()) {
        result->success = false;
        result->message = "LegControl canceled";
        gh->canceled(result);
        RCLCPP_INFO(get_logger(), "LegControl goal canceled");
        return;
      }
      feedback->progress = static_cast<float>(i) / static_cast<float>(steps);
      gh->publish_feedback(feedback);
      RCLCPP_DEBUG(get_logger(), "Progress: %.2f", feedback->progress);
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    if (rclcpp::ok()) {
      result->success = true;
      result->message = "LegControl done";
      gh->succeed(result);
      RCLCPP_INFO(get_logger(), "LegControl goal succeeded");
    }
  }

  rclcpp_action::Server<LegControl>::SharedPtr action_server_;
};


// 测试：
// ros2 action send_goal /LegControl interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}}}'


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