#include <Arduino.h>

// --- Pins ---
#define CUTTER_PIN 2
#define BED_PIN    3
#define WIPER_PIN  4

// --- State flags ---
bool armed = false;
bool encoding = false;
bool filamentEncoded = false;

// Placeholder for VL53L0X result
int encodedSlot = -1;

// Debounce
unsigned long lastEventTime = 0;
const unsigned long debounceMs = 150;

void setup() {
  pinMode(CUTTER_PIN, INPUT_PULLUP);
  pinMode(BED_PIN, INPUT_PULLUP);
  pinMode(WIPER_PIN, INPUT_PULLUP);

  Serial.begin(115200);
  Serial.println("EFAC-A1 Logic Test Started (distance-based)");
}

void loop() {
  handleCutter();
  handleBed();
  handleWiper();
}

// -------------------- Handlers --------------------

void handleCutter() {
  if (digitalRead(CUTTER_PIN) == LOW && debounce()) {
    armed = true;
    filamentEncoded = false;
    encodedSlot = -1;

    Serial.println("Cutter pressed → System armed");
    Serial.println("Pulling current filament (toolhead only)");
  }
}

void handleBed() {
  if (!armed) return;

  if (digitalRead(BED_PIN) == LOW && debounce()) {

    // First press → enter encoding
    if (!encoding && !filamentEncoded) {
      encoding = true;
      encoderValue = 0;
      Serial.println("Bed pressed → Encoding STARTED");
      return;
    }

    // Second press → exit encoding
    if (encoding) {
      encoding = false;
      filamentEncoded = true;
      encodedSlot = encoderValue;

      Serial.print("Bed pressed again → Encoding LOCKED, Slot ");
      Serial.println(encodedSlot);
      return;
    }
  }
}

void handleWiper() {

  // During encoding → update distance
  if (encoding && digitalRead(WIPER_PIN) == LOW && debounce()) {
    encoderValue++; // replace with VL53L0X later
    Serial.print("Wiper distance update → Encoder = ");
    Serial.println(encoderValue);
    return;
  }

  // After encoding → execute swap
  if (!encoding && filamentEncoded &&
      digitalRead(WIPER_PIN) == LOW && debounce()) {

    Serial.print("Wiper pressed → Executing swap to slot ");
    Serial.println(encodedSlot);

    Serial.println("Pulling current filament...");
    Serial.println("Pushing encoded filament...");
    Serial.println("Swap complete");

    // Reset
    armed = false;
    filamentEncoded = false;
    encodedSlot = -1;
  }
}


// -------------------- Utility --------------------

bool debounce() {
  if (millis() - lastEventTime > debounceMs) {
    lastEventTime = millis();
    return true;
  }
  return false;
}