/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test bảng chuyển state.
 *
 * Mỗi transition trong docs/STATE_MACHINE.md phải có ít nhất một test ở đây. Bảng đó là nguồn
 * chuẩn; test này là thứ chứng minh code khớp với bảng.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <string>

#include <move_base2/core/state_machine.h>

using move_base2::ActionFeedback;
using move_base2::ControllerFeedback;
using move_base2::NavigationOutcome;
using move_base2::NavigationState;
using move_base2::PlannerFeedback;
using move_base2::RecoveryFeedback;
using move_base2::RecoveryOutputKind;
using move_base2::RecoveryTrigger;
using move_base2::StateMachine;
using move_base2::StateMachineConfig;
using move_base2::StateMachineInput;
using move_base2::StateMachineOutput;
using move_base2::VelocitySource;

namespace
{

/// @brief Cấu hình cơ sở cho test: ngưỡng ngắn để không phải tua thời gian dài.
StateMachineConfig baseConfig()
{
  StateMachineConfig config;
  config.planner_patience = 1.0;      // [s]
  config.controller_patience = 2.0;   // [s]
  config.oscillation_timeout = 0.0;   // tắt trừ khi test bật riêng
  config.oscillation_distance = 0.5;  // [m]
  config.max_planning_retries = -1;
  config.recovery_behavior_count = 2;
  config.recovery_enabled = true;
  return config;
}

/**
 * @class Driver
 * @brief Vỏ mỏng quanh StateMachine để test đọc như kịch bản chứ không như lời gọi API.
 */
class Driver
{
public:
  explicit Driver(const StateMachineConfig& config = baseConfig())
  {
    std::string error;
    configured_ = machine_.configure(config, error);
    EXPECT_TRUE(configured_) << error;
    now_ = 1000.0;
  }

  /// @param seconds [s] Thời gian trôi trước cycle này.
  Driver& advance(double seconds)
  {
    now_ += seconds;
    return *this;
  }

  StateMachineOutput tick(StateMachineInput input)
  {
    input.now = robot::Time(now_);
    last_ = machine_.update(input);
    return last_;
  }

  /// @brief Cycle không có sự kiện gì.
  StateMachineOutput idleTick()
  {
    return tick(StateMachineInput());
  }

  /// @brief Đưa máy từ kIdle tới kControlling: nhận yêu cầu rồi cấp một plan hợp lệ.
  void driveToControlling()
  {
    StateMachineInput request;
    request.has_pending_request = true;
    ASSERT_EQ(tick(request).state, NavigationState::kPlanning);

    StateMachineInput plan_ready;
    plan_ready.planner = PlannerFeedback::kPlanReady;
    ASSERT_EQ(tick(plan_ready).state, NavigationState::kControlling);
  }

  StateMachine& machine()
  {
    return machine_;
  }

  const StateMachineOutput& last() const
  {
    return last_;
  }

  double now() const
  {
    return now_;
  }

private:
  StateMachine machine_;
  StateMachineOutput last_;
  double now_ = 1000.0;
  bool configured_ = false;
};

StateMachineInput planReady()
{
  StateMachineInput input;
  input.planner = PlannerFeedback::kPlanReady;
  return input;
}

StateMachineInput plannerFailed()
{
  StateMachineInput input;
  input.planner = PlannerFeedback::kFailed;
  return input;
}

StateMachineInput controller(ControllerFeedback feedback)
{
  StateMachineInput input;
  input.controller = feedback;
  return input;
}

StateMachineInput recovery(RecoveryFeedback feedback)
{
  StateMachineInput input;
  input.recovery = feedback;
  return input;
}

StateMachineInput actionFb(ActionFeedback feedback)
{
  StateMachineInput input;
  input.action = feedback;
  return input;
}

/// @brief Yêu cầu mới với hình dạng D8: có goal hay không, và bao nhiêu action.
StateMachineInput requestWithActions(std::size_t action_count, bool has_goal = true)
{
  StateMachineInput input;
  input.has_pending_request = true;
  input.pending_request_has_goal = has_goal;
  input.pending_request_action_count = action_count;
  return input;
}

}  // namespace

// ================================================================================================
// Cấu hình
// ================================================================================================

TEST(StateMachineConfig, RejectsOscillationTimeoutWithoutDistance)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_timeout = 5.0;
  config.oscillation_distance = 0.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("oscillation_distance"), std::string::npos);
}

TEST(StateMachineConfig, RejectsNegativeOscillationDistance)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_distance = -1.0;

  std::string error;
  EXPECT_FALSE(config.validate(error));
}

TEST(StateMachineConfig, RejectsRecoveryEnabledWithZeroBehaviors)
{
  StateMachineConfig config = baseConfig();
  config.recovery_behavior_count = 0;
  config.recovery_enabled = true;

  std::string error;
  EXPECT_FALSE(config.validate(error)) << "this config would go ABORTED at runtime on the very "
                                          "first failure";
}

