#include "cmd_handler.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.h"
#include "uart_messages.pb.h"
#include <stdlib.h>
#include <string.h>

#define MAX_HANDLERS 32

static const char *TAG = "[CMDH]";
static QueueHandle_t cmd_queue;
static msg_binding_t handlers[MAX_HANDLERS];
static int handler_count;

static const char *message_type_name(uint16_t id) {
  switch ((alox_MessageType)id) {
  case alox_MessageType_ACK:
    return "ACK";
  case alox_MessageType_ECHO:
    return "ECHO";
  case alox_MessageType_VERSION:
    return "VERSION";
  case alox_MessageType_CLIENT_INFO:
    return "CLIENT_INFO";
  case alox_MessageType_CLIENT_INPUT:
    return "CLIENT_INPUT";
  case alox_MessageType_ACCEL_DEADZONE:
    return "ACCEL_DEADZONE";
  case alox_MessageType_ESPNOW_UNICAST_TEST:
    return "ESPNOW_UNICAST_TEST";
  case alox_MessageType_LED_RING:
    return "LED_RING";
  case alox_MessageType_OTA_START:
    return "OTA_START";
  case alox_MessageType_OTA_PAYLOAD:
    return "OTA_PAYLOAD";
  case alox_MessageType_OTA_END:
    return "OTA_END";
  case alox_MessageType_OTA_STATUS:
    return "OTA_STATUS";
  case alox_MessageType_OTA_START_ESPNOW:
    return "OTA_START_ESPNOW";
  case alox_MessageType_OTA_SLAVE_PROGRESS:
    return "OTA_SLAVE_PROGRESS";
  case alox_MessageType_FIND_ME:
    return "FIND_ME";
  case alox_MessageType_RESTART:
    return "RESTART";
  default:
    return "UNKNOWN";
  }
}

void init_cmdHandler(QueueHandle_t queue) {
  cmd_queue = queue;
  if (xTaskCreate(vCmdDispatcherTask, "cmd_dispatch", 8192, NULL, 5, NULL) !=
      pdPASS) {
    ESP_LOGE(TAG, "failed to create cmd_dispatch task");
  }
}

esp_err_t msg_register_handler(uint16_t id, msg_callback_t cb) {
  if (cb == NULL) {
    return ESP_ERR_INVALID_ARG;
  }
  for (int i = 0; i < handler_count; i++) {
    if (handlers[i].msg_id == id) {
      handlers[i].callback = cb;
      return ESP_OK;
    }
  }
  if (handler_count >= MAX_HANDLERS) {
    return ESP_ERR_NO_MEM;
  }
  handlers[handler_count].msg_id = id;
  handlers[handler_count].callback = cb;
  handler_count++;
  return ESP_OK;
}

esp_err_t msg_post(uint16_t id, const uint8_t *data, size_t len) {
  if (cmd_queue == NULL) {
    return ESP_ERR_INVALID_STATE;
  }

  generic_msg_t msg = {.msg_id = id, .len = len, .payload = NULL};
  if (len > 0) {
    msg.payload = malloc(len);
    if (msg.payload == NULL) {
      return ESP_ERR_NO_MEM;
    }
    memcpy(msg.payload, data, len);
  }

  if (xQueueSend(cmd_queue, &msg, pdMS_TO_TICKS(100)) != pdPASS) {
    free(msg.payload);
    return ESP_ERR_TIMEOUT;
  }
  return ESP_OK;
}

void vCmdDispatcherTask(void *param) {
  (void)param;
  generic_msg_t msg;

  while (1) {
    if (xQueueReceive(cmd_queue, &msg, portMAX_DELAY) == pdPASS) {
      bool handled = false;
      for (int i = 0; i < handler_count; i++) {
        if (handlers[i].msg_id == msg.msg_id) {
          ESP_LOGI(TAG, "trigger command %s (0x%02x)", message_type_name(msg.msg_id),
                   (unsigned)msg.msg_id);
          handlers[i].callback(msg.payload, msg.len);
          handled = true;
          break;
        }
      }
      if (!handled) {
        ESP_LOGW(TAG, "no handler for %s (0x%02x)", message_type_name(msg.msg_id),
                 (unsigned)msg.msg_id);
      }
      free(msg.payload);
    }
  }
}