#include <Arduino_FreeRTOS.h>
// #include <atomic.h>
// #include <event_groups.h>
// #include <FreeRTOSConfig.h>
// #include <FreeRTOSVariant.h>
// #include <list.h>
// #include <message_buffer.h>
// #include <mpu_wrappers.h>
// #include <portable.h>
// #include <portmacro.h>
// #include <projdefs.h>
// #include <queue.h>
// #include <semphr.h>
// #include <stack_macros.h>
// #include <stream_buffer.h>
// #include <task.h>
// #include <timers.h>

#include "Adafruit_MLX90393.h"

// Declarations
#define PRINT true

// Global variables
// const int bufferSize = 64;
// static char inputBuffer[bufferSize];

// Sensor objects
Adafruit_MLX90393 magnetometer = Adafruit_MLX90393();

// Task stubs
void vTask_serialHandler(void *pvParameters);

void setup() {
    //Initialize the serial monitor
    Serial.begin(115200);
    while(!Serial){
        delay(10);
    }
    if(PRINT){
        Serial.println("Serial monitor initialized.");
    }

    // Initialize the MLX90393
    // if(!magnetometer.begin_I2C()){
    //     if(PRINT){
    //         Serial.println("MLX90393 not found.");
    //         for(;;);
    //     }
    // }
    // else if(PRINT){
    //     Serial.println("MLX90393 found.");
    // }

    // // Set parameters for MLX90393
    // magnetometer.setGain(MLX90393_GAIN_2_5X);
    // magnetometer.setResolution(MLX90393_X, MLX90393_RES_19);
    // magnetometer.setResolution(MLX90393_Y, MLX90393_RES_19);
    // magnetometer.setResolution(MLX90393_Z, MLX90393_RES_16);
    // magnetometer.setOversampling(MLX90393_OSR_2);
    // magnetometer.setFilter(MLX90393_FILTER_6);

    if(PRINT){
        Serial.println("MLX90393 parameters set.");
        Serial.println("Initializing tasks...");
    }
    
    // delay(10);
    xTaskCreate(vTask_serialHandler, "vTask_serialHandler", 1024, NULL, configMAX_PRIORITIES, NULL);
    vTaskStartScheduler();
}

void loop() {
    // Nothing here, keep reading
}

void vTask_serialHandler(void *pvParameters) {
    (void) pvParameters;

    int i = 4;
    for (;;) {
    if (PRINT) {
        Serial.println("Serial handler initialized.");
    }
    vTaskDelay(10 / portTICK_PERIOD_MS);
        // if (Serial.available() > 0) {
        //     int bytesRead = Serial.readBytesUntil('\n', inputBuffer, bufferSize-1);

        //     inputBuffer[bytesRead] = '\0';

        //     if (strncmp(inputBuffer, "RM", 2)) {
        //         if (PRINT) {
        //             Serial.println("Valid input received.");
        //         }
        //         // Perform actions based on the received data
        //     } else {
        //         if (PRINT) {
        //             Serial.println("Invalid input received.");
        //         }
        //         // Handle invalid input
        //     }
        // }
    }
}