#include <Arduino.

h>
#include <esp_task_wdt.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>

// --- System Configuration Constants ---

#define SAMPLING_FREQUENCY_HZ 200 // Target sampling rate
#define WDT_TIMEOUT_S 10 // Task Watchdog Timer timeout
#define BUFFER_SIZE 1024 // Number of samples per buffer
#define QUEUE_SIZE 2 // Double buffering queue size

// --- GPIO Pin Definitions (Hardware Contract) ---

// I2C Bus (MPU-6050)
#define MPU_SCL_PIN 39
#define MPU_SDA_PIN 40

// Digital I/O (HX711)

#define HX711_DT_PIN 5
#define HX711_SCK_PIN 4

// 1-Wire Protocol (DS18B20)

#define DS18B20_DQ_PIN 17

// SPI Bus (LoRa Ra-02)

#define LORA_NSS_PIN 10
#define LORA_MOSI_PIN 11
#define LORA_SCK_PIN 12
#define LORA_MISO_PIN 13

// --- System State Machine ---

enum SystemState {
INITIALIZING,
SAMPLING,
PROCESSING,
TRANSMITTING,
LOW_POWER_IDLE
};

volatile SystemState systemState = INITIALIZING;

// --- Sensor Data Buffer Structure ---

struct SensorDataBuffer {
uint32_t timestamp; // Buffer timestamp
uint16_t sampleCount; // Number of valid samples
struct {
float accel_x, accel_y, accel_z; // MPU-6050 accelerometer data
float gyro_x, gyro_y, gyro_z; // MPU-6050 gyroscope data
int32_t strain; // HX711 strain reading
float temperature; // DS18B20 temperature
} samples[BUFFER_SIZE];
};

// --- Global State and Synchronization ---

// Double Buffers for sensor data
SensorDataBuffer bufferA;
SensorDataBuffer bufferB;
SensorDataBuffer* activeBuffer = &bufferA; // Currently being filled
SensorDataBuffer* processingBuffer = nullptr; // Currently being processed
// FreeRTOS Handles
TaskHandle_t samplingTaskHandle = nullptr;
TaskHandle_t processingTaskHandle = nullptr;
TaskHandle_t loraTaskHandle = nullptr;
TaskHandle_t mainControlTaskHandle = nullptr;

// Inter-core communication queue - passes buffer pointers from Core 0 to Core 1

QueueHandle_t dataBufferQueue = nullptr;

// Hardware Timer for deterministic sampling

hw_timer_t *samplingTimer = nullptr;
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;

// Buffer management variables

volatile uint16_t currentSampleIndex = 0;
volatile bool bufferReady = false;

// --- Minimalist Interrupt Service Routine (ISR) ---

// This ISR is kept minimal: its only job is to notify the sampling_task
void IRAM_ATTR onTimer() {
portENTER_CRITICAL_ISR(&timerMux);

// Signal the sampling task to acquire a new data point

// This is the ONLY operation performed in the ISR to minimize interrupt
latency
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(samplingTaskHandle, &xHigherPriorityTaskWoken);

// Yield to higher priority task if necessary

if (xHigherPriorityTaskWoken) {
portYIELD_FROM_ISR();
}

portEXIT_CRITICAL_ISR(&timerMux);
}

// --- FreeRTOS Task Implementations ---

// --- Placeholder Sensor Functions ---

// These represent the actual sensor interface functions that would be implemented
// with the appropriate libraries (Adafruit MPU6050, HX711, DallasTemperature)

struct MPU6050_Data {
float accel_x, accel_y, accel_z; // Accelerometer readings in g
float gyro_x, gyro_y, gyro_z; // Gyroscope readings in deg/s
};

// Placeholder function to read complete MPU-6050 sensor data in one I2C

transaction
MPU6050_Data read_mpu6050() {
// TODO: Replace with actual Adafruit MPU6050 library calls
// Example: mpu.getEvent(&a, &g, &temp);
MPU6050_Data data;
data.accel_x = 0.1f; // Placeholder accelerometer X (g)
data.accel_y = 0.2f; // Placeholder accelerometer Y (g)
data.accel_z = 1.0f; // Placeholder accelerometer Z (g)
data.gyro_x = 0.5f; // Placeholder gyroscope X (deg/s)
data.gyro_y = -0.3f; // Placeholder gyroscope Y (deg/s)
 data.gyro_z = 0.1f; // Placeholder gyroscope Z (deg/s)
return data;
}

