#include "HostBluezBleInterface.h"

#include <Utilities/OS.h>

#include <algorithm>
#include <cstdio>
#include <cstring>

using namespace RNS;

static uint64_t now_ms() {
  return RNS::Utilities::OS::ltime();
}

static void put_u16_be(uint8_t* data, uint16_t value) {
  data[0] = (uint8_t)((value >> 8) & 0xFF);
  data[1] = (uint8_t)(value & 0xFF);
}

static void put_u32_be(uint8_t* data, uint32_t value) {
  data[0] = (uint8_t)((value >> 24) & 0xFF);
  data[1] = (uint8_t)((value >> 16) & 0xFF);
  data[2] = (uint8_t)((value >> 8) & 0xFF);
  data[3] = (uint8_t)(value & 0xFF);
}

static uint16_t get_u16_be(const uint8_t* data) {
  return ((uint16_t)data[0] << 8) | data[1];
}

static uint32_t get_u32_be(const uint8_t* data) {
  return ((uint32_t)data[0] << 24) |
         ((uint32_t)data[1] << 16) |
         ((uint32_t)data[2] << 8) |
         (uint32_t)data[3];
}

static bool uuid_matches(const char* found, const char* expected) {
  return found && g_ascii_strcasecmp(found, expected) == 0;
}

static void log_error(const char* what, GError* error) {
  if (error) {
    std::fprintf(stderr, "%s: %s\n", what, error->message);
    g_error_free(error);
  } else {
    std::fprintf(stderr, "%s\n", what);
  }
}

static std::string peer_address_from_path(const std::string& path) {
  size_t pos = path.find("/dev_");
  if (pos == std::string::npos) {
    return path.empty() ? std::string("unknown") : path;
  }
  std::string addr = path.substr(pos + 5);
  std::replace(addr.begin(), addr.end(), '_', ':');
  return addr;
}

static void log_reticulum_frame(const char* direction,
                                const std::string& node_label,
                                const std::string& peer_path,
                                const std::string& object_path,
                                const void* iface,
                                const RNS::Bytes& packet) {
  unsigned first = packet.size() > 0 ? packet[0] : 0;
  std::printf("BLE-FRAME t=%llu node=%s role=central dir=%s peer=%s object=%s if=%p len=%zu packet0=0x%02X link_hint=unknown\n",
              (unsigned long long)now_ms(),
              node_label.c_str(),
              direction,
              peer_address_from_path(peer_path).c_str(),
              object_path.empty() ? "unknown" : object_path.c_str(),
              iface,
              packet.size(),
              first);
}

HostBluezBleInterface::HostBluezBleInterface(const std::string& node_label,
                                             bool frame_log_enabled,
                                             const char* name)
    : InterfaceImpl(name), node_label_(node_label), frame_log_enabled_(frame_log_enabled) {
  _IN = true;
  _OUT = true;
  _bitrate = 1000000;
  _HW_MTU = BLE_PAYLOAD_SIZE;
}

HostBluezBleInterface::~HostBluezBleInterface() {
  stop();
  if (bus_) {
    g_object_unref(bus_);
  }
}

bool HostBluezBleInterface::start() {
  if (online_) {
    return true;
  }
  if (!connect_bus() || !find_adapter()) {
    return false;
  }
  online_ = true;
  _online = true;
  std::printf("BLE linux-central: adapter=%s label=%s\n", adapter_path_.c_str(), node_label_.c_str());
  start_discovery();
  return true;
}

void HostBluezBleInterface::stop() {
  online_ = false;
  _online = false;
  disconnect_peer();
  stop_discovery();
}

bool HostBluezBleInterface::connect_bus() {
  GError* error = nullptr;
  bus_ = g_bus_get_sync(G_BUS_TYPE_SYSTEM, nullptr, &error);
  if (!bus_) {
    log_error("BLE linux-central: cannot connect to system D-Bus", error);
    return false;
  }
  return true;
}

