// Define constants for module stat
const int EMPTY = 0;
const int REFRIGERATING = 1;
const int INCUBATING = 2;
const int DONE = 3;
const float BETA = 3950; // should match the Beta Coefficient of the thermistor
const int serialDelay = 5000;
// Define the number of modules, default is 3
const int NUM_MODULES = 3;

// active output pins for each module
// Need some kind of indicator or LED to show plate is inserted,
int modulePins[NUM_MODULES][3] = {
  {2, 3, A0}, // Module A: heatingPin, coolingPin, temperaturePin, [statusPin]
  {4, 5, A1}, // Module B: heatingPin, coolingPin, temperaturePin, [statusPin]
  {6, 7, A2}  // Module C: heatingPin, coolingPin, temperaturePin, [statusPin]
};

// This array contains the timings for each
float moduleTimers[NUM_MODULES][3] = {
  {0, 0, 0}, // Module A: startIncTime, delayTime, finishTime
  {0, 0, 0}, // Module B: startIncTime, delayTime, finishTime
  {0, 0, 0}  // Module C: startIncTime, delayTime, finishTime
};

// Define pins for plate detection
int plateDetectPins[NUM_MODULES] = {8, 12, 13}; 

// Define pins for "plate done" LED
int plateDone[NUM_MODULES] = {9,10,11};

// Define variables for incubation settings
float incTime = 1;     // Incubation time in minutes
float delayTime = 0.5;    // Delay time in minutes
int tolerance = 0;    // Incubation time tolerance percentage

// Define variables for temperature control settings
int refrigeratingTemperature = 4; // Target temperature for refrigerating in Celsius
int incubatingTemperature = 35;     // Target temperature for incubating in Celsius

// Define variables to track module status and timers
int moduleStatus[NUM_MODULES] = {EMPTY};

// Define a variable to keep track of the last time module statuses were printed
unsigned long lastStatusPrintTime = 0;

void setup() {
  // Initialize module pins
  for (int i = 0; i < NUM_MODULES; i++) {
    pinMode(modulePins[i][0], OUTPUT); // Heating
    pinMode(modulePins[i][1], OUTPUT); // Cooling
    pinMode(modulePins[i][2], INPUT);  // Temperature
    //pinMode(plateDetectPins[i], INPUT);  // Plate Detection
    pinMode(plateDone[i], OUTPUT); // Plate Done
  }
  // Initialize serial communication for debugging
  Serial.begin(9600);
}

void loop() {
  // Check for plates and update module statuses
  for (int i = 0; i < NUM_MODULES; i++) {
    int currentStatus = moduleStatus[i];
    switch (currentStatus) {
      case EMPTY:
        // Check for a plate insertion
        if (plateInserted(i)) {
          if (!anyModuleIncubating()) {
            // No module is currently incubating, so start incubating immediately. This only triggers if all modules are empty
            moduleStatus[i] = INCUBATING;
            moduleTimers[i][0] = millis(); // Push incubation start time to array
            moduleTimers[i][1] = millis() + (delayTime * 60000); // Push delay threshold to array
            moduleTimers[i][2] = millis() + (incTime * 60000); // Push end time to array
          } else {
            moduleStatus[i] = REFRIGERATING;
          }
        } else {
          if (digitalRead(plateDone[i]) == HIGH) {
            digitalWrite(plateDone[i], LOW);
          }
        }
        break;

      case INCUBATING:
        // Check if incubation time has elapsed. If condition is met, turn off pins and turn on done light
        if (millis() >= moduleTimers[i][2]) {
          moduleStatus[i] = DONE;
          Serial.print("Timepoint: ");
          Serial.print(millis() / 1000);
          Serial.print("s Module ");
          Serial.print(i);
          Serial.println(" is done incubating.");
          digitalWrite(modulePins[i][0],LOW);
          digitalWrite(modulePins[i][1],LOW);
          digitalWrite(plateDone[i],HIGH);
        }

        // If the plate is incubating, and the plate is removed, turn off the control pins and set status to empty.
        if (!plateInserted(i)) {
          moduleStatus[i] = EMPTY;
          Serial.print("Timepoint: ");
          Serial.print(millis() / 1000);
          Serial.print("s Module ");
          Serial.print(i);
          Serial.println(" has been removed.");
          digitalWrite(modulePins[i][0], LOW);
          digitalWrite(modulePins[i][1], LOW);
          digitalWrite(plateDone[i], LOW);
        }
        break;

      case REFRIGERATING:
        // If refrigerating, and canStartIncubation function returns true, then set status to incubating and write timepoints to the array.
        if (canStartIncubation(i)) {
            moduleStatus[i] = INCUBATING;
            moduleTimers[i][0] = millis(); // Push incubation start time to array
            moduleTimers[i][1] = millis() + (delayTime * 60000); // Push delay threshold to array
            moduleTimers[i][2] = millis() + (incTime * 60000); // Push end time to array;
        }
        // If plate is removed, set status to empty and turn off control pins.
        if (!plateInserted(i)) {
          moduleStatus[i] = EMPTY;
          Serial.print("Timepoint: ");
          Serial.print(millis() / 1000);
          Serial.print("s Module ");
          Serial.print(i);
          Serial.println(" has been removed.");
          digitalWrite(modulePins[i][0], LOW);
          digitalWrite(modulePins[i][1], LOW);
          digitalWrite(plateDone[i],LOW);
        }
        break;

      case DONE:
        // If plate is done, turn off the control pins. Remain at "done" status until the plate is removed.

        if (!plateInserted(i)) {
          moduleStatus[i] = EMPTY;
          Serial.print("Timepoint: ");
          Serial.print(millis() / 1000);
          Serial.print("s Module ");
          Serial.print(i);
          Serial.println(" is empty.");
          digitalWrite(modulePins[i][0], LOW);
          digitalWrite(modulePins[i][1], LOW);
          digitalWrite(plateDone[i], LOW);
        }
        break;
    }
   
    // Temperature control logic
    int currentTemperature = readTemperature(modulePins[i][2]);
    if (moduleStatus[i] == REFRIGERATING) {
      // Control temperature for refrigerating
      controlTemperature(i, refrigeratingTemperature, currentTemperature);
    } else if (moduleStatus[i] == INCUBATING) {
      // Control temperature for incubating
      controlTemperature(i, incubatingTemperature, currentTemperature);
    }
  }

  // Print module statuses every 10 seconds
  unsigned long currentTime = millis();
  if (currentTime - lastStatusPrintTime >= serialDelay) { // 10 seconds have passed
    printModuleStatus();
    lastStatusPrintTime = currentTime; // Update the last print time
  }
}

// Function to print the status of each module
void printModuleStatus() {
  Serial.print("********* Timepoint ");
  Serial.print(millis());
  Serial.println("ms *********");
  for (int i = 0; i < NUM_MODULES; i++) {
    Serial.print("PLATE LOADED: ");
    Serial.print(plateInserted(i));
    Serial.print("|| Module ");
    Serial.print(i);
    Serial.print(" Status: ");
    switch (moduleStatus[i]) {
      case EMPTY:
        Serial.println("Empty");
        break;
      case REFRIGERATING:
        Serial.print("Refrigerating ");
        Serial.print(readTemperature(modulePins[i][2]));
        Serial.println("C");
        break;
      case INCUBATING:
        Serial.print("Incubating ") ;
        Serial.print(readTemperature(modulePins[i][2]));
        Serial.print("C || ");
        Serial.print("START: ");
        Serial.print(moduleTimers[i][0]);
        Serial.print(" ENDS: ");
        Serial.print(moduleTimers[i][2]);
        Serial.print(" NEXT PLATE AT: ");
        Serial.println(moduleTimers[i][1]);
        break;
      case DONE:
        Serial.println("Done");
        break;
      default:
        Serial.println("Unknown");
        break;
    }
  }
}

// Check if a plate is inserted into the module
bool plateInserted(int moduleIndex) {
  // Read the plate detection input pin
  return digitalRead(plateDetectPins[moduleIndex]) == HIGH;
}

// This boolean facilitates immediate incubating if all modules are empty.
bool anyModuleIncubating() {
  for (int i = 0; i < NUM_MODULES; i++) {
    if (moduleStatus[i] == INCUBATING) {
      return true;
    }
  }
  return false;
}

// Iterate through index 1 (delayTime) to determine if incubation can begin. 
bool canStartIncubation(int moduleIndex) {
  unsigned long currentTime = millis();
  // Find the highest value of "delayTime"
  float longestDelay = findMaxInVerticalSlice(moduleTimers,1);
  if (currentTime > longestDelay) {
    return true;
  } else {
    return false;
  }
}

float findMaxInVerticalSlice(float arr[][3], int columnIndex) {
  float maxVal = arr[0][columnIndex];  // Initialize maxVal with the first element in the specified column
  for (int i = 1; i < 3; i++) {       // Loop through the remaining elements in the column
    if (arr[i][columnIndex] > maxVal) {
      maxVal = arr[i][columnIndex];  // Update maxVal if a greater value is found
    }
  }
  
  return maxVal;  // Return the maximum value in the specified column
}

// Read temperature from the temperature sensor
float readTemperature(int temperaturePin) {
  // Read and convert the temperature using the TMP36 sensor
  int rawValue = analogRead(temperaturePin);
  float celsius = 1 / (log(1 / (1023. / rawValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  return celsius;
}

// Control temperature for a module
void controlTemperature(int moduleIndex, int targetTemperature, float currentTemperature) {
  // Implement your temperature control logic here
  if ((moduleStatus[moduleIndex] == INCUBATING) || (moduleStatus[moduleIndex == REFRIGERATING])) {
    if (currentTemperature < targetTemperature -2) {
      // It's too cold, turn on heating
      digitalWrite(modulePins[moduleIndex][0], HIGH);
      digitalWrite(modulePins[moduleIndex][1], LOW);

    } else if (currentTemperature > targetTemperature + 2) {
      // It's too hot, turn on cooling
      digitalWrite(modulePins[moduleIndex][0], LOW);
      digitalWrite(modulePins[moduleIndex][1], HIGH);

    } else {
      // Temperature is within tolerance, turn off both
      digitalWrite(modulePins[moduleIndex][0], LOW);
      digitalWrite(modulePins[moduleIndex][1], LOW);
    }
  } else if ((moduleStatus[moduleIndex] == DONE) || (moduleStatus[moduleIndex == EMPTY])) {
    digitalWrite(modulePins[moduleIndex][0], LOW);
    digitalWrite(modulePins[moduleIndex][1], LOW);
    }
  }