/* UART asynchronous example, that uses separate RX and TX tasks

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "driver/uart.h"
#include "string.h"
#include "driver/gpio.h"
#include "soc/uart_struct.h"

#include <stdio.h>
#include <stdint.h>
#include <string.h>

static const int RX_BUF_SIZE = 256;

#define TXD_PIN (GPIO_NUM_16)
#define RXD_PIN (GPIO_NUM_17)

typedef uint8_t   u8;  // 8-bit unsigned
typedef uint16_t  u16; // 16-bit unsigned

/* IrDA CRC-16 table (polynomial 0x1021, LSB-first) */
static const u16 fcstab[256] = {
    0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
    0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
    0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
    0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
    0x2102, 0x308B, 0x0210, 0x1299, 0x6726, 0x76AF, 0x4434, 0x55BD,
    0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
    0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
    0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
    0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
    0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
    0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
    0xDEcd, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
    0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
    0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
    0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
    0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
    0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
    0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
    0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
    0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
    0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
    0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
    0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
    0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
    0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
    0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
    0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
    0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
    0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
    0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
    0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
    0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
};

#define PPPINITFCS 0xFFFF
#define PPPGOODFCS 0xF0B8

u16 irda_crc(u16 fcs, unsigned char *cp, int len) {
    printf("irda_crc - FCS len: %d\n", len);

    printf("irda_crc - FCS_Payload: ");
    for (size_t j = 0; j < len; j++) {
        uint8_t byte = cp[j];
        printf("%02X ", byte);
    }
    printf("\n");

    while (len--) {
        fcs = (fcs >> 8) ^ fcstab[(fcs ^ *cp++) & 0xff];
    }
    ESP_LOGI("irda_crc1", "CRC:    0x%04X", fcs);

    return fcs;
}

int sendData(const char* logName, const char* data, int data_length)
{
    for (size_t j = 0; j < data_length; j++) {
        uint8_t byte = data[j];
        printf("%02X ", byte);
        }
    printf("\n");

    const int txBytes = uart_write_bytes(UART_NUM_1, data, data_length);
    ESP_LOGI(logName, "Wrote %d bytes", txBytes);
    return txBytes;
}


uint32_t read_address(const uint8_t* buffer) {
    return (uint32_t)(
        (buffer[0] << 24) | 
        (buffer[1] << 16) | 
        (buffer[2] << 8)  | 
        buffer[3]
    );
}

uint32_t parse_pxid_packets(const uint8_t *data, size_t len) {
    static const char *TASK_TAG = "PXID_TASK";
    esp_log_level_set(TASK_TAG, ESP_LOG_INFO);

    uint32_t destination_address = 0;
    uint32_t source_address = 0;

    ESP_LOGI(TASK_TAG, "Parsing P-XID Packet: %d bytes", len);
    // Print payload in hex
    for (size_t j = 0; j < len; j++) {
        uint8_t byte = data[j];
        printf("%02X ", byte);

        // Start new line every 16 bytes
        if ((j + 1) % 16 == 0) {
            printf("\n");
        }
    }
    printf("\n");

    size_t i = 0;
    int packet_count = 0;

    while (i < len) {
        // Find beginning of P-XID Payload
        if (data[i] != 0x01) {
            i++;
            continue;
        }

        // Extract address fields

        uint8_t source_address_arr[4];
        source_address_arr[0] = data[i+1];
        source_address_arr[1] = data[i+2];
        source_address_arr[2] = data[i+3];
        source_address_arr[3] = data[i+4];

        source_address = read_address(source_address_arr);

        ESP_LOGI(TASK_TAG, "Source Address: 0x%04lX", source_address);

        uint8_t destination_address_arr[4];
        destination_address_arr[0] = data[i+5];
        destination_address_arr[1] = data[i+6];
        destination_address_arr[2] = data[i+7];
        destination_address_arr[3] = data[i+8];
        destination_address = read_address(destination_address_arr);

        ESP_LOGI(TASK_TAG, "Destination Address: 0x%04lX", destination_address);
        
        uint8_t discovery_flag = data[i+9];
        uint8_t slot_num = data[i+10];
        uint8_t version_num = data[i+11];

        ESP_LOGI(TASK_TAG, "Discovery Flag: 0x%02X", discovery_flag);
        ESP_LOGI(TASK_TAG, "Slot Number: 0x%02X", slot_num);
        ESP_LOGI(TASK_TAG, "Version Number: 0x%02X", version_num);

        // Move up i
        i = i + 12;
        // Print payload in hex and ASCII
        for (size_t j = 0; j < (len - i); j++) {
            uint8_t byte = data[i + j];
            printf("%02X ", byte);

            // Start new line every 16 bytes
            if ((j + 1) % 16 == 0) {
                printf("\n");
            }
        }
        printf("\n");

        // Print ASCII representation
        for (size_t j = 0; j < (len - i); j++) {
            uint8_t byte = data[i + j];
            putchar((byte >= 0x20 && byte <= 0x7E) ? byte : '.');
        }
        printf("\n\n");

        break;
    }   
    return source_address; 
}