// Placeholder function to check if HX711 has new data ready (non-blocking)

bool hx711_is_ready() {
// TODO: Replace with actual HX711 library call
// Example: return scale.is_ready();
return true; // Placeholder - assume data is always ready
}

// Placeholder function to read strain gauge data from HX711

int32_t read_hx711_strain() {
// TODO: Replace with actual HX711 library call
// Example: return scale.read();
return 12345; // Placeholder raw ADC value
}

// Placeholder function to read temperature from DS18B20 (blocking operation)

float read_ds18b20_temperature() {
// TODO: Replace with actual Dallas Temperature library calls
// Example: sensors.requestTemperatures(); return sensors.getTempCByIndex(0);
return 23.5f; // Placeholder temperature in Celsius
}

/**
* Core 0 (PRO_CPU) - Real-Time Sampling Task
* Highest priority task dedicated to deterministic data acquisition
* Pinned to Core 0 for hardware-enforced isolation from application logic
*/
void sampling_task(void *pvParameters) {
Serial.println("[Core 0] Sampling task started - deterministic data
acquisition");

// Local buffer management variables (Core 0 exclusive)

SensorDataBuffer* activeBuffer = &bufferA; // Start with buffer A
uint16_t sampleIndex = 0; // Current position in active
buffer

// Temperature reading management (slower update rate)

static uint32_t temperatureCounter = 0;
static float lastTemperature = 25.0f; // Cache last temperature reading

// Initialize the active buffer

activeBuffer->sampleCount = 0;
activeBuffer->timestamp = millis();

Serial.printf("[Core 0] Starting deterministic sampling at %d Hz\n",

SAMPLING_FREQUENCY_HZ);
Serial.printf("[Core 0] Buffer size: %d samples, Double-buffering active\n",
BUFFER_SIZE);

// Task main loop - runs on Core 0 (Real-Time Core)

for (;;) {
// CRITICAL: Wait for notification from hardware timer ISR
// This blocks the task efficiently until woken by the precise hardware
timer
// The pdTRUE parameter clears the notification count, ensuring we process
exactly one sample per notification
 ulTaskNotifyTake(pdTRUE, portMAX_DELAY);

// --- TIME-CRITICAL DATA ACQUISITION SEQUENCE ---

// From this point forward, execution time must be minimized to maintain
deterministic timing
// The hardware timer ISR has fired, indicating it's time to acquire a new
sample

// 1. IMMEDIATE: Read MPU-6050 IMU data (highest priority - vibration

analysis)
// This is the most time-sensitive reading as vibration analysis requires
precise timing
MPU6050_Data imu_data = read_mpu6050();

// Store IMU data in the active buffer at current sample index

activeBuffer->samples[sampleIndex].accel_x = imu_data.accel_x;
activeBuffer->samples[sampleIndex].accel_y = imu_data.accel_y;
activeBuffer->samples[sampleIndex].accel_z = imu_data.accel_z;
activeBuffer->samples[sampleIndex].gyro_x = imu_data.gyro_x;
activeBuffer->samples[sampleIndex].gyro_y = imu_data.gyro_y;
activeBuffer->samples[sampleIndex].gyro_z = imu_data.gyro_z;

// 2. CONDITIONAL: Read HX711 strain gauge data (non-blocking approach)

// The HX711 ADC may not have new data ready at our sampling rate
// Using non-blocking check to avoid disrupting the deterministic timing
if (hx711_is_ready()) {
activeBuffer->samples[sampleIndex].strain = read_hx711_strain();
} else {
// HX711 not ready - use previous value or zero
// In a production system, this might interpolate or use the last valid
reading
activeBuffer->samples[sampleIndex].strain = 0; // Placeholder behavior
}

// 3. PERIODIC: Read DS18B20 temperature data (thermal compensation)

// Temperature changes slowly, so we only read it periodically (e.g., once
per second)
// This reduces I2C bus traffic and processing overhead while maintaining
accuracy
temperatureCounter++;
if (temperatureCounter >= SAMPLING_FREQUENCY_HZ) {
// Time to read temperature (approximately once per second)
lastTemperature = read_ds18b20_temperature();
temperatureCounter = 0; // Reset counter
}

// Store the cached temperature value (thermal compensation requirement)

activeBuffer->samples[sampleIndex].temperature = lastTemperature;

// 4. METADATA: Update sample metadata with current timestamp

// This could be enhanced to store per-sample timestamps for precise timing
analysis
if (sampleIndex == 0) {
// Update buffer timestamp when starting a new buffer
activeBuffer->timestamp = millis();
}

// 5. ADVANCE: Move to next sample position in buffer

sampleIndex++;
 // 6. CRITICAL: Double-Buffer Management (Ping-Pong Buffer Logic)
// Check if the current buffer is now full
if (sampleIndex >= BUFFER_SIZE) {
// Buffer is full - time to switch to the alternate buffer

// Set the final sample count for the completed buffer

activeBuffer->sampleCount = sampleIndex;

// ATOMIC OPERATION: Send completed buffer pointer to Core 1

// Using non-blocking queue send to avoid any delay in the real-time
loop
// The buffer pointer (not the data) is passed to maintain zero-copy
semantics
if (xQueueSend(dataBufferQueue, &activeBuffer, 0) == pdPASS) {
// SUCCESS: Buffer successfully queued for Core 1 processing

// IMMEDIATE: Switch to alternate buffer for continuous sampling

// This is the "ping-pong" operation that prevents data loss
activeBuffer = (activeBuffer == &bufferA) ? &bufferB : &bufferA;

// Reset sample index for the new buffer

sampleIndex = 0;

// Initialize the new active buffer

activeBuffer->sampleCount = 0;
activeBuffer->timestamp = millis();

// Optional: Debug output (remove in production for timing

precision)
// Serial.printf("[Core 0] Buffer swap complete, now using %s\n",
// (activeBuffer == &bufferA) ? "Buffer A" : "Buffer
B");

} else {
// CRITICAL ERROR: Queue is full - Core 1 processing is falling
behind
// This indicates a serious system performance issue

// RECOVERY: Reset to beginning of current buffer (data loss

scenario)
// This maintains system stability but results in lost samples
sampleIndex = 0;
activeBuffer->sampleCount = 0;
activeBuffer->timestamp = millis();

// Log warning (this should be monitored in production)

// Note: Serial.println in real-time task should be avoided in
production
static uint32_t lastWarning = 0;
uint32_t now = millis();
if (now - lastWarning > 5000) { // Rate-limit warnings to every 5
seconds
Serial.println("[CRITICAL] Core 0: Processing queue full - data
loss occurring!");
lastWarning = now;
}
}
}
 // --- END OF CRITICAL SECTION ---
