#include <stdio.h>
#include "esp_log.h"
#include "driver/uart.h"
#include "string.h"

#define TAG "UART"

#define TXD_PIN 4
#define RXD_PIN 5

#define RX_BUF_SIZE 1024
#define TX_BUF_SIZE 1024
#define PATTERN_LEN 3

#define SOM 0x02      // Start of Message
#define EOM 0x03      // End of Message
#define CMD_STATUS 0x01
#define CMD_CONFIG 0x02
#define MAX_PAYLOAD_SIZE 256

QueueHandle_t uart_queue;

// Helper function to calculate checksum
uint8_t calculate_checksum(uint8_t *data, size_t length) {
    uint8_t checksum = 0;
    for (size_t i = 0; i < length; i++) {
        checksum ^= data[i];
    }
    return checksum;
}

// Function to send a message
void send_message(uint8_t command_id, uint8_t *payload, uint8_t payload_length) {
    uint8_t buffer[MAX_PAYLOAD_SIZE + 4];
    buffer[0] = SOM;
    buffer[1] = command_id;
    buffer[2] = payload_length;

    // Copy payload
    memcpy(buffer + 3, payload, payload_length);

    // Add checksum
    buffer[3 + payload_length] = calculate_checksum(buffer + 1, 2 + payload_length);

    // Add End of Message
    buffer[4 + payload_length] = EOM;

    // Send data over UART
    uart_write_bytes(UART_NUM_1, (const char *)buffer, 5 + payload_length);
    printf ("message sent\n");
}

// Function to process received message
void process_received_message(uint8_t *data, size_t length) {
    // Validate Start and End of Message
    if (data[0] != SOM || data[length - 1] != EOM) {
        ESP_LOGE(TAG, "Invalid message format");
        return;
    }

    uint8_t command_id = data[1];
    uint8_t payload_length = data[2];

    // Validate payload length
    if (payload_length > MAX_PAYLOAD_SIZE || payload_length + 4 != length) {
        ESP_LOGE(TAG, "Invalid message length");
        return;
    }

    uint8_t checksum = data[length - 2];
    if (checksum != calculate_checksum(data + 1, length - 3)) {
        ESP_LOGE(TAG, "Invalid checksum");
        return;
    }

    // Handle commands
    switch (command_id) {
        case CMD_STATUS:
            ESP_LOGI(TAG, "Received STATUS: %.*s", payload_length, data + 3);
            break;
        case CMD_CONFIG:
            ESP_LOGI(TAG, "Received CONFIG: %.*s", payload_length, data + 3);
            break;
        default:
            ESP_LOGE(TAG, "Unknown command: %d", command_id);
            break;
    }
}

void uart_event_task(void *params) {
    uart_event_t uart_event;
    uint8_t *received_buffer = malloc(RX_BUF_SIZE);
    size_t datalen;

    while (true) {
        if (xQueueReceive(uart_queue, &uart_event, portMAX_DELAY)) {
            switch (uart_event.type) {
                case UART_DATA:
                    datalen = uart_event.size;
                    uart_read_bytes(UART_NUM_1, received_buffer, datalen, portMAX_DELAY);

                    // Process received message if valid
                    if (received_buffer[0] == SOM && received_buffer[datalen - 1] == EOM) {
                        process_received_message(received_buffer, datalen);
                    } else {
                        ESP_LOGE(TAG, "Invalid message received");
                    }
                    break;
                case UART_BUFFER_FULL:
                    ESP_LOGW(TAG, "UART buffer full");
                    uart_flush_input(UART_NUM_1);
                    xQueueReset(uart_queue);
                    break;
                case UART_FIFO_OVF:
                    ESP_LOGW(TAG, "UART FIFO overflow");
                    uart_flush_input(UART_NUM_1);
                    xQueueReset(uart_queue);
                    break;
                case UART_PATTERN_DET:
                    ESP_LOGI(TAG, "UART pattern detected");
                    break;
                default:
                    ESP_LOGI(TAG, "Unhandled UART event type: %d", uart_event.type);
                    break;
            }
        }
    }
}

void app_main(void) {
    uart_config_t uart_config = {
        .baud_rate = 9600,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE
    };

    uart_param_config(UART_NUM_1, &uart_config);
    uart_set_pin(UART_NUM_1, TXD_PIN, RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    uart_driver_install(UART_NUM_1, RX_BUF_SIZE, TX_BUF_SIZE, 20, &uart_queue, 0);

    // Pattern detection (optional)
    uart_enable_pattern_det_baud_intr(UART_NUM_1, '+', PATTERN_LEN, 10000, 10, 10);
    uart_pattern_queue_reset(UART_NUM_1, 20);

    // Start UART event task
    xTaskCreate(uart_event_task, "uart_event_task", 2048, NULL, 10, NULL);

    // Example: Periodically send a STATUS message
    while (true) {
        char status_message[] = "Hello from ESP32";
        send_message(CMD_STATUS, (uint8_t *)status_message, strlen(status_message));
        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}