#include "client_registry.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include <stdio.h>
#include <string.h>

static const char *TAG = "[CLIENTS]";

typedef struct {
  client_info_t info;
  bool active;
} client_slot_t;

static client_slot_t s_clients[CLIENT_REGISTRY_MAX];

uint32_t client_registry_now_ms(void) {
  return (uint32_t)(xTaskGetTickCount() * portTICK_PERIOD_MS);
}

uint32_t client_registry_ms_since(uint32_t timestamp) {
  if (timestamp == 0) {
    return 0;
  }
  return client_registry_now_ms() - timestamp;
}

static uint32_t now_ms(void) { return client_registry_now_ms(); }

static bool mac_equal(const uint8_t *a, const uint8_t *b) {
  return memcmp(a, b, CLIENT_MAC_LEN) == 0;
}

static void mac_to_str(const uint8_t *mac, char *out, size_t out_len) {
  snprintf(out, out_len, "%02x:%02x:%02x:%02x:%02x:%02x", mac[0], mac[1],
           mac[2], mac[3], mac[4], mac[5]);
}

static client_slot_t *find_slot(const uint8_t mac[CLIENT_MAC_LEN]) {
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (s_clients[i].active && mac_equal(s_clients[i].info.mac, mac)) {
      return &s_clients[i];
    }
  }
  return NULL;
}

static void evict_stale_id(uint32_t id, const uint8_t mac[CLIENT_MAC_LEN]) {
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active || s_clients[i].info.id != id) {
      continue;
    }
    if (mac_equal(s_clients[i].info.mac, mac)) {
      continue;
    }
    char old_mac[18];
    mac_to_str(s_clients[i].info.mac, old_mac, sizeof(old_mac));
    ESP_LOGW(TAG, "dropping stale id=%lu mac=%s (now %02x:…:%02x)", (unsigned long)id,
             old_mac, mac[0], mac[5]);
    s_clients[i].active = false;
  }
}

static client_slot_t *alloc_slot(const uint8_t mac[CLIENT_MAC_LEN],
                                 bool *out_is_new) {
  client_slot_t *slot = find_slot(mac);
  if (slot != NULL) {
    if (out_is_new != NULL) {
      *out_is_new = false;
    }
    return slot;
  }

  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active) {
      slot = &s_clients[i];
      slot->active = true;
      memcpy(slot->info.mac, mac, CLIENT_MAC_LEN);
      slot->info.accel_deadzone = CLIENT_REGISTRY_DEFAULT_ACCEL_DEADZONE;
      if (out_is_new != NULL) {
        *out_is_new = true;
      }
      return slot;
    }
  }
  return NULL;
}

void client_registry_init(void) { memset(s_clients, 0, sizeof(s_clients)); }

const client_info_t *
client_registry_find_by_mac(const uint8_t mac[CLIENT_MAC_LEN]) {
  client_slot_t *slot = find_slot(mac);
  return slot != NULL ? &slot->info : NULL;
}

esp_err_t client_registry_upsert(const uint8_t mac[CLIENT_MAC_LEN], uint32_t id,
                                 uint32_t version, bool available, bool used,
                                 bool *out_is_new) {
  if (mac == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  uint32_t ts = now_ms();
  bool is_new = false;
  client_slot_t *slot = alloc_slot(mac, &is_new);
  if (slot == NULL) {
    return ESP_ERR_NO_MEM;
  }

  slot->info.id = id;
  slot->info.version = version;
  slot->info.available = available;
  slot->info.used = used;
  slot->info.last_ping_at = ts;
  slot->info.last_success_ping_at = ts;
  evict_stale_id(id, mac);

  if (out_is_new != NULL) {
    *out_is_new = is_new;
  }
  return ESP_OK;
}

esp_err_t client_registry_heartbeat(const uint8_t mac[CLIENT_MAC_LEN],
                                    uint32_t id, uint32_t version, bool used,
                                    bool *out_is_new, bool *out_reactivated) {
  if (mac == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  uint32_t ts = now_ms();
  bool is_new = false;
  bool reactivated = false;
  client_slot_t *slot = alloc_slot(mac, &is_new);
  if (slot == NULL) {
    return ESP_ERR_NO_MEM;
  }

  if (!is_new && !slot->info.available) {
    reactivated = true;
  }

  slot->info.id = id;
  slot->info.version = version;
  slot->info.used = used;
  slot->info.available = true;
  slot->info.last_ping_at = ts;
  slot->info.last_success_ping_at = ts;
  evict_stale_id(id, mac);

  if (out_is_new != NULL) {
    *out_is_new = is_new;
  }
  if (out_reactivated != NULL) {
    *out_reactivated = reactivated || is_new;
  }
  return ESP_OK;
}

void client_registry_check_timeouts(uint32_t timeout_ms) {
  uint32_t now = now_ms();

  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active || !s_clients[i].info.available) {
      continue;
    }

    uint32_t elapsed = now - s_clients[i].info.last_success_ping_at;
    if (elapsed > timeout_ms) {
      s_clients[i].info.available = false;
      char mac_str[18];
      mac_to_str(s_clients[i].info.mac, mac_str, sizeof(mac_str));
      ESP_LOGW(TAG, "client inactive id=%lu mac=%s (no heartbeat for %lu ms)",
               (unsigned long)s_clients[i].info.id, mac_str,
               (unsigned long)elapsed);
    }
  }
}

