//Project: Midterm_Harrell.ino
//Author: Sarah Harrell
//Date: 02/28/25
//Description:  implements a FreeRTOS-based sensor simulation, with producer tasks (button, 
// potentiometer, and ultrasonic sensor) that send data to a queue, and consumer tasks 
// (LED, buzzer, and serial monitor) process the data while ensuring it meets the semaphore.

// Libraries
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

// Pin Defines
#define LED_PIN        2
#define BUTTON_PIN     0
#define POT_PIN        5
#define BUZZER_PIN     4
#define TRIG_PIN       35
#define ECHO_PIN       36

// Task Handles
TaskHandle_t Task_Button_Handle = NULL;
TaskHandle_t Task_Pot_Handle = NULL;
TaskHandle_t Task_Ultrasonic_Handle = NULL;
TaskHandle_t Task_LED_Handle = NULL;
TaskHandle_t Task_Buzzer_Handle = NULL;
TaskHandle_t Task_Serial_Handle = NULL;

// Queue & Semaphore
QueueHandle_t sensorQueue;
SemaphoreHandle_t queueSemaphore;

// Sensor Data Structure
typedef struct {
    int value;
    String source;
    unsigned long timestamp;
} SensorData;

void setup() {
    Serial.begin(115200);
  
    // Pin Modes Setuo
    pinMode(LED_PIN, OUTPUT);
    pinMode(BUTTON_PIN, INPUT_PULLUP);
    pinMode(POT_PIN, INPUT);
    pinMode(BUZZER_PIN, OUTPUT);
    pinMode(TRIG_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);

    // Queue for up to 10 sensor readings
    sensorQueue = xQueueCreate(10, sizeof(SensorData));
    if (sensorQueue == NULL) {
        Serial.println("Error creating the queue");
        while (true);  // Stop if queue failed
    }

    // Semaphore for queue access
    queueSemaphore = xSemaphoreCreateBinary();
    if (queueSemaphore == NULL) {
        Serial.println("Error creating the semaphore");
        while (true);  // Stop if semaphore failed
    }
    xSemaphoreGive(queueSemaphore);

    // Create FreeRTOS Tasks
    xTaskCreate(Task_ButtonSensor, "ButtonSensor", 2048, NULL, 1, &Task_Button_Handle);
    xTaskCreate(Task_Potentiometer, "Potentiometer", 2048, NULL, 1, &Task_Pot_Handle);
    xTaskCreate(Task_Ultrasonic, "Ultrasonic", 2048, NULL, 1, &Task_Ultrasonic_Handle);
    xTaskCreate(Task_LED, "LEDTask", 2048, NULL, 1, &Task_LED_Handle);
    xTaskCreate(Task_Buzzer, "BuzzerTask", 2048, NULL, 1, &Task_Buzzer_Handle);
    xTaskCreate(Task_SerialMonitor, "SerialMonitor", 2048, NULL, 1, &Task_Serial_Handle);
}

void loop() {
    // FreeRTOS handles everything, no need for loop()
}

// ===================== PRODUCER TASKS =====================

// Task: Button Sensor
void Task_ButtonSensor(void *pvParameters) {
    while (true) {
        if (digitalRead(BUTTON_PIN) == LOW) {  // Button Press Detected
            SensorData data = {1, "Button", millis()}; // Create sensor data
            
            if (xSemaphoreTake(queueSemaphore, portMAX_DELAY)) {
                if (xQueueSend(sensorQueue, &data, portMAX_DELAY) != pdTRUE) { // Send data to queue
                    Serial.println("Queue Full! Data Lost.");
                } 
                xSemaphoreGive(queueSemaphore); // Give the semaphore
            }
            
            vTaskDelay(pdMS_TO_TICKS(40)); // Debounce delay
        }
        vTaskDelay(pdMS_TO_TICKS(40)); //Delay for button 
    }
}

// Task: Potentiometer Sensor
void Task_Potentiometer(void *pvParameters) {
    int potValue;
    int mappedValue;
    SensorData data;
    while (true) {
        potValue = analogRead(POT_PIN); // Read potentiometer value
        mappedValue = map(potValue, 0, 4095, 1, 100); // Map the value to a range
        data = (SensorData){mappedValue, "Potentiometer", millis()}; // Create sensor data

        if (xSemaphoreTake(queueSemaphore, portMAX_DELAY)) { // Take the semaphore
            if (xQueueSend(sensorQueue, &data, portMAX_DELAY) != pdTRUE) { // Send data to queue
                Serial.println("Queue Full! Data Lost.");
            }
            xSemaphoreGive(queueSemaphore); // Give the semaphore
        }
        
        vTaskDelay(pdMS_TO_TICKS(300)); // Delay for potentiometer 
    }
}

// Task: Ultrasonic Sensor
void Task_Ultrasonic(void *pvParameters) {
    long duration;
    long distance;
    SensorData data;
    while (true) {
        digitalWrite(TRIG_PIN, LOW); // Set trigger pin on and off
        delayMicroseconds(2);
        digitalWrite(TRIG_PIN, HIGH);
        delayMicroseconds(10);
        digitalWrite(TRIG_PIN, LOW);
        
        duration = pulseIn(ECHO_PIN, HIGH); // Read echo pin duration
        distance = duration * 0.0344 / 2;  // Convert to cm
        data = (SensorData){distance, "Ultrasonic", millis()}; // Create sensor data

        if (xSemaphoreTake(queueSemaphore, portMAX_DELAY)) {
            if (xQueueSend(sensorQueue, &data, portMAX_DELAY) != pdTRUE) {
                Serial.println("Queue Full! Data Lost.");
            }
            xSemaphoreGive(queueSemaphore);
        }

        vTaskDelay(pdMS_TO_TICKS(500)); // Delay for ultrasonic sensor 
    }
}

// ===================== CONSUMER TASKS =====================

// Task: LED Controller
void Task_LED(void *pvParameters) {
    SensorData receivedData; 
    while (true) {
        if (xQueueReceive(sensorQueue, &receivedData, portMAX_DELAY) == pdTRUE) { // Receive data from queue
            digitalWrite(LED_PIN, HIGH); //turn led on and off
            vTaskDelay(pdMS_TO_TICKS(200));
            digitalWrite(LED_PIN, LOW);
        }
    }
}

// Task: Buzzer Controller
void Task_Buzzer(void *pvParameters) {
    SensorData receivedData;
    while (true) {
        if (xQueueReceive(sensorQueue, &receivedData, portMAX_DELAY) == pdTRUE) { // Receive data from queue
            if (receivedData.source == "Ultrasonic" && receivedData.value < 20) { // If ultrasonic sensor detects object within 20 cm
                Serial.println("Buzzer ON"); //turn buzzer on and off
                tone(BUZZER_PIN, 1000);
                vTaskDelay(pdMS_TO_TICKS(500));
                noTone(BUZZER_PIN);
                Serial.println("Buzzer OFF");
            }
        }
    }
}

// Task: Serial Monitor
void Task_SerialMonitor(void *pvParameters) {
    SensorData receivedData;
    while (true) {
        if (xQueueReceive(sensorQueue, &receivedData, portMAX_DELAY) == pdTRUE) {
            Serial.print("Source: ");
            Serial.print(receivedData.source); // Print to serial monitor info on sensor data
            Serial.print(" | Value: ");
            Serial.print(receivedData.value);
            Serial.print(" | Timestamp: ");
            Serial.println(receivedData.timestamp);
        }
    }
}