/* action_client.cpp
 * Action client node template for ROS2
 *
 * Node description:
 * Action client that sends goals to action server and handles responses,
 * feedback, and results. Demonstrates async goal sending and cancellation.
 *
 * Reviewed by: <x>
 * Changelog:
 * [18-09-2025] Wessel T: Implement action client template
 */

// General includes
#include <cstdlib>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

// Custom includes
#include "les_interface/action/template_a.hpp"

// Using declarations
using TemplateA = les_interface::action::TemplateA;
using TemplateAGoalHandle = rclcpp_action::ClientGoalHandle<TemplateA>;
using namespace std::placeholders;

namespace lessons::three::actions {

class NodeActionClient : public rclcpp::Node {
public:
    // Constructor:
    NodeActionClient() : Node("node_les3_action_client")
    {
        // Communication and timer objects:
        template_actionclient_ = rclcpp_action::create_client<TemplateA>(this, "template_actionserver");

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

    // Communication and timer functions
    void sendGoal(std::string gname)
    {
        template_actionclient_->wait_for_action_server();
        auto goal = TemplateA::Goal();

        goal.gname = gname;

        auto options = rclcpp_action::Client<TemplateA>::SendGoalOptions();
        options.goal_response_callback = std::bind(&NodeActionClient::goalResponseCallback, this, _1);
        options.result_callback = std::bind(&NodeActionClient::goalResultCallback, this, _1);
        options.feedback_callback = std::bind(&NodeActionClient::goalFeedbackCallback, this, _1, _2);

        template_actionclient_->async_send_goal(goal, options);
        RCLCPP_INFO(this->get_logger(), "Sent request");
    }

    // Method to send a cancel request
    void cancelGoal()
    {
        RCLCPP_INFO(this->get_logger(), "Sent a cancel goal request");
        template_actionclient_->async_cancel_all_goals();
    }

private:
    // Communication and timer functions
    void goalResponseCallback(const TemplateAGoalHandle::SharedPtr &goal_handle)
    {
        if (!goal_handle) {
            RCLCPP_INFO(this->get_logger(), "Goal got rejected");
        }
        else {
            RCLCPP_INFO(this->get_logger(), "Goal got accepted");
        }
    }

    void goalResultCallback(const TemplateAGoalHandle::WrappedResult &result)
    {
        auto status = result.code;
        if (status == rclcpp_action::ResultCode::SUCCEEDED) {
            RCLCPP_INFO(this->get_logger(), "Succeeded");
        }
        else if (status == rclcpp_action::ResultCode::ABORTED) {
            RCLCPP_ERROR(this->get_logger(), "Aborted");
        }
        else if (status == rclcpp_action::ResultCode::CANCELED) {
            RCLCPP_WARN(this->get_logger(), "Canceled");
        }
        int number = result.result->rnumber;
        RCLCPP_INFO(this->get_logger(), "Result: %d", number);
    }

    void goalFeedbackCallback(const TemplateAGoalHandle::SharedPtr &goal_handle,
        const std::shared_ptr<const TemplateA::Feedback> feedback)
    {
        (void)goal_handle;
        int number = feedback->fnumber;
        RCLCPP_INFO(this->get_logger(), "Got feedback: %d", number);

        // Now here the option to cancel request if
        // feedback has certain value

        // if (number >= 2) {
        //     cancelGoal();
        // }
    }

    // Custom functions:
    // ...

    // ROS2 variables:
    rclcpp_action::Client<TemplateA>::SharedPtr template_actionclient_;
};

} // namespace lessons::three::actions

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<lessons::three::actions::NodeActionClient>();

    std::string name = "jan";
    node->sendGoal(name);

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
