// Deterministic, deliberately narrow Gazebo-side adapter for the Kaptan
// quad-X host-SITL transport contract. It is simulator tooling, not flight code.

#include <gz/common/Console.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
#include <gz/msgs/imu.pb.h>
#include <gz/plugin/Register.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/Link.hh>
#include <gz/sim/Model.hh>
#include <gz/sim/System.hh>
#include <gz/transport/Node.hh>

#include <algorithm>
#include <array>
#include <bit>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <memory>
#include <mutex>
#include <limits>
#include <optional>
#include <random>
#include <string>

#include <arpa/inet.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <unistd.h>

namespace kaptan::sitl
{
namespace
{
constexpr std::uint8_t kPacketVersion = 1;
constexpr std::array<std::uint8_t, 4> kImuMagic{'K', 'S', 'I', '1'};
constexpr std::array<std::uint8_t, 4> kLocalStateMagic{'K', 'S', 'V', '1'};
constexpr std::array<std::uint8_t, 4> kClockMagic{'K', 'S', 'C', '1'};
constexpr std::array<std::uint8_t, 4> kTruthMagic{'K', 'S', 'T', '2'};
constexpr std::array<std::uint8_t, 4> kOutputMagic{'K', 'S', 'O', '1'};
constexpr std::size_t kImuPacketSize = 44;
constexpr std::size_t kClockPacketSize = 16;
constexpr std::size_t kTruthPacketSize = 76;
constexpr std::size_t kOutputPacketSize = 40;
constexpr std::size_t kMaximumPacketsPerStep = 4;
constexpr std::size_t kQuadMotorCount = 4;

struct QuadMotorGeometry
{
  gz::math::Vector3d offset_flu;
  double reaction_torque_sign;
};

// This test model is FLU / ENU. It is not an electrical Kakute timer map:
// M1 front-right CW, M2 rear-right CCW, M3 rear-left CW, M4 front-left CCW.
constexpr std::array<QuadMotorGeometry, kQuadMotorCount> kQuadXGeometry{{
    {{0.17, -0.17, 0.0}, -1.0},
    {{-0.17, -0.17, 0.0}, 1.0},
    {{-0.17, 0.17, 0.0}, -1.0},
    {{0.17, 0.17, 0.0}, 1.0},
}};

bool IsFiniteFraction(const float value)
{
  return std::isfinite(value) && value >= 0.0F && value <= 1.0F;
}

bool IsZero(const float value)
{
  return value == 0.0F;
}

void WriteU32(std::uint8_t *destination, const std::uint32_t value)
{
  for (std::size_t index = 0; index < sizeof(value); ++index)
  {
    destination[index] = static_cast<std::uint8_t>(value >> (index * 8));
  }
}

void WriteU64(std::uint8_t *destination, const std::uint64_t value)
{
  for (std::size_t index = 0; index < sizeof(value); ++index)
  {
    destination[index] = static_cast<std::uint8_t>(value >> (index * 8));
  }
}

std::uint32_t ReadU32(const std::uint8_t *source)
{
  std::uint32_t value = 0;
  for (std::size_t index = 0; index < sizeof(value); ++index)
  {
    value |= static_cast<std::uint32_t>(source[index]) << (index * 8);
  }
  return value;
}

std::uint64_t ReadU64(const std::uint8_t *source)
{
  std::uint64_t value = 0;
  for (std::size_t index = 0; index < sizeof(value); ++index)
  {
    value |= static_cast<std::uint64_t>(source[index]) << (index * 8);
  }
  return value;
}

void WriteF32(std::uint8_t *destination, const float value)
{
  WriteU32(destination, std::bit_cast<std::uint32_t>(value));
}

float ReadF32(const std::uint8_t *source)
{
  return std::bit_cast<float>(ReadU32(source));
}

bool SequenceAfter(const std::uint32_t next, const std::uint32_t previous)
{
  const std::uint32_t distance = next - previous;
  return distance != 0 && distance <= UINT32_MAX / 2;
}

class UdpSocket
{
 public:
  UdpSocket() = default;
  ~UdpSocket()
  {
    if (this->descriptor_ >= 0)
    {
      close(this->descriptor_);
    }
  }

  UdpSocket(const UdpSocket &) = delete;
  UdpSocket &operator=(const UdpSocket &) = delete;

