#include <Arduino.h>
#include "types.h"
#include "config.h"

void simulate() {
  static unsigned long lastChangeTime = 0;
  static int state = 10;
  
  // Change state every 2 seconds for demonstration
  if (millis() - lastChangeTime > 2000) {
    lastChangeTime = millis();


    // Cycle through different states
    switch (state) {
      case 0:
        digitalWrite(slots[0].filamentSensorPin, SLOT_DETECT_LEVEL);
        break;
      case 1:
        digitalWrite(slots[0].selectorInputPin, SLOT_DETECT_LEVEL);
        break;
      case 2:
        // Simulate selector output
        digitalWrite(SELECTOR_END_PIN, SLOT_DETECT_LEVEL);
        break;
      case 3:
        // Simulate buffer feed
        digitalWrite(BUFFER_FEED_PIN, LOW);
        break;
      case 4:
        // Simulate buffer feed
        digitalWrite(BUFFER_FEED_PIN, BUFFER_FEED_LEVEL);
        break;
      case 5:
        // Simulate buffer retract
        digitalWrite(BUFFER_RETRACT_PIN, !BUFFER_FEED_LEVEL);
        break;
      case 6:
        // Simulate buffer retract
        digitalWrite(BUFFER_RETRACT_PIN, BUFFER_RETRACT_LEVEL);
        break;
      case 7:
        // Simulate buffer retract
        digitalWrite(BUFFER_RETRACT_PIN, !BUFFER_RETRACT_LEVEL);
        break;

      // Add more cases as needed for different scenarios
      default:
        // Reset all states to default
        for (int i = 0; i < MAX_SLOTS; i++) {
          digitalWrite(slots[i].filamentSensorPin, HIGH); // Assuming HIGH means no filament
          digitalWrite(slots[i].selectorInputPin, HIGH);  // Assuming HIGH means no filament
        }
        digitalWrite(SELECTOR_END_PIN, HIGH);             // Assuming HIGH means no filament at selector output
        digitalWrite(BUFFER_FEED_PIN, HIGH);              // Assuming HIGH means no need to feed
        digitalWrite(BUFFER_RETRACT_PIN, HIGH);           // Assuming HIGH means no need to retract
    }

    state = (state + 1) % 10; // Cycle through 5 states
  }
}


void visualizeSystemStatus() {
  Serial.print("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
  Serial.write(27); // Attempt to clear the terminal screen
  Serial.println();

  // Calculate total width of SLOT headers
  int slotHeadersWidth = 10 + 2 * MAX_SLOTS; // "SLOT  " is 6 chars + 2 chars per slot

  // Header
  for (uint8_t i = 1; i <= MAX_SLOTS; i++) {
    Serial.print(' ');
    Serial.print(i);
  }
  Serial.print(' ');
  Serial.println("\tSLOTS");
  
  // Feeder
  for (uint8_t i = 1; i <= MAX_SLOTS; i++) {
    Serial.print(' ');
    Serial.print(digitalRead(slots[i - 1].filamentSensorPin) == SLOT_DETECT_LEVEL ? '|' : '.');
  }
  Serial.print(' ');
  Serial.println("\tFeeder input");
  
  // Selector Inputs
  for (uint8_t i = 1; i <= MAX_SLOTS; i++) {
    Serial.print(' ');
    Serial.print(digitalRead(slots[i - 1].selectorInputPin) == SELECTOR_DETECT_LEVEL ? '|' : '.');
  }
  Serial.println("\tSelector input");

  // Selector Output
  int centeringSpaces = (slotHeadersWidth - 9) / 2; // "Output: " is 9 chars
  for (int i = 0; i < centeringSpaces; i++) Serial.print(' ');
  Serial.print(digitalRead(SELECTOR_END_PIN) == SELECTOR_DETECT_LEVEL ? 'V' : '.');
  Serial.println("\t\tSelector output");

  // Buffer
  bool bufferFeed = digitalRead(BUFFER_FEED_PIN) == BUFFER_FEED_LEVEL;
  bool bufferRetract = digitalRead(BUFFER_RETRACT_PIN) == BUFFER_FEED_LEVEL;
  Serial.print(" [");
  Serial.print(bufferFeed ? '=' : '-');
  Serial.print(!bufferFeed && !bufferRetract ? '=' : '-');
  Serial.print(bufferRetract ? '=' : '-');
  Serial.print("] ");
  Serial.println("\tBuffer");
}


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


  for (int i = 0; i < MAX_SLOTS; i++) {
    pinMode(slots[i].filamentSensorPin, INPUT_PULLUP);
    pinMode(slots[i].selectorInputPin, INPUT_PULLUP);  
  }
  pinMode(SELECTOR_END_PIN, INPUT_PULLUP);             
  pinMode(BUFFER_FEED_PIN, INPUT_PULLUP);              
  pinMode(BUFFER_RETRACT_PIN, INPUT_PULLUP);           
}

void loop() {
  //simulate();
  visualizeSystemStatus();
  delay(500); // Update every 2 seconds
}