/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — hiện thực ScenarioDriver để chạy kịch bản khai báo của nav_test_harness.
 *
 * Đặt trong test/ của move_base2 chứ không trong harness: harness không được phụ thuộc ngược vào
 * package tiêu thụ nào. Harness biết cách CHẠY và cách KIỂM; "chạy cái gì" do bên tiêu thụ hiện
 * thực — đây chính là chỗ đó.
 *
 * Author: DuongTD
 *********************************************************************/
#ifndef MOVE_BASE2_TEST_SCENARIO_DRIVER_H_
#define MOVE_BASE2_TEST_SCENARIO_DRIVER_H_

#include <cmath>
#include <cstddef>
#include <string>
#include <vector>

#include <nav_test_harness/scenario.h>
#include <nav_test_harness/scenario_runner.h>

#include <move_base2/control_loop.h>

#include "fake_ports.h"

namespace move_base2
{
namespace testing
{

/**
 * @class MoveBase2ScenarioDriver
 * @brief Chạy một kịch bản qua @ref ControlLoop với toàn bộ cổng giả.
 *
 * Kịch bản mô tả **hành vi của các cổng**, không mô tả thế giới vật lý: `planner_script` là chuỗi
 * kết quả `makePlan`, `controller_script` là chuỗi kết quả `computeVelocityCommands`. Nhờ vậy các
 * bất biến an toàn (mất TF -> dừng, NaN -> chặn, vượt trần -> clamp) kiểm được **xác định**, không
 * phụ thuộc vào việc dựng đúng một tình huống hình học.
 *
 * @note `obstacles` trong kịch bản **chưa được dùng** ở driver này: cổng recovery là fake theo kịch
 *       bản nên không tra costmap. Kịch bản cần va chạm thật (`obstacle_behind_during_backup`) phải
 *       chạy qua một driver khác nạp `recovery_core` thật kèm `FakeCollisionChecker` — thuộc phần
 *       sau của Phase 5. Driver này **báo lỗi setup** nếu kịch bản khai obstacle, thay vì lặng lẽ
 *       chạy một ca test không kiểm đúng thứ nó nói là đang kiểm.
 */
class MoveBase2ScenarioDriver final : public nav_test_harness::ScenarioDriver
{
public:
  bool setup(const nav_test_harness::Scenario& scenario, std::string& error) override
  {
    scenario_ = scenario;

    if (!scenario.obstacles.empty())
    {
      error = "this driver does not simulate obstacles (the recovery port is faked by the "
              "scenario); use the driver with the real recovery_core for collision scenarios";
      return false;
    }

    std::vector<PlannerScript> planner_script;
    for (const std::string& item : scenario.planner_script)
    {
      if (item == "ok")
      {
        planner_script.push_back(PlannerScript::kOk);
      }
      else if (item == "fail")
      {
        planner_script.push_back(PlannerScript::kFail);
      }
      else if (item == "empty")
      {
        planner_script.push_back(PlannerScript::kEmpty);
      }
      else
      {
        error = "unknown planner_script: '" + item + "'";
        return false;
      }
    }

    std::vector<ControllerScript> controller_script;
    for (const std::string& item : scenario.controller_script)
    {
      if (item == "ok")
      {
        controller_script.push_back(ControllerScript::kOk);
      }
      else if (item == "fail")
      {
        controller_script.push_back(ControllerScript::kFail);
      }
      else if (item == "goal_reached")
      {
        controller_script.push_back(ControllerScript::kGoalReached);
      }
      else if (item == "nan")
      {
        controller_script.push_back(ControllerScript::kNaN);
      }
      else if (item == "too_fast")
      {
        controller_script.push_back(ControllerScript::kTooFast);
      }
      else
      {
        error = "unknown controller_script: '" + item + "'";
        return false;
      }
    }

    std::vector<RecoveryScript> recovery_script;
    for (const std::string& item : scenario.recovery_script)
    {
      if (item == "running")
      {
        recovery_script.push_back(RecoveryScript::kRunning);
      }
      else if (item == "succeeded")
      {
        recovery_script.push_back(RecoveryScript::kSucceeded);
      }
      else if (item == "failed")
      {
        recovery_script.push_back(RecoveryScript::kFailed);
      }
      else
      {
        error = "unknown recovery_script: '" + item + "'";
        return false;
      }
    }

    for (const nav_test_harness::ScenarioEvent& event : scenario.events)
    {
      if (event.action != "cancel" && event.action != "pause" && event.action != "resume" &&
          event.action != "lose_pose" && event.action != "restore_pose" &&
          event.action != "sensors_stale" && event.action != "sensors_ok")
      {
        error = "events: unknown action: '" + event.action + "'";
        return false;
      }
    }

    planner_.setScript(planner_script);
    controller_.setScript(controller_script);
    recovery_.setScript(recovery_script);

    // Recovery thế hệ 2 có thể tự lái. Kịch bản nào khai `recovery_velocity` thì behavior được coi
    // là họ velocity; 0 nghĩa là behavior chỉ đợi/xoá costmap và lõi phải giữ nguồn vận tốc kNone.
    const bool recovery_drives = std::abs(scenario.recovery_velocity) > 0.0;
    recovery_.setRecoveryVelocity(recovery_drives, scenario.recovery_velocity);  // [m/s]
    recovery_.setDefaultOutputKind(recovery_drives ? RecoveryOutputKind::kVelocity
                                                   : RecoveryOutputKind::kNone);

    pose_.setPosition(scenario.initial_pose.x, scenario.initial_pose.y);

    ControlLoopConfig config;
    config.nominal_control_period = scenario.control_period;  // [s]
    config.state_machine.planner_patience = 0.5;              // [s]
    config.state_machine.controller_patience = 0.5;           // [s]
    config.state_machine.oscillation_timeout = 0.0;           // tắt trừ khi kịch bản cần
    config.state_machine.oscillation_distance = 0.5;          // [m]
    config.state_machine.max_planning_retries = -1;
    config.state_machine.recovery_behavior_count = 2;
    config.state_machine.recovery_enabled = true;

    // Trần vận tốc lấy từ kỳ vọng của kịch bản: `expect_max_speed` vừa là điều được kiểm, vừa là
    // trần mà arbiter phải áp — nếu hai con số đó lệch nhau thì ca test không kiểm đúng thứ nó nói.
    config.velocity.max_vel_x = scenario.expect_max_speed > 0.0 ? scenario.expect_max_speed : 0.5;
    config.velocity.min_vel_x = -config.velocity.max_vel_x;
    config.velocity.max_vel_theta =
        scenario.expect_max_yaw_rate > 0.0 ? scenario.expect_max_yaw_rate : 1.0;
    config.velocity.max_accel_x = 100.0;      // [m/s^2] lớn: kịch bản kiểm state và trần, không kiểm ramp
    config.velocity.max_accel_theta = 100.0;  // [rad/s^2]

    config.position.global_planner_name = "ScenarioGlobalPlanner";
    config.position.local_planner_name = "ScenarioLocalPlanner";
    config.docking = config.position;
    config.go_straight = config.position;
    config.rotate = config.position;

    deps_.clock = &clock_;
    deps_.pose = &pose_;
    deps_.planner = &planner_;
    deps_.controller = &controller_;
    deps_.recovery = &recovery_;
    deps_.mission = &mission_;
    deps_.action = &action_;
    deps_.costmap_status = &costmap_status_;

    if (!loop_.configure(config, deps_, error))
    {
      return false;
    }

    NavigationRequest request;
    request.profile = MotionProfile::kPosition;
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = scenario.goal.x;  // [m]
    request.goal.pose.position.y = scenario.goal.y;  // [m]
    request.goal.pose.orientation.z = std::sin(scenario.goal.theta * 0.5);
    request.goal.pose.orientation.w = std::cos(scenario.goal.theta * 0.5);

    if (!loop_.submit(request, error))
    {
      return false;
    }

    cycle_ = 0;
    started_ = false;
    return true;
  }

