#include "client_registry.h"
#include "cmd_accel_snapshot.h"
#include "uart_cmd.h"

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

static void fill_accel_snapshot(alox_AccelSnapshotResponse *out,
                                uint32_t filter_client_id) {
  if (out == NULL) {
    return;
  }

  out->samples_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;
    }
    if (filter_client_id != 0 && client->id != filter_client_id) {
      continue;
    }
    if (!client->accel_stream_enabled) {
      continue;
    }
    if (out->samples_count >=
        sizeof(out->samples) / sizeof(out->samples[0])) {
      break;
    }

    alox_AccelSample *sample = &out->samples[out->samples_count++];
    sample->client_id = client->id;
    sample->valid = client->accel_valid;
    sample->x = client->accel_x;
    sample->y = client->accel_y;
    sample->z = client->accel_z;
    if (client->accel_valid) {
      sample->age_ms = client_registry_ms_since(client->accel_updated_at);
    }
  }
}

static void handle_accel_snapshot(const uint8_t *data, size_t len) {
  uint32_t filter_client_id = 0;

  if (len > 0) {
    alox_UartMessage req;
    if (uart_cmd_decode(data, len, &req) == ESP_OK) {
      alox_AccelSnapshotRequest *snap_req = UART_CMD_REQ(
          &req, alox_UartMessage_accel_snapshot_request_tag, accel_snapshot_request);
      if (snap_req != NULL) {
        filter_client_id = snap_req->client_id;
      }
    }
  }

  alox_UartMessage response;
  uart_cmd_init_response(&response, alox_MessageType_ACCEL_SNAPSHOT,
                         alox_UartMessage_accel_snapshot_response_tag);
  fill_accel_snapshot(&response.payload.accel_snapshot_response, filter_client_id);

  uart_cmd_send(&response, TAG);
}

void cmd_accel_snapshot_register(void) {
  uart_cmd_register(alox_MessageType_ACCEL_SNAPSHOT, handle_accel_snapshot);
}