TEST(StateMachineConfig, AcceptsRecoveryDisabledWithZeroBehaviors)
{
  StateMachineConfig config = baseConfig();
  config.recovery_behavior_count = 0;
  config.recovery_enabled = false;

  std::string error;
  EXPECT_TRUE(config.validate(error)) << error;
}

TEST(StateMachineConfig, RejectsResolvedRouteWithAnOutOfRangeBehavior)
{
  StateMachineConfig config = baseConfig();
  config.recovery_routes.planning_failed = {0};
  config.recovery_routes.controlling_failed = {1};
  config.recovery_routes.oscillation = {2};  // behavior_count chỉ là 2

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("loaded behavior"), std::string::npos);
}

TEST(StateMachineConfig, DescribeMentionsEveryParameter)
{
  const std::string text = baseConfig().describe();
  EXPECT_NE(text.find("planner_patience"), std::string::npos);
  EXPECT_NE(text.find("controller_patience"), std::string::npos);
  EXPECT_NE(text.find("oscillation_timeout"), std::string::npos);
  EXPECT_NE(text.find("action_patience"), std::string::npos);
  EXPECT_NE(text.find("max_planning_retries"), std::string::npos);
}

TEST(StateMachine, RefusesToRunBeforeConfigure)
{
  StateMachine machine;
  EXPECT_FALSE(machine.initialized());

  StateMachineInput input;
  input.has_pending_request = true;
  const StateMachineOutput out = machine.update(input);

  EXPECT_EQ(out.state, NavigationState::kIdle);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
  EXPECT_FALSE(out.accept_request);
}

TEST(StateMachine, ConfigureFailsLoudlyOnBadConfig)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_timeout = 5.0;
  config.oscillation_distance = 0.0;

  StateMachine machine;
  std::string error;
  EXPECT_FALSE(machine.configure(config, error));
  EXPECT_FALSE(machine.initialized());
  EXPECT_FALSE(error.empty());
}

// ================================================================================================
// IDLE
// ================================================================================================

TEST(StateMachineIdle, StaysIdleWithoutRequest)
{
  Driver driver;
  const StateMachineOutput out = driver.idleTick();

  EXPECT_EQ(out.state, NavigationState::kIdle);
  EXPECT_FALSE(out.state_changed);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineIdle, RequestMovesToPlanningAndStartsPlanner)
{
  Driver driver;
  StateMachineInput input;
  input.has_pending_request = true;

  const StateMachineOutput out = driver.tick(input);

  EXPECT_EQ(out.state, NavigationState::kPlanning);
  EXPECT_TRUE(out.state_changed);
  EXPECT_TRUE(out.accept_request);
  EXPECT_TRUE(out.start_planner);
  EXPECT_TRUE(out.reset_oscillation_origin);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineIdle, CancelWithoutRequestIsIgnored)
{
  Driver driver;
  StateMachineInput input;
  input.cancel_requested = true;

  const StateMachineOutput out = driver.tick(input);
  EXPECT_EQ(out.state, NavigationState::kIdle);
  EXPECT_FALSE(out.report_outcome);
}

// ================================================================================================
// PLANNING
// ================================================================================================

TEST(StateMachinePlanning, PlanReadyMovesToControllingAndAppliesPlan)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  const StateMachineOutput out = driver.tick(planReady());

  EXPECT_EQ(out.state, NavigationState::kControlling);
  EXPECT_TRUE(out.apply_plan);
  EXPECT_TRUE(out.run_controller);
  EXPECT_EQ(out.velocity_source, VelocitySource::kController);
}

TEST(StateMachinePlanning, PlannerPatienceEscalatesToRecovery)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  driver.advance(0.5);
  EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning);

  driver.advance(0.6);  // tổng 1.1 s > planner_patience = 1.0 s
  const StateMachineOutput out = driver.tick(plannerFailed());

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_TRUE(out.start_recovery);
  EXPECT_EQ(out.recovery_index, 0u);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
  EXPECT_TRUE(out.stop_planner);
}

TEST(StateMachinePlanning, MaxRetriesEscalatesBeforePatienceExpires)
{
  StateMachineConfig config = baseConfig();
  config.max_planning_retries = 1;
  config.planner_patience = 100.0;  // dài, để chắc chắn cái chặn là số lượt chứ không phải thời gian
  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  EXPECT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning);  // retries = 1
  const StateMachineOutput out = driver.tick(plannerFailed());                // retries = 2 > 1

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}

TEST(StateMachinePlanning, UsesPlanningRouteInsteadOfRegistryOrder)
{
  StateMachineConfig config = baseConfig();
  config.max_planning_retries = 0;
  config.planner_patience = 100.0;
  config.recovery_routes.planning_failed = {1};
  config.recovery_routes.controlling_failed = {0};
  config.recovery_routes.oscillation = {0};
  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  const StateMachineOutput out = driver.tick(plannerFailed());
  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_TRUE(out.start_recovery);
  EXPECT_EQ(out.recovery_index, 1u);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kPlanningFailed);
}

