#include "cmd_handler.h"
#include "driver/uart.h"
#include "esp_err.h"
#include "esp_log.h"
#include "freertos/idf_additions.h"
#include "hal/uart_types.h"
#include "uart.h"
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

static const char *TAG = "[UART]";
static QueueHandle_t uart_cmd_queue;

static bool uart_enqueue_packet(const uart_packet_t *packet) {
  if (packet->len == 0) {
    return false;
  }

  generic_msg_t msg = {
      .msg_id = packet->payload[0],
      .len = packet->len > 1 ? packet->len - 1 : 0,
      .payload = NULL,
  };

  if (msg.len > 0) {
    msg.payload = malloc(msg.len);
    if (msg.payload == NULL) {
      ESP_LOGE(TAG, "failed to allocate command payload");
      return false;
    }
    memcpy(msg.payload, &packet->payload[1], msg.len);
  }

  if (xQueueSend(uart_cmd_queue, &msg, pdMS_TO_TICKS(500)) != pdPASS) {
    free(msg.payload);
    ESP_LOGW(TAG, "command queue full (cmd 0x%02x)", (unsigned)msg.msg_id);
    return false;
  }
  return true;
}

void init_uart(QueueHandle_t cmd_queue) {
  esp_err_t err;

  uart_cmd_queue = cmd_queue;
  uart_config_t uart_config = {
      .baud_rate = UART_BAUD_RATE,
      .data_bits = UART_DATA_8_BITS,
      .parity = UART_PARITY_DISABLE,
      .stop_bits = UART_STOP_BITS_1,
      .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
  };

  err = uart_driver_install(UART_NUM, UART_DRIVER_RX_BUF_SIZE,
                            UART_DRIVER_TX_BUF_SIZE, 0, NULL, 0);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "uart_driver_install failed: %s", esp_err_to_name(err));
    return;
  }
  err = uart_param_config(UART_NUM, &uart_config);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "uart_param_config failed: %s", esp_err_to_name(err));
    return;
  }
  err = uart_set_pin(UART_NUM, UART_TXD_PIN, UART_RXD_PIN, UART_PIN_NO_CHANGE,
                     UART_PIN_NO_CHANGE);
  if (err != ESP_OK) {
    ESP_LOGE(TAG, "uart_set_pin failed: %s", esp_err_to_name(err));
    return;
  }

  if (xTaskCreate(uart_read_task, "uart_rx", 4096, NULL, 5, NULL) != pdPASS) {
    ESP_LOGE(TAG, "failed to create uart_read_task");
  }
}

void uart_read_task(void *param) {
  (void)param;

  uint8_t *data = (uint8_t *)malloc(UART_BUF_SIZE);
  if (data == NULL) {
    ESP_LOGE(TAG, "out of memory");
    vTaskDelete(NULL);
    return;
  }

  int len = 0;
  uart_packet_t packet = {.state = STATE_START};
  TickType_t last_byte_time = xTaskGetTickCount();
  const TickType_t timeout_ticks = pdMS_TO_TICKS(50);

  while (1) {
    len = uart_read_bytes(UART_NUM, data, UART_BUF_SIZE, pdMS_TO_TICKS(20));
    if (len > 0) {
      for (int i = 0; i < len; ++i) {
        if (parse_uart_byte(data[i], &packet)) {
          ESP_LOGI(TAG, "received message cmd=0x%02x len=%u",
                   packet.len > 0 ? packet.payload[0] : 0, (unsigned)packet.len);
          uart_enqueue_packet(&packet);
        }
      }
      last_byte_time = xTaskGetTickCount();
    } else if (packet.state != STATE_START) {
      TickType_t now = xTaskGetTickCount();
      if ((now - last_byte_time) > timeout_ticks) {
        packet.state = STATE_START;
      }
    }
  }
}

bool parse_uart_byte(uint8_t byte, uart_packet_t *p) {
  switch (p->state) {
  case STATE_START:
    if (byte == START_MARKER) {
      p->index = 0;
      p->checksum = 0;
      p->state = STATE_LEN;
    }
    break;

  case STATE_LEN:
    if (byte > MAX_PAYLOAD_SIZE || byte == 0) {
      p->state = STATE_START;
    } else {
      p->len = byte;
      p->state = STATE_DATA;
    }
    break;

  case STATE_DATA:
    p->payload[p->index++] = byte;
    p->checksum ^= byte;
    if (p->index >= p->len) {
      p->state = STATE_CHECKSUM;
    }
    break;

  case STATE_CHECKSUM:
    if (byte == p->checksum) {
      p->state = STATE_STOP;
    } else {
      p->state = STATE_START;
    }
    break;

  case STATE_STOP:
    p->state = STATE_START;
    if (byte == STOP_MARKER) {
      return true;
    }
    break;
  }
  return false;
}

esp_err_t uart_send_framed(const uint8_t *payload, size_t len) {
  if (payload == NULL || len == 0 || len > MAX_PAYLOAD_SIZE) {
    return ESP_ERR_INVALID_ARG;
  }

  uint8_t checksum = 0;
  for (size_t i = 0; i < len; i++) {
    checksum ^= payload[i];
  }

  uint8_t frame[4 + MAX_PAYLOAD_SIZE];
  size_t pos = 0;
  frame[pos++] = START_MARKER;
  frame[pos++] = (uint8_t)len;
  memcpy(&frame[pos], payload, len);
  pos += len;
  frame[pos++] = checksum;
  frame[pos++] = STOP_MARKER;

  int written = uart_write_bytes(UART_NUM, frame, pos);
  if (written < 0 || (size_t)written != pos) {
    return ESP_FAIL;
  }
  return ESP_OK;
}