bool HostBluezBleInterface::find_adapter() {
  GError* error = nullptr;
  GVariant* managed = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, "/",
                                                  "org.freedesktop.DBus.ObjectManager",
                                                  "GetManagedObjects", nullptr,
                                                  G_VARIANT_TYPE("(a{oa{sa{sv}}})"),
                                                  G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!managed) {
    log_error("BLE linux-central: GetManagedObjects failed", error);
    return false;
  }

  GVariantIter* objects = nullptr;
  g_variant_get(managed, "(a{oa{sa{sv}}})", &objects);
  const gchar* object_path = nullptr;
  GVariant* ifaces = nullptr;
  while (g_variant_iter_next(objects, "{&o@a{sa{sv}}}", &object_path, &ifaces)) {
    if (g_variant_lookup_value(ifaces, "org.bluez.Adapter1", G_VARIANT_TYPE("a{sv}"))) {
      adapter_path_ = object_path;
      g_variant_unref(ifaces);
      break;
    }
    g_variant_unref(ifaces);
  }
  g_variant_iter_free(objects);
  g_variant_unref(managed);

  if (adapter_path_.empty()) {
    std::fprintf(stderr, "BLE linux-central: no BlueZ adapter found\n");
    return false;
  }

  GVariantBuilder props;
  g_variant_builder_init(&props, G_VARIANT_TYPE("a{sv}"));
  g_variant_builder_add(&props, "{sv}", "Powered", g_variant_new_boolean(TRUE));
  GVariantBuilder invalidated;
  g_variant_builder_init(&invalidated, G_VARIANT_TYPE("as"));
  g_dbus_connection_emit_signal(bus_, nullptr, adapter_path_.c_str(),
                                "org.freedesktop.DBus.Properties", "PropertiesChanged",
                                g_variant_new("(sa{sv}as)", "org.bluez.Adapter1", &props, &invalidated),
                                nullptr);

  error = nullptr;
  GVariant* powered = g_dbus_connection_call_sync(
      bus_, BLUEZ_BUS, adapter_path_.c_str(), "org.freedesktop.DBus.Properties", "Set",
      g_variant_new("(ssv)", "org.bluez.Adapter1", "Powered", g_variant_new_boolean(TRUE)),
      nullptr, G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!powered) {
    log_error("BLE linux-central: could not power adapter", error);
    return false;
  }
  g_variant_unref(powered);
  return true;
}

bool HostBluezBleInterface::start_discovery() {
  if (discovering_ || adapter_path_.empty()) {
    return true;
  }

  GVariantBuilder uuids;
  g_variant_builder_init(&uuids, G_VARIANT_TYPE("as"));
  g_variant_builder_add(&uuids, "s", SERVICE_UUID);
  GVariantBuilder filter;
  g_variant_builder_init(&filter, G_VARIANT_TYPE("a{sv}"));
  g_variant_builder_add(&filter, "{sv}", "UUIDs", g_variant_builder_end(&uuids));
  g_variant_builder_add(&filter, "{sv}", "Transport", g_variant_new_string("le"));

  GError* error = nullptr;
  GVariant* result = g_dbus_connection_call_sync(
      bus_, BLUEZ_BUS, adapter_path_.c_str(), "org.bluez.Adapter1", "SetDiscoveryFilter",
      g_variant_new("(a{sv})", &filter), nullptr, G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!result) {
    log_error("BLE linux-central: SetDiscoveryFilter failed", error);
  } else {
    g_variant_unref(result);
  }

  error = nullptr;
  result = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, adapter_path_.c_str(),
                                       "org.bluez.Adapter1", "StartDiscovery",
                                       nullptr, nullptr, G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!result) {
    log_error("BLE linux-central: StartDiscovery failed", error);
    next_scan_ms_ = now_ms() + SCAN_RETRY_MS;
    return false;
  }
  g_variant_unref(result);
  discovering_ = true;
  next_scan_ms_ = now_ms() + SCAN_RETRY_MS;
  std::printf("BLE linux-central: scanning for Reticulum service\n");
  return true;
}