  bool BindLoopback(const std::uint16_t port)
  {
    this->descriptor_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (this->descriptor_ < 0)
    {
      return false;
    }
    const int flags = fcntl(this->descriptor_, F_GETFL, 0);
    if (flags < 0 || fcntl(this->descriptor_, F_SETFL, flags | O_NONBLOCK) < 0)
    {
      return false;
    }
    sockaddr_in address{};
    address.sin_family = AF_INET;
    address.sin_port = htons(port);
    address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    return bind(
               this->descriptor_,
               reinterpret_cast<const sockaddr *>(&address),
               sizeof(address)) == 0;
  }

  bool SendToLoopback(
      const std::uint16_t port, const std::uint8_t *data, const std::size_t size) const
  {
    sockaddr_in address{};
    address.sin_family = AF_INET;
    address.sin_port = htons(port);
    address.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    return sendto(
               this->descriptor_, data, size, MSG_DONTWAIT,
               reinterpret_cast<const sockaddr *>(&address), sizeof(address)) ==
           static_cast<ssize_t>(size);
  }

  ssize_t Receive(std::uint8_t *data, const std::size_t size) const
  {
    sockaddr_in sender{};
    socklen_t sender_size = sizeof(sender);
    const ssize_t received = recvfrom(
        this->descriptor_, data, size, MSG_DONTWAIT,
        reinterpret_cast<sockaddr *>(&sender), &sender_size);
    if (received < 0 || sender.sin_addr.s_addr != htonl(INADDR_LOOPBACK))
    {
      return -1;
    }
    return received;
  }

