powerpods/main/client_registry.c

#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];

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

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

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) {
    bool is_new = false;
    esp_err_t err = client_registry_upsert(mac, slave_id, 0, true, false, &is_new);
    if (err != ESP_OK) {
      return err;
    }
    slot = find_slot(mac);
    if (slot == NULL) {
      return ESP_ERR_NOT_FOUND;
    }
    ESP_LOGI(TAG, "battery auto-registered id=%lu (report before heartbeat)",
             (unsigned long)slave_id);
  }

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

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