size_t client_registry_count(void) {
  size_t n = 0;
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (s_clients[i].active) {
      n++;
    }
  }
  return n;
}

const client_info_t *client_registry_find_by_id(uint32_t id) {
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (s_clients[i].active && s_clients[i].info.id == id) {
      return &s_clients[i].info;
    }
  }
  return NULL;
}

esp_err_t client_registry_set_accel_deadzone(uint32_t client_id,
                                             uint32_t deadzone) {
  const client_info_t *info = client_registry_find_by_id(client_id);
  if (info == NULL) {
    return ESP_ERR_NOT_FOUND;
  }

  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (s_clients[i].active && s_clients[i].info.id == client_id) {
      s_clients[i].info.accel_deadzone = deadzone;
      return ESP_OK;
    }
  }
  return ESP_ERR_NOT_FOUND;
}

esp_err_t client_registry_get_accel_deadzone(uint32_t client_id,
                                             uint32_t *deadzone_out) {
  if (deadzone_out == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  const client_info_t *info = client_registry_find_by_id(client_id);
  if (info == NULL) {
    return ESP_ERR_NOT_FOUND;
  }

  *deadzone_out = info->accel_deadzone;
  return ESP_OK;
}

size_t client_registry_set_accel_deadzone_all(uint32_t deadzone) {
  size_t n = 0;

  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active) {
      continue;
    }
    s_clients[i].info.accel_deadzone = deadzone;
    n++;
  }
  return n;
}

static void clear_client_accel(client_slot_t *slot) {
  if (slot == NULL) {
    return;
  }
  slot->info.accel_valid = false;
  slot->info.accel_x = 0;
  slot->info.accel_y = 0;
  slot->info.accel_z = 0;
  slot->info.accel_updated_at = 0;
}

esp_err_t client_registry_set_accel_stream(uint32_t client_id, bool enabled) {
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active || s_clients[i].info.id != client_id) {
      continue;
    }
    s_clients[i].info.accel_stream_enabled = enabled;
    if (!enabled) {
      clear_client_accel(&s_clients[i]);
    }
    return ESP_OK;
  }
  return ESP_ERR_NOT_FOUND;
}

esp_err_t client_registry_get_accel_stream(uint32_t client_id,
                                           bool *enabled_out) {
  if (enabled_out == NULL) {
    return ESP_ERR_INVALID_ARG;
  }
  const client_info_t *info = client_registry_find_by_id(client_id);
  if (info == NULL) {
    return ESP_ERR_NOT_FOUND;
  }
  *enabled_out = info->accel_stream_enabled;
  return ESP_OK;
}

size_t client_registry_set_accel_stream_all(bool enabled) {
  size_t n = 0;
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active) {
      continue;
    }
    s_clients[i].info.accel_stream_enabled = enabled;
    if (!enabled) {
      clear_client_accel(&s_clients[i]);
    }
    n++;
  }
  return n;
}

esp_err_t client_registry_update_accel(const uint8_t mac[CLIENT_MAC_LEN],
                                       uint32_t slave_id, int16_t x, int16_t y,
                                       int16_t z) {
  if (mac == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  client_slot_t *slot = find_slot(mac);
  if (slot == NULL) {
    return ESP_ERR_NOT_FOUND;
  }
  if (slot->info.id != slave_id) {
    return ESP_ERR_INVALID_ARG;
  }
  if (!slot->info.accel_stream_enabled) {
    return ESP_ERR_INVALID_STATE;
  }

  slot->info.accel_x = x;
  slot->info.accel_y = y;
  slot->info.accel_z = z;
  slot->info.accel_valid = true;
  slot->info.accel_updated_at = now_ms();
  return ESP_OK;
}

const client_info_t *client_registry_at(size_t index) {
  size_t n = 0;
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active) {
      continue;
    }
    if (n == index) {
      return &s_clients[i].info;
    }
    n++;
  }
  return NULL;
}