 private:
  int descriptor_{-1};
};

struct OutputCommand
{
  std::uint64_t controller_time_usec{};
  std::uint32_t control_sequence{};
  bool permitted{};
  std::array<float, kQuadMotorCount> motors{};
};

bool DecodeOutput(const std::array<std::uint8_t, kOutputPacketSize> &packet,
                  OutputCommand &output)
{
  if (!std::equal(kOutputMagic.begin(), kOutputMagic.end(), packet.begin()) ||
      packet[4] != kPacketVersion || packet[5] != 0 || packet[6] != 0 || packet[7] != 0)
  {
    return false;
  }
  output.controller_time_usec = ReadU64(packet.data() + 8);
  output.control_sequence = ReadU32(packet.data() + 16);
  if (packet[20] > 1 || packet[21] != 0 || packet[22] != 0 || packet[23] != 0)
  {
    return false;
  }
  output.permitted = packet[20] == 1;
  for (std::size_t index = 0; index < output.motors.size(); ++index)
  {
    output.motors[index] = ReadF32(packet.data() + 24 + index * sizeof(float));
    if (!IsFiniteFraction(output.motors[index]))
    {
      return false;
    }
  }
  return output.permitted || std::all_of(output.motors.begin(), output.motors.end(), IsZero);
}
}  // namespace

class Quad final : public gz::sim::System,
                   public gz::sim::ISystemConfigure,
                   public gz::sim::ISystemPreUpdate,
                   public gz::sim::ISystemPostUpdate,
                   public gz::sim::ISystemReset
{
 public:
  Quad() : data_(std::make_unique<Data>()) {}

  void Configure(const gz::sim::Entity &entity,
                 const std::shared_ptr<const sdf::Element> &sdf,
                 gz::sim::EntityComponentManager &ecm,
                 gz::sim::EventManager &) override;

  void PreUpdate(const gz::sim::UpdateInfo &info,
                 gz::sim::EntityComponentManager &ecm) override;

  void PostUpdate(const gz::sim::UpdateInfo &info,
                  const gz::sim::EntityComponentManager &) override;

  void Reset(const gz::sim::UpdateInfo &,
             gz::sim::EntityComponentManager &) override;

 private:
  struct Data
  {
    void OnImu(const gz::msgs::IMU &message)
    {
      std::lock_guard lock(this->imu_mutex);
      this->latest_imu = message;
      ++this->imu_generation;
      if (!this->received_first_imu)
      {
        gzmsg << "Kaptan SITL quad received its first Gazebo IMU sample.\n";
        this->received_first_imu = true;
      }
    }

    gz::sim::Link body;
    gz::transport::Node node;
    UdpSocket socket;
    std::uint16_t sensor_port{};
    std::uint16_t actuator_port{};
    std::uint16_t truth_port{};
    std::uint16_t operator_clock_port{};
    std::chrono::microseconds actuator_timeout{};
    double maximum_motor_thrust_newtons{};
    double motor_time_constant_seconds{};
    std::normal_distribution<double> local_position_noise{0.0, 0.0};
    std::normal_distribution<double> local_velocity_noise{0.0, 0.0};
    std::mt19937 local_state_random{};
    double reaction_torque_per_newton_metre{};
    std::optional<gz::math::Vector3d> launch_origin_enu;
    std::optional<gz::math::Quaterniond> launch_attitude_enu;
    std::mutex imu_mutex;
    gz::msgs::IMU latest_imu;
    std::uint64_t imu_generation{};
    std::uint64_t published_imu_generation{};
    bool received_first_imu{};
    std::optional<OutputCommand> output;
    std::array<double, kQuadMotorCount> applied_motor_fractions{};
    std::optional<std::pair<std::uint64_t, std::uint32_t>> previous_output_order;
    // Actuator freshness belongs to the simulated vehicle timeline, never
    // host scheduling time. That keeps timeout behavior deterministic when
    // Gazebo is paused, stepped, or runs at a non-real-time factor.
    std::optional<std::chrono::microseconds> last_output_sim_time;
  };

  std::unique_ptr<Data> data_;
};

void Quad::Configure(const gz::sim::Entity &entity,
                     const std::shared_ptr<const sdf::Element> &sdf,
                     gz::sim::EntityComponentManager &ecm,
                     gz::sim::EventManager &)
{
  const gz::sim::Model model(entity);
  if (!model.Valid(ecm))
  {
    gzerr << "Kaptan SITL quad must be attached to a model.\n";
    return;
  }
  const auto body_entity = model.LinkByName(ecm, "base_link");
  if (body_entity == gz::sim::kNullEntity)
  {
    gzerr << "Kaptan SITL quad requires a base_link.\n";
    return;
  }
  this->data_->body = gz::sim::Link(body_entity);
  this->data_->body.EnableVelocityChecks(ecm);

  const std::string imu_topic = sdf->Get<std::string>("imu_topic", "").first;
  const unsigned int sensor_port = sdf->Get<unsigned int>("sensor_udp_port", 0U).first;
  const unsigned int actuator_port =
      sdf->Get<unsigned int>("actuator_udp_port", 0U).first;
  const unsigned int truth_port = sdf->Get<unsigned int>("truth_udp_port", 0U).first;
  const unsigned int operator_clock_port =
      sdf->Get<unsigned int>("operator_clock_udp_port", 0U).first;
  const std::uint64_t timeout_usec =
      sdf->Get<std::uint64_t>("actuator_timeout_usec", 0).first;
  this->data_->maximum_motor_thrust_newtons =
      sdf->Get<double>("maximum_motor_thrust_newtons", 0.0).first;
  this->data_->motor_time_constant_seconds =
      sdf->Get<double>("motor_time_constant_seconds", 0.0).first;
  const double local_position_noise_stddev =
      sdf->Get<double>("local_position_noise_stddev_m", -1.0).first;
  const double local_velocity_noise_stddev =
      sdf->Get<double>("local_velocity_noise_stddev_mps", -1.0).first;
  const unsigned int local_state_noise_seed =
      sdf->Get<unsigned int>("local_state_noise_seed", 0U).first;
  this->data_->reaction_torque_per_newton_metre =
      sdf->Get<double>("reaction_torque_per_newton_metre", 0.0).first;
  if (imu_topic.empty() || sensor_port == 0 || actuator_port == 0 || truth_port == 0 ||
      operator_clock_port == 0 ||
      sensor_port > std::numeric_limits<std::uint16_t>::max() ||
      actuator_port > std::numeric_limits<std::uint16_t>::max() ||
      truth_port > std::numeric_limits<std::uint16_t>::max() ||
      operator_clock_port > std::numeric_limits<std::uint16_t>::max() ||
      timeout_usec == 0 || !std::isfinite(this->data_->maximum_motor_thrust_newtons) ||
      this->data_->maximum_motor_thrust_newtons <= 0.0 ||
      !std::isfinite(this->data_->motor_time_constant_seconds) ||
      this->data_->motor_time_constant_seconds <= 0.0 ||
      !std::isfinite(local_position_noise_stddev) || local_position_noise_stddev < 0.0 ||
      !std::isfinite(local_velocity_noise_stddev) || local_velocity_noise_stddev < 0.0 ||
      !std::isfinite(this->data_->reaction_torque_per_newton_metre) ||
      this->data_->reaction_torque_per_newton_metre < 0.0)
  {
    gzerr << "Kaptan SITL quad has an invalid static transport or vehicle configuration.\n";
    return;
  }
  this->data_->sensor_port = static_cast<std::uint16_t>(sensor_port);
  this->data_->actuator_port = static_cast<std::uint16_t>(actuator_port);
  this->data_->truth_port = static_cast<std::uint16_t>(truth_port);
  this->data_->operator_clock_port = static_cast<std::uint16_t>(operator_clock_port);
  this->data_->actuator_timeout = std::chrono::microseconds(timeout_usec);
  this->data_->local_position_noise =
      std::normal_distribution<double>(0.0, local_position_noise_stddev);
  this->data_->local_velocity_noise =
      std::normal_distribution<double>(0.0, local_velocity_noise_stddev);
  this->data_->local_state_random.seed(local_state_noise_seed);
  if (!this->data_->socket.BindLoopback(this->data_->actuator_port))
  {
    gzerr << "Kaptan SITL quad could not bind its loopback actuator socket.\n";
    return;
  }
  if (!this->data_->node.Subscribe(imu_topic, &Data::OnImu, this->data_.get()))
  {
    gzerr << "Kaptan SITL quad could not subscribe to its configured IMU topic.\n";
  }
  else
  {
    gzmsg << "Kaptan SITL quad subscribed to Gazebo IMU topic [" << imu_topic << "].\n";
  }
}

void Quad::PreUpdate(const gz::sim::UpdateInfo &info,
                     gz::sim::EntityComponentManager &ecm)
{
  if (info.paused || info.simTime.count() < 0 || !this->data_->body.Valid(ecm))
  {
    return;
  }
  const auto simulation_time =
      std::chrono::duration_cast<std::chrono::microseconds>(info.simTime);
  for (std::size_t attempt = 0; attempt < kMaximumPacketsPerStep; ++attempt)
  {
    std::array<std::uint8_t, kOutputPacketSize> packet{};
    if (this->data_->socket.Receive(packet.data(), packet.size()) !=
        static_cast<ssize_t>(packet.size()))
    {
      break;
    }
    OutputCommand command{};
    if (!DecodeOutput(packet, command))
    {
      continue;
    }
    if (!command.permitted)
    {
      this->data_->output = command;
      continue;
    }
    if (this->data_->previous_output_order.has_value() &&
        (command.controller_time_usec <= this->data_->previous_output_order->first ||
         !SequenceAfter(command.control_sequence,
                        this->data_->previous_output_order->second)))
    {
      continue;
    }
    this->data_->previous_output_order =
        std::pair{command.controller_time_usec, command.control_sequence};
    this->data_->last_output_sim_time = simulation_time;
    this->data_->output = command;
  }

  const bool command_is_fresh =
      this->data_->output.has_value() && this->data_->output->permitted &&
      this->data_->last_output_sim_time.has_value() &&
      simulation_time >= *this->data_->last_output_sim_time &&
      simulation_time - *this->data_->last_output_sim_time <= this->data_->actuator_timeout;
  const auto pose = this->data_->body.WorldPose(ecm);
  if (!pose.has_value())
  {
    return;
  }
  const double dt_seconds = std::chrono::duration<double>(info.dt).count();
  if (!std::isfinite(dt_seconds) || dt_seconds <= 0.0)
  {
    return;
  }
  const double alpha = 1.0 - std::exp(-dt_seconds / this->data_->motor_time_constant_seconds);
  const auto desired_motors = command_is_fresh
      ? this->data_->output->motors
      : std::array<float, kQuadMotorCount>{};
  const gz::math::Vector3d thrust_axis_world = pose->Rot().RotateVector({0.0, 0.0, 1.0});
  for (std::size_t index = 0; index < kQuadMotorCount; ++index)
  {
    this->data_->applied_motor_fractions[index] +=
        alpha * (static_cast<double>(desired_motors[index]) -
                 this->data_->applied_motor_fractions[index]);
    const double thrust =
        this->data_->applied_motor_fractions[index] *
        this->data_->maximum_motor_thrust_newtons;
    this->data_->body.AddWorldWrench(
        ecm, thrust_axis_world * thrust,
        thrust_axis_world *
            (kQuadXGeometry[index].reaction_torque_sign * thrust *
             this->data_->reaction_torque_per_newton_metre),
        kQuadXGeometry[index].offset_flu);
  }
}

void Quad::PostUpdate(const gz::sim::UpdateInfo &info,
                      const gz::sim::EntityComponentManager &_ecm)
{
  if (info.paused || info.simTime.count() < 0)
  {
    return;
  }
  // This is deliberately emitted for every unpaused Gazebo step, even when
  // an IMU sample is absent. The host runtime uses it as its sole freshness
  // and lease time base; host scheduling cannot advance safety state.
  const auto send_clock = [this, &info]() {
    std::array<std::uint8_t, kClockPacketSize> clock_packet{};
    std::copy(kClockMagic.begin(), kClockMagic.end(), clock_packet.begin());
    clock_packet[4] = kPacketVersion;
    WriteU64(clock_packet.data() + 8, static_cast<std::uint64_t>(
                                          std::chrono::duration_cast<std::chrono::microseconds>(
                                              info.simTime)
                                              .count()));
    (void)this->data_->socket.SendToLoopback(
        this->data_->sensor_port, clock_packet.data(), clock_packet.size());
    (void)this->data_->socket.SendToLoopback(
        this->data_->operator_clock_port, clock_packet.data(), clock_packet.size());
  };
  gz::msgs::IMU imu;
  {
    std::lock_guard lock(this->data_->imu_mutex);
    if (this->data_->imu_generation == this->data_->published_imu_generation)
    {
      send_clock();
      return;
    }
    imu = this->data_->latest_imu;
    this->data_->published_imu_generation = this->data_->imu_generation;
  }

  // Gazebo's standard body sensor convention is FLU. Kaptan's is FRD, so the
  // single explicit boundary conversion is x, -y, -z for both vectors.
  std::array<std::uint8_t, kImuPacketSize> packet{};
  std::copy(kImuMagic.begin(), kImuMagic.end(), packet.begin());
  packet[4] = kPacketVersion;
  WriteU64(packet.data() + 8, static_cast<std::uint64_t>(
                                  std::chrono::duration_cast<std::chrono::microseconds>(
                                      info.simTime)
                                      .count()));
  WriteU32(packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
  const std::array<float, 6> frd{
      static_cast<float>(imu.linear_acceleration().x()),
      -static_cast<float>(imu.linear_acceleration().y()),
      -static_cast<float>(imu.linear_acceleration().z()),
      static_cast<float>(imu.angular_velocity().x()),
      -static_cast<float>(imu.angular_velocity().y()),
      -static_cast<float>(imu.angular_velocity().z()),
  };
  if (!std::all_of(frd.begin(), frd.end(), [](const float value) { return std::isfinite(value); }))
  {
    send_clock();
    return;
  }
  for (std::size_t index = 0; index < frd.size(); ++index)
  {
    WriteF32(packet.data() + 20 + index * sizeof(float), frd[index]);
  }
  (void)this->data_->socket.SendToLoopback(
      this->data_->sensor_port, packet.data(), packet.size());

  const auto pose = this->data_->body.WorldPose(_ecm);
  const auto velocity = this->data_->body.WorldLinearVelocity(_ecm);
  if (!pose.has_value() || !velocity.has_value())
  {
    send_clock();
    return;
  }
  if (!this->data_->launch_origin_enu.has_value())
  {
    this->data_->launch_origin_enu = pose->Pos();
    this->data_->launch_attitude_enu = pose->Rot();
  }
  // This fixed test vehicle is yawed so ENU world +Y/+X/-Z are respectively
  // its launch-local NED north/east/down axes. The conversion happens once at
  // the Gazebo boundary; all flight code receives only explicit NED SI data.
  const auto &origin = *this->data_->launch_origin_enu;
  const std::array<float, 6> local_ned{
      static_cast<float>(pose->Pos().Y() - origin.Y() +
                         this->data_->local_position_noise(this->data_->local_state_random)),
      static_cast<float>(pose->Pos().X() - origin.X() +
                         this->data_->local_position_noise(this->data_->local_state_random)),
      static_cast<float>(origin.Z() - pose->Pos().Z() +
                         this->data_->local_position_noise(this->data_->local_state_random)),
      static_cast<float>(velocity->Y() +
                         this->data_->local_velocity_noise(this->data_->local_state_random)),
      static_cast<float>(velocity->X() +
                         this->data_->local_velocity_noise(this->data_->local_state_random)),
      static_cast<float>(-velocity->Z() +
                         this->data_->local_velocity_noise(this->data_->local_state_random)),
  };
  if (!std::all_of(
          local_ned.begin(), local_ned.end(),
          [](const float value) { return std::isfinite(value); }))
  {
    send_clock();
    return;
  }
  std::array<std::uint8_t, kImuPacketSize> state_packet{};
  std::copy(kLocalStateMagic.begin(), kLocalStateMagic.end(), state_packet.begin());
  state_packet[4] = kPacketVersion;
  WriteU64(state_packet.data() + 8, static_cast<std::uint64_t>(
                                        std::chrono::duration_cast<std::chrono::microseconds>(
                                            info.simTime)
                                            .count()));
  WriteU32(state_packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
  for (std::size_t index = 0; index < local_ned.size(); ++index)
  {
    WriteF32(state_packet.data() + 20 + index * sizeof(float), local_ned[index]);
  }
  (void)this->data_->socket.SendToLoopback(
      this->data_->sensor_port, state_packet.data(), state_packet.size());

  // This packet is intentionally sent to a separate test-only port. The
  // controller never binds that port, so these noise-free values cannot form a
  // parallel control path or mask estimator behavior. The quaternion is the
  // body attitude relative to launch; its rotation magnitude is frame
  // invariant, which permits the test client to verify attitude without
  // duplicating the ENU/FLU-to-NED/FRD conversion path.
  const auto launch_attitude = *this->data_->launch_attitude_enu;
  const auto relative_attitude = launch_attitude.Inverse() * pose->Rot();
  const std::array<float, 14> truth_ned{
      static_cast<float>(pose->Pos().Y() - origin.Y()),
      static_cast<float>(pose->Pos().X() - origin.X()),
      static_cast<float>(origin.Z() - pose->Pos().Z()),
      static_cast<float>(velocity->Y()),
      static_cast<float>(velocity->X()),
      static_cast<float>(-velocity->Z()),
      static_cast<float>(relative_attitude.W()),
      static_cast<float>(relative_attitude.X()),
      static_cast<float>(relative_attitude.Y()),
      static_cast<float>(relative_attitude.Z()),
      static_cast<float>(this->data_->applied_motor_fractions[0]),
      static_cast<float>(this->data_->applied_motor_fractions[1]),
      static_cast<float>(this->data_->applied_motor_fractions[2]),
      static_cast<float>(this->data_->applied_motor_fractions[3]),
  };
  if (!std::all_of(
          truth_ned.begin(), truth_ned.end(),
          [](const float value) { return std::isfinite(value); }))
  {
    send_clock();
    return;
  }
  std::array<std::uint8_t, kTruthPacketSize> truth_packet{};
  std::copy(kTruthMagic.begin(), kTruthMagic.end(), truth_packet.begin());
  truth_packet[4] = kPacketVersion;
  WriteU64(truth_packet.data() + 8, static_cast<std::uint64_t>(
                                        std::chrono::duration_cast<std::chrono::microseconds>(
                                            info.simTime)
                                            .count()));
  WriteU32(truth_packet.data() + 16, static_cast<std::uint32_t>(this->data_->imu_generation));
  for (std::size_t index = 0; index < truth_ned.size(); ++index)
  {
    WriteF32(truth_packet.data() + 20 + index * sizeof(float), truth_ned[index]);
  }
  (void)this->data_->socket.SendToLoopback(
      this->data_->truth_port, truth_packet.data(), truth_packet.size());
  send_clock();
}

void Quad::Reset(const gz::sim::UpdateInfo &,
                 gz::sim::EntityComponentManager &)
{
  this->data_->output.reset();
  this->data_->applied_motor_fractions = {};
  this->data_->previous_output_order.reset();
  this->data_->last_output_sim_time.reset();
  this->data_->launch_origin_enu.reset();
  this->data_->launch_attitude_enu.reset();
  std::lock_guard lock(this->data_->imu_mutex);
  this->data_->published_imu_generation = this->data_->imu_generation;
}

}  // namespace kaptan::sitl

GZ_ADD_PLUGIN(kaptan::sitl::Quad,
              gz::sim::System,
              kaptan::sitl::Quad::ISystemConfigure,
              kaptan::sitl::Quad::ISystemPreUpdate,
              kaptan::sitl::Quad::ISystemPostUpdate,
              kaptan::sitl::Quad::ISystemReset)
GZ_ADD_PLUGIN_ALIAS(kaptan::sitl::Quad, "kaptan::sitl::Quad")