bool HostBluezBleInterface::stop_discovery() {
  if (!discovering_ || adapter_path_.empty()) {
    return true;
  }
  GError* error = nullptr;
  GVariant* result = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, adapter_path_.c_str(),
                                                 "org.bluez.Adapter1", "StopDiscovery",
                                                 nullptr, nullptr, G_DBUS_CALL_FLAGS_NONE, -1,
                                                 nullptr, &error);
  if (!result) {
    log_error("BLE linux-central: StopDiscovery failed", error);
    return false;
  }
  g_variant_unref(result);
  discovering_ = false;
  return true;
}

bool HostBluezBleInterface::scan_for_peer() {
  GError* error = nullptr;
  GVariant* managed = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, "/",
                                                  "org.freedesktop.DBus.ObjectManager",
                                                  "GetManagedObjects", nullptr,
                                                  G_VARIANT_TYPE("(a{oa{sa{sv}}})"),
                                                  G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!managed) {
    log_error("BLE linux-central: scan GetManagedObjects failed", error);
    return false;
  }

  std::string found_path;
  std::string found_name;
  GVariantIter* objects = nullptr;
  g_variant_get(managed, "(a{oa{sa{sv}}})", &objects);
  const gchar* object_path = nullptr;
  GVariant* ifaces = nullptr;
  while (g_variant_iter_next(objects, "{&o@a{sa{sv}}}", &object_path, &ifaces)) {
    GVariant* device = g_variant_lookup_value(ifaces, "org.bluez.Device1", G_VARIANT_TYPE("a{sv}"));
    if (!device) {
      g_variant_unref(ifaces);
      continue;
    }

    const gchar* name = nullptr;
    g_variant_lookup(device, "Name", "&s", &name);
    bool has_rns_name = name && std::strncmp(name, "RNS-", 4) == 0;
    bool has_service = false;
    GVariant* uuids = g_variant_lookup_value(device, "UUIDs", G_VARIANT_TYPE("as"));
    if (uuids) {
      GVariantIter uuid_iter;
      const gchar* uuid = nullptr;
      g_variant_iter_init(&uuid_iter, uuids);
      while (g_variant_iter_next(&uuid_iter, "&s", &uuid)) {
        if (uuid_matches(uuid, SERVICE_UUID)) {
          has_service = true;
          break;
        }
      }
      g_variant_unref(uuids);
    }

    if (has_service || has_rns_name) {
      found_path = object_path;
      found_name = name ? name : "";
      g_variant_unref(device);
      g_variant_unref(ifaces);
      break;
    }

    g_variant_unref(device);
    g_variant_unref(ifaces);
  }
  g_variant_iter_free(objects);
  g_variant_unref(managed);

  if (found_path.empty()) {
    return false;
  }

  std::printf("BLE linux-central: peer candidate path=%s name=%s\n", found_path.c_str(), found_name.c_str());
  stop_discovery();
  return connect_device(found_path);
}