uint8_t reply_to_pxid(const uint32_t destination_address) {
    static const char *TASK_TAG = "PXID REPLY";
    esp_log_level_set(TASK_TAG, ESP_LOG_INFO);

    ESP_LOGI(TASK_TAG, "Construct reply packet");
    uint8_t reply_packet[36];

    //XBOF
    reply_packet[0] = 0xFF;
    reply_packet[1] = 0xFF;
    reply_packet[2] = 0xFF;
    reply_packet[3] = 0xFF;
    reply_packet[4] = 0xFF;
    reply_packet[5] = 0xFF;
    reply_packet[6] = 0xFF;
    reply_packet[7] = 0xFF;
    reply_packet[8] = 0xFF;
    reply_packet[9] = 0xFF;

    //BOF
    reply_packet[10] = 0xC0;
    //A
    reply_packet[11] = 0xfe;
    //C
    reply_packet[12] = 0xbf;
    //I
    reply_packet[13] = 0x01;
    reply_packet[14] = 0xfe;
    reply_packet[15] = 0x42;
    reply_packet[16] = 0x28;
    reply_packet[17] = 0x66;
    reply_packet[18] = 0x06;
    reply_packet[19] = (destination_address >> 24) & 0xFF;
    reply_packet[20] = (destination_address >> 16) & 0xFF;
    reply_packet[21] = (destination_address >> 8) & 0xFF;
    reply_packet[22] = (destination_address) & 0xFF;
    reply_packet[23] = 0x01;
    reply_packet[24] = 0x00;
    reply_packet[25] = 0x00;
    //IrDA Link Manager
    //Service Hint
    reply_packet[26] = 0x90;
    reply_packet[27] = 0x24;
    //Char Set
    reply_packet[28] = 0x00;
    //Device Nickname
    reply_packet[29] = 0x54;
    reply_packet[30] = 0x45;
    reply_packet[31] = 0x53;
    reply_packet[32] = 0x54;

    // CRC
    reply_packet[33] = 0x00;
    reply_packet[34] = 0x00;
    //EOF
    reply_packet[35] = 0xC1;

    //FCS = A + C + I (not FCS + EOF)
    uint8_t fcs_payload[22];
    memcpy(fcs_payload, &reply_packet[11], 22 * sizeof(uint8_t));

    printf("FCS_Payload: ");
    for (size_t j = 0; j < sizeof(fcs_payload); j++) {
        uint8_t byte = fcs_payload[j];
        printf("%02X ", byte);
    }
    printf("\n");

    // Calculate CRC over Address + Control + Payload + CRC bytes
    uint16_t calculated = irda_crc(PPPINITFCS, fcs_payload, 22);

    calculated = ~calculated;

    reply_packet[33] = (calculated) & 0xFF;
    reply_packet[34] = (calculated >> 8) & 0xFF;

    // Calculate CRC over Address + Control + Payload + CRC bytes

    uint16_t calculated_crc = irda_crc(PPPINITFCS, &reply_packet[11], 24 * sizeof(uint8_t));  // Total length including CRC

    ESP_LOGI(TASK_TAG, "CRC Check: %s\n", (calculated_crc == PPPGOODFCS) ? "VALID" : "INVALID");

    for (size_t j = 0; j < sizeof(reply_packet); j++) {
        uint8_t byte = reply_packet[j];
        printf("%02X ", byte);
    }

    printf("\n");

    ESP_LOGI(TASK_TAG, "Send reply packet");

    char char_array[sizeof(reply_packet)];
    memcpy(char_array, reply_packet, sizeof(char_array));

    sendData(TASK_TAG, char_array, sizeof(char_array));

    ESP_LOGI(TASK_TAG, "Sent reply packet");
    return 1;
}

