// Timeout continuation test
#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"
#include "interfaces/action/hand_control.hpp"

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

namespace
{
class HangingArmServer
{
public:
  explicit HangingArmServer(const rclcpp::Node::SharedPtr & node)
  {
    using namespace std::placeholders; server_ = rclcpp_action::create_server<ArmSpaceControl>(
      node,
      "ArmSpaceControl", std::bind(
        &HangingArmServer::on_goal, this, _1, _2), std::bind(
        &HangingArmServer::on_cancel, this,
        _1),
      std::bind(&HangingArmServer::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>>)   /* never finish */
  {
  }
};
class HandQuickServer
{
public:
  explicit HandQuickServer(const rclcpp::Node::SharedPtr & node)
  {
    using namespace std::placeholders; server_ = rclcpp_action::create_server<HandControl>(
      node,
      "HandControl", std::bind(
        &HandQuickServer::on_goal, this, _1, _2), std::bind(
        &HandQuickServer::on_cancel, this,
        _1),
      std::bind(&HandQuickServer::on_accept, this, _1));
  }

private:
  rclcpp_action::Server<HandControl>::SharedPtr server_; rclcpp_action::GoalResponse on_goal(
    const rclcpp_action::GoalUUID &, std::shared_ptr<const HandControl::Goal>)
  {
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  rclcpp_action::CancelResponse on_cancel(
    const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> & gh)
  {
    auto r = std::make_shared<HandControl::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<HandControl>> gh)
  {
    std::thread(
      [gh]() {
        auto r = std::make_shared<HandControl::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(ExecuteBtActionTimeoutContinue, FirstTimeoutSecondSucceeds) {
  if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
  auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("abort_on_failure", false);
  opts.append_parameter_override("default_action_timeout_sec", 0.5);
  opts.append_parameter_override("stats_log_interval_sec", 0.0);
  auto node = std::make_shared<brain::CerebellumNode>(opts);
  auto hang = std::make_shared<HangingArmServer>(node);
  auto hand = std::make_shared<HandQuickServer>(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 = "ArmSpaceControl,HandControl";
  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(
        10)), rclcpp::FutureReturnCode::SUCCESS);
  auto wrapped = res_future.get();
  EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::ABORTED);
  ASSERT_TRUE(wrapped.result);
  EXPECT_EQ(wrapped.result->total_skills, 2);
  EXPECT_EQ(wrapped.result->succeeded_skills, 1);
  rclcpp::shutdown();
}