// Task now returns to blocked state, efficiently waiting for the next
timer interrupt
// The hardware timer guarantees precise, jitter-free intervals between
samples
// No additional delays or processing should occur here to maintain
deterministic timing
}

// This point should never be reached in normal operation

Serial.println("[ERROR] Core 0: Sampling task exited unexpectedly");
}

/**
* Core 1 (APP_CPU) - Signal Processing Task
* Handles computationally intensive DSP operations and feature extraction
* Pinned to Core 1 to prevent interference with real-time sampling
*/
void processing_task(void *pvParameters) {
Serial.println("[Core 1] Processing task started");

// Subscribe this task to the Task Watchdog Timer

esp_task_wdt_add(nullptr);

SensorDataBuffer* filledBuffer = nullptr;

// Task main loop - runs on Core 1 (Application Core)

for (;;) {
// Feed the Task Watchdog Timer to indicate this task is healthy
esp_task_wdt_reset();

// Wait for a full buffer pointer from the sampling_task on Core 0

// This blocks until Core 0 signals that a complete buffer is ready
if (xQueueReceive(dataBufferQueue, &filledBuffer, portMAX_DELAY) == pdPASS)
{
Serial.printf("[Core 1] Processing buffer with %d samples\n",
filledBuffer->sampleCount);

// --- COMPUTATIONALLY INTENSIVE DSP LOGIC ---

// The buffer is now safely accessible - Core 0 is writing to the other
buffer

// 1. PRIORITY: Vibration Analysis (FFT) - Safety Critical

// Process accelerometer data for structural resonance detection
// TODO: Apply Hann windowing function to reduce spectral leakage
// TODO: Perform FFT on windowed accelerometer data
// TODO: Extract dominant frequencies and amplitudes
// TODO: Check for dangerous resonance conditions (immediate alert)

// 2. Strain Analysis Pipeline

// TODO: Apply thermal compensation to all strain readings
// for (int i = 0; i < filledBuffer->sampleCount; i++) {
// float corrected_strain = apply_thermal_compensation(
// filledBuffer->samples[i].strain,
// filledBuffer->samples[i].temperature
// );
// // Store corrected value back or in separate array
// }
 // TODO: Perform Rainflow counting on thermally compensated strain data
// TODO: Update cumulative fatigue cycle histograms in RTC memory

// 3. Feature Extraction and Data Packetization

// TODO: Package extracted features into LoRa transmission payload
// TODO: Signal lora_task that data packet is ready for transmission

// Placeholder processing delay to simulate computational load

vTaskDelay(pdMS_TO_TICKS(10));

Serial.printf("[Core 1] Buffer processing complete\n");

}

// Continue monitoring for new buffers from Core 0

// The double-buffering mechanism ensures continuous data flow
}
}

