/*
本程序用于创建 action 服务器，接收客户端发送的 goal，并执行相应的操作。
*/

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

using MoveTo = cpp_prococol::action::MoveTo;

class MoveToActionServer : public rclcpp::Node
{
public:
    MoveToActionServer() : Node("move_to_action_server")
    {
        /*
         * 创建一个 action 服务器，指定 action 类型为 MoveTo，名称为 "move_to"。
         * 绑定三个回调函数：handle_goal 用于处理接收到的 goal 请求，handle_cancel 用于处理取消请求，handle_accepted 用于处理接受请求。
         */
        action_server_ = rclcpp_action::create_server<MoveTo>(
            this,
            "move_to",
            std::bind(&MoveToActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&MoveToActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&MoveToActionServer::handle_accepted, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "Action server 'move_to' has been started.");
    }

    /*
        * 处理接收到的 goal 请求的回调函数。
        Params:
        - uuid: 目标的唯一标识符。
        - goal: 接收到的目标消息。
        Returns:
        - GoalResponse: 表示是否接受该目标的响应。
    */
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const MoveTo::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request with target position:(%.2f, %.2f)", goal->target_x, goal->target_y);
        // 目标位置验证
        if (goal->target_x < 0 || goal->target_y < 0)
        {
            // 如果目标位置无效，拒绝该目标请求
            return rclcpp_action::GoalResponse::REJECT;
        }
        // 目标位置有效,接收此请求并开始执行
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    /*
        * 处理接收到的取消请求的回调函数。
        Params:
        - goal_handle: 目标句柄。
        Returns:
        - CancelResponse: 表示是否接受取消请求的响应。
    */
    rclcpp_action::CancelResponse handle_cancel(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
        // 这里可以添加取消目标的逻辑
        auto goal = goal_handle->get_goal();
        RCLCPP_INFO(this->get_logger(), "Canceling goal with target position: (%.2f, %.2f)", goal->target_x, goal->target_y);

        return rclcpp_action::CancelResponse::ACCEPT;
    }

    /*
        * 处理接受请求的回调函数。
        Params:
        - goal_handle: 目标句柄。
    */
    void handle_accepted(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
    {
        std::thread{std::bind(&MoveToActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
    }

    /*
        * 执行目标的函数。
        Params:
        - goal_handle: 目标句柄。
    */
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveTo>> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        // 这里可以添加执行目标的逻辑，并在适当的时候调用 goal_handle->succeed() 或 goal_handle->abort()
        // 设置每个周期的频率:10hz = 0.1s/次
        rclcpp::Rate loop_rate(10);
        // 初始化progress == 0
        auto feedback = std::make_shared<MoveTo::Feedback>();
        feedback->progress = 0.0;
        // 发布初始反馈
        goal_handle->publish_feedback(feedback);

        for (int ii = 0; ii < 100; ii++)
        {
            // 如果节点被关闭
            if (!rclcpp::ok())
            {
                return;
            }

            // 处理中途取消请求
            if (goal_handle->is_canceling())
            {
                RCLCPP_INFO(this->get_logger(), "Goal is canceling");
                auto result = std::make_shared<MoveTo::Result>();
                result->success = false;
                result->status = MoveTo::Result::STATUS_CANCELED;
                result->message = "Goal was canceled";
                goal_handle->canceled(result);
                return;
            }

            // 模拟执行过程
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            feedback->progress = (ii + 1) / 100.0f;
            goal_handle->publish_feedback(feedback);
        }

        // 执行完成，设置目标状态为成功
        auto result = std::make_shared<MoveTo::Result>();
        result->success = true;
        result->status = MoveTo::Result::STATUS_SUCCEEDED;
        result->message = "Goal was succeeded";
        goal_handle->succeed(result);
    }

private:
    rclcpp_action::Server<MoveTo>::SharedPtr action_server_;
};

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