TEST(StateMachinePlanning, PatienceDisabledMeansNeverTimesOut)
{
  // `planner_patience = 0` vẫn nghĩa là "tắt đồng hồ kiên nhẫn". Nhưng từ khi planner chạy trên
  // thread riêng, tắt CẢ HAI cận (kiên nhẫn và số lượt) bị `validate()` từ chối — không còn gì phát
  // hiện được planner treo. Test này kiểm đúng thứ nó vẫn luôn kiểm: đồng hồ kiên nhẫn không kêu.
  // Cận số lượt đặt cao hơn hẳn số vòng lặp để nó không phải là thứ kết thúc test.
  StateMachineConfig config = baseConfig();
  config.planner_patience = 0.0;
  config.max_planning_retries = 1000;
  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  for (int i = 0; i < 100; ++i)
  {
    driver.advance(1.0);
    ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kPlanning) << "round " << i;
  }
}

TEST(StateMachinePlanning, CancelMovesToCancelling)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  StateMachineInput cancel;
  cancel.cancel_requested = true;
  const StateMachineOutput out = driver.tick(cancel);

  EXPECT_EQ(out.state, NavigationState::kCancelling);
  EXPECT_TRUE(out.stop_planner);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachinePlanning, PauseMovesToPausedAndResumeComesBack)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  StateMachineInput pause;
  pause.pause_requested = true;
  EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  StateMachineInput resume;
  resume.resume_requested = true;
  const StateMachineOutput out = driver.tick(resume);

  EXPECT_EQ(out.state, NavigationState::kPlanning);
  EXPECT_TRUE(out.start_planner);
}

TEST(StateMachinePlanning, NoRecoveryAvailableAbortsInsteadOfHanging)
{
  StateMachineConfig config = baseConfig();
  config.recovery_enabled = false;
  config.recovery_behavior_count = 0;
  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  driver.advance(2.0);
  const StateMachineOutput out = driver.tick(plannerFailed());

  EXPECT_EQ(out.state, NavigationState::kAborted);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
}

// ================================================================================================
// CONTROLLING
// ================================================================================================

TEST(StateMachineControlling, GoalReachedFinishesSucceeded)
{
  Driver driver;
  driver.driveToControlling();

  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kGoalReached));

  EXPECT_EQ(out.state, NavigationState::kSucceeded);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kSucceeded);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineControlling, ValidCommandKeepsControlling)
{
  Driver driver;
  driver.driveToControlling();

  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));

  EXPECT_EQ(out.state, NavigationState::kControlling);
  EXPECT_TRUE(out.run_controller);
  EXPECT_EQ(out.velocity_source, VelocitySource::kController);
}

TEST(StateMachineControlling, NoValidCommandGoesBackToPlanning)
{
  Driver driver;
  driver.driveToControlling();

  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kNoValidCommand));

  EXPECT_EQ(out.state, NavigationState::kPlanning);
  EXPECT_TRUE(out.start_planner);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineControlling, ControllerPatienceSurvivesReplanLoop)
{
  // Vòng lặp CONTROLLING -> PLANNING -> CONTROLLING không được làm mới đồng hồ controller_patience,
  // nếu không thì controller hỏng vĩnh viễn sẽ không bao giờ dẫn tới recovery.
  Driver driver;
  driver.driveToControlling();

  for (int i = 0; i < 20; ++i)
  {
    driver.advance(0.15);
    const NavigationState state = driver.tick(controller(ControllerFeedback::kNoValidCommand)).state;
    if (state == NavigationState::kRecovering)
    {
      EXPECT_EQ(driver.last().recovery_trigger, RecoveryTrigger::kControllingFailed);
      SUCCEED();
      return;
    }
    ASSERT_EQ(state, NavigationState::kPlanning) << "round " << i;

    driver.advance(0.05);
    ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling) << "round " << i;
  }

  FAIL() << "controller_patience never expires — the replanning loop kept refreshing the clock";
}

TEST(StateMachineControlling, OscillationTimeoutEscalatesToRecovery)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_timeout = 1.0;   // [s]
  config.oscillation_distance = 0.5;  // [m]
  Driver driver(config);
  driver.driveToControlling();

  // Robot vẫn sinh được lệnh nhưng không đi đâu cả.
  driver.advance(0.5);
  ASSERT_EQ(driver.tick(controller(ControllerFeedback::kCommandValid)).state,
            NavigationState::kControlling);

  driver.advance(0.6);  // tổng 1.1 s > oscillation_timeout
  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
}

TEST(StateMachineControlling, UsesOscillationRouteIndependently)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_timeout = 1.0;
  config.oscillation_distance = 0.5;
  config.recovery_routes.planning_failed = {0};
  config.recovery_routes.controlling_failed = {0};
  config.recovery_routes.oscillation = {1};
  Driver driver(config);
  driver.driveToControlling();

  driver.advance(1.1);
  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kOscillation);
  EXPECT_EQ(out.recovery_index, 1u);
}

