/* node_template.cpp
 * Action server node template for ROS2
 *
 * Node description:
 * Template action server that demonstrates action server implementation
 * with goal handling, feedback publishing, and cancellation support
 *
 * Reviewed by: <x>
 * Changelog:
 * [04-09-2025] Wessel T: Implement template
 * [18-09-2025] Wessel T: Add action server functionality
 */

#include <cstdlib>

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

#include "les_interface/action/template_a.hpp"

namespace lessons::three::actions {

using TemplateA = les_interface::action::TemplateA;
using TemplateAGoalHandle = rclcpp_action::ServerGoalHandle<TemplateA>;
using namespace std::placeholders;

class NodeActionServer : public rclcpp::Node {
public:
    NodeActionServer()
    : Node("node_les3_action_server")
    {
        // Callback groups make thread handling possible
        cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        template_actionserver_ = rclcpp_action::create_server<TemplateA>(
            this,
            "template_actionserver",
            std::bind(&NodeActionServer::goalCallback, this, _1, _2),
            std::bind(&NodeActionServer::cancelCallback, this, _1),
            std::bind(&NodeActionServer::executeCallback, this, _1),
            rcl_action_server_get_default_options(),
            cb_group_
        );

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

private:
    rclcpp_action::Server<TemplateA>::SharedPtr template_actionserver_;
    rclcpp::CallbackGroup::SharedPtr cb_group_;

    // Communication and timer functions
    rclcpp_action::GoalResponse goalCallback(
        const rclcpp_action::GoalUUID &uuid,
        std::shared_ptr<const TemplateA::Goal> goal)
    {
        (void)uuid;  // Needed to avoid warning in compiler - every sent goal gets an id
        RCLCPP_INFO(this->get_logger(), "Received a goal");

        // Your code - optionally you can decide when you want to reject a goal
        if (goal->gname == "Tilmann") {
            RCLCPP_WARN(this->get_logger(), "Rejecting the goal, target name is Tilmann");
            return rclcpp_action::GoalResponse::REJECT;
        }
        // End of your code

        RCLCPP_INFO(this->get_logger(), "Accepting the goal");
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    // Any cancel request will be processed here, we can accept or reject it
    rclcpp_action::CancelResponse cancelCallback(
        const std::shared_ptr<TemplateAGoalHandle> goal_handle)
    {
        (void)goal_handle; // Avoiding compiler warning
        RCLCPP_INFO(this->get_logger(), "Received a cancel request");
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void executeCallback(const std::shared_ptr<TemplateAGoalHandle> goal_handle)
    {
        // The goal from defined action (here TemplateA)
        std::string gname = goal_handle->get_goal()->gname;

        // Defines result and feedback
        auto result = std::make_shared<TemplateA::Result>();
        auto feedback = std::make_shared<TemplateA::Feedback>();
        RCLCPP_INFO(this->get_logger(), "Executing the goal");

        // Your real code starts here !!!
        rclcpp::Rate loop_rate(0.5);
        int i = 0;

        while(i < 10) {
            i++;
            feedback->fnumber = i;
            goal_handle->publish_feedback(feedback);

            // Here you should handle as well when a cancel request is sent
            if(goal_handle->is_canceling()) {
                RCLCPP_INFO(this->get_logger(), "Canceling goal");
                result->rnumber = i; // If you want to send something back as result
                goal_handle->canceled(result);
                return;
            }

            RCLCPP_INFO(this->get_logger(), "looping");
            loop_rate.sleep();
        }
        // End of your code

        result->rnumber = i;
        goal_handle->succeed(result);
    }
};

} // namespace lessons::three::actions

int main(int argc,char *argv[]) {
    rclcpp::init(argc,argv);

    auto node = std::make_shared<lessons::three::actions::NodeActionServer>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();

    return 0;
}
