#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 "host_transport.h"
#include "uart.h"
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

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

static esp_err_t uart_host_tx(const uint8_t *frame, size_t frame_len) {
  int written = uart_write_bytes(UART_NUM, frame, frame_len);
  if (written < 0 || (size_t)written != frame_len) {
    return ESP_FAIL;
  }
  return ESP_OK;
}

void init_uart(QueueHandle_t cmd_queue) {
  esp_err_t err;

  host_transport_set_cmd_queue(cmd_queue);
  host_transport_set_tx(uart_host_tx);

  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;
  }

  while (1) {
    int len = uart_read_bytes(UART_NUM, data, UART_BUF_SIZE, pdMS_TO_TICKS(20));
    if (len > 0) {
      host_rx_bytes(data, (size_t)len);
    }
  }
}

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) {
  return host_send_framed(payload, len);
}