TEST(StateMachineControlling, MovingFarEnoughResetsOscillationClock)
{
  StateMachineConfig config = baseConfig();
  config.oscillation_timeout = 1.0;
  config.oscillation_distance = 0.5;
  Driver driver(config);
  driver.driveToControlling();

  for (int i = 0; i < 20; ++i)
  {
    driver.advance(0.5);
    StateMachineInput input = controller(ControllerFeedback::kCommandValid);
    input.travelled_since_oscillation_reset = 0.6;  // [m] đi đủ xa mỗi lần

    const StateMachineOutput out = driver.tick(input);
    ASSERT_EQ(out.state, NavigationState::kControlling) << "round " << i;
    ASSERT_TRUE(out.reset_oscillation_origin) << "round " << i;
  }
}

TEST(StateMachineControlling, NewPlanIsAppliedWithoutLeavingControlling)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput input = planReady();
  input.controller = ControllerFeedback::kCommandValid;
  const StateMachineOutput out = driver.tick(input);

  EXPECT_EQ(out.state, NavigationState::kControlling);
  EXPECT_TRUE(out.apply_plan);
}

TEST(StateMachineControlling, CancelMovesToCancelling)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput cancel = controller(ControllerFeedback::kCommandValid);
  cancel.cancel_requested = true;

  const StateMachineOutput out = driver.tick(cancel);
  EXPECT_EQ(out.state, NavigationState::kCancelling);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineControlling, PauseThenResumeReturnsToControlling)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput pause;
  pause.pause_requested = true;
  EXPECT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  StateMachineInput resume;
  resume.resume_requested = true;
  const StateMachineOutput out = driver.tick(resume);

  EXPECT_EQ(out.state, NavigationState::kControlling);
  EXPECT_TRUE(out.run_controller);
}

TEST(StateMachineControlling, LongPauseDoesNotBlowControllerPatience)
{
  // Tạm dừng 60 giây rồi tiếp tục không được bị tính là "controller hỏng 60 giây".
  Driver driver;
  driver.driveToControlling();

  StateMachineInput pause;
  pause.pause_requested = true;
  ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  driver.advance(60.0);

  StateMachineInput resume;
  resume.resume_requested = true;
  ASSERT_EQ(driver.tick(resume).state, NavigationState::kControlling);

  const StateMachineOutput out = driver.tick(controller(ControllerFeedback::kCommandValid));
  EXPECT_EQ(out.state, NavigationState::kControlling);
}

TEST(StateMachineControlling, LostPoseBlocksVelocityAndEventuallyRecovers)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput blind = controller(ControllerFeedback::kCommandValid);
  blind.pose_available = false;

  const StateMachineOutput first = driver.tick(blind);
  EXPECT_EQ(first.velocity_source, VelocitySource::kNone)
      << "when the robot position is unknown no source may publish velocity";
  EXPECT_FALSE(first.run_controller);

  // Mất TF kéo dài phải dẫn tới recovery, không được treo im lặng.
  driver.advance(3.0);
  const StateMachineOutput out = driver.tick(blind);
  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_EQ(out.recovery_trigger, RecoveryTrigger::kControllingFailed);
}

// ================================================================================================
// RECOVERING
// ================================================================================================

TEST(StateMachineRecovering, TicksWhileRunningAndOwnsVelocity)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);

  const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kRunning));

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_TRUE(out.tick_recovery);
  EXPECT_FALSE(out.run_controller);
  EXPECT_EQ(out.velocity_source, VelocitySource::kRecovery);
}

TEST(StateMachineRecovering, OneShotBehaviorDoesNotOwnVelocity)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);

  // Behavior đang chạy là loại không lái robot (đợi, xoá costmap).
  StateMachineInput failed = plannerFailed();
  failed.active_recovery_output = RecoveryOutputKind::kNone;
  ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering);

  // Ngay tại cycle KHỞI ĐỘNG recovery đã phải là kNone, không chờ tới tick sau: đổi nguồn vận tốc
  // qua lại tốn một cycle zero mỗi lần, và ở đây không có vận tốc nào để trao quyền.
  EXPECT_TRUE(driver.last().start_recovery);
  EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone);

  StateMachineInput running = recovery(RecoveryFeedback::kRunning);
  running.active_recovery_output = RecoveryOutputKind::kNone;
  const StateMachineOutput out = driver.tick(running);

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_TRUE(out.tick_recovery);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineRecovering, PathBehaviorDoesNotOwnVelocityEither)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);

  StateMachineInput failed = plannerFailed();
  failed.active_recovery_output = RecoveryOutputKind::kPath;
  ASSERT_EQ(driver.tick(failed).state, NavigationState::kRecovering);

  EXPECT_EQ(driver.last().velocity_source, VelocitySource::kNone);
}

