#include "cmd_battery.h"
#include "board_input.h"
#include "client_registry.h"
#include "esp_log.h"
#include "uart_cmd.h"

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

static void fill_lipo(alox_LipoReading *dst, bool *has_dst, bool valid,
                      uint32_t mv) {
  if (dst == NULL || has_dst == NULL) {
    return;
  }
  *has_dst = true;
  dst->valid = valid;
  dst->voltage_mv = valid ? mv : 0;
}

static bool append_battery_sample(alox_BatteryStatusResponse *resp,
                                  uint32_t client_id, bool lipo1_valid,
                                  uint32_t lipo1_mv, bool lipo2_valid,
                                  uint32_t lipo2_mv, uint32_t age_ms) {
  if (resp->samples_count >=
      sizeof(resp->samples) / sizeof(resp->samples[0])) {
    return false;
  }

  alox_BatterySample *sample = &resp->samples[resp->samples_count++];
  sample->client_id = client_id;
  fill_lipo(&sample->lipo1, &sample->has_lipo1, lipo1_valid, lipo1_mv);
  fill_lipo(&sample->lipo2, &sample->has_lipo2, lipo2_valid, lipo2_mv);
  sample->age_ms = age_ms;
  return lipo1_valid || lipo2_valid;
}

static bool append_master_cached(alox_BatteryStatusResponse *resp) {
  board_lipo_reading_t reading;
  uint32_t age_ms = 0;

  if (!client_registry_get_master_battery(&reading, &age_ms)) {
    board_input_read_lipo(&reading);
    client_registry_set_master_battery(&reading);
    age_ms = 0;
  }

  return append_battery_sample(resp, 0, reading.lipo1_valid, reading.lipo1_mv,
                               reading.lipo2_valid, reading.lipo2_mv, age_ms);
}

static bool append_slave_cached(alox_BatteryStatusResponse *resp,
                                const client_info_t *client) {
  if (client == NULL) {
    return false;
  }
  if (client->battery_updated_at == 0) {
    return false;
  }

  return append_battery_sample(
      resp, client->id, client->lipo1_valid, client->lipo1_mv,
      client->lipo2_valid, client->lipo2_mv,
      client_registry_ms_since(client->battery_updated_at));
}

static void handle_battery_status(const uint8_t *data, size_t len) {
  alox_BatteryStatusRequest req = alox_BatteryStatusRequest_init_zero;

  if (len > 0) {
    alox_UartMessage uart_msg;
    if (uart_cmd_decode(data, len, &uart_msg) == ESP_OK) {
      const alox_BatteryStatusRequest *req_ptr = UART_CMD_REQ(
          &uart_msg, alox_UartMessage_battery_status_request_tag,
          battery_status_request);
      if (req_ptr != NULL) {
        req = *req_ptr;
      }
    }
  }

  alox_UartMessage response;
  uart_cmd_init_response(&response, alox_MessageType_BATTERY_STATUS,
                         alox_UartMessage_battery_status_response_tag);
  alox_BatteryStatusResponse *resp =
      &response.payload.battery_status_response;
  resp->success = false;
  resp->samples_count = 0;

  bool any = false;

  if (req.all_clients) {
    any |= append_master_cached(resp);
    for (size_t i = 0; i < client_registry_count(); i++) {
      const client_info_t *client = client_registry_at(i);
      if (client == NULL) {
        continue;
      }
      any |= append_slave_cached(resp, client);
    }
    ESP_LOGI(TAG, "battery cache all_clients → %u samples",
             (unsigned)resp->samples_count);
  } else if (req.client_id == 0) {
    any = append_master_cached(resp);
    ESP_LOGI(TAG, "battery cache master");
  } else {
    const client_info_t *client = client_registry_find_by_id(req.client_id);
    if (client != NULL) {
      any = append_slave_cached(resp, client);
    } else {
      ESP_LOGW(TAG, "client %lu not in registry", (unsigned long)req.client_id);
    }
  }

  resp->success = any;
  uart_cmd_send(&response, TAG);
}

void cmd_battery_register(void) {
  uart_cmd_register(alox_MessageType_BATTERY_STATUS, handle_battery_status);
}