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

#define BUFFER_SIZE 256

char NULLCHAR = 0x0;
QueueHandle_t MESSAGES = NULL;


void vReadSerial(void* params) {
	// Setup
	MESSAGES = xQueueCreate(4, BUFFER_SIZE); // Message queue
	unsigned int len = 0; // Buffer ptr
	int ret;
	char ret_char;
	char* buffer = pvPortMalloc(BUFFER_SIZE * sizeof(char));
	
	// start loop
	while(true) {
		ret = getchar_timeout_us(0);
		if(ret != PICO_ERROR_TIMEOUT) {
			// If returned char exists, or...
			ret_char = (char)ret;
			if (ret_char & 0xE0) {
				// If char is printable, echo it, copy to buffer, and increment ptr
				printf("%c", ret_char);
				memcpy(buffer+len*sizeof(char), &ret_char, sizeof(char));
				len++;
			} else if (len) {
				// Otw, and if buffer is not empty, check special chars
				switch(ret_char) {
					// Backspace
					// "Delete" char, ie decrement ptr
					case 8:
						len--;
						break;
					// CR, LF
					// Send message to queue with null char and reset ptr
					case 10:
					case 13:
						memcpy(buffer+len*sizeof(char), &NULLCHAR, sizeof(char));
						xQueueSendToBack(MESSAGES, buffer, 50);
						printf("\n");
						len = 0;
						break;
					// Otw do nothing
					default:
						break;
				};
			}
		}
	}
}

void vProcessMessages(void* params) {
	// Wait for queue to be intialized
	while(true) {
		if (MESSAGES != NULL) { break; }
	}
	char* command = pvPortMalloc(BUFFER_SIZE * sizeof(char));
	while(MESSAGES != NULL) {
		if (xQueueReceive(MESSAGES, command, 50)) {
			printf("Recieved message: ");
			printf("%s\n", command);
		}
	}

}

void main() {
	stdio_init_all();
	busy_wait_ms(1000);
	gpio_init(PICO_DEFAULT_LED_PIN);
	gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
	xTaskCreate(vReadSerial, "Read serial", 128, NULL, 1, NULL);
	xTaskCreate(vProcessMessages, "Process messages", 128, NULL, 1, NULL);
	vTaskStartScheduler();
}