bool HostBluezBleInterface::connect_device(const std::string& device_path) {
  GError* error = nullptr;
  GVariant* result = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, device_path.c_str(),
                                                 "org.bluez.Device1", "Connect",
                                                 nullptr, nullptr, G_DBUS_CALL_FLAGS_NONE, 30000,
                                                 nullptr, &error);
  if (!result) {
    log_error("BLE linux-central: Device1.Connect failed", error);
    next_scan_ms_ = now_ms() + SCAN_RETRY_MS;
    return false;
  }
  g_variant_unref(result);
  device_path_ = device_path;
  device_subscription_ = g_dbus_connection_signal_subscribe(
      bus_, BLUEZ_BUS, "org.freedesktop.DBus.Properties", "PropertiesChanged",
      device_path_.c_str(), "org.bluez.Device1",
      G_DBUS_SIGNAL_FLAGS_NONE, device_properties_changed, this, nullptr);
  std::printf("BLE linux-central: connected to %s\n", device_path_.c_str());

  uint64_t deadline = now_ms() + 10000;
  while (now_ms() < deadline) {
    while (g_main_context_iteration(nullptr, FALSE)) {
    }
    if (discover_characteristics()) {
      break;
    }
    RNS::Utilities::OS::sleep(0.1);
  }

  if (rx_char_path_.empty() || tx_char_path_.empty()) {
    std::fprintf(stderr, "BLE linux-central: Reticulum characteristics not found\n");
    disconnect_peer();
    return false;
  }

  if (!start_notify()) {
    disconnect_peer();
    return false;
  }

  uint8_t identity[16] = {};
  std::hash<std::string> hasher;
  size_t h1 = hasher(node_label_);
  size_t h2 = hasher(node_label_ + ":exercise306");
  std::memcpy(identity, &h1, std::min(sizeof(h1), sizeof(identity)));
  std::memcpy(identity + 8, &h2, std::min(sizeof(h2), sizeof(identity) - 8));
  connected_ = true;
  if (!write_characteristic(rx_char_path_, identity, sizeof(identity), true)) {
    return false;
  }

  std::printf("BLE linux-central: notifications active; identity handshake sent\n");
  return true;
}

bool HostBluezBleInterface::discover_characteristics() {
  GError* error = nullptr;
  GVariant* managed = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, "/",
                                                  "org.freedesktop.DBus.ObjectManager",
                                                  "GetManagedObjects", nullptr,
                                                  G_VARIANT_TYPE("(a{oa{sa{sv}}})"),
                                                  G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!managed) {
    log_error("BLE linux-central: characteristic discovery failed", error);
    return false;
  }

  tx_char_path_.clear();
  rx_char_path_.clear();
  identity_char_path_.clear();

  GVariantIter* objects = nullptr;
  g_variant_get(managed, "(a{oa{sa{sv}}})", &objects);
  const gchar* object_path = nullptr;
  GVariant* ifaces = nullptr;
  while (g_variant_iter_next(objects, "{&o@a{sa{sv}}}", &object_path, &ifaces)) {
    if (device_path_.empty() || std::strncmp(object_path, device_path_.c_str(), device_path_.size()) != 0) {
      g_variant_unref(ifaces);
      continue;
    }
    GVariant* characteristic = g_variant_lookup_value(ifaces, "org.bluez.GattCharacteristic1", G_VARIANT_TYPE("a{sv}"));
    if (!characteristic) {
      g_variant_unref(ifaces);
      continue;
    }
    const gchar* uuid = nullptr;
    if (g_variant_lookup(characteristic, "UUID", "&s", &uuid)) {
      if (uuid_matches(uuid, TX_UUID)) {
        tx_char_path_ = object_path;
      } else if (uuid_matches(uuid, RX_UUID)) {
        rx_char_path_ = object_path;
      } else if (uuid_matches(uuid, IDENTITY_UUID)) {
        identity_char_path_ = object_path;
      }
    }
    g_variant_unref(characteristic);
    g_variant_unref(ifaces);
  }
  g_variant_iter_free(objects);
  g_variant_unref(managed);
  return !tx_char_path_.empty() && !rx_char_path_.empty();
}

bool HostBluezBleInterface::start_notify() {
  notify_subscription_ = g_dbus_connection_signal_subscribe(
      bus_, BLUEZ_BUS, "org.freedesktop.DBus.Properties", "PropertiesChanged",
      tx_char_path_.c_str(), "org.bluez.GattCharacteristic1",
      G_DBUS_SIGNAL_FLAGS_NONE, properties_changed, this, nullptr);

  GError* error = nullptr;
  GVariant* result = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, tx_char_path_.c_str(),
                                                 "org.bluez.GattCharacteristic1", "StartNotify",
                                                 nullptr, nullptr, G_DBUS_CALL_FLAGS_NONE, -1,
                                                 nullptr, &error);
  if (!result) {
    log_error("BLE linux-central: StartNotify failed", error);
    return false;
  }
  g_variant_unref(result);
  return true;
}

