#include "cmd_handler.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/idf_additions.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;

void init_cmdHandler(QueueHandle_t queue) {
  cmd_queue = queue;
  xTaskCreate(vCmdDispatcherTask, "cmd_dispatch", 4096, NULL, 5, NULL);
}

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) {
  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) {
          handlers[i].callback(msg.payload, msg.len);
          handled = true;
          break;
        }
      }
      if (!handled) {
        ESP_LOGW(TAG, "no handler for msg_id 0x%04x (%u bytes)", msg.msg_id,
                 (unsigned)msg.len);
      }
      free(msg.payload);
    }
  }
}