/**
* Core 1 - LoRa Communication Task (Placeholder)
* Manages LoRa radio operations and data transmission
*/
void lora_task(void *pvParameters) {
Serial.println("[Core 1] LoRa task started");

for (;;) {
// TODO: Wait for signal from processing_task that data packet is ready
// TODO: Configure LoRa radio parameters (frequency, spreading factor,
etc.)
// TODO: Transmit data packet
// TODO: Wait for TxDone confirmation
// TODO: Put radio back into low-power mode
// TODO: Signal main_control_task that transmission cycle is complete

vTaskDelay(pdMS_TO_TICKS(1000)); // Placeholder delay

}
}

/**
* Main Control Task - System State Machine Management
* Handles overall system coordination and power management
*/
void main_control_task(void *pvParameters) {
Serial.println("[Core 1] Main control task started");

// Subscribe to watchdog timer

esp_task_wdt_add(nullptr);

for (;;) {
esp_task_wdt_reset(); // Feed watchdog

// TODO: Implement main system state machine

switch (systemState) {
case INITIALIZING:
// Handled in setup()
systemState = SAMPLING;
break;
 case SAMPLING:
// Monitor sampling progress, manage timeouts
// Transition to PROCESSING when buffer thresholds are met
break;

case PROCESSING:
// Monitor processing task progress
// Transition to TRANSMITTING when features are extracted
break;

case TRANSMITTING:
// Monitor LoRa transmission progress
// Transition to LOW_POWER_IDLE when transmission complete
break;

case LOW_POWER_IDLE:
// Prepare for and enter deep sleep mode
// TODO: Configure wake-up timer
// TODO: Store critical state in RTC memory
// esp_sleep_enable_timer_wakeup(SLEEP_DURATION_S * 1000000ULL);
// esp_deep_sleep_start();
Serial.println("Would enter deep sleep here...");
vTaskDelay(pdMS_TO_TICKS(5000)); // Placeholder
systemState = SAMPLING; // Resume cycle
break;
}

vTaskDelay(pdMS_TO_TICKS(100)); // Yield to other tasks

}
}

// --- Main Setup and Loop ---