bool HostBluezBleInterface::write_characteristic(const std::string& path,
                                                 const uint8_t* data,
                                                 size_t len,
                                                 bool with_response) {
  GVariantBuilder bytes;
  g_variant_builder_init(&bytes, G_VARIANT_TYPE("ay"));
  for (size_t i = 0; i < len; ++i) {
    g_variant_builder_add(&bytes, "y", data[i]);
  }
  GVariantBuilder options;
  g_variant_builder_init(&options, G_VARIANT_TYPE("a{sv}"));
  g_variant_builder_add(&options, "{sv}", "type", g_variant_new_string(with_response ? "request" : "command"));

  GError* error = nullptr;
  GVariant* result = g_dbus_connection_call_sync(
      bus_, BLUEZ_BUS, path.c_str(), "org.bluez.GattCharacteristic1", "WriteValue",
      g_variant_new("(aya{sv})", &bytes, &options), nullptr,
      G_DBUS_CALL_FLAGS_NONE, -1, nullptr, &error);
  if (!result) {
    std::string message = error && error->message ? error->message : "unknown write failure";
    log_error("BLE linux-central: WriteValue failed", error);
    mark_disconnected(message.c_str());
    return false;
  }
  g_variant_unref(result);
  return true;
}

void HostBluezBleInterface::loop() {
  if (!online_) {
    return;
  }
  while (g_main_context_iteration(nullptr, FALSE)) {
  }

  if (!connected_) {
    uint64_t now = now_ms();
    if (!discovering_ && now >= next_scan_ms_) {
      start_discovery();
    }
    if (discovering_) {
      scan_for_peer();
    }
    return;
  }

  if (reassembly_started_ms_ != 0 && now_ms() - reassembly_started_ms_ > REASSEMBLY_TIMEOUT_MS) {
    std::fprintf(stderr, "BLE linux-central: reassembly timeout; dropping partial packet\n");
    reset_reassembly();
  }

  RNS::Bytes packet({RNS::Type::NONE});
  while (dequeue_packet(packet)) {
    InterfaceImpl::handle_incoming(packet);
  }
}

void HostBluezBleInterface::send_outgoing(const RNS::Bytes& data) {
  if (!online_ || !connected_ || rx_char_path_.empty()) {
    return;
  }

  if (frame_log_enabled_) {
    log_reticulum_frame("tx", node_label_, device_path_, rx_char_path_, this, data);
  }

  size_t total = (data.size() + BLE_PAYLOAD_SIZE - 1) / BLE_PAYLOAD_SIZE;
  if (total == 0 || total > 65535) {
    std::fprintf(stderr, "BLE linux-central: cannot fragment packet len=%zu fragments=%zu\n", data.size(), total);
    return;
  }

  uint32_t msg_id = ++tx_message_id_;
  for (size_t i = 0; i < total; ++i) {
    uint8_t fragment[BLE_VALUE_SIZE];
    uint8_t fragment_type = FRAG_CONTINUE;
    if (i == 0) {
      fragment_type = FRAG_START;
    } else if (i == total - 1) {
      fragment_type = FRAG_END;
    }

    size_t offset = i * BLE_PAYLOAD_SIZE;
    size_t chunk = std::min(BLE_PAYLOAD_SIZE, data.size() - offset);
    fragment[0] = fragment_type;
    fragment[1] = FRAG_HEADER_VERSION;
    put_u16_be(fragment + 2, (uint16_t)i);
    put_u16_be(fragment + 4, (uint16_t)total);
    put_u32_be(fragment + 6, msg_id);
    put_u32_be(fragment + 10, (uint32_t)data.size());
    std::memcpy(fragment + FRAG_HEADER_SIZE, data.data() + offset, chunk);
    if (!send_fragment(fragment, FRAG_HEADER_SIZE + chunk)) {
      return;
    }
    RNS::Utilities::OS::sleep(0.008);
  }

  InterfaceImpl::handle_outgoing(data);
}

