/*
本程序用于创建 ROS2 客户端Action节点，演示如何发送Action请求并处理结果。
*/

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "cpp_prococol/action/move_to.hpp"

using MoveTo = cpp_prococol::action::MoveTo;

class MoveToClient : public rclcpp::Node
{
public:
    MoveToClient() : Node("move_to_client")
    {
        // 创建一个 action 客户端，指定 action 类型为 MoveTo，名称为 "move_to"。
        action_client_ = rclcpp_action::create_client<MoveTo>(this, "move_to");

        if (!WaitServer())
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
        }
        MoveToPos(5.0f, 5.0f);
    }

    // 发送一个目标请求，并处理结果的回调函数。
    void MoveToPos(float x, float y)
    {
        MoveTo::Goal goal_msg;
        goal_msg.target_x = x;
        goal_msg.target_y = y;

        // 发送目标请求，并绑定结果回调函数。
        rclcpp_action::Client<MoveTo>::SendGoalOptions options;
        options.goal_response_callback = std::bind(&MoveToClient::goal_response_callback, this, std::placeholders::_1);
        options.feedback_callback = std::bind(&MoveToClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        options.result_callback = std::bind(&MoveToClient::result_callback, this, std::placeholders::_1);

        // 异步发送目标请求，结果通过回调函数处理。
        action_client_->async_send_goal(goal_msg, options);
    }

    // *** 定义回调 ***
    //  绑定目标响应回调函数，当服务器接受或拒绝目标时调用。
    void goal_response_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<MoveTo>> goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    // 绑定反馈回调函数，当服务器发送反馈时调用。
    void feedback_callback(std::shared_ptr<rclcpp_action::ClientGoalHandle<MoveTo>> goal_handle, const std::shared_ptr<const MoveTo::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Received feedback: current progress:(%.2f)", feedback->progress);
    }

    // 绑定结果回调函数，当服务器发送结果时调用。
    void result_callback(const rclcpp_action::ClientGoalHandle<MoveTo>::WrappedResult & result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "Goal succeeded: %s", result.result->message.c_str());
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_WARN(this->get_logger(), "Goal was canceled");
            break;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            break;
        }
    }


    // 等待服务端就绪就绪
    bool WaitServer()
    {
        // 等待 action 服务器就绪，超时时间为 10 秒。
        return action_client_->wait_for_action_server(std::chrono::seconds(10));
    }

private:
    rclcpp_action::Client<MoveTo>::SharedPtr action_client_;
};

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