// 本文件测试板卡运动控制适配层。验证 MotionControlBridge 是否会在处理请求前
// 发布当前位姿/状态，并在合法写模式、目标位姿和纠偏量后调用板卡适配接口。

#include "line_laser_modbus/host.hpp"
#include "line_laser_modbus/motion_adapter.hpp"
#include "test_support.hpp"

#include <iostream>

using namespace line_laser_modbus;

namespace {

class FakeMotion final : public MotionControlAdapter {
 public:
  void on_mode_changed(const WorkMode mode) override {
    last_mode = mode;
    ++mode_calls;
  }

  void on_target_pose(const Pose6D& pose) override {
    last_target = pose;
    ++target_calls;
  }

  void on_correction(const Pose6D& correction) override {
    last_correction = correction;
    ++correction_calls;
  }

  [[nodiscard]] Pose6D current_pose() const override {
    return pose;
  }

  [[nodiscard]] DeviceState current_state() const override {
    return state;
  }

  Pose6D pose{123U, 1.0F, 2.0F, 3.0F, 4.0F, 5.0F, 6.0F};
  DeviceState state = DeviceState::OnlineTrackingNormal;
  WorkMode last_mode = WorkMode::StandbyReset;
  Pose6D last_target{};
  Pose6D last_correction{};
  int mode_calls = 0;
  int target_calls = 0;
  int correction_calls = 0;
};

void test_bridge_publishes_feedback_before_read() {
  HostClient host;
  DeviceServer server;
  FakeMotion motion;
  MotionControlBridge bridge(server, motion);

  const auto response = bridge.process_request(host.make_read_current_pose_request());
  const auto parsed_pose = host.parse_current_pose_response(response);

  test_support::require_true(parsed_pose.has_value(),
                             "bridge must return current pose response");
  test_support::require_equal(parsed_pose->timestamp_ms, 123U,
                              "bridge must publish adapter timestamp");
  test_support::require_float_close(parsed_pose->x, 1.0F,
                                    "bridge must publish adapter pose x");
}

void test_bridge_notifies_mode_after_valid_write() {
  HostClient host;
  DeviceServer server;
  FakeMotion motion;
  MotionControlBridge bridge(server, motion);

  const auto response =
      bridge.process_request(host.make_write_mode_request(WorkMode::OnlineTracking));

  test_support::require_true(host.parse_write_ack(response, kModeCommandAddress, 1U),
                             "valid mode write must be acknowledged");
  test_support::require_equal(motion.mode_calls, 1,
                              "bridge must notify mode write once");
  test_support::require_true(motion.last_mode == WorkMode::OnlineTracking,
                             "bridge must pass written mode to adapter");
}

void test_bridge_notifies_pose_writes() {
  HostClient host;
  DeviceServer server;
  FakeMotion motion;
  MotionControlBridge bridge(server, motion);

  const Pose6D target{200U, 10.0F, 20.0F, 30.0F, 1.0F, 2.0F, 3.0F};
  const auto target_response =
      bridge.process_request(host.make_write_target_pose_request(target));
  test_support::require_true(
      host.parse_write_ack(target_response, kTargetPoseAddress, kPoseRegisterCount),
      "target pose write must be acknowledged");
  test_support::require_equal(motion.target_calls, 1,
                              "bridge must notify target pose write");
  test_support::require_equal(motion.last_target.timestamp_ms, 200U,
                              "bridge must pass target timestamp");
  test_support::require_float_close(motion.last_target.z, 30.0F,
                                    "bridge must pass target z");

  const Pose6D correction{201U, 0.5F, -0.25F, 1.5F, 0.0F, 0.0F, 0.0F};
  const auto correction_response =
      bridge.process_request(host.make_write_correction_request(correction));
  test_support::require_true(
      host.parse_write_ack(correction_response, kCorrectionAddress,
                           kPoseRegisterCount),
      "correction write must be acknowledged");
  test_support::require_equal(motion.correction_calls, 1,
                              "bridge must notify correction write");
  test_support::require_float_close(motion.last_correction.y, -0.25F,
                                    "bridge must pass correction y");
}

void test_bridge_does_not_notify_rejected_mode_write() {
  HostClient host;
  DeviceServer server;
  FakeMotion motion;
  MotionControlBridge bridge(server, motion);

  const auto emergency_response =
      bridge.process_request(host.make_write_mode_request(WorkMode::EmergencyStop));
  test_support::require_true(
      host.parse_write_ack(emergency_response, kModeCommandAddress, 1U),
      "emergency mode write must be acknowledged");
  const auto rejected =
      bridge.process_request(host.make_write_mode_request(WorkMode::OnlineTracking));

  test_support::require_true(!host.parse_write_ack(rejected, kModeCommandAddress, 1U),
                             "illegal mode transition must not be acknowledged");
  test_support::require_equal(motion.mode_calls, 1,
                              "bridge must not notify rejected mode write");
  test_support::require_true(motion.last_mode == WorkMode::EmergencyStop,
                             "last notified mode must remain emergency");
}

}  // 匿名命名空间

int main() {
  test_bridge_publishes_feedback_before_read();
  test_bridge_notifies_mode_after_valid_write();
  test_bridge_notifies_pose_writes();
  test_bridge_does_not_notify_rejected_mode_write();

  std::cout << "adapter_tests passed\n";
  return 0;
}