bool HostBluezBleInterface::send_fragment(const uint8_t* data, size_t len) {
  return write_characteristic(rx_char_path_, data, len, true);
}

void HostBluezBleInterface::properties_changed(GDBusConnection* connection,
                                               const gchar* sender_name,
                                               const gchar* object_path,
                                               const gchar* interface_name,
                                               const gchar* signal_name,
                                               GVariant* parameters,
                                               gpointer user_data) {
  (void)connection;
  (void)sender_name;
  (void)interface_name;
  (void)signal_name;
  auto* self = static_cast<HostBluezBleInterface*>(user_data);
  if (!self || self->tx_char_path_ != object_path) {
    return;
  }

  const gchar* changed_iface = nullptr;
  GVariant* changed = nullptr;
  GVariant* invalidated = nullptr;
  g_variant_get(parameters, "(&s@a{sv}@as)", &changed_iface, &changed, &invalidated);
  if (std::strcmp(changed_iface, "org.bluez.GattCharacteristic1") == 0) {
    GVariant* value = g_variant_lookup_value(changed, "Value", G_VARIANT_TYPE("ay"));
    if (value) {
      gsize len = 0;
      const uint8_t* data = static_cast<const uint8_t*>(g_variant_get_fixed_array(value, &len, sizeof(uint8_t)));
      if (data && len > 0) {
        self->handle_fragment(data, len);
      }
      g_variant_unref(value);
    }
  }
  g_variant_unref(changed);
  g_variant_unref(invalidated);
}

void HostBluezBleInterface::device_properties_changed(GDBusConnection* connection,
                                                      const gchar* sender_name,
                                                      const gchar* object_path,
                                                      const gchar* interface_name,
                                                      const gchar* signal_name,
                                                      GVariant* parameters,
                                                      gpointer user_data) {
  (void)connection;
  (void)sender_name;
  (void)interface_name;
  (void)signal_name;
  auto* self = static_cast<HostBluezBleInterface*>(user_data);
  if (!self || self->device_path_ != object_path) {
    return;
  }

  const gchar* changed_iface = nullptr;
  GVariant* changed = nullptr;
  GVariant* invalidated = nullptr;
  g_variant_get(parameters, "(&s@a{sv}@as)", &changed_iface, &changed, &invalidated);
  if (std::strcmp(changed_iface, "org.bluez.Device1") == 0) {
    gboolean connected = TRUE;
    if (g_variant_lookup(changed, "Connected", "b", &connected) && !connected) {
      self->mark_disconnected("device disconnected");
    }
  }
  g_variant_unref(changed);
  g_variant_unref(invalidated);
}

