#include "client_registry.h"
#include "pod_settings.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];
static pod_id_map_t s_id_map;

static struct {
  board_lipo_reading_t reading;
  uint32_t updated_at;
} s_master_battery;

static bool id_in_use(uint32_t id, const uint8_t *except_mac) {
  for (uint8_t i = 0; i < s_id_map.count; i++) {
    if (s_id_map.entries[i].id != id) {
      continue;
    }
    if (except_mac != NULL &&
        memcmp(s_id_map.entries[i].mac, except_mac, CLIENT_MAC_LEN) == 0) {
      continue;
    }
    return true;
  }
  return false;
}

static int find_map_index(const uint8_t mac[CLIENT_MAC_LEN]) {
  for (uint8_t i = 0; i < s_id_map.count; i++) {
    if (memcmp(s_id_map.entries[i].mac, mac, CLIENT_MAC_LEN) == 0) {
      return (int)i;
    }
  }
  return -1;
}

static esp_err_t persist_id_map(void) {
  return pod_settings_save_id_map(&s_id_map);
}

static uint32_t alloc_next_free_id(void) {
  if (s_id_map.count >= POD_ID_MAP_MAX) {
    return 0;
  }
  uint8_t start = s_id_map.next_id;
  if (start < POD_CLIENT_ID_MIN) {
    start = (uint8_t)POD_CLIENT_ID_MIN;
  }
  for (uint32_t n = 0; n < POD_CLIENT_ID_MAX; n++) {
    uint32_t id = (uint32_t)start + n;
    if (id > POD_CLIENT_ID_MAX) {
      id = id - POD_CLIENT_ID_MAX;
    }
    if (id < POD_CLIENT_ID_MIN) {
      continue;
    }
    if (!id_in_use(id, NULL)) {
      uint32_t next = id + 1;
      if (next > POD_CLIENT_ID_MAX) {
        next = POD_CLIENT_ID_MIN;
      }
      s_id_map.next_id = (uint8_t)next;
      return id;
    }
  }
  return 0;
}

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;
      {
        static const bma456_tap_config_t k_tap_default = BMA456_TAP_CONFIG_DEFAULT;
        slot->info.tap_config = k_tap_default;
      }
      slot->info.uv_protection_enabled = false;
      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));
  memset(&s_id_map, 0, sizeof(s_id_map));
  s_id_map.next_id = (uint8_t)POD_CLIENT_ID_MIN;
  (void)pod_settings_load_id_map(&s_id_map);
  if (s_id_map.next_id < POD_CLIENT_ID_MIN) {
    s_id_map.next_id = (uint8_t)POD_CLIENT_ID_MIN;
  }
}

esp_err_t client_registry_assign_id(const uint8_t mac[CLIENT_MAC_LEN],
                                    uint32_t preferred_id, uint32_t *out_id) {
  if (mac == NULL || out_id == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  int idx = find_map_index(mac);
  if (idx >= 0) {
    *out_id = s_id_map.entries[idx].id;
    return ESP_OK;
  }

  uint32_t id = 0;
  if (preferred_id >= POD_CLIENT_ID_MIN && preferred_id <= POD_CLIENT_ID_MAX &&
      !id_in_use(preferred_id, mac)) {
    id = preferred_id;
  } else {
    id = alloc_next_free_id();
  }
  if (id == 0) {
    return ESP_ERR_NO_MEM;
  }

  if (s_id_map.count >= POD_ID_MAP_MAX) {
    return ESP_ERR_NO_MEM;
  }

  memcpy(s_id_map.entries[s_id_map.count].mac, mac, CLIENT_MAC_LEN);
  s_id_map.entries[s_id_map.count].id = (uint8_t)id;
  s_id_map.count++;
  if (id >= s_id_map.next_id) {
    uint32_t next = id + 1;
    if (next > POD_CLIENT_ID_MAX) {
      next = POD_CLIENT_ID_MIN;
    }
    s_id_map.next_id = (uint8_t)next;
  }

  esp_err_t err = persist_id_map();
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "persist id_map failed after assign id=%lu: %s",
             (unsigned long)id, esp_err_to_name(err));
  }
  *out_id = id;
  return ESP_OK;
}

