// Stats topic content test
#include <gtest/gtest.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <std_msgs/msg/string.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 ArmFastServer
{
public:
  explicit ArmFastServer(const rclcpp::Node::SharedPtr & node)
  {
    using namespace std::placeholders; server_ = rclcpp_action::create_server<ArmSpaceControl>(
      node,
      "ArmSpaceControl", std::bind(
        &ArmFastServer::on_goal, this, _1, _2), std::bind(
        &ArmFastServer::on_cancel, this,
        _1),
      std::bind(&ArmFastServer::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 r = std::make_shared<ArmSpaceControl::Result>(); r->success = true; r->message = "ok"; gh->succeed(
          r);
      }).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(ExecuteBtActionStats, SequenceStatsPublishOrEmptyWhenTimerDisabled) {
  if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
  auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.2); // enable fast timer
  auto node = std::make_shared<brain::CerebellumNode>(opts);
  auto server = std::make_shared<ArmFastServer>(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)));
  std::mutex m; std::string last;
  auto sub =
    node->create_subscription<std_msgs::msg::String>(
    "/brain/skill_stats", 10,
    [&](std_msgs::msg::String::ConstSharedPtr msg) {
      std::lock_guard<std::mutex> lk(m); last = msg->data;
    });
  ExecuteBtAction::Goal g; g.action_name = "ArmSpaceControl";
  auto gh_f = client->async_send_goal(g);
  ASSERT_EQ(
    exec.spin_until_future_complete(
      gh_f, std::chrono::seconds(
        2)), rclcpp::FutureReturnCode::SUCCESS);
  auto gh = gh_f.get(); ASSERT_TRUE(gh);
  auto r_f = client->async_get_result(gh);
  ASSERT_EQ(
    exec.spin_until_future_complete(
      r_f, std::chrono::seconds(
        5)), rclcpp::FutureReturnCode::SUCCESS);
  // wait for at least one stats timer publish
  spin_until(
    exec, [&] {
      std::lock_guard<std::mutex> lk(
        m); return !last.empty();
    }, std::chrono::milliseconds(600));
  std::lock_guard<std::mutex> lk(m);
  ASSERT_FALSE(last.empty());
  EXPECT_NE(last.find("seq_ok="), std::string::npos);
  rclcpp::shutdown();
}