void parse_irda_packets(const uint8_t *data, size_t len) {
    static const char *RX_TASK_TAG = "RX_TASK";
    esp_log_level_set(RX_TASK_TAG, ESP_LOG_INFO);

    size_t i = 0;
    int packet_count = 0;
    ESP_LOGI(RX_TASK_TAG, "Starting to Parse: %d", len);

    while (i < len) {
        // Find beginning of frame (BOF)
        if (data[i] != 0xC0) {
            i++;
            continue;
        }

        // Basic packet validation
        if (i + 5 > len) {  // Minimum valid packet size: C0 FF 3F [CRC] C1
            ESP_LOGI(RX_TASK_TAG, "Truncated packet at offset %zu\n", i);
            break;
        }

        // Extract header fields
        uint8_t address = data[i+1];
        uint8_t control = data[i+2];

        // Find end of frame (EOF)
        size_t eof_pos = i;
        while (eof_pos < len && data[eof_pos] != 0xC1) eof_pos++;

        if (eof_pos >= len) {
            ESP_LOGI(RX_TASK_TAG, "Unterminated packet at offset %zu\n", i);
            break;
        }

        // Calculate packet length
        size_t packet_len = eof_pos - i + 1;

        // Verify packet structure
        if (address != 0xFF || control != 0x3F) {
            ESP_LOGI(RX_TASK_TAG, "Invalid header in packet %d at offset %zu\n", packet_count+1, i);
            i = eof_pos + 1;
            continue;
        }

        // Extract payload and CRC
        size_t payload_start = i + 3;
        size_t payload_len = packet_len - 6;  // Subtract header, CRC, and EOF
        uint16_t crc = (data[eof_pos-2] << 8) | data[eof_pos-1];

        ESP_LOGI(RX_TASK_TAG, "\n=== Packet %d (offset %zu, length %zu) ===\n", 
               ++packet_count, i, packet_len);

        ESP_LOGI(RX_TASK_TAG, "Address: 0x%02X", address);

        // Check if the command bit (7th bit) is set
        if (address & 0x80) {
            // Bit is 1 (command frame)
            ESP_LOGI(RX_TASK_TAG, "Command bit: 1");
        } else {
            // Bit is 0 (response frame)
            ESP_LOGI(RX_TASK_TAG, "Command bit: 0");
        }

        ESP_LOGI(RX_TASK_TAG, "Control: 0x%02X", control);

        if (control == 0x3F){
            ESP_LOGI(RX_TASK_TAG, "Control: P-XID");
        }
        else if (control == 0xBF){
            ESP_LOGI(RX_TASK_TAG, "Control: F-XID");
        }
        else if (control == 0x93){
            ESP_LOGI(RX_TASK_TAG, "Control: P-NRM");
        }
        else if (control == 0x73){
            ESP_LOGI(RX_TASK_TAG, "Control: F-UA");
        }
        else if (control == 0x11){
            ESP_LOGI(RX_TASK_TAG, "Control: P-RR");
        }
        else if (control == 0x54){
            ESP_LOGI(RX_TASK_TAG, "Control: P-DISC");
        }

        ESP_LOGI(RX_TASK_TAG, "CRC:    0x%04X", crc);

        // Calculate CRC over Address + Control + Payload + CRC bytes
        uint16_t calculated_crc = irda_crc(PPPINITFCS, 
                                  &data[i+1],  // Address byte
                                  (eof_pos - i - 1));  // Total length including CRC

        ESP_LOGI(RX_TASK_TAG, "CRC Check: %s\n", 
           (calculated_crc == PPPGOODFCS) ? "VALID" : "INVALID");

        ESP_LOGI(RX_TASK_TAG, "Payload (%zu bytes):", payload_len);

        uint8_t payload[payload_len + 1];

        // Print payload in hex and ASCII
        for (size_t j = 0; j < payload_len; j++) {
            uint8_t byte = data[payload_start + j];
            printf("%02X ", byte);
            payload[j] = data[payload_start + j];

            // Start new line every 16 bytes
            if ((j + 1) % 16 == 0) { 
                printf("\n");
            }
        }
        printf("\n");

        // Print ASCII representation
        ESP_LOGI(RX_TASK_TAG, "\nASCII: ");
        for (size_t j = 0; j < payload_len; j++) {
            uint8_t byte = data[payload_start + j];
            putchar((byte >= 0x20 && byte <= 0x7E) ? byte : '.');
        }
        printf("\n\n");


        //Now process the payload
        // This is a P-XID
        if (control == 0x3F){

            uint32_t destination_address = parse_pxid_packets(payload, payload_len);
            uint8_t result = reply_to_pxid(destination_address);

            if (result == 1){
                ESP_LOGI(RX_TASK_TAG, "Send Success");
            }
            else {
                ESP_LOGI(RX_TASK_TAG, "Send Failed");
            }
        }

        // Move to next potential packet
        i = eof_pos + 1;
    }
}