TEST(StateMachineRecovering, SucceededGoesBackToPlanningWithNextIndex)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
  ASSERT_EQ(driver.last().recovery_index, 0u);

  const StateMachineOutput out = driver.tick(recovery(RecoveryFeedback::kSucceeded));

  EXPECT_EQ(out.state, NavigationState::kPlanning);
  EXPECT_TRUE(out.start_planner);
  EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u);
}

TEST(StateMachineRecovering, FailedAlsoAdvancesToNextBehavior)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);

  EXPECT_EQ(driver.tick(recovery(RecoveryFeedback::kFailed)).state, NavigationState::kPlanning);
  EXPECT_EQ(driver.machine().nextRecoveryIndex(), 1u);
}

TEST(StateMachineRecovering, ExhaustingAllBehaviorsAborts)
{
  StateMachineConfig config = baseConfig();
  config.recovery_behavior_count = 2;
  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  // Behavior 0
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
  ASSERT_EQ(driver.last().recovery_index, 0u);
  ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);

  // Behavior 1
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
  ASSERT_EQ(driver.last().recovery_index, 1u);
  ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);

  // Hết behavior
  driver.advance(2.0);
  const StateMachineOutput out = driver.tick(plannerFailed());

  EXPECT_EQ(out.state, NavigationState::kAborted);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
}

TEST(StateMachineRecovering, CancelMidRecoveryStopsBehaviorAndCancels)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);

  StateMachineInput cancel = recovery(RecoveryFeedback::kRunning);
  cancel.cancel_requested = true;
  const StateMachineOutput out = driver.tick(cancel);

  EXPECT_EQ(out.state, NavigationState::kCancelling);
  EXPECT_TRUE(out.cancel_recovery);
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

TEST(StateMachineRecovering, PauseMidRecoveryCancelsBehaviorAndResumesToPlanning)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);

  StateMachineInput pause = recovery(RecoveryFeedback::kRunning);
  pause.pause_requested = true;
  const StateMachineOutput paused = driver.tick(pause);

  EXPECT_EQ(paused.state, NavigationState::kPaused);
  EXPECT_TRUE(paused.cancel_recovery)
      << "keeping a behavior half-finished across a long pause is unsafe";

  StateMachineInput resume;
  resume.resume_requested = true;
  EXPECT_EQ(driver.tick(resume).state, NavigationState::kPlanning);
}

TEST(StateMachineRecovering, LostPoseStillTicksButBlocksVelocity)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);

  StateMachineInput blind = recovery(RecoveryFeedback::kRunning);
  blind.pose_available = false;
  const StateMachineOutput out = driver.tick(blind);

  EXPECT_EQ(out.state, NavigationState::kRecovering);
  EXPECT_TRUE(out.tick_recovery) << "still ticked so the behavior reports its own failure per its "
                                    "contract";
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

// ================================================================================================
// PAUSED / CANCELLING / terminal
// ================================================================================================

TEST(StateMachinePaused, CancelFromPausedGoesToCancelling)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput pause;
  pause.pause_requested = true;
  ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  StateMachineInput cancel;
  cancel.cancel_requested = true;
  EXPECT_EQ(driver.tick(cancel).state, NavigationState::kCancelling);
}

TEST(StateMachinePaused, StaysPausedIndefinitelyWithoutResume)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput pause;
  pause.pause_requested = true;
  ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  for (int i = 0; i < 50; ++i)
  {
    driver.advance(1.0);
    ASSERT_EQ(driver.idleTick().state, NavigationState::kPaused) << "round " << i;
    ASSERT_EQ(driver.last().velocity_source, VelocitySource::kNone);
  }
}

TEST(StateMachineCancelling, WaitsForRobotToStopBeforeCancelled)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput cancel;
  cancel.cancel_requested = true;
  ASSERT_EQ(driver.tick(cancel).state, NavigationState::kCancelling);

  StateMachineInput still_moving;
  still_moving.robot_stopped = false;
  EXPECT_EQ(driver.tick(still_moving).state, NavigationState::kCancelling);
  EXPECT_FALSE(driver.last().report_outcome);

  StateMachineInput stopped;
  stopped.robot_stopped = true;
  const StateMachineOutput out = driver.tick(stopped);

  EXPECT_EQ(out.state, NavigationState::kCancelled);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled);
}

TEST(StateMachineTerminal, OutcomeIsReportedExactlyOnce)
{
  Driver driver;
  driver.driveToControlling();

  const StateMachineOutput finish = driver.tick(controller(ControllerFeedback::kGoalReached));
  ASSERT_TRUE(finish.report_outcome);

  // Mọi cycle sau đó không được báo lại lần nữa.
  for (int i = 0; i < 10; ++i)
  {
    const StateMachineOutput out = driver.idleTick();
    ASSERT_FALSE(out.report_outcome) << "reported again at round " << i;
    ASSERT_EQ(out.state, NavigationState::kIdle);
  }
}

