powerpods/main/ble_stream.c

#include "ble_stream.h"
#include "app_config.h"
#include "ble_host.h"
#include "client_registry.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "pb_encode.h"
#include "uart.h"
#include "uart_cmd.h"
#include "uart_messages.pb.h"
#include <string.h>

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

#define BLE_STREAM_PERIOD_MIN_MS 8u
#define BLE_STREAM_PERIOD_MAX_MS 16u
#define BLE_STREAM_PERIOD_DEFAULT_MS 16u

static uint32_t s_period_ms = BLE_STREAM_PERIOD_DEFAULT_MS;
static TaskHandle_t s_task;
static volatile bool s_running;

static bool tap_notify_any(const client_info_t *client) {
  return client != NULL &&
         (client->tap_notify_single || client->tap_notify_double ||
          client->tap_notify_triple);
}

static alox_TapKind tap_kind_from_registry(uint32_t kind) {
  switch (kind) {
  case 1:
    return alox_TapKind_TAP_SINGLE;
  case 2:
    return alox_TapKind_TAP_DOUBLE;
  case 3:
    return alox_TapKind_TAP_TRIPLE;
  default:
    return alox_TapKind_TAP_NONE;
  }
}

static void fill_clients(alox_BleStreamTick *tick) {
  tick->clients_count = 0;
  size_t count = client_registry_count();
  for (size_t i = 0; i < count; i++) {
    const client_info_t *client = client_registry_at(i);
    if (client == NULL) {
      continue;
    }
    const bool want_accel = client->accel_stream_enabled;
    const bool want_tap = tap_notify_any(client);
    if (!want_accel && !want_tap) {
      continue;
    }
    if (tick->clients_count >=
        sizeof(tick->clients) / sizeof(tick->clients[0])) {
      break;
    }

    alox_CacheClientStatus *entry = &tick->clients[tick->clients_count++];
    entry->client_id = client->id;
    entry->has_accel = false;
    entry->has_tap = false;

    if (want_accel) {
      entry->has_accel = true;
      entry->accel.valid = client->accel_valid;
      if (client->accel_valid) {
        entry->accel.x = client->accel_x;
        entry->accel.y = client->accel_y;
        entry->accel.z = client->accel_z;
        entry->accel.age_ms =
            client_registry_ms_since(client->accel_updated_at);
      }
    }

    if (want_tap) {
      uint32_t kind = 0;
      uint32_t age_ms = 0;
      if (client_registry_take_tap(client->id, &kind, &age_ms)) {
        entry->has_tap = true;
        entry->tap.kind = tap_kind_from_registry(kind);
        entry->tap.age_ms = age_ms;
      }
    }
  }
}

static bool any_accel_stream(void) {
  size_t count = client_registry_count();
  for (size_t i = 0; i < count; i++) {
    const client_info_t *client = client_registry_at(i);
    if (client != NULL && client->accel_stream_enabled) {
      return true;
    }
  }
  return false;
}

static esp_err_t send_tick_truncated(alox_UartMessage *msg) {
  uint8_t pb_buf[MAX_PAYLOAD_SIZE];
  pb_ostream_t stream = pb_ostream_from_buffer(pb_buf, sizeof(pb_buf));

  while (true) {
    stream = pb_ostream_from_buffer(pb_buf, sizeof(pb_buf));
    if (!pb_encode(&stream, alox_UartMessage_fields, msg)) {
      return ESP_FAIL;
    }

    size_t payload_len = stream.bytes_written + 1;
    uint16_t max_pay = ble_host_max_payload();
    /* Framed size = payload + 4; notify must fit ATT MTU-3. */
    size_t frame_len = payload_len + 4;
    if (payload_len <= MAX_PAYLOAD_SIZE && frame_len <= max_pay) {
      uint8_t payload[MAX_PAYLOAD_SIZE];
      payload[0] = (uint8_t)msg->type;
      memcpy(&payload[1], pb_buf, stream.bytes_written);
      return uart_send_framed(payload, payload_len);
    }

    if (msg->payload.ble_stream_tick.clients_count == 0) {
      ESP_LOGW(TAG, "tick too large even with 0 clients (need %u max %u)",
               (unsigned)frame_len, (unsigned)max_pay);
      return ESP_ERR_NO_MEM;
    }

    msg->payload.ble_stream_tick.clients_count--;
    ESP_LOGW(TAG, "truncating tick clients to %u for MTU",
             (unsigned)msg->payload.ble_stream_tick.clients_count);
  }
}

static void ble_stream_task(void *param) {
  (void)param;
  ESP_LOGI(TAG, "BLE stream task started period_ms=%lu",
           (unsigned long)s_period_ms);

  while (s_running) {
    uint32_t period = s_period_ms;
    if (period < BLE_STREAM_PERIOD_MIN_MS) {
      period = BLE_STREAM_PERIOD_MIN_MS;
    }
    if (period > BLE_STREAM_PERIOD_MAX_MS) {
      period = BLE_STREAM_PERIOD_MAX_MS;
    }

    if (ble_host_ready() && any_accel_stream()) {
      alox_UartMessage msg = alox_UartMessage_init_zero;
      uart_cmd_init_response(&msg, alox_MessageType_BLE_STREAM_TICK,
                             alox_UartMessage_ble_stream_tick_tag);
      msg.payload.ble_stream_tick.esp_timestamp_us =
          (uint64_t)esp_timer_get_time();
      msg.payload.ble_stream_tick.period_ms = period;
      fill_clients(&msg.payload.ble_stream_tick);

      esp_err_t err = send_tick_truncated(&msg);
      if (err != ESP_OK) {
        ESP_LOGW(TAG, "tick send failed: %s", esp_err_to_name(err));
      }
    }

    vTaskDelay(pdMS_TO_TICKS(period));
  }

  s_task = NULL;
  vTaskDelete(NULL);
}

void ble_stream_set_period_ms(uint32_t period_ms) {
  if (period_ms == 0) {
    period_ms = BLE_STREAM_PERIOD_DEFAULT_MS;
  }
  if (period_ms < BLE_STREAM_PERIOD_MIN_MS) {
    period_ms = BLE_STREAM_PERIOD_MIN_MS;
  }
  if (period_ms > BLE_STREAM_PERIOD_MAX_MS) {
    period_ms = BLE_STREAM_PERIOD_MAX_MS;
  }
  s_period_ms = period_ms;
}

uint32_t ble_stream_period_ms(void) { return s_period_ms; }

void ble_stream_update(void) {
  const app_config_t *cfg = app_config_get();
  bool want = cfg != NULL && cfg->host_ble && any_accel_stream();

  if (want && !s_running) {
    s_running = true;
    if (xTaskCreate(ble_stream_task, "ble_stream", 4096, NULL, 5, &s_task) !=
        pdPASS) {
      s_running = false;
      s_task = NULL;
      ESP_LOGE(TAG, "failed to create ble_stream task");
    }
  } else if (!want && s_running) {
    s_running = false;
  }
}