#include <bsec2.h>
#include "commMux.h"
#include <SD.h>
#include <SPI.h>
#include <Wire.h>
#include "esp_task_wdt.h"
#include <bme68xLibrary.h>

#define SD_CS 33  
#define SPI_SS 5  
#define NUM_OF_SENS 8  
#define PANIC_LED LED_BUILTIN
#define ERROR_DUR 1000

void errLeds(void);
void checkBsecStatus(Bsec2 bsec);
void newDataCallback(const bme68x_data data, const bsecOutputs outputs, Bsec2 bsec);

Bsec2 envSensor[NUM_OF_SENS];  
comm_mux communicationSetup[NUM_OF_SENS];
uint8_t bsecMemBlock[NUM_OF_SENS][BSEC_INSTANCE_SIZE];

void logFreeHeap();
void retrySDInitialization();
void stopProgram(const char* errorMessage);
void clearSerialMonitor();
bool checkConfigFilePresence();

bool sdInitialized = false;  

void errLeds(void) {
    pinMode(PANIC_LED, OUTPUT); 
    for (int i = 0; i < 10; i++) {  
        digitalWrite(PANIC_LED, HIGH);
        delay(ERROR_DUR);
        digitalWrite(PANIC_LED, LOW);
        delay(ERROR_DUR);
    }
    ESP.restart();  
}

void setup() {
    Serial.begin(115200);
    clearSerialMonitor();

    Serial.println("Waiting for 'run' to start...");
    while (!Serial.available()) {
        delay(100);  
    }
    String input = Serial.readString();
    input.trim();  
    if (input != "run") {
        stopProgram("Invalid input. Expected 'run'.");
    }
    Serial.println("Starting program...");

    retrySDInitialization();
    if (!sdInitialized) {
        stopProgram("SD card initialization failed.");
    }

    if (!checkConfigFilePresence()) {
        stopProgram("BSEC configuration file not found. Please upload it to the SD card.");
    }

    SPI.begin();

    comm_mux_begin(Wire, SPI);

    for (uint8_t i = 0; i < NUM_OF_SENS; i++) {
        Serial.println("Initializing sensor " + String(i) + "...");

        communicationSetup[i] = comm_mux_set_config(Wire, SPI, i, communicationSetup[i]);

        envSensor[i].allocateMemory(bsecMemBlock[i]);

        if (!envSensor[i].begin(BME68X_SPI_INTF, comm_mux_read, comm_mux_write, comm_mux_delay, &communicationSetup[i])) {
            checkBsecStatus(envSensor[i]);
            stopProgram(("Failed to initialize sensor " + String(i)).c_str());
        }

        bsecSensor sensorList[] = {
            BSEC_OUTPUT_IAQ,
            BSEC_OUTPUT_RAW_TEMPERATURE,
            BSEC_OUTPUT_RAW_PRESSURE,
            BSEC_OUTPUT_RAW_HUMIDITY,
            BSEC_OUTPUT_RAW_GAS,
            BSEC_OUTPUT_STABILIZATION_STATUS,
            BSEC_OUTPUT_RUN_IN_STATUS
        };

        if (!envSensor[i].updateSubscription(sensorList, ARRAY_LEN(sensorList), BSEC_SAMPLE_RATE_LP)) {
            checkBsecStatus(envSensor[i]);
            stopProgram(("Subscription failed for sensor " + String(i)).c_str());
        }

        envSensor[i].attachCallback(newDataCallback);

        Serial.println("Sensor " + String(i) + " initialized successfully.");
    }

    Serial.println("BSEC library version: " +
        String(envSensor[0].version.major) + "." +
        String(envSensor[0].version.minor) + "." +
        String(envSensor[0].version.major_bugfix) + "." +
        String(envSensor[0].version.minor_bugfix));

    logFreeHeap();
}

void loop() {
    for (uint8_t i = 0; i < NUM_OF_SENS; i++) {
        if (!envSensor[i].run()) {
            checkBsecStatus(envSensor[i]);
        }
    }
    logFreeHeap();  
    yield();  
}

void newDataCallback(const bme68x_data data, const bsecOutputs outputs, Bsec2 bsec) {
    String filename = "dataSample1.bmerawdata";  
    File dataFile;

    if (!SD.exists(filename)) {
        dataFile = SD.open(filename, FILE_WRITE);
        if (dataFile) {
            dataFile.println("Timestamp,IAQ,Accuracy,Temperature,Pressure,Humidity,GasResistance");
            dataFile.close();
        } else {
            Serial.println("Failed to create new .bmerawdata file.");
            return;
        }
    }

    // Append new data
    dataFile = SD.open(filename, FILE_APPEND);
    if (!dataFile) {
        Serial.println("Failed to open file for appending!");
        return;
    }

    float iaq = NAN, accuracy = NAN, temperature = NAN, pressure = NAN, humidity = NAN, gasResistance = NAN;

    for (uint8_t i = 0; i < outputs.nOutputs; i++) {
        const bsecData output = outputs.output[i];
        switch (output.sensor_id) {
            case BSEC_OUTPUT_IAQ:
                iaq = output.signal;
                accuracy = output.accuracy;
                break;
            case BSEC_OUTPUT_RAW_TEMPERATURE:
                temperature = output.signal;
                break;
            case BSEC_OUTPUT_RAW_PRESSURE:
                pressure = output.signal;
                break;
            case BSEC_OUTPUT_RAW_HUMIDITY:
                humidity = output.signal;
                break;
            case BSEC_OUTPUT_RAW_GAS:
                gasResistance = output.signal;
                break;
        }
    }

    dataFile.print(millis());
    dataFile.print(",");
    dataFile.print(iaq);
    dataFile.print(",");
    dataFile.print(accuracy);
    dataFile.print(",");
    dataFile.print(temperature);
    dataFile.print(",");
    dataFile.print(pressure);
    dataFile.print(",");
    dataFile.print(humidity);
    dataFile.print(",");
    dataFile.println(gasResistance);

    dataFile.close();
    Serial.println("Data successfully written to .bmerawdata file.");
}

void checkBsecStatus(Bsec2 bsec) {
    if (bsec.status < BSEC_OK) {
        Serial.println("BSEC error code: " + String(bsec.status));
        errLeds();
    } else if (bsec.status > BSEC_OK) {
        Serial.println("BSEC warning code: " + String(bsec.status));
    }
}

void retrySDInitialization() {
    int retries = 3;
    while (retries > 0 && !SD.begin(SD_CS)) {
        Serial.println("SD initialization failed. Retrying...");
        delay(6000);
        retries--;
    }
    sdInitialized = (retries > 0);
}

bool checkConfigFilePresence() {
    return SD.exists("/newconfig.bmeconfig");
}

void stopProgram(const char* errorMessage) {
    Serial.println(errorMessage);
    errLeds();
    while (true) {
        delay(1000);  
    }
}

void clearSerialMonitor() {
    while (Serial.available() > 0) {
        Serial.read();  
    }
}

void logFreeHeap() {
    Serial.println("Free heap: " + String(ESP.getFreeHeap()));
}