#include <functional>
#include <memory>
#include <thread>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_components/register_node_macro.hpp"

// action libs
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "sensor_msgs/msg/joint_state.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"

namespace rbs_skill_actions {

class GripperControlActionServer : public rclcpp::Node {

public:
  using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
  explicit GripperControlActionServer(rclcpp::NodeOptions options)
      : Node("gripper_control_action_server",
             options.allow_undeclared_parameters(true)) {
    this->actionServer_ = rclcpp_action::create_server<GripperCommand>(
        this, "gripper_control",
        std::bind(&GripperControlActionServer::goal_callback, this,
                  std::placeholders::_1, std::placeholders::_2),
        std::bind(&GripperControlActionServer::cancel_callback, this,
                  std::placeholders::_1),
        std::bind(&GripperControlActionServer::accepted_callback, this,
                  std::placeholders::_1));
    publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
        "~/gripper_controller/commands", 10);
  }

private:
  rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;

  using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;

  rclcpp_action::GoalResponse
  goal_callback(const rclcpp_action::GoalUUID &uuid,
                std::shared_ptr<const GripperCommand::Goal> goal) {

    RCLCPP_INFO(this->get_logger(),
                "goal request recieved for gripper [%.6f m]", goal->position);
    (void)uuid;
    // if (goal->position > 0.008 or goal->position < 0) {
    //   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 request to cancel goal");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
    std::thread{std::bind(&GripperControlActionServer::execute, this,
                          std::placeholders::_1),
                goal_handle}
        .detach();
  }

  void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {

    const auto goal = goal_handle->get_goal();
    auto result = std::make_shared<GripperCommand::Result>();
    auto feedback = std::make_shared<GripperCommand::Feedback>();

    if (goal_handle->is_canceling()) {
      goal_handle->canceled(result);
      RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
      return;
    }

    std_msgs::msg::Float64MultiArray command;

    using namespace std::chrono_literals;

    command.data.push_back(goal->position);
    RCLCPP_INFO(this->get_logger(), "Publishing goal to gripper");
    publisher->publish(command);
    std::this_thread::sleep_for(3s);

    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
  }
};
} // namespace rbs_skill_actions

RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);