// RUN interpolation multiple RUN feedback test isolated
#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
{
class DelayedArmServer
{
public:
  DelayedArmServer(const rclcpp::Node::SharedPtr & node, int delay_ms)
  : delay_(delay_ms)
  {
    using namespace std::placeholders;
    server_ = rclcpp_action::create_server<ArmSpaceControl>(
      node, "ArmSpaceControl",
      std::bind(&DelayedArmServer::on_goal, this, _1, _2),
      std::bind(&DelayedArmServer::on_cancel, this, _1),
      std::bind(&DelayedArmServer::on_accept, this, _1));
  }

private:
  int delay_;
  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>> & gh)
  {
    auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
    gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
  }
  void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> gh)
  {
    std::thread(
      [gh, d = delay_]() {
        std::this_thread::sleep_for(
          std::chrono::milliseconds(
            d)); auto res = std::make_shared<ArmSpaceControl::Result>(); res->success = true; res->message = "ok"; gh->succeed(
          res);
      }).detach();
  }
};
static bool spin_until(
  rclcpp::executors::SingleThreadedExecutor & ex,
  std::function<bool()> pred, std::chrono::milliseconds to)
{
  auto start = std::chrono::steady_clock::now();
  while (std::chrono::steady_clock::now() - start < to) {
    ex.spin_some(); if (pred()) {
      return true;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
  return pred();
}
} // namespace

TEST(ExecuteBtActionRunInterp, MultipleRunFeedbacks) {
  if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
  auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.0);
  opts.append_parameter_override("default_action_timeout_sec", 3.0);
  auto node = std::make_shared<brain::CerebellumNode>(opts);
  auto srv = std::make_shared<DelayedArmServer>(node, 1200);
  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)));
  std::atomic<int> run_count{0};
  ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl";
  auto optsSend = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
  optsSend.feedback_callback =
    [&](auto, const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
      if (fb && fb->stage == "RUN") {
        run_count.fetch_add(1);
      }
    };
  auto gh_future = client->async_send_goal(goal, optsSend);
  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);
  EXPECT_GE(run_count.load(), 3);
  rclcpp::shutdown();
}