void client_registry_clear_id_map(void) {
  memset(s_clients, 0, sizeof(s_clients));
  memset(&s_id_map, 0, sizeof(s_id_map));
  s_id_map.next_id = (uint8_t)POD_CLIENT_ID_MIN;
  esp_err_t err = pod_settings_clear_id_map();
  if (err != ESP_OK) {
    ESP_LOGW(TAG, "clear id_map NVS failed: %s", esp_err_to_name(err));
  }
  ESP_LOGI(TAG, "cleared id map and live registry");
}

void client_registry_mark_assign_sent(const uint8_t mac[CLIENT_MAC_LEN]) {
  if (mac == NULL) {
    return;
  }
  client_slot_t *slot = find_slot(mac);
  if (slot == NULL) {
    return;
  }
  slot->info.last_assign_sent_at = now_ms();
}

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) {
      continue;
    }

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

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;
}

esp_err_t client_registry_set_tap_config(uint32_t client_id,
                                         const bma456_tap_config_t *cfg) {
  if (cfg == NULL || !bma456_tap_config_valid(cfg)) {
    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;
  }

  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.tap_config = *cfg;
      return ESP_OK;
    }
  }
  return ESP_ERR_NOT_FOUND;
}

esp_err_t client_registry_get_tap_config(uint32_t client_id,
                                         bma456_tap_config_t *cfg_out) {
  if (cfg_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;
  }

  *cfg_out = info->tap_config;
  return ESP_OK;
}

size_t client_registry_set_tap_config_all(const bma456_tap_config_t *cfg) {
  if (cfg == NULL || !bma456_tap_config_valid(cfg)) {
    return 0;
  }
  size_t n = 0;

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

esp_err_t client_registry_set_uv_protection(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) {
      s_clients[i].info.uv_protection_enabled = enabled;
      return ESP_OK;
    }
  }
  return ESP_ERR_NOT_FOUND;
}

esp_err_t client_registry_get_uv_protection(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->uv_protection_enabled;
  return ESP_OK;
}

size_t client_registry_set_uv_protection_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.uv_protection_enabled = enabled;
    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;
}

static void clear_client_tap(client_slot_t *slot) {
  if (slot == NULL) {
    return;
  }
  slot->info.tap_valid = false;
  slot->info.tap_kind = 0;
  slot->info.tap_updated_at = 0;
}

static bool tap_kind_enabled(const client_info_t *info, uint32_t kind) {
  if (info == NULL) {
    return false;
  }
  switch (kind) {
  case 1:
    return info->tap_notify_single;
  case 2:
    return info->tap_notify_double;
  case 3:
    return info->tap_notify_triple;
  default:
    return false;
  }
}

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;
}

esp_err_t client_registry_set_tap_notify(uint32_t client_id, bool single,
                                         bool double_tap, bool triple) {
  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.tap_notify_single = single;
    s_clients[i].info.tap_notify_double = double_tap;
    s_clients[i].info.tap_notify_triple = triple;
    if (!single && !double_tap && !triple) {
      clear_client_tap(&s_clients[i]);
    }
    return ESP_OK;
  }
  return ESP_ERR_NOT_FOUND;
}

esp_err_t client_registry_get_tap_notify(uint32_t client_id, bool *single_out,
                                         bool *double_tap_out,
                                         bool *triple_out) {
  if (single_out == NULL || double_tap_out == NULL || triple_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;
  }
  *single_out = info->tap_notify_single;
  *double_tap_out = info->tap_notify_double;
  *triple_out = info->tap_notify_triple;
  return ESP_OK;
}

