/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * recovery_core — tiện ích dùng chung cho test.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_
#define RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_

#include <cmath>
#include <string>
#include <utility>
#include <vector>

#include <nav_test_harness/fake_clock.h>
#include <nav_test_harness/fake_collision_checker.h>
#include <nav_test_harness/fake_costmap.h>
#include <nav_test_harness/fake_pose_provider.h>

#include <recovery_core/recovery_behavior.h>
#include <recovery_core/recovery_context.h>
#include <recovery_core/recovery_math.h>
#include <recovery_core/recovery_registry.h>

namespace recovery_test
{

/// @brief Behavior trong @p registry mang tên @p name, hoặc nullptr.
inline recovery_core::RecoveryBehavior* findBehavior(const recovery_core::RecoveryRegistry& registry,
                                                     const std::string& name)
{
  for (std::size_t i = 0; i < registry.size(); ++i)
  {
    if (registry.nameAt(i) == name)
    {
      return registry.at(i);
    }
  }
  return nullptr;
}

/**
 * @brief Nối `nav_test_harness::FakePoseProvider` vào cổng của recovery_core.
 *
 * Hai interface cố ý tách nhau: `recovery_core` không được phụ thuộc gói test harness, và
 * `nav_test_harness` phục vụ nhiều gói khác nhau. Adapter mỏng ở đây chính là thứ `RecoveryRunner`
 * sẽ làm với `move_base2::PosePort` ở Phase 4.
 */
class HarnessPoseProvider final : public recovery_core::PoseProvider
{
public:
  explicit HarnessPoseProvider(nav_test_harness::FakePoseProvider* fake) : fake_(fake)
  {
  }

  bool getRobotPose(robot_geometry_msgs::PoseStamped& pose) const override
  {
    return fake_ != nullptr && fake_->getRobotPose(pose);
  }

private:
  nav_test_harness::FakePoseProvider* fake_ = nullptr;
};

/// @brief Nối `nav_test_harness::FakeCollisionChecker` vào cổng của recovery_core.
class HarnessCollisionChecker final : public recovery_core::CollisionChecker
{
public:
  explicit HarnessCollisionChecker(nav_test_harness::FakeCollisionChecker* fake) : fake_(fake)
  {
  }

  double footprintCost(double x, double y, double theta) const override
  {
    return fake_ == nullptr ? -1.0 : fake_->footprintCost(x, y, theta);
  }

private:
  nav_test_harness::FakeCollisionChecker* fake_ = nullptr;
};

/// @brief Nguồn plan đơn giản do test bơm thẳng.
class StubPlanProvider final : public recovery_core::PlanProvider
{
public:
  void setPlan(std::vector<robot_geometry_msgs::PoseStamped> plan)
  {
    plan_ = std::move(plan);
  }

  bool getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped>& out) const override
  {
    if (plan_.empty())
    {
      return false;
    }
    out = plan_;
    return true;
  }

private:
  std::vector<robot_geometry_msgs::PoseStamped> plan_;
};

/**
 * @brief Behavior giả, cho phép test điều khiển từng hook.
 *
 * Dùng để kiểm phần **base** (guard vòng đời, timeout, cưỡng chế họ output) mà không phụ thuộc vào
 * hành vi của plugin thật.
 */
class MockBehavior final : public recovery_core::RecoveryBehavior
{
public:
  explicit MockBehavior(recovery_core::RecoveryOutputType kind) : kind_(kind)
  {
    next_result = recovery_core::RecoveryResult::Running();
  }

  recovery_core::RecoveryOutputType outputKind() const override
  {
    return kind_;
  }

  // Núm điều khiển cho test.
  bool configure_ok = true;
  bool start_ok = true;
  recovery_core::RecoveryResult next_result;

  // Ghi nhận để assert.
  int configure_calls = 0;
  int start_calls = 0;
  int update_calls = 0;
  int cancel_calls = 0;
  double last_dt = -1.0;
  recovery_core::RecoveryGoal last_goal;

protected:
  bool onConfigure(robot::NodeHandle& /*nh*/) override
  {
    ++configure_calls;
    return configure_ok;
  }

  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
    ++start_calls;
    last_goal = goal;
    return start_ok;
  }

  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
  {
    ++update_calls;
    last_dt = dt;
    return next_result;
  }

  recovery_core::RecoveryResult onCancel() override
  {
    ++cancel_calls;
    return recovery_core::RecoveryBehavior::onCancel();
  }

private:
  recovery_core::RecoveryOutputType kind_;
};

/**
 * @brief Bộ đồ nghề đầy đủ cho một test plugin họ velocity.
 *
 * Gom costmap giả, pose giả, collision checker giả và đồng hồ giả, kèm hàm mô phỏng robot chạy
 * theo đúng lệnh vận tốc mà behavior phát ra.
 */
struct VelocityRig
{
  VelocityRig(double span_m = 8.0, double resolution = 0.05,
              double footprint_length = 0.6, double footprint_width = 0.4)
    : costmap(nav_test_harness::FakeCostmap::centered(span_m, resolution))
    , checker(&costmap,
              nav_test_harness::FakeCollisionChecker::rectangleFootprint(footprint_length,
                                                                        footprint_width))
    , pose_port(&pose)
    , collision_port(&checker)
  {
    ctx.pose = &pose_port;
    ctx.collision = &collision_port;
    ctx.plan = &plan;
  }

  /// @brief Cho robot chạy theo @p command trong @p dt giây, cập nhật pose giả.
  void applyCommand(const robot_geometry_msgs::Twist& command, double dt)
  {
    const double yaw = pose.rawPose().theta;
    pose.moveBy(command.linear.x * std::cos(yaw) * dt, command.linear.x * std::sin(yaw) * dt,
                command.angular.z * dt);
  }

  nav_test_harness::FakeCostmap costmap;
  nav_test_harness::FakeCollisionChecker checker;
  nav_test_harness::FakePoseProvider pose;
  nav_test_harness::FakeClock clock;
  StubPlanProvider plan;

  HarnessPoseProvider pose_port;
  HarnessCollisionChecker collision_port;
  recovery_core::RecoveryContext ctx;
};

}  // namespace recovery_test

#endif  // RECOVERY_CORE_TEST_RECOVERY_TEST_UTILS_H_