void init(void)
{
    const 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,
        .source_clk = UART_SCLK_DEFAULT,
    };
    // We won't use a buffer for sending data.
    uart_driver_install(UART_NUM_1, RX_BUF_SIZE * 2, 0, 0, NULL, 0);
    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_set_mode(UART_NUM_1, UART_MODE_IRDA);
    uart_set_line_inverse(UART_NUM_1, UART_SIGNAL_IRDA_RX_INV);
    //ESP_LOGI("INIT", "%d", outcome);
}  

static void tx_task(void *arg)
{
    static const char *TX_TASK_TAG = "TX_TASK";
    esp_log_level_set(TX_TASK_TAG, ESP_LOG_INFO);
    while (1) {
        sendData(TX_TASK_TAG, "Hello world", 10);
        vTaskDelay(10000 / portTICK_PERIOD_MS);
    }
}

static void rx_task(void *arg)
{
    static const char *RX_TASK_TAG = "RX_TASK";
    esp_log_level_set(RX_TASK_TAG, ESP_LOG_INFO);
    uint8_t* data = (uint8_t*) malloc(RX_BUF_SIZE + 1);
    while (1) {
        const int rxBytes = uart_read_bytes(UART_NUM_1, data, RX_BUF_SIZE, 1000 / portTICK_PERIOD_MS);
        if (rxBytes > 0) {
            data[rxBytes] = 0;
            ESP_LOGI(RX_TASK_TAG, "Read %d bytes: '%s'", rxBytes, data);
            ESP_LOG_BUFFER_HEXDUMP(RX_TASK_TAG, data, rxBytes, ESP_LOG_INFO);

            parse_irda_packets(data, rxBytes);            

        }
    }
    free(data);
}

void app_main(void)
{
    init();

    xTaskCreate(rx_task, "uart_rx_task", 1024 * 2, NULL, configMAX_PRIORITIES - 1, NULL);
//    xTaskCreate(tx_task, "uart_tx_task", 1024 * 2, NULL, configMAX_PRIORITIES - 2, NULL);
}