// HandControl action server implementation

#include <memory>
#include <string>
#include <thread>
#include <chrono>
#include <atomic>

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

#include "interfaces/action/hand_control.hpp"

using HandControl = interfaces::action::HandControl;

class HandControlNode : public rclcpp::Node {
public:
  HandControlNode()
  : Node("hand_control_node"), active_goal_count_(0)
  {
    using namespace std::placeholders;
    server_ = rclcpp_action::create_server<HandControl>(
      this,
      "hand_control",
      std::bind(&HandControlNode::handle_goal, this, _1, _2),
      std::bind(&HandControlNode::handle_cancel, this, _1),
      std::bind(&HandControlNode::handle_accepted, this, _1));

    declare_parameter<double>("default_duration_sec", 2.0);
    declare_parameter<int>("open_mode_value", 0);
    declare_parameter<int>("close_mode_value", 1);
    declare_parameter<bool>("allow_concurrent_goals", false);
    RCLCPP_INFO(get_logger(), "HandControl action server started");
  }

private:
  rclcpp_action::Server<HandControl>::SharedPtr server_;
  std::atomic<int> active_goal_count_;

  rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const HandControl::Goal> goal)
  {
    (void)uuid;
    if (!get_parameter("allow_concurrent_goals").as_bool() && active_goal_count_.load() > 0) {
      RCLCPP_WARN(get_logger(), "Reject goal: another goal already active");
      return rclcpp_action::GoalResponse::REJECT;
    }
    if (goal->mode != get_parameter("open_mode_value").as_int() && goal->mode != get_parameter("close_mode_value").as_int()) {
      RCLCPP_WARN(get_logger(), "Reject goal: unsupported mode %d", goal->mode);
      return rclcpp_action::GoalResponse::REJECT;
    }
    RCLCPP_INFO(get_logger(), "Accept goal: mode=%d effort=%.2f", goal->mode, goal->effort);
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }

  rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
  {
    RCLCPP_INFO(get_logger(), "Cancel request received");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }

  void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
  {
    active_goal_count_.fetch_add(1);
    std::thread(&HandControlNode::execute, this, goal_handle).detach();
  }

  void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
  {
    const auto goal = goal_handle->get_goal();
    const double total_duration = get_parameter("default_duration_sec").as_double();
    const auto start = now();
    rclcpp::Rate r(20.0); // 50ms cycle
    float last_progress = 0.0f;

    while (rclcpp::ok()) {
      if (goal_handle->is_canceling()) {
        auto result = std::make_shared<HandControl::Result>();
        result->success = false;
        result->message = "canceled";
        goal_handle->canceled(result);
        RCLCPP_INFO(get_logger(), "Goal canceled");
        active_goal_count_.fetch_sub(1);
        return;
      }

      auto elapsed = (now() - start).seconds();
      float progress = static_cast<float>(elapsed / total_duration);
      if (progress > 1.0f) progress = 1.0f;
      if (progress < last_progress) progress = last_progress; // monotonic
      last_progress = progress;

      auto feedback = std::make_shared<HandControl::Feedback>();
      feedback->progress = progress;
      goal_handle->publish_feedback(feedback);

      if (progress >= 1.0f) {
        break;
      }
      r.sleep();
    }

    if (!rclcpp::ok()) {
      auto result = std::make_shared<HandControl::Result>();
      result->success = false;
      result->message = "shutdown";
      try { goal_handle->abort(result); } catch(...) {}
      active_goal_count_.fetch_sub(1);
      return;
    }

    // Simulate success/failure based on mode and effort (simple heuristic)
    bool success = true;
    std::string msg = (goal->mode == get_parameter("open_mode_value").as_int()) ? "opened" : "closed";
    if (goal->effort < 0.0f) { success = false; msg = "negative effort"; }

    auto result = std::make_shared<HandControl::Result>();
    result->success = success;
    result->message = msg;
    if (success) {
      goal_handle->succeed(result);
      RCLCPP_INFO(get_logger(), "Goal succeeded: %s", msg.c_str());
    } else {
      goal_handle->abort(result);
      RCLCPP_WARN(get_logger(), "Goal aborted: %s", msg.c_str());
    }
    active_goal_count_.fetch_sub(1);
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<HandControlNode>();
  rclcpp::executors::MultiThreadedExecutor exec;
  exec.add_node(node);
  exec.spin();
  rclcpp::shutdown();
  return 0;
}

