// Extended scenario tests for ExecuteBtAction (trimmed to only multi-skill progress segmentation)
// Other scenarios have been split into their own dedicated test translation units to avoid
// cross-test interference with rclcpp global state.

#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
// no stats topic usage here

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

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

namespace
{

// Simple delayed success server for each action type
template<typename ActionT>
class SimpleDelayedServer
{
public:
  SimpleDelayedServer(const rclcpp::Node::SharedPtr & node, const std::string & name, int delay_ms)
  : delay_ms_(delay_ms)
  {
    using namespace std::placeholders;
    server_ = rclcpp_action::create_server<ActionT>(
      node, name,
      std::bind(&SimpleDelayedServer::on_goal, this, _1, _2),
      std::bind(&SimpleDelayedServer::on_cancel, this, _1),
      std::bind(&SimpleDelayedServer::on_accept, this, _1));
  }

private:
  int delay_ms_;
  typename rclcpp_action::Server<ActionT>::SharedPtr server_;
  rclcpp_action::GoalResponse on_goal(
    const rclcpp_action::GoalUUID &,
    std::shared_ptr<const typename ActionT::Goal>)
  {return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;}
  rclcpp_action::CancelResponse on_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> & gh)
  {
    auto res = std::make_shared<typename ActionT::Result>(); res->success = false;
    res->message = "cancel"; gh->canceled(res); return rclcpp_action::CancelResponse::ACCEPT;
  }
  void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> gh)
  {
    std::thread(
      [gh, d = delay_ms_]() {
        std::this_thread::sleep_for(
          std::chrono::milliseconds(
            d)); auto res = std::make_shared<typename ActionT::Result>(); res->success = true; res->message = "ok"; gh->succeed(
          res);
      }).detach();
  }
};

static bool spin_until(
  rclcpp::executors::SingleThreadedExecutor & exec,
  std::function<bool()> pred,
  std::chrono::milliseconds timeout)
{
  auto start = std::chrono::steady_clock::now();
  while (std::chrono::steady_clock::now() - start < timeout) {
    exec.spin_some();
    if (pred()) {return true;}
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
  return pred();
}

TEST(ExecuteBtActionExtended, MultiSkillProgressSegmentation)
{
  if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} // safe re-init if already init
  auto options = rclcpp::NodeOptions();
  options.append_parameter_override("stats_log_interval_sec", 0.0);
  options.append_parameter_override("default_action_timeout_sec", 5.0);
  auto node = std::make_shared<brain::CerebellumNode>(options);
  // Three skills with distinct durations to create progress partitions
  auto arm = std::make_shared<SimpleDelayedServer<ArmSpaceControl>>(node, "ArmSpaceControl", 150);
  auto hand = std::make_shared<SimpleDelayedServer<HandControl>>(node, "HandControl", 120);
  auto leg = std::make_shared<SimpleDelayedServer<LegControl>>(node, "LegControl", 180);

  rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
  auto client = rclcpp_action::create_client<ExecuteBtAction>(node, brain::kExecuteBtActionName);
  ASSERT_TRUE(
    spin_until(
      exec,
      [&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));

  ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl,HandControl,LegControl";
  std::vector<double> end_progresses;
  auto send_opts = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
  send_opts.feedback_callback =
    [&](auto, const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
      if (!fb) {return;} if (fb->stage == "END") {end_progresses.push_back(fb->progress);}};
  auto gh_future = client->async_send_goal(goal, send_opts);
  ASSERT_EQ(
    exec.spin_until_future_complete(
      gh_future, std::chrono::seconds(
        2)), rclcpp::FutureReturnCode::SUCCESS);
  auto gh = gh_future.get(); ASSERT_TRUE(gh);
  auto res_future = client->async_get_result(gh);
  ASSERT_EQ(
    exec.spin_until_future_complete(
      res_future, std::chrono::seconds(
        10)), rclcpp::FutureReturnCode::SUCCESS);
  auto wrapped = res_future.get(); ASSERT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED);
  ASSERT_TRUE(wrapped.result); EXPECT_EQ(wrapped.result->total_skills, 3); EXPECT_EQ(
    wrapped.result->succeeded_skills, 3);
  // We expect three phase end progress marks roughly near 1/3, 2/3, and 1.0 (allowing tolerance)
  ASSERT_EQ(end_progresses.size(), 3u);
  EXPECT_NEAR(end_progresses[0], 1.0 / 3.0, 0.25);
  EXPECT_NEAR(end_progresses[1], 2.0 / 3.0, 0.25);
  EXPECT_NEAR(end_progresses[2], 1.0, 0.05);
  rclcpp::shutdown();
}

}  // namespace
