#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "robossembler_interfaces/action/gripper_command.hpp"

using GripperCmdAction = robossembler_interfaces::action::GripperCommand;

namespace robossembler_servers
{
class GripperCmd : public rclcpp::Node
{
public:

    explicit GripperCmd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) 
    : Node("gripper_cmd", options)
    {
        using namespace std::placeholders;
        this->action_server_ = rclcpp_action::create_server<GripperCmdAction>(
            this,
            "/gripper_cmd",
            std::bind(&GripperCmd::goal_callback, this, _1, _2),
            std::bind(&GripperCmd::cancel_callback, this, _1),
            std::bind(&GripperCmd::accepted_callback, this, _1));

        this->gripper_cmd_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>
                                        ("/rasmt_hand_controller/command", 10);

    }

private:
    rclcpp_action::Server<GripperCmdAction>::SharedPtr action_server_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_publisher_;

    using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCmdAction>;

    rclcpp_action::GoalResponse goal_callback(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const GripperCmdAction::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "Received goal request [%.3f]", goal->value);
        (void)uuid;
        if (false) {
            return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    
    rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Received cancel request");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    
    void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        using namespace std::placeholders;
        std::thread(std::bind(&robossembler_servers::GripperCmd::execute, this, _1), goal_handle).detach();
    }
    
    void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing goal");
        const auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<GripperCmdAction::Feedback>();
        auto result = std::make_shared<GripperCmdAction::Result>();
    
        if (goal_handle->is_canceling()) {
            goal_handle->canceled(result);
            RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
            return;
        }
        std_msgs::msg::Float64MultiArray commands;
        RCLCPP_INFO(this->get_logger(), "Initialize commands");
        commands.data.push_back(goal->value);
        commands.data.push_back(goal->value);
        //commands.data[0] = goal->value;
        //commands.data[1] = goal->value;
        RCLCPP_INFO(this->get_logger(), "Publish commands");
        this->gripper_cmd_publisher_->publish(commands);

        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
    }

};
}  // namespace robossembler_servers

RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_servers::GripperCmd)