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

#include <iomanip>
#include <iostream>

namespace {

void print_frame(const char* title, const line_laser_modbus::ByteVector& frame) {
  std::cout << title;
  for (const std::uint8_t byte : frame) {
    std::cout << std::hex << std::uppercase << std::setw(2) << std::setfill('0')
              << static_cast<int>(byte) << ' ';
  }
  std::cout << std::dec << '\n';
}

class DemoBoardMotion final : public line_laser_modbus::MotionControlAdapter {
 public:
  void on_mode_changed(const line_laser_modbus::WorkMode mode) override {
    mode_ = mode;
    std::cout << "adapter: mode changed to "
              << static_cast<std::uint16_t>(mode_) << '\n';
  }

  void on_target_pose(const line_laser_modbus::Pose6D& pose) override {
    target_pose_ = pose;
    std::cout << "adapter: target pose timestamp "
              << target_pose_.timestamp_ms << '\n';
  }

  void on_correction(const line_laser_modbus::Pose6D& correction) override {
    correction_ = correction;
    current_pose_.x += correction_.x;
    current_pose_.y += correction_.y;
    current_pose_.z += correction_.z;
    std::cout << "adapter: correction applied dx=" << correction_.x
              << ", dy=" << correction_.y << ", dz=" << correction_.z << '\n';
  }

  [[nodiscard]] line_laser_modbus::Pose6D current_pose() const override {
    return current_pose_;
  }

  [[nodiscard]] line_laser_modbus::DeviceState current_state() const override {
    if (mode_ == line_laser_modbus::WorkMode::OnlineTracking) {
      return line_laser_modbus::DeviceState::OnlineTrackingNormal;
    }
    return line_laser_modbus::DeviceState::StandbyReady;
  }

 private:
  line_laser_modbus::WorkMode mode_ =
      line_laser_modbus::WorkMode::StandbyReset;
  line_laser_modbus::Pose6D current_pose_{100U, 10.0F, 20.0F, 30.0F,
                                          1.0F, 2.0F, 3.0F};
  line_laser_modbus::Pose6D target_pose_{};
  line_laser_modbus::Pose6D correction_{};
};

}  // 匿名命名空间

int main() {
  line_laser_modbus::HostClient host;
  line_laser_modbus::DeviceServer server;
  DemoBoardMotion board;
  line_laser_modbus::MotionControlBridge bridge(server, board);

  const auto mode_request =
      host.make_write_mode_request(line_laser_modbus::WorkMode::OnlineTracking);
  const auto mode_response = bridge.process_request(mode_request);
  print_frame("write mode response: ", mode_response);

  const line_laser_modbus::Pose6D target{200U, 100.0F, 200.0F, 300.0F,
                                         10.0F, 20.0F, 30.0F};
  const auto target_response =
      bridge.process_request(host.make_write_target_pose_request(target));
  print_frame("write target response: ", target_response);

  const line_laser_modbus::Pose6D correction{250U, 0.5F, -0.25F, 1.0F,
                                             0.0F, 0.0F, 0.0F};
  const auto correction_response =
      bridge.process_request(host.make_write_correction_request(correction));
  print_frame("write correction response: ", correction_response);

  const auto pose_response =
      bridge.process_request(host.make_read_current_pose_request());
  print_frame("read pose response: ", pose_response);

  return 0;
}