#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/twai.h"
#include <vars.h>


#define QUEUE_SIZE 1000

// Define a structure for the message
typedef struct {
    uint32_t id;           // Message ID
    uint8_t data[8];   // Data payload
} Message;

// Create a queue handle for TWAI messages
QueueHandle_t rx_queue;
QueueHandle_t tx_queue;

void twai_tx_task(void *arg) {
    Message message;

    while (1) {
        // Wait for a message from the queue
        if (xQueueReceive(tx_queue, &message, portMAX_DELAY) == pdTRUE) {
            twai_message_t twai_msg;
            twai_msg.identifier = message.id;
            twai_msg.data_length_code = 8;

            // Copy data into TWAI message format
            memcpy(twai_msg.data, message.data, 8);
            UBaseType_t message_count = uxQueueMessagesWaiting(tx_queue);
            Serial.printf("Number of messages in TX queue: %d\n", message_count);
            // Send the TWAI message
            if (twai_transmit(&twai_msg, pdMS_TO_TICKS(1)) == ESP_OK) {
                Serial.printf("Sent message with ID: %d\n", twai_msg.identifier);
            } else {
                Serial.println("Failed to send message.");
            }
        }

        vTaskDelay(pdMS_TO_TICKS(50)); // Delay for 50 ms before checking the queue again
    }
    vTaskDelay(1);
}

// Task function to receive TWAI messages
void twai_rx_task(void *arg) {
    twai_message_t twai_message;
    while (1) {
        // Receive message from TWAI
        if (twai_receive(&twai_message, pdMS_TO_TICKS(1)) == ESP_OK) {
            // Prepare the custom message format
            Message msg;
            msg.id = twai_message.identifier; // Use TWAI message ID
            for (int i = 0; i < 8; i++)
            {
                msg.data[i] = twai_message.data[i];
            }
            //snprintf(msg.data, sizeof(msg.data), "Data: %s", twai_message.data); // Convert data to string
            UBaseType_t message_count = uxQueueMessagesWaiting(rx_queue);
            Serial.printf("Number of messages in rX queue: %d\n", message_count);
            Serial.printf("Task Recieve processing message ID: %d with data: %s\n", msg.id, msg.data);
            // Send received message to the processing queue
            switch (msg.id) {
            case REAR_PAM_ID: xQueueSend(rx_queue, &msg, portMAX_DELAY);
                break;
            case DIM: xQueueSend(rx_queue, &msg, portMAX_DELAY);
                break;
            default: ;
            }

        }
    }
    vTaskDelay(1);
}

// Task function to process messages in Task 1
void PAM_QUEUE(void *pvParameters) {
    Message msg;
    while (1) {
        // Wait indefinitely for a message
        if (xQueueReceive(rx_queue, &msg, portMAX_DELAY) == pdTRUE) {
            if (msg.id == REAR_PAM_ID)
            {
                // Process the message (task 1 specific logic)
                if ((msg.data[4] == 0x39) or (msg.data[4] == 0xB9)) {
                    msg.data[4] = msg.data[0];
                    Serial.println("PAM_SEND");
                    msg.id=Front_PAM_XC70;
                    xQueueSend(tx_queue, &msg, portMAX_DELAY);
                    SEC = 0;
                    MIN = 0;
                }
            }
        }
    }
    vTaskDelay(1);
}

// Task function to process messages in Task 2
void DIM_queue(void *pvParameters) {
    Message msg;
    bool zeroActionPerformed = false;
    int lastSpDIM = 0;
    while (1) {
        // Wait indefinitely for a message
        if (xQueueReceive(rx_queue, &msg, portMAX_DELAY) == pdTRUE) {
            if (msg.id == DIM)
            {
                // Process the message (task 1 specific logic)
                spdim = bytes_to_u16(msg.data[4], msg.data[5]);
                if ((msg.data[0] == 0xCC) and (spdim != 0) and spdim != lastSpDIM) {
                    lastSpDIM = spdim;
                    spdim = spdim / 96;
                    msg.data[2] = (spdim >> 8) & 0xFF;
                    msg.data[3] = spdim & 0xFF;
                    Serial.println("SPEED_SEND");
                    msg.id=IHU;
                    xQueueSend(tx_queue, &msg, portMAX_DELAY);
                    SEC = 0;
                    MIN = 0;
                    zeroActionPerformed = false;
                }else
                {
                    if (!zeroActionPerformed and spdim == 0) {
                        // Perform the action once for the first zero encountered
                        msg.data[2] = 0x00;
                        msg.data[3] = 0x00;
                        Serial.println("SPEED_SEND_STOP");
                        msg.id=IHU;
                        xQueueSend(tx_queue, &msg, portMAX_DELAY);
                        zeroActionPerformed = true; // Set flag to indicate that action has been performed
                    }
                }
            }
        }
    }
    vTaskDelay(1);
}

void setup() {
    Serial.begin(115200);

    // Configure TWAI
    twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(GPIO_NUM_27, GPIO_NUM_25, TWAI_MODE_NORMAL);
    twai_timing_config_t t_config = TWAI_TIMING_CONFIG_125KBITS();
    twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

    // Install TWAI driver
    twai_driver_install(&g_config, &t_config, &f_config);

    // Start TWAI
    twai_start();

    // Create queues for received and processed messages
    rx_queue = xQueueCreate(QUEUE_SIZE, sizeof(twai_message_t));
    tx_queue = xQueueCreate(QUEUE_SIZE, sizeof(twai_message_t));

    // Create tasks for receiving CAN messages and processing them
    // Чем больше число приоритета, тем больше процессорного времени получает
    xTaskCreate(twai_rx_task, "twai_rx_task", 2048, NULL, 8, NULL);
    xTaskCreate(PAM_QUEUE, "PAM_QUEUE", 2048, NULL, 7, NULL);
    xTaskCreate(DIM_queue, "Dim_queue", 2048, NULL, 6, NULL);
    // Create a task for sending CAN messages
    xTaskCreate(twai_tx_task, "twai_tx_task", 2048, NULL, 9, NULL);

}

void loop() {
   // The loop function can remain empty if all processing is done in tasks.

}