  bool step(nav_test_harness::ScenarioStep& step) override
  {
    if (!started_)
    {
      // Ảnh chụp TRƯỚC khi cycle nào chạy. Không phải chi tiết kỹ thuật: nó là bằng chứng rằng lõi
      // chưa phát lệnh nào trước khi nhận yêu cầu — và không có nó thì `expect_no_cmd_in_states`
      // không kiểm được state ban đầu, vì state đó không bao giờ xuất hiện trong trace.
      started_ = true;
      step.cycle = 0;
      step.state = toString(loop_.state());
      step.linear_x = 0.0;
      step.angular_z = 0.0;
      return true;
    }

    applyEventsFor(cycle_);

    const bool running = loop_.step();

    step.cycle = cycle_;
    step.state = toString(loop_.state());
    step.linear_x = loop_.lastCommand().linear.x;    // [m/s]
    step.angular_z = loop_.lastCommand().angular.z;  // [rad/s]

    clock_.advance(scenario_.control_period);
    ++cycle_;
    return running;
  }

  std::string outcome() const override
  {
    const char* text = loop_.lastOutcome();
    return text != nullptr ? std::string(text) : std::string();
  }

private:
  void applyEventsFor(std::size_t cycle)
  {
    for (const nav_test_harness::ScenarioEvent& event : scenario_.events)
    {
      if (event.cycle != cycle)
      {
        continue;
      }
      if (event.action == "cancel")
      {
        loop_.requestCancel();
      }
      else if (event.action == "pause")
      {
        loop_.requestPause();
      }
      else if (event.action == "resume")
      {
        loop_.requestResume();
      }
      else if (event.action == "lose_pose")
      {
        pose_.setAvailable(false);
      }
      else if (event.action == "restore_pose")
      {
        pose_.setAvailable(true);
      }
      else if (event.action == "sensors_stale")
      {
        // Observation buffer của costmap hết hạn — lõi phải ngừng cho lái bánh xe.
        costmap_status_.setCurrent(false);
      }
      else if (event.action == "sensors_ok")
      {
        costmap_status_.setCurrent(true);
      }
    }
  }

  nav_test_harness::Scenario scenario_;

  ControlLoop loop_;
  ControlLoopDeps deps_;
  FakeClockPort clock_;
  FakePosePort pose_;
  FakePlannerPort planner_;
  FakeControllerPort controller_;
  FakeRecoveryPort recovery_{ 2 };
  FakeMissionPort mission_;
  FakeActionPort action_;
  FakeCostmapStatusPort costmap_status_;

  std::size_t cycle_ = 0;
  bool started_ = false;
};

}  // namespace testing
}  // namespace move_base2

#endif  // MOVE_BASE2_TEST_SCENARIO_DRIVER_H_