TEST(StateMachineTerminal, NewRequestAfterTerminalStartsFreshWithRecoveryIndexZero)
{
  Driver driver;
  StateMachineInput request;
  request.has_pending_request = true;
  driver.tick(request);

  // Đốt hết behavior để ABORTED.
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
  ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kRecovering);
  ASSERT_EQ(driver.tick(recovery(RecoveryFeedback::kSucceeded)).state, NavigationState::kPlanning);
  driver.advance(2.0);
  ASSERT_EQ(driver.tick(plannerFailed()).state, NavigationState::kAborted);

  // Yêu cầu mới phải bắt đầu lại từ behavior 0.
  const StateMachineOutput out = driver.tick(request);
  EXPECT_EQ(out.state, NavigationState::kPlanning);
  EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u);
}

TEST(StateMachineTerminal, TerminalStateLastsExactlyOneCycle)
{
  Driver driver;
  driver.driveToControlling();

  ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state,
            NavigationState::kSucceeded);
  EXPECT_EQ(driver.idleTick().state, NavigationState::kIdle);
}

// ================================================================================================
// EXECUTING_ACTIONS (D8) — action chạy sau goal, hoặc thay goal khi yêu cầu chỉ-có-action
// ================================================================================================

TEST(StateMachineActions, ActionOnlyRequestSkipsPlanningEntirely)
{
  Driver driver;

  const StateMachineOutput accepted = driver.tick(requestWithActions(1, /*has_goal=*/false));
  EXPECT_EQ(accepted.state, NavigationState::kExecutingActions);
  EXPECT_TRUE(accepted.accept_request);
  EXPECT_TRUE(accepted.start_action);
  EXPECT_EQ(accepted.action_index, 0u);
  EXPECT_FALSE(accepted.start_planner) << "with no goal there is nothing to plan";
  EXPECT_EQ(accepted.velocity_source, VelocitySource::kNone);

  ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
            NavigationState::kExecutingActions);
  EXPECT_TRUE(driver.last().tick_action);

  const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded));
  EXPECT_EQ(done.state, NavigationState::kSucceeded);
  EXPECT_TRUE(done.report_outcome);
  EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded);
}

TEST(StateMachineActions, RequestWithoutGoalOrActionsAbortsDefensively)
{
  Driver driver;

  const StateMachineOutput out = driver.tick(requestWithActions(0, /*has_goal=*/false));
  EXPECT_EQ(out.state, NavigationState::kAborted);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
  EXPECT_FALSE(out.start_action);
}

TEST(StateMachineActions, ActionsRunInOrderAfterGoalReached)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(2)).state, NavigationState::kPlanning);
  ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling);

  // Tới goal: chưa được SUCCEEDED — còn 2 action phải chạy, và chưa được báo kết quả.
  const StateMachineOutput at_goal = driver.tick(controller(ControllerFeedback::kGoalReached));
  ASSERT_EQ(at_goal.state, NavigationState::kExecutingActions);
  EXPECT_FALSE(at_goal.report_outcome);
  EXPECT_TRUE(at_goal.start_action);
  EXPECT_EQ(at_goal.action_index, 0u);

  // Action 0 xong -> khởi động action 1, vẫn ở kExecutingActions.
  const StateMachineOutput next = driver.tick(actionFb(ActionFeedback::kSucceeded));
  ASSERT_EQ(next.state, NavigationState::kExecutingActions);
  EXPECT_TRUE(next.start_action);
  EXPECT_EQ(next.action_index, 1u);
  EXPECT_FALSE(next.report_outcome);

  // Action cuối xong -> SUCCEEDED, báo kết quả đúng một lần.
  const StateMachineOutput done = driver.tick(actionFb(ActionFeedback::kSucceeded));
  EXPECT_EQ(done.state, NavigationState::kSucceeded);
  EXPECT_TRUE(done.report_outcome);
  EXPECT_EQ(done.outcome, NavigationOutcome::kSucceeded);
}

TEST(StateMachineActions, ActionFailureAbortsWithoutRecovery)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(1)).state, NavigationState::kPlanning);
  ASSERT_EQ(driver.tick(planReady()).state, NavigationState::kControlling);
  ASSERT_EQ(driver.tick(controller(ControllerFeedback::kGoalReached)).state,
            NavigationState::kExecutingActions);

  const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kFailed));
  EXPECT_EQ(out.state, NavigationState::kAborted);
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
  EXPECT_FALSE(out.start_recovery) << "recovery repairs navigation, it cannot rescue an action";
}

TEST(StateMachineActions, CancelDuringActionCancelsPortAndEndsCancelled)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  StateMachineInput cancel;
  cancel.cancel_requested = true;
  const StateMachineOutput cancelling = driver.tick(cancel);
  EXPECT_EQ(cancelling.state, NavigationState::kCancelling);
  EXPECT_TRUE(cancelling.cancel_action);

  StateMachineInput stopped;
  stopped.robot_stopped = true;
  const StateMachineOutput out = driver.tick(stopped);
  EXPECT_EQ(out.state, NavigationState::kCancelled);
  EXPECT_EQ(out.outcome, NavigationOutcome::kCancelled);
}

