#include <stdio.h>
#include "pico/stdlib.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

// Define the queue handle globally
QueueHandle_t dataQueue;

// Task 1: Produces data and sends it to the queue
void producerTask(void *params) {
    int counter = 0;

    while (1) {
        // Send the counter value to the queue
        if (xQueueSend(dataQueue, &counter, 0) == pdPASS) {
            printf("Producer: Sent %d to queue\n", counter);
        } else {
            printf("Producer: Failed to send to queue\n");
        }
        counter++;

        // Delay to simulate periodic data production
        vTaskDelay(2000 / portTICK_PERIOD_MS); // 1 second delay
    }
}

// Task 2: Consumes data from the queue and processes it
void consumerTask(void *params) {
    int receivedValue;

    while (1) {
        // Receive data from the queue
        if (xQueueReceive(dataQueue, &receivedValue, portMAX_DELAY) == pdPASS) {
            printf("Consumer: Received %d from queue\n", receivedValue);
        }
    }
}

void usbInputTask(void *params) {
    char inputBuffer[100];
    int bufferIndex = 0;

    while (1) {
        // Check if a character is available from USB
        if (stdio_usb_connected()) { // Make sure USB is connected
            int ch = getchar_timeout_us(0); // Non-blocking read
            if (ch != PICO_ERROR_TIMEOUT) { // Data available
                if (ch == '\n' || ch == '\r') {
                    if (ch == '\r') {
                        putchar('\n');
                    }
                    inputBuffer[bufferIndex] = '\0'; // Null-terminate the string

                    // Parse the buffer as needed
                    int value;
                    if (sscanf(inputBuffer, "%d", &value) == 1) {
                        printf("Received integer: %d\n", value);
                    }

                    bufferIndex = 0; // Reset buffer index for next input
                } else if (bufferIndex < sizeof(inputBuffer) - 1) {
                    inputBuffer[bufferIndex++] = ch;
                    putchar(ch);
                }
            }
        }
        vTaskDelay(10 / portTICK_PERIOD_MS); // Small delay to prevent task hogging
    }
}

int main() {
    stdio_init_all(); // Initialize stdio over USB

    // Create a queue to hold integers, with a capacity of 5 items
    dataQueue = xQueueCreate(5, sizeof(int));
    if (dataQueue == NULL) {
        printf("Failed to create queue\n");
        while (1); // Loop indefinitely if queue creation fails
    }

    xTaskCreate(producerTask, "Producer Task", 1024, NULL, 1, NULL);
    xTaskCreate(consumerTask, "Consumer Task", 1024, NULL, 1, NULL);
    xTaskCreate(usbInputTask, "USB Input", 1024, NULL, 1, NULL);

    vTaskStartScheduler(); // Start FreeRTOS scheduler

    for (;;);
}