size_t client_registry_set_tap_notify_all(bool single, bool double_tap,
                                          bool triple) {
  size_t n = 0;
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active) {
      continue;
    }
    s_clients[i].info.tap_notify_single = single;
    s_clients[i].info.tap_notify_double = double_tap;
    s_clients[i].info.tap_notify_triple = triple;
    if (!single && !double_tap && !triple) {
      clear_client_tap(&s_clients[i]);
    }
    n++;
  }
  return n;
}

esp_err_t client_registry_update_tap(const uint8_t mac[CLIENT_MAC_LEN],
                                     uint32_t slave_id, uint32_t kind) {
  if (mac == NULL || kind < 1 || kind > 3) {
    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 (!tap_kind_enabled(&slot->info, kind)) {
    return ESP_ERR_INVALID_STATE;
  }

  slot->info.tap_kind = kind;
  slot->info.tap_valid = true;
  slot->info.tap_updated_at = now_ms();
  return ESP_OK;
}

void client_registry_expire_tap(client_info_t *info) {
  if (info == NULL || !info->tap_valid) {
    return;
  }
  if (client_registry_ms_since(info->tap_updated_at) >
      CLIENT_REGISTRY_TAP_MAX_AGE_MS) {
    info->tap_valid = false;
    info->tap_kind = 0;
    info->tap_updated_at = 0;
  }
}

void client_registry_clear_tap(client_info_t *info) {
  if (info == NULL) {
    return;
  }
  info->tap_valid = false;
  info->tap_kind = 0;
  info->tap_updated_at = 0;
}

bool client_registry_take_tap(uint32_t client_id, uint32_t *kind_out,
                              uint32_t *age_ms_out) {
  for (size_t i = 0; i < CLIENT_REGISTRY_MAX; i++) {
    if (!s_clients[i].active || s_clients[i].info.id != client_id) {
      continue;
    }
    client_info_t *info = &s_clients[i].info;
    client_registry_expire_tap(info);
    if (!info->tap_valid) {
      return false;
    }
    if (kind_out != NULL) {
      *kind_out = info->tap_kind;
    }
    if (age_ms_out != NULL) {
      *age_ms_out = client_registry_ms_since(info->tap_updated_at);
    }
    client_registry_clear_tap(info);
    return true;
  }
  return false;
}

void client_registry_set_master_battery(const board_lipo_reading_t *reading) {
  if (reading == NULL) {
    return;
  }
  s_master_battery.reading = *reading;
  s_master_battery.updated_at = now_ms();
}

bool client_registry_get_master_battery(board_lipo_reading_t *reading_out,
                                        uint32_t *age_ms_out) {
  if (reading_out == NULL) {
    return false;
  }
  *reading_out = s_master_battery.reading;
  if (age_ms_out != NULL) {
    *age_ms_out = client_registry_ms_since(s_master_battery.updated_at);
  }
  return s_master_battery.updated_at != 0;
}

esp_err_t client_registry_update_battery(const uint8_t mac[CLIENT_MAC_LEN],
                                         uint32_t slave_id, bool lipo1_valid,
                                         uint32_t lipo1_mv, bool lipo2_valid,
                                         uint32_t lipo2_mv) {
  if (mac == NULL) {
    return ESP_ERR_INVALID_ARG;
  }

  client_slot_t *slot = find_slot(mac);
  if (slot == NULL) {
    /* Ids are assigned on presence only; ignore early battery reports. */
    return ESP_ERR_NOT_FOUND;
  }

  if (slave_id != 0 && slot->info.id != slave_id) {
    ESP_LOGW(TAG, "battery id mismatch slot=%lu report=%lu mac %02x:…:%02x",
             (unsigned long)slot->info.id, (unsigned long)slave_id, mac[0],
             mac[5]);
    return ESP_ERR_INVALID_STATE;
  }

  slot->info.lipo1_valid = lipo1_valid;
  slot->info.lipo2_valid = lipo2_valid;
  slot->info.lipo1_mv = lipo1_mv;
  slot->info.lipo2_mv = lipo2_mv;
  slot->info.battery_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;
}