#include <Arduino_FreeRTOS.h>
#include <math.h> // For log() and pow()

// Define the sensor pin
#define NTC_PIN A0

// Task handles
TaskHandle_t TaskReadSensor;
TaskHandle_t TaskDisplayData;

// Global variables
volatile float temperature = 0.0;
volatile bool sensorError = false;
volatile bool temperatureAlert = false;
volatile bool systemStateOK = true;

// Temperature alert thresholds
const float ALERT_THRESHOLD_HIGH = 30.0;
const float ALERT_THRESHOLD_LOW = 10.0;

// Noise filtering parameters
float previousTemperature = 0.0; // Store last valid reading
const float NOISE_THRESHOLD = 5.0; // Max allowed jump

// Moving Average Filter parameters
#define WINDOW_SIZE 5
float tempBuffer[WINDOW_SIZE] = {0}; 
int bufferIndex = 0;

// Saturation limits
const float TEMP_MIN = -40.0;  // Example: NTC thermistor range
const float TEMP_MAX = 100.0;

void setup() {
    Serial.begin(115200);
    while (!Serial);  // Wait for Serial Monitor

    // Create tasks for FreeRTOS
    xTaskCreate(readSensorTask, "ReadSensor", 128, NULL, 1, &TaskReadSensor);
    xTaskCreate(displayDataTask, "DisplayData", 128, NULL, 1, &TaskDisplayData);
}

void loop() {
    // FreeRTOS handles task scheduling, so loop() remains empty
}

// Function: Salt & Pepper Noise Filter
float applySaltPepperFilter(float newTemp) {
    if (abs(newTemp - previousTemperature) > NOISE_THRESHOLD) {
        return previousTemperature;  // Ignore sudden spikes
    }
    previousTemperature = newTemp;  // Accept valid data
    return newTemp;
}

// Function: Weighted Moving Average Filter
float applyWeightedMovingAverage(float newTemp) {
    tempBuffer[bufferIndex] = newTemp; 
    bufferIndex = (bufferIndex + 1) % WINDOW_SIZE;

    float sum = 0, weightSum = 0;
    for (int i = 0; i < WINDOW_SIZE; i++) {
        float weight = (i + 1);  // Higher weight for recent values
        sum += tempBuffer[i] * weight;
        weightSum += weight;
    }
    return sum / weightSum;
}

// Function: Apply Saturation
float applySaturation(float temp) {
    if (temp < TEMP_MIN) return TEMP_MIN;
    if (temp > TEMP_MAX) return TEMP_MAX;
    return temp;
}

// Task: Read Sensor Data
void readSensorTask(void *pvParameters) {
    const TickType_t xDelay = pdMS_TO_TICKS(1000);  
    TickType_t xLastWakeTime = xTaskGetTickCount();

    while (1) {
        int rawValue = analogRead(NTC_PIN);

        // 🔍 Debugging: Print the raw ADC value
        // Serial.print("Raw ADC Value: ");
        // Serial.println(rawValue);

        if (rawValue < 0 || rawValue > 1023) {
            sensorError = true;
            systemStateOK = false;
        } else {
            sensorError = false;
            systemStateOK = true;

            float voltage = rawValue * (5.0 / 1023.0);
            
            // 🔍 Debugging: Print the calculated voltage
            // Serial.print("Voltage: ");
            // Serial.println(voltage, 3);

            float resistance = (10000.0 * voltage) / (5.0 - voltage);
            
            // 🔍 Debugging: Print the calculated resistance
            // Serial.print("Resistance: ");
            // Serial.println(resistance);

            if (resistance > 0) { // Avoid division by zero
                temperature = 1 / (0.001129148 + (0.000234125 * log(resistance)) + (0.0000000876741 * pow(log(resistance), 3))) - 273.15;
            } else {
                temperature = -999; // Indicate an error
            }

            // 🔍 Debugging: Print the calculated temperature
            // Serial.print("Temperature: ");
            // Serial.println(temperature, 2);

            if (temperature < ALERT_THRESHOLD_LOW || temperature > ALERT_THRESHOLD_HIGH) {
                temperatureAlert = true;
            } else {
                temperatureAlert = false;
            }
        }

        vTaskDelayUntil(&xLastWakeTime, xDelay);
    }
}


// Task: Display Processed Data
void displayDataTask(void *pvParameters) {
    const TickType_t xDelay = pdMS_TO_TICKS(500);

    while (1) {
        Serial.print("[Temperature]: ");
        Serial.print(temperature, 2);
        Serial.print(" °C  |  ");

        if (sensorError) {
            Serial.print("[Error]: Sensor failure  |  ");
        }
        if (temperatureAlert) {
            Serial.print("[ALERT]: Out of range!  |  ");
        }

        Serial.println(systemStateOK ? "[Status]: OK" : "[Status]: ERROR");

        vTaskDelay(xDelay);
    }
}