TEST(StateMachineActions, PauseDuringActionFreezesWithoutCancelling)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  StateMachineInput pause;
  pause.pause_requested = true;
  const StateMachineOutput paused = driver.tick(pause);
  EXPECT_EQ(paused.state, NavigationState::kPaused);
  EXPECT_FALSE(paused.cancel_action) << "the action is not idempotent — pausing must not cancel it";
  EXPECT_FALSE(paused.tick_action);

  StateMachineInput resume;
  resume.resume_requested = true;
  const StateMachineOutput resumed = driver.tick(resume);
  EXPECT_EQ(resumed.state, NavigationState::kExecutingActions);
  EXPECT_TRUE(resumed.tick_action) << "resume keeps ticking the unfinished action";
  EXPECT_FALSE(resumed.start_action) << "an action already in progress must not be started again";
}

TEST(StateMachineActions, ActionPatienceIsDisabledByDefault)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  // Action dài hợp lệ (ví dụ sạc pin): mặc định không có trần nào ở tầng navigation.
  driver.advance(3600.0);
  const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
  EXPECT_EQ(out.state, NavigationState::kExecutingActions);
  EXPECT_TRUE(out.tick_action);
}

TEST(StateMachineActions, ActionPatienceAbortsStuckActionAndCancelsPort)
{
  StateMachineConfig config = baseConfig();
  config.action_patience = 1.0;  // [s]

  Driver driver(config);
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  driver.advance(0.5);
  ASSERT_EQ(driver.tick(actionFb(ActionFeedback::kRunning)).state,
            NavigationState::kExecutingActions);

  driver.advance(1.0);  // Tổng 1.5 s > action_patience.
  const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
  EXPECT_EQ(out.state, NavigationState::kAborted);
  EXPECT_TRUE(out.cancel_action) << "the port must be told to stop the device safely before "
                                    "finishing";
  EXPECT_TRUE(out.report_outcome);
  EXPECT_EQ(out.outcome, NavigationOutcome::kFailed);
}

TEST(StateMachineActions, PauseRearmsActionPatienceClock)
{
  StateMachineConfig config = baseConfig();
  config.action_patience = 1.0;  // [s]

  Driver driver(config);
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  StateMachineInput pause;
  pause.pause_requested = true;
  ASSERT_EQ(driver.tick(pause).state, NavigationState::kPaused);

  // Dừng lâu hơn hẳn action_patience rồi mới resume — quãng dừng không được tính vào trần.
  driver.advance(30.0);
  StateMachineInput resume;
  resume.resume_requested = true;
  ASSERT_EQ(driver.tick(resume).state, NavigationState::kExecutingActions);

  driver.advance(0.5);  // Mới 0.5 s sau resume, chưa chạm trần.
  const StateMachineOutput out = driver.tick(actionFb(ActionFeedback::kRunning));
  EXPECT_EQ(out.state, NavigationState::kExecutingActions) << "must not be wrongly ABORTED after "
                                                              "resume";
  EXPECT_TRUE(out.tick_action);
}

TEST(StateMachineActions, ActionTicksWithoutPoseAndVelocityStaysZero)
{
  Driver driver;
  ASSERT_EQ(driver.tick(requestWithActions(1, /*has_goal=*/false)).state,
            NavigationState::kExecutingActions);

  StateMachineInput no_pose;
  no_pose.pose_available = false;
  const StateMachineOutput out = driver.tick(no_pose);

  EXPECT_EQ(out.state, NavigationState::kExecutingActions);
  EXPECT_TRUE(out.tick_action) << "operating a device in place needs no localization";
  EXPECT_EQ(out.velocity_source, VelocitySource::kNone);
}

// ================================================================================================
// Bất biến áp cho mọi state
// ================================================================================================

