// Unsupported skill failure detail test split out for isolation
#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 ArmQuickServer
{
public:
  explicit ArmQuickServer(const rclcpp::Node::SharedPtr & node)
  {
    using namespace std::placeholders;
    server_ = rclcpp_action::create_server<ArmSpaceControl>(
      node, "ArmSpaceControl",
      std::bind(&ArmQuickServer::on_goal, this, _1, _2),
      std::bind(&ArmQuickServer::on_cancel, this, _1),
      std::bind(&ArmQuickServer::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>> & 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]() {
        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(ExecuteBtActionUnsupported, UndefinedFirstSkillAborts) {
  if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
  auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.0);
  auto node = std::make_shared<brain::CerebellumNode>(opts);
  auto arm = std::make_shared<ArmQuickServer>(node);
  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 = "NonExistingSkill,ArmSpaceControl";
  auto gh_future = client->async_send_goal(goal);
  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(
        5)), rclcpp::FutureReturnCode::SUCCESS);
  auto wrapped = res_future.get();
  EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::ABORTED);
  ASSERT_TRUE(wrapped.result);
  EXPECT_FALSE(wrapped.result->success);
  EXPECT_NE(wrapped.result->message.find("NonExistingSkill(FAIL)"), std::string::npos);
  rclcpp::shutdown();
}