void HostBluezBleInterface::handle_fragment(const uint8_t* data, size_t len) {
  if (len < FRAG_HEADER_SIZE) {
    std::fprintf(stderr, "BLE linux-central: fragment too short len=%zu\n", len);
    return;
  }

  uint8_t fragment_type = data[0];
  uint8_t version = data[1];
  uint16_t sequence = get_u16_be(data + 2);
  uint16_t total = get_u16_be(data + 4);
  uint32_t msg_id = get_u32_be(data + 6);
  uint32_t msg_len = get_u32_be(data + 10);
  const uint8_t* payload = data + FRAG_HEADER_SIZE;
  size_t payload_len = len - FRAG_HEADER_SIZE;
  uint32_t expected_fragments = (msg_len + BLE_PAYLOAD_SIZE - 1) / BLE_PAYLOAD_SIZE;
  size_t expected_offset = (size_t)sequence * BLE_PAYLOAD_SIZE;

  if ((fragment_type != FRAG_START && fragment_type != FRAG_CONTINUE && fragment_type != FRAG_END) ||
      version != FRAG_HEADER_VERSION ||
      total == 0 || sequence >= total ||
      msg_id == 0 || msg_len == 0 ||
      expected_fragments == 0 || expected_fragments != total ||
      expected_offset >= msg_len || payload_len > (msg_len - expected_offset)) {
    std::fprintf(stderr, "BLE linux-central: invalid fragment header\n");
    reset_reassembly();
    return;
  }

  if (sequence == 0) {
    reset_reassembly();
    expected_total_ = total;
    received_fragments_ = 0;
    expected_message_len_ = msg_len;
    reassembly_started_ms_ = now_ms();
    current_rx_message_id_ = msg_id;
  } else if (reassembly_started_ms_ == 0 || msg_id != current_rx_message_id_) {
    return;
  }

  if (expected_total_ != total || expected_message_len_ != msg_len || sequence != received_fragments_) {
    std::fprintf(stderr, "BLE linux-central: out-of-order fragment; dropping partial packet\n");
    reset_reassembly();
    return;
  }

  reassembly_buffer_.append(payload, payload_len);
  received_fragments_++;

  if (received_fragments_ == expected_total_) {
    if (reassembly_buffer_.size() == expected_message_len_) {
      enqueue_packet(reassembly_buffer_);
    }
    reset_reassembly();
  }
}

void HostBluezBleInterface::enqueue_packet(const RNS::Bytes& packet) {
  if (frame_log_enabled_) {
    log_reticulum_frame("rx", node_label_, device_path_, tx_char_path_, this, packet);
  }
  incoming_packets_.push_back(packet);
}

bool HostBluezBleInterface::dequeue_packet(RNS::Bytes& packet) {
  if (incoming_packets_.empty()) {
    return false;
  }
  packet = incoming_packets_.front();
  incoming_packets_.pop_front();
  return true;
}

void HostBluezBleInterface::reset_reassembly() {
  reassembly_buffer_.clear();
  expected_total_ = 0;
  received_fragments_ = 0;
  expected_message_len_ = 0;
  reassembly_started_ms_ = 0;
  current_rx_message_id_ = 0;
}

void HostBluezBleInterface::mark_disconnected(const char* reason) {
  bool was_connected = connected_ || !device_path_.empty() || !tx_char_path_.empty() || !rx_char_path_.empty();
  connected_ = false;
  if (bus_ && notify_subscription_) {
    g_dbus_connection_signal_unsubscribe(bus_, notify_subscription_);
    notify_subscription_ = 0;
  }
  if (bus_ && device_subscription_) {
    g_dbus_connection_signal_unsubscribe(bus_, device_subscription_);
    device_subscription_ = 0;
  }
  device_path_.clear();
  tx_char_path_.clear();
  rx_char_path_.clear();
  identity_char_path_.clear();
  reset_reassembly();
  next_scan_ms_ = now_ms() + 1000;
  if (online_) {
    if (was_connected) {
      std::printf("BLE linux-central: disconnected; %s; returning to scan\n", reason ? reason : "link lost");
    }
    start_discovery();
  } else if (was_connected) {
    std::printf("BLE linux-central: disconnected; %s\n", reason ? reason : "link lost");
  }
}

void HostBluezBleInterface::disconnect_peer() {
  std::string path = device_path_;
  bool should_disconnect = connected_ && !path.empty();
  mark_disconnected("local stop");
  if (should_disconnect && bus_) {
    GError* error = nullptr;
    GVariant* result = g_dbus_connection_call_sync(bus_, BLUEZ_BUS, path.c_str(),
                                                   "org.bluez.Device1", "Disconnect",
                                                   nullptr, nullptr, G_DBUS_CALL_FLAGS_NONE, -1,
                                                   nullptr, &error);
    if (!result) {
      log_error("BLE linux-central: Device1.Disconnect failed", error);
    } else {
      g_variant_unref(result);
    }
  }
}