void setup() {
Serial.begin(115200);
delay(2000); // Allow serial monitor to connect

Serial.println("=== SETU Super-Node Firmware Starting ===");

Serial.println("Dual-Core FreeRTOS Architecture with Hardware Timer Sampling");

// --- Initialize Task Watchdog Timer ---

Serial.println("Initializing Task Watchdog Timer...");
esp_task_wdt_init(WDT_TIMEOUT_S, true); // Enable panic on timeout
esp_task_wdt_add(nullptr); // Subscribe setup/loop task

// --- Initialize Inter-Core Communication ---

Serial.println("Creating FreeRTOS synchronization primitives...");

// Create queue for passing buffer pointers from Core 0 to Core 1

dataBufferQueue = xQueueCreate(QUEUE_SIZE, sizeof(SensorDataBuffer*));
if (dataBufferQueue == nullptr) {
Serial.println("ERROR: Failed to create data buffer queue");
while(1); // Halt on critical failure
}

// --- Initialize Peripheral Hardware ---

Serial.println("Initializing peripheral hardware...");
// TODO: Initialize I2C bus for MPU-6050
// TODO: Initialize SPI bus for LoRa module
// TODO: Initialize digital I/O for HX711
// TODO: Initialize 1-Wire bus for DS18B20

Serial.println("Hardware initialization complete (placeholder)");

// --- Configure Hardware Timer for Deterministic Sampling ---

Serial.println("Configuring hardware timer for deterministic sampling...");

// Use timer 0 with 80 prescaler (80MHz APB clock / 80 = 1MHz timer clock)
// This gives us 1μs resolution, suitable for precise timing control
samplingTimer = timerBegin(0, 80, true);

if (samplingTimer == nullptr) {
Serial.println("ERROR: Failed to initialize hardware timer");
while(1); // Halt on critical failure
}

// Attach our minimalist ISR to the timer

timerAttachInterrupt(samplingTimer, &onTimer, true);

// Calculate alarm value for desired sampling frequency

// For 200 Hz: 1,000,000 μs / 200 Hz = 5,000 μs per sample
uint32_t timerAlarmValue = 1000000 / SAMPLING_FREQUENCY_HZ;
Serial.printf("Setting timer alarm for %d Hz sampling (alarm value: %d μs)\n",
SAMPLING_FREQUENCY_HZ, timerAlarmValue);

timerAlarmWrite(samplingTimer, timerAlarmValue, true); // Auto-reload enabled

// --- Create FreeRTOS Tasks with Core Affinity ---

Serial.println("Creating FreeRTOS tasks with core affinity...");

// Create Core 0 (Real-Time) sampling task - HIGHEST PRIORITY

BaseType_t result = xTaskCreatePinnedToCore(
sampling_task, // Task function
"Sampling Task", // Task name (for debugging)
8192, // Stack size (bytes)
nullptr, // Task parameters
configMAX_PRIORITIES - 1, // Priority (highest possible)
&samplingTaskHandle, // Task handle
0 // Core affinity: Core 0 (PRO_CPU)
);

if (result != pdPASS) {
Serial.println("ERROR: Failed to create sampling task");
while(1);
}

// Create Core 1 (Application) processing task - HIGH PRIORITY

result = xTaskCreatePinnedToCore(
processing_task, // Task function
"Processing Task", // Task name
16384, // Stack size (larger for DSP operations)
nullptr, // Task parameters
5, // Priority (high, but lower than sampling)
&processingTaskHandle, // Task handle
1 // Core affinity: Core 1 (APP_CPU)
);

if (result != pdPASS) {
 Serial.println("ERROR: Failed to create processing task");
while(1);
}

// Create Core 1 LoRa communication task - MEDIUM PRIORITY

result = xTaskCreatePinnedToCore(
lora_task, // Task function
"LoRa Task", // Task name
8192, // Stack size
nullptr, // Task parameters
4, // Priority (medium)
&loraTaskHandle, // Task handle
1 // Core affinity: Core 1 (APP_CPU)
);

if (result != pdPASS) {
Serial.println("ERROR: Failed to create LoRa task");
while(1);
}

// Create Core 1 main control task - LOWER PRIORITY

result = xTaskCreatePinnedToCore(
main_control_task, // Task function
"Main Control Task", // Task name
8192, // Stack size
nullptr, // Task parameters
3, // Priority (lower than processing/LoRa)
&mainControlTaskHandle, // Task handle
1 // Core affinity: Core 1 (APP_CPU)
);

if (result != pdPASS) {
Serial.println("ERROR: Failed to create main control task");
while(1);
}

// --- Start the Hardware Timer ---

Serial.println("Starting deterministic sampling timer...");
timerAlarmEnable(samplingTimer);

// --- System Ready ---

systemState = SAMPLING;
Serial.println("=== SETU Super-Node Firmware Ready ===");
Serial.printf("Core 0: Real-time sampling at %d Hz\n", SAMPLING_FREQUENCY_HZ);
Serial.println("Core 1: Signal processing, LoRa communication, and system
control");
Serial.println("Hardware timer ISR configured for deterministic data
acquisition");
Serial.println("Double-buffering mechanism active for zero-copy inter-core
communication");
}

void loop() {
// The main Arduino loop() represents the main_control_task functionality
// In this architecture, most work is done by dedicated FreeRTOS tasks
// The loop() is kept minimal and primarily feeds the watchdog

esp_task_wdt_reset(); // Feed watchdog from main loop

 // Optional: Print system status periodically
static uint32_t lastStatusPrint = 0;
if (millis() - lastStatusPrint > 10000) { // Every 10 seconds
Serial.printf("[STATUS] System State: %d, Active Buffer: %s, Sample Index:
%d\n",
systemState,
(activeBuffer == &bufferA) ? "A" : "B",
currentSampleIndex);

// Print memory usage

Serial.printf("[MEMORY] Free heap: %d bytes, Min free heap: %d bytes\n",
ESP.getFreeHeap(), ESP.getMinFreeHeap());

lastStatusPrint = millis();
}

// Yield to other tasks - this prevents the Arduino loop from monopolizing CPU
vTaskDelay(pdMS_TO_TICKS(1000));
}