// FreeRTOS related headers
#include <freertos/FreeRTOS.h>  // Core FreeRTOS functionality
#include <freertos/task.h>      // For creating and managing tasks
#include <freertos/queue.h>     // For inter-task communication using queues
#include <freertos/semphr.h>    // For synchronization primitives like semaphores
#include <memory>

// ESP32 specific libraries
#include <esp_task_wdt.h>  // ESP32 task watchdog timer
#include <driver/rtc_io.h> //
#include <Preferences.h>   // For storing data in non-volatile storage

// Custom project headers
#include "enhancedEMA.h"              // Enhanced Exponential Moving Average implementation
#include "enumerate.h"
#include "WashingMachineController.h" // Main controller for the washing machine
#include "hardwareConfig.h"           // Hardware configuration and pin definitions
#include "sensorManager.h"            // Managing various sensors
#include "taskManager.h"              // Handling different tasks in the system
#include "safetyManager.h"            // Ensuring safe operation of the machine
#include "buzzer.h"                   // For audio feedback or alerts
#include "cyclesManager.h" // Assume the above code is in this header file
#include "machineInstruction.h"
#include "errorHandler.h"

// WIFI/Networking
#include "WIFI_controller.h"
const char* ssid = "YourWiFiSSID";
const char* password = "YourWiFiPassword";
WIFIController wifiController;

MachineInstruction instruction = {};

ErrorHandler& errorHandler = ErrorHandler::getInstance();

// Global instance (this will call the constructor and initialize cycles)
CyclesManager cyclesManager;

Preferences preferences;
EnhancedEMA pressureEMA(0.3, 0.1, 1.0);
Buzzer buzzer(BUZZER_PIN);

enum class MachineState {
  IDLE,
  WASHING,
  RINSING,
  COMPLETED
};

MachineState currentState = MachineState::IDLE;

std::unique_ptr<WashingMachineController> controller;

SensorManager sensorManager;
SafetyManager safetyManager;

TaskManager taskManager(sensorManager, safetyManager, errorHandler);

void initializeController() {
  controller = std::make_unique<WashingMachineController>(instruction);
}

void setup() {

  initializeSerialBus(); //Only for debugging purposes

  setupPinModes();
  safetyManager.setupInterrupts();
  safetyManager.checkSafetyStatus();

  sensorManager.initialize();
  taskManager.initialize();
  cyclesManager.initialize();
  errorHandler.initialize(instruction);

  wifiController.begin();

  buzzer.playStartupSequence();
}

void listCycles() {
  auto cycles = cyclesManager.loadAllCycles();
  Serial.println("Available cycles:");
  for (const auto& [index, cycle] : enumerate(cycles)) {
    Serial.print(index + 1);  // Adding 1 to start counting from 1 instead of 0
    Serial.print(". ");
    Serial.println(cycle.name);
  }
}

CycleStages currentCycle;
bool loadCycle(String name) {
  auto maybeCycle = cyclesManager.getCycleByName(name);
  if (maybeCycle) {
    // Use the cycle
    currentCycle = *maybeCycle;
    // Serial.println(currentCycle.name);
    return true;
  } else {
    Serial.println("Cycle not found!");
    return false;
  }
}

void loop() {
  wifiController.handleClient();
  if (wifiController.isConnected()) {
    Serial.println("WIFI Connected");
  }

  if (!loadCycle("Normal")) return;
  String temp = safetyManager.isHeaterSafe() ? "Yes" : "No";
  Serial.println("Is Heater Safe: " + temp);
  temp = safetyManager.checkBucketEmpty() ? "Yes" : "No";
  Serial.println("Is Bucket Empty: " + temp);

  // Run main washing machine.
  switch (currentState) {
    case MachineState::IDLE:
      initializeController();
      currentState = MachineState::RINSING;
      break;
    case MachineState::WASHING:
      // Serial.println("Washing.");
      // loopWashCycle(controller, currentCycle);
      // loopWashRinseCycleTest(controller, currentCycle);
      break;
    case MachineState::RINSING:
      // Serial.println("Rinsing.");
      // loopRinseCycle(controller, currentCycle);
      loopWashRinseCycleTest(*controller, currentCycle);
      break;
    case MachineState::COMPLETED:
      controller.reset();
      Serial.println("Cycle completed.");
      currentState = MachineState::IDLE;

      break;
  }

  // Action any changed state
  (*controller).applyInstructions();

  Serial.println("________________Reached end of loop________________");

  // Show sensor dashboard
  // showDashboard();

  delay(1000);
}

// TODO: but not as important for main function.
// 1. Make sure power button puts esp32 to sleep then restarts it upon waking. DONE BUT NEED TO TEST ON ACTUAL DEVICE.
// 2. Let users save their custom cycles to the device.
// 3. Give users a user interface to create and start their custom/non-custom wash cycles.
// 4. Implement the watchdog timer.
// 5. Put wash cycles into a config file.



















// ###################################################################
// WASHER FUNCTIONS - PRODUCTION
// ###################################################################
void loopWashCycle(WashingMachineController& controller, const CycleStages& cycleStages) {
  Serial.println("Starting wash cycle: " + cycleStages.name);

  float currentPressureKPa = sensorManager.getCurrentPressure();
  float currentAverageTemp = sensorManager.getAverageTemperature();

  controller.handleWashStage(cycleStages.washStage, currentPressureKPa, currentAverageTemp);

  if (controller.washComplete()) {
    currentState = MachineState::RINSING;
    Serial.println("Wash cycle complete. Starting rinse cycle.");
  }
}

// Run continuously.
void loopRinseCycle(WashingMachineController& controller, const CycleStages& cycleStages) {
  Serial.println("Running rinse cycle for: " + cycleStages.name);

  float currentPressureKPa = sensorManager.getCurrentPressure();
  float currentAverageTemp = sensorManager.getAverageTemperature();

  controller.handleRinseStage(cycleStages.rinseStage, currentPressureKPa, currentAverageTemp);

  if (controller.rinseComplete()) {
    currentState = MachineState::COMPLETED;
    Serial.println("Rinse cycle complete. MACHINE DONE.");
  }
}






















// ###################################################################
// DEBUGGING
// ###################################################################
void initializeSerialBus() {
  // Serial bus only needed in pre-production environments;
  Serial.begin(115200);
  unsigned long startTime = millis();
  while (!Serial && millis() - startTime < 5000) {
    ; // wait up to 5 seconds for serial port to connect
  }
  Serial.println("Serial initialized");
}

#include "sensorDashboard.h"          // Displaying sensor data
SensorDashboard dashboard(sensorManager, 8); // Show last x readings
void showDashboard() {
  static unsigned long lastDashboardUpdate = 0;
  const unsigned long dashboardUpdateInterval = 5000;

  unsigned long currentTime = millis();
  if (currentTime - lastDashboardUpdate >= dashboardUpdateInterval) {
    dashboard.printDashboard();
    lastDashboardUpdate = currentTime;
  }
}

// Primary template (this will cause a compilation error if used with an unsupported type)
template<typename T>
const char* enumToString(T) {
  return "UNKNOWN";
}

// Template specialization for MotorState
const char* enumToString(MotorState state) {
  switch (state) {
    case MotorState::LEFT: return "LEFT";
    case MotorState::RIGHT: return "RIGHT";
    case MotorState::PAUSE1: return "PAUSE1";
    case MotorState::PAUSE2: return "PAUSE2";
    default: return "UNKNOWN";
  }
}

// Template specialization for StageState
const char* enumToString(StageState state) {
  switch (state) {
    case StageState::IDLE: return "IDLE";
    case StageState::INITIALIZING: return "INITIALIZING";
    case StageState::FILLING: return "FILLING";
    case StageState::RUNNING: return "RUNNING";
    case StageState::DRAINING: return "DRAINING";
    case StageState::COMPLETED: return "COMPLETED";
    case StageState::ERROR: return "ERROR";
    default: return "UNKNOWN";
  }
}

// Global variables to simulate sensor readings
float currentPressureKPa = 0.0f;
float currentAverageTemperature = 25.0f;
float roomTemp = 21.0f;

// Function to simulate changing pressure and temperature
void updateSimulatedSensorReadings(MachineInstruction& instruction) {
  // Simulate pressure changes
  if (instruction.turnInletPumpOn) {
    Serial.println("FILLING");
    currentPressureKPa += 0.5f;  // Simulate filling
  }

  if (instruction.turnOutletPumpOn) {
    Serial.println("DRAINING");
    if (!currentPressureKPa <= 0) {
      currentPressureKPa -= 1.0f;  // Simulate draining
    }
  }

  if (instruction.turnHeaterOn) {
    Serial.println("HEATING");
    currentAverageTemperature += 0.1f;
  }

  if (!instruction.turnHeaterOn) {
    Serial.println("PASSIVE COOLING");
    if (currentAverageTemperature > roomTemp) {
      currentAverageTemperature -= 0.1f;
    }
  }
}

void loopWashRinseCycleTest(WashingMachineController& controller, const CycleStages& cycleStages) {
  static unsigned long lastPrintTime = 0;
  static bool testStarted = false;
  const unsigned long printInterval = 1000;  // Print every second

  if (!testStarted) {
    Serial.println("Starting Rinse Cycle Test");
    testStarted = true;
  }

  enum class StageName {
    WASH,
    RINSE
  };
  StageName stageName = StageName::RINSE; //---

  if (stageName == StageName::WASH) {
    controller.handleWashStage(cycleStages.washStage, currentPressureKPa, currentAverageTemperature);
  } else if (stageName == StageName::RINSE) {
    controller.handleRinseStage(cycleStages.rinseStage, currentPressureKPa, currentAverageTemperature);
  } else {
    Serial.println("Error: Unknown stage!");
  }

  MachineInstruction& instruction = controller.getInstructions();

  // Print status every second
  if (millis() - lastPrintTime >= printInterval) {

    lastPrintTime = millis();

    Serial.println("----------------------------------------");
    Serial.print("Time: ");
    Serial.print(millis() / 1000);
    if (stageName == StageName::WASH) {
      Serial.print("s   Wash time left: ");
      Serial.print(controller.getWashTimer().getTimeRemaining());
      Serial.print(" / ");
      Serial.print(controller.getWashTimer().getDuration());
      Serial.println("s");
    }
    if (stageName == StageName::RINSE) {
      Serial.print("s   Rinse time left: ");
      Serial.print(controller.getRinseTimer().getTimeRemaining());
      Serial.print(" / ");
      Serial.print(controller.getRinseTimer().getDuration());
      Serial.println("s");
    }

    Serial.print("Pressure: ");
    Serial.print(currentPressureKPa);
    Serial.print(" / ");
    if (stageName == StageName::RINSE) {
      Serial.print(cycleStages.rinseStage.waterPressureKPa);
    }
    if (stageName == StageName::WASH) {
      Serial.print(cycleStages.washStage.waterPressureKPa);
    }

    Serial.print(" kPa, AvgTemp: ");
    Serial.print(currentAverageTemperature);
    Serial.print(" / ");
    if (stageName == StageName::RINSE) {
      Serial.print(cycleStages.rinseStage.temperature);
    }
    if (stageName == StageName::WASH) {
      Serial.print(cycleStages.washStage.temperature);
    }
    Serial.println(" °C");

    if (stageName == StageName::RINSE) {
      Serial.print("Rinse State: ");
      Serial.print(enumToString(controller.getRinseState()));
    }
    if (stageName == StageName::WASH) {
      Serial.print("Wash State: ");
      Serial.print(enumToString(controller.getWashState()));
    }
    Serial.print("  Motor State: ");
    Serial.print(enumToString(controller.getMotorState()));
    Serial.print("  Current Rinse Count: ");
    Serial.print(controller.getCurrentRinseCount());

    Serial.print("\nMachine Instructions: ");
    Serial.print("  Inlet Pump: ");
    Serial.print(instruction.turnInletPumpOn ? "ON" : "OFF");
    Serial.print("  Outlet Pump: ");
    Serial.print(instruction.turnOutletPumpOn ? "ON" : "OFF");
    Serial.print("\nWashing Motor: ");
    Serial.print(instruction.turnWashingMotorOn ? "ON" : "OFF");
    Serial.print("    Rotate Left: ");
    Serial.print(instruction.rotateWashingMotorLeft ? "YES" : "NO");
    Serial.print("    Rotate Right: ");
    Serial.print(instruction.rotateWashingMotorRight ? "YES" : "NO");
    Serial.print("  Heater: ");
    Serial.print(instruction.turnHeaterOn ? "ON" : "OFF");

    Serial.print("\nIndicator Instructions:");
    Serial.print("  Cycle Complete LED: ");
    Serial.print(instruction.turnCycleCompleteLEDOn ? "ON" : "OFF");
    Serial.print("  Error LED: ");
    Serial.print(instruction.turnErrorLEDOn ? "ON" : "OFF");
    Serial.print("  Cycle Complete Sound: ");
    Serial.print(instruction.playCycleCompleteSound ? "PLAY" : "OFF");
    Serial.print("  Error Sound: ");
    Serial.print(instruction.playErrorSound ? "PLAY" : "OFF");

    if (controller.rinseComplete()) {
      Serial.println("++++++++++++++++Rinse Cycle Test Completed++++++++++++++++");
      testStarted = false;  // Reset for next test run
    }

    Serial.println("\n----------------------------------------");

    // Update simulated sensor readings
    updateSimulatedSensorReadings(instruction);
  }
}