#include <Arduino.h>
#include <FreeRTOS.h> 
#include <task.h>

// --- PIN MAPPING CONFIGURATION ---
#define MOTOR_L_STEP  2   // Pin D2
#define MOTOR_L_DIR   3   // Pin D3
#define MOTOR_R_STEP  4   // Pin D4
#define MOTOR_R_DIR   5   // Pin D5
#define VACUUM_ESC    9   // Pin D9 (PWM)

// --- SIMULATION GLOBALS ---
bool isCleaningActive = false;
uint32_t oneHourCounter = 0; 
const uint32_t ONE_HOUR_TARGET = 10; // Accelerated to 10 seconds for fast browser testing

// --- TASK HANDLES ---
TaskHandle_t SafetyTaskHandle;
TaskHandle_t NavigationTaskHandle;
TaskHandle_t TimerTaskHandle;

void TaskSafety(void *pvParameters);
void TaskNavigation(void *pvParameters);
void TaskTimer(void *pvParameters);

void setup() {
  Serial.begin(115200);
  Serial.println("--- Classroom Smart Duster Initializing ---");

  // Define Motor Output Directions
  pinMode(MOTOR_L_STEP, OUTPUT);
  pinMode(MOTOR_L_DIR, OUTPUT);
  pinMode(MOTOR_R_STEP, OUTPUT);
  pinMode(MOTOR_R_DIR, OUTPUT);
  pinMode(VACUUM_ESC, OUTPUT);

  // Initialize ESC to armed state (Low signal / 1.0ms pulse)
  digitalWrite(VACUUM_ESC, LOW);

  // Create FreeRTOS Task Architecture
  xTaskCreate(TaskSafety,     "Safety",     128, NULL, 3, &SafetyTaskHandle);     // Highest Priority
  xTaskCreate(TaskNavigation, "Navigation", 128, NULL, 2, &NavigationTaskHandle); // High Priority
  xTaskCreate(TaskTimer,      "Timer",      128, NULL, 1, &TimerTaskHandle);      // Low Priority
}

void loop() {
  // FreeRTOS handles execution. The standard loop must remain empty.
}

/**
 * @brief Task 1: Safety Check Loop (Priority: 3 - Real-Time)
 */
void TaskSafety(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    int simulatedPressure = 2500; // 2.5 kPa hold simulation

    if(simulatedPressure < 2000) {
      Serial.println("🚨 EMERGENCY ERROR: Vacuum Seal Broken! Halting!");
      isCleaningActive = false;
    }
    vTaskDelay(pdMS_TO_TICKS(10)); // Evaluate vacuum integrity every 10ms
  }
}

/**
 * @brief Task 2: Drivetrain Navigation Sweep (Priority: 2 - High)
 */
void TaskNavigation(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    if (isCleaningActive) {
      Serial.println("🌪️ Vacuum Fan Engaged: Spinning up to 25,000 RPM...");
      
      // Simulate systematic Zig-Zag Horizontal Sweep Rows
      for (int row = 1; row <= 3; row++) {
        Serial.print("➡️ Sweeping Horizontal Row #"); Serial.println(row);
        digitalWrite(MOTOR_L_DIR, HIGH);
        digitalWrite(MOTOR_R_DIR, HIGH);
        
        // Simulate running across the board width
        for (int steps = 0; steps < 150; steps++) {
          digitalWrite(MOTOR_L_STEP, HIGH);
          digitalWrite(MOTOR_R_STEP, HIGH);
          delayMicroseconds(2000);
          digitalWrite(MOTOR_L_STEP, LOW);
          digitalWrite(MOTOR_R_STEP, LOW);
          delayMicroseconds(2000);
        }
        
        vTaskDelay(pdMS_TO_TICKS(500)); // Brief pause at edge boundary
        Serial.println("🔄 Reached Board Frame! Executing 90° Turn Downward.");
        vTaskDelay(pdMS_TO_TICKS(500));
      }
      
      Serial.println("✅ Board Cleaned Successfully! Returning to Base Station Dock.");
      isCleaningActive = false;
    }
    vTaskDelay(pdMS_TO_TICKS(100)); // Check if deployment is requested every 100ms
  }
}

/**
 * @brief Task 3: Background Interval Countdown (Priority: 1 - Normal)
 */
void TaskTimer(void *pvParameters) {
  (void) pvParameters;
  for (;;) {
    if (!isCleaningActive) {
      oneHourCounter++;
      Serial.print("⏱️ Interval Timer Count: "); Serial.print(oneHourCounter); Serial.println("s / 10s");
      
      if (oneHourCounter >= ONE_HOUR_TARGET) {
        Serial.println("🔔 1-Hour Interval Expired! Deploying Smart Duster...");
        isCleaningActive = true;
        oneHourCounter = 0; // Reset countdown
      }
    }
    vTaskDelay(pdMS_TO_TICKS(1000)); // Increment clock tick precisely once a second
  }
}