// Test ExecuteBtAction end-to-end for a single action skill (ArmSpaceControl)
// Verifies: server availability, feedback stages sequencing, final result fields.

#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include "brain/cerebellum_node.hpp"
#include "brain/constants.hpp"
#include "brain_interfaces/action/execute_bt_action.hpp"
#include "interfaces/action/arm_space_control.hpp"

using ExecuteBtAction = brain_interfaces::action::ExecuteBtAction;
using ArmSpaceControl = interfaces::action::ArmSpaceControl;

namespace
{

// Minimal dummy ArmSpaceControl action server that succeeds almost immediately.
class DummyArmServer
{
public:
  explicit DummyArmServer(const rclcpp::Node::SharedPtr & node)
  {
    using namespace std::placeholders;
    server_ = rclcpp_action::create_server<ArmSpaceControl>(
      node,
      "ArmSpaceControl",
      std::bind(&DummyArmServer::on_goal, this, _1, _2),
      std::bind(&DummyArmServer::on_cancel, this, _1),
      std::bind(&DummyArmServer::on_accept, this, _1));
  }

private:
  rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;

  rclcpp_action::GoalResponse on_goal(
    const rclcpp_action::GoalUUID &,
    std::shared_ptr<const ArmSpaceControl::Goal>)
  {
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  rclcpp_action::CancelResponse on_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> &)
  {
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> handle)
  {
    std::thread(
      [handle]() {
        auto feedback = std::make_shared<ArmSpaceControl::Feedback>();
        // (Feedback fields intentionally left default if none defined.)
        handle->publish_feedback(feedback);
        auto result = std::make_shared<ArmSpaceControl::Result>();
        result->success = true;
        result->message = "ok";
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
        handle->succeed(result);
      }).detach();
  }
};

} // namespace

TEST(ExecuteBtAction, SingleSkillArmSpaceControlSuccess) {
  rclcpp::init(0, nullptr);
  auto options = rclcpp::NodeOptions();
  // Keep stats interval small to avoid side effects; not strictly needed
  options.append_parameter_override("stats_log_interval_sec", 0.0);
  auto cerebellum = std::make_shared<brain::CerebellumNode>(options);
  auto dummy_server_holder = std::make_shared<DummyArmServer>(cerebellum); // ensure server lifetime

  // Create ExecuteBtAction client directly (bypassing Cerebrum for unit scope)
  // Use explicit action name (constant not exported in public headers)
  auto client = rclcpp_action::create_client<ExecuteBtAction>(cerebellum, brain::kExecuteBtActionName);
  rclcpp::executors::SingleThreadedExecutor exec;
  exec.add_node(cerebellum);

  // Wait for both servers: ExecuteBtAction (hosted by cerebellum) and ArmSpaceControl (dummy)
  auto start = std::chrono::steady_clock::now();
  while (std::chrono::steady_clock::now() - start < std::chrono::seconds(5)) {
    exec.spin_some();
    if (client->action_server_is_ready()) {break;}
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
  }
  ASSERT_TRUE(client->action_server_is_ready()) << "ExecuteBtAction server not ready";

  // Track feedback
  std::mutex fb_mutex;
  std::vector<std::string> stages;
  float last_progress = 0.f;
  auto send_goal_options = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
  send_goal_options.feedback_callback = [&](auto,
      const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
      if (!fb) {return;}
      std::lock_guard<std::mutex> lk(fb_mutex);
      stages.push_back(fb->stage + std::string("|") + fb->current_skill);
      last_progress = fb->progress;
    };

  ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl";
  auto future_handle = client->async_send_goal(goal, send_goal_options);

  // Wait for accept
  while (exec.spin_until_future_complete(
      future_handle,
      std::chrono::milliseconds(50)) == rclcpp::FutureReturnCode::TIMEOUT)
  {
    // loop
  }
  auto handle = future_handle.get();
  ASSERT_TRUE(handle);

  auto result_future = client->async_get_result(handle);
  while (exec.spin_until_future_complete(
      result_future,
      std::chrono::milliseconds(50)) == rclcpp::FutureReturnCode::TIMEOUT)
  {
    // keep spinning to pump feedback
  }
  auto wrapped = result_future.get();
  ASSERT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED);
  ASSERT_TRUE(wrapped.result);
  EXPECT_TRUE(wrapped.result->success);
  EXPECT_EQ(wrapped.result->total_skills, 1);
  EXPECT_EQ(wrapped.result->succeeded_skills, 1);
  {
    std::lock_guard<std::mutex> lk(fb_mutex);
    // Expect at least START and END for ArmSpaceControl; optional WAIT/RUN may appear depending on timing
    bool saw_start = false, saw_end = false;
    for (auto & s : stages) {
      if (s.rfind("START|ArmSpaceControl", 0) == 0) {saw_start = true;}
      if (s.rfind("END|ArmSpaceControl", 0) == 0) {saw_end = true;}
    }
    EXPECT_TRUE(saw_start);
    EXPECT_TRUE(saw_end);
    EXPECT_NEAR(last_progress, 1.0f, 1e-3);
  }

  rclcpp::shutdown();
}