TEST(StateMachineInvariants, NeverRunsControllerRecoveryOrActionSimultaneously)
{
  // Quét một chuỗi sự kiện dài, đủ để đi qua mọi state — kể cả kExecutingActions (D8) — và kiểm
  // bất biến ở từng cycle.
  Driver driver;

  const StateMachineInput sequence[] = {
    requestWithActions(1),
    planReady(),
    controller(ControllerFeedback::kCommandValid),
    controller(ControllerFeedback::kNoValidCommand),
    plannerFailed(),
    plannerFailed(),
    recovery(RecoveryFeedback::kRunning),
    recovery(RecoveryFeedback::kSucceeded),
    planReady(),
    controller(ControllerFeedback::kGoalReached),
    actionFb(ActionFeedback::kRunning),
    actionFb(ActionFeedback::kSucceeded),
  };

  for (const auto& input : sequence)
  {
    driver.advance(0.6);
    const StateMachineOutput out = driver.tick(input);

    ASSERT_FALSE(out.run_controller && out.tick_recovery)
        << "state " << move_base2::toString(out.state) << ": two command sources running at once";
    ASSERT_FALSE(out.run_controller && out.tick_action)
        << "state " << move_base2::toString(out.state) << ": controller running together with an "
                                                          "action";
    ASSERT_FALSE(out.tick_recovery && out.tick_action)
        << "state " << move_base2::toString(out.state) << ": recovery running together with an "
                                                          "action";

    if (move_base2::mustBeStopped(out.state))
    {
      ASSERT_EQ(out.velocity_source, VelocitySource::kNone)
          << "state " << move_base2::toString(out.state) << " must be stopped";
    }
    if (out.velocity_source == VelocitySource::kController)
    {
      ASSERT_EQ(out.state, NavigationState::kControlling);
    }
    if (out.velocity_source == VelocitySource::kRecovery)
    {
      ASSERT_EQ(out.state, NavigationState::kRecovering);
    }
  }
}

TEST(StateMachineInvariants, ResetReturnsToPristineState)
{
  Driver driver;
  driver.driveToControlling();
  driver.tick(controller(ControllerFeedback::kCommandValid));

  driver.machine().reset();

  EXPECT_EQ(driver.machine().state(), NavigationState::kIdle);
  EXPECT_EQ(driver.machine().nextRecoveryIndex(), 0u);
  EXPECT_EQ(driver.machine().planningRetries(), 0);
}

TEST(StateMachineInvariants, EveryStateHasANonEmptyName)
{
  const NavigationState states[] = {
    NavigationState::kIdle,       NavigationState::kPlanning,         NavigationState::kControlling,
    NavigationState::kRecovering, NavigationState::kExecutingActions, NavigationState::kPaused,
    NavigationState::kCancelling, NavigationState::kSucceeded,        NavigationState::kAborted,
    NavigationState::kCancelled,
  };

  for (const auto state : states)
  {
    const std::string name = move_base2::toString(state);
    EXPECT_FALSE(name.empty());
    EXPECT_NE(name, "UNKNOWN");
  }
}

// ================================================================================================
// kBusy — planner bất đồng bộ
// ================================================================================================

TEST(StateMachineAsyncPlanner, BusyDoesNotLeavePlanning)
{
  // kBusy phải hành xử đúng như kIdle ở mọi điểm quyết định: chờ tiếp, KHÔNG tính là lượt hỏng.
  Driver driver;

  StateMachineInput request;
  request.has_pending_request = true;
  ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning);

  StateMachineInput busy;
  busy.planner = PlannerFeedback::kBusy;

  for (int i = 1; i <= 5; ++i)
  {
    driver.advance(0.05);
    EXPECT_EQ(driver.tick(busy).state, NavigationState::kPlanning)
        << "left PLANNING at cycle " << i;
  }
}

TEST(StateMachineAsyncPlanner, BusyDoesNotCountAsAFailedPlanningAttempt)
{
  StateMachineConfig config = baseConfig();
  config.max_planning_retries = 2;
  config.planner_patience = 100.0;  // [s] loại đường kiên nhẫn để chỉ còn đếm lượt

  Driver driver(config);

  StateMachineInput request;
  request.has_pending_request = true;
  ASSERT_EQ(driver.tick(request).state, NavigationState::kPlanning);

  StateMachineInput busy;
  busy.planner = PlannerFeedback::kBusy;

  for (int i = 1; i <= 10; ++i)
  {
    driver.advance(0.05);
    driver.tick(busy);
  }

  EXPECT_EQ(driver.machine().state(), NavigationState::kPlanning)
      << "kBusy was counted as a failed planning attempt so max_planning_retries ran out";
}

TEST(StateMachineAsyncPlanner, BusyWhileControllingKeepsFollowingTheCurrentPlan)
{
  Driver driver;
  driver.driveToControlling();

  StateMachineInput busy;
  busy.planner = PlannerFeedback::kBusy;
  busy.controller = ControllerFeedback::kCommandValid;

  driver.advance(0.05);
  const StateMachineOutput out = driver.tick(busy);

  EXPECT_EQ(out.state, NavigationState::kControlling);
  EXPECT_FALSE(out.apply_plan) << "pushed a plan down to the controller while the planner had no "
                                  "plan yet";
}

TEST(StateMachineConfigTest, RejectsDisablingEveryHungPlannerDetector)
{
  // Từ khi planner chạy trên thread riêng, đây là thứ duy nhất phát hiện được plugin không bao giờ
  // trả lời. Ở chế độ đồng bộ trước đây, planner treo làm treo luôn control loop — hỏng thì thấy ngay.
  StateMachineConfig config = baseConfig();
  config.planner_patience = 0.0;
  config.max_planning_retries = -1;

  std::string error;
  EXPECT_FALSE(config.validate(error));
  EXPECT_NE(error.find("hung planner"), std::string::npos) << error;
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}