#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rover_interfaces/action/move_arm.hpp"

class MoveArmClient : public rclcpp::Node {
public:
  using MoveArm = rover_interfaces::action::MoveArm;

  MoveArmClient() : Node("move_arm_client") {

    // Create a client that connects to the "move_arm" action server
    // The action type must match the server exactly
    client_ = rclcpp_action::create_client<MoveArm>(this, "move_arm");

    // Build the goal — target_positions maps to the Goal field in our .action file
    // These six values represent the target angle for each of the arm's 6 joints
    auto goal = MoveArm::Goal();
    goal.target_positions = {0.0, 1.57, 0.0, 0.0, 0.0, 0.0};

    // SendGoalOptions bundles the three callbacks the client uses to track the goal
    auto send_goal_options = rclcpp_action::Client<MoveArm>::SendGoalOptions();

    // Feedback callback — fires repeatedly while the server is executing
    // Use this to track progress, update a UI, or trigger safety checks
    send_goal_options.feedback_callback =
      [this](auto, const std::shared_ptr<const MoveArm::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "Progress: %.0f%%", feedback->progress * 100);
      };

    // Result callback — fires once when the server finishes (success, failure, or cancel)
    // This is where you handle what happens after the arm reaches its target
    // or fails to do so
    send_goal_options.result_callback =
      [this](const rclcpp_action::ClientGoalHandle<MoveArm>::WrappedResult & result) {
        RCLCPP_INFO(this->get_logger(), "Result: %s", result.result->message.c_str());
      };

    // Send the goal asynchronously — doesn't block, callbacks fire when relevant
    client_->async_send_goal(goal, send_goal_options);
  }

private:
  rclcpp_action::Client<MoveArm>::SharedPtr client_;
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MoveArmClient>());
  rclcpp::shutdown();
  return 0;
}

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"        // ROS2 action library, separate from core rclcpp
#include "rover_interfaces/action/move_arm.hpp"   // Our custom action type defined in the .action file

class MoveArmServer : public rclcpp::Node {
public:
  // Type aliases to avoid writing the full type every time
  // MoveArm refers to our custom action type
  // GoalHandle represents an active goal being executed — used to send feedback and results
  using MoveArm = rover_interfaces::action::MoveArm;
  using GoalHandle = rclcpp_action::ServerGoalHandle<MoveArm>;

  MoveArmServer() : Node("move_arm_server") {

    // Create the action server with three callbacks:
    // 1. Goal callback    — decides whether to accept or reject an incoming goal
    // 2. Cancel callback  — decides whether to accept a cancellation request
    // 3. Accepted callback — called when a goal is accepted, starts execution
    action_server_ = rclcpp_action::create_server<MoveArm>(
      this, "move_arm",

      // 1. Goal callback
      // Called when a client sends a new goal
      // Here we always accept, but you could reject based on current state
      // e.g. reject if the arm is already moving or in an error state
      [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const MoveArm::Goal>) {
        RCLCPP_INFO(this->get_logger(), "Received goal");
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
      },

      // 2. Cancel callback
      // Called when a client requests cancellation of an active goal
      // Here we always accept cancellation
      [this](const std::shared_ptr<GoalHandle>) {
        RCLCPP_INFO(this->get_logger(), "Goal cancelled");
        return rclcpp_action::CancelResponse::ACCEPT;
      },

      // 3. Accepted callback
      // Called once a goal is accepted — we spin up a separate thread to execute it
      // This is important: execute() can be long running and we don't want to block
      // the node's main thread which needs to stay free to handle other callbacks
      [this](const std::shared_ptr<GoalHandle> goal_handle) {
        std::thread{[this, goal_handle]() { execute(goal_handle); }}.detach();
      });
  }

private:
  rclcpp_action::Server<MoveArm>::SharedPtr action_server_;

  // The actual execution logic runs here in its own thread
  void execute(const std::shared_ptr<GoalHandle> goal_handle) {

    // Create the feedback and result objects we'll send back to the client
    auto feedback = std::make_shared<MoveArm::Feedback>();
    auto result = std::make_shared<MoveArm::Result>();

    // Simulate the arm moving in 10 steps
    for (int i = 0; i <= 10; i++) {

      // Check at each step if the client has requested cancellation
      // This is important — without this check the arm would ignore cancel requests
      // and keep moving even after the client asked it to stop
      if (goal_handle->is_canceling()) {
        result->success = false;
        result->message = "Cancelled";
        goal_handle->canceled(result);  // Notify client that we cancelled cleanly
        return;
      }

      // Publish feedback so the client can track progress
      // In a real implementation this would be actual joint positions
      feedback->progress = i / 10.0;
      goal_handle->publish_feedback(feedback);

      // Simulate time passing while the arm moves
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    // Execution complete — send the final result to the client
    result->success = true;
    result->message = "Arm reached target";
    goal_handle->succeed(result);
  }
};

int main(int argc, char * argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MoveArmServer>());
  rclcpp::shutdown();
  return 0;
}