#include "driver/uart.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "esp_log_buffer.h"
#include "freertos/idf_additions.h"
#include "hal/uart_types.h"
#include "nvs_flash.h"
#include "portmacro.h"
#include "uart.h"
#include <stdbool.h>
#include <string.h>

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

void init_uart(QueueHandle_t cmd_queue) {
  uart_cmd_queue = cmd_queue;
  uart_config_t uart_config = {// .baud_rate = 115200, // 921600, 115200
                               .baud_rate = 921600,
                               .data_bits = UART_DATA_8_BITS,
                               .parity = UART_PARITY_DISABLE,
                               .stop_bits = UART_STOP_BITS_1,
                               .flow_ctrl = UART_HW_FLOWCTRL_DISABLE};

  uart_driver_install(UART_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0);
  uart_param_config(UART_NUM, &uart_config);
  uart_set_pin(UART_NUM, UART_TXD_PIN, UART_RXD_PIN, UART_PIN_NO_CHANGE,
               UART_PIN_NO_CHANGE);

  xTaskCreate(uart_read_task, "Read Uart", 4096, NULL, 1, NULL);
}

void uart_read_task(void *param) {
  // Send all Input from Uart to the Message Handler for Parsing
  uint8_t *data = (uint8_t *)malloc(UART_BUF_SIZE);
  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 = 0;
    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("UART", "Paket empfangen! Länge: %d", packet.len);
          xQueueSend(uart_cmd_queue, &packet, 0);
        }
      }
      last_byte_time = xTaskGetTickCount();
    } else { // reset message receive when there comes nothing new
      if (packet.state != STATE_START) {
        TickType_t now = xTaskGetTickCount();
        if ((now - last_byte_time) > timeout_ticks) {
          packet.state = STATE_START;
          ESP_LOGW("UART", "Parser Timeout - Resetting State");
        }
      }
    }
  }
}

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) {
      // corrupt length or fake start
      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 {
      // CRC wrong: reset
      p->state = STATE_START;
    }
    break;

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