#include <LiquidCrystal.h>
#include <Servo.h>

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Servo myServo;

const int sensorPin1 = A0; 
const int sensorPin2 = A1; 
const int servoPin = 9;   

float baseA0 = 0, baseA1 = 0;

int deviationThreshold = 25; 
int contactThreshold = 18; 

unsigned long lastLCDUpdateTime = 0; 
unsigned long pullStartTime = 0;
unsigned long pullDuration = 0; 
unsigned long messageStartTime = 0; 
unsigned long sleepStartTime = 0; 
unsigned long rejectionStartTime = 0; 

int cycleCount = 0; 
const int maxCycles = 3; 

const int stopUs = 1495;    
const int pullSpeed = 1560;    
const int releaseSpeed = 1440; 

int currentState = 0; 

void setup() {
  Serial.begin(9600); 
  myServo.attach(servoPin);
  myServo.writeMicroseconds(stopUs); 
  
  lcd.begin(16, 2);
  lcd.print("System Ready");
  
  delay(3000); 
  
  lcd.clear();
  lcd.print("Calibrating...");
  
  long sumA0 = 0, sumA1 = 0;
  for (int i = 0; i < 50; i++) {
    sumA0 += analogRead(sensorPin1);
    sumA1 += analogRead(sensorPin2);
    delay(20);
  }
  baseA0 = (float)sumA0 / 50.0;
  baseA1 = (float)sumA1 / 50.0;
  
  myServo.writeMicroseconds(stopUs);
  lcd.clear();

  Serial.println("--- LOG START ---");
  Serial.print("Baselines set -> A0: "); Serial.print(baseA0);
  Serial.print(" | A1: "); Serial.println(baseA1);
}

void loop() {
  int valA0 = analogRead(sensorPin1);
  int valA1 = analogRead(sensorPin2);

  if (currentState == 0 || currentState == 5) {
    baseA0 = (baseA0 * 0.99) + (valA0 * 0.01);
    baseA1 = (baseA1 * 0.99) + (valA1 * 0.01);
  }

  int deltaA0 = abs(valA0 - (int)baseA0);
  int deltaA1 = abs(valA1 - (int)baseA1);

  // --- LCD Logic ---
  if (millis() - lastLCDUpdateTime >= 250) {
    lastLCDUpdateTime = millis();
    lcd.setCursor(0, 0);
    lcd.print("A0D:"); lcd.print(deltaA0);
    lcd.print(" A1R:"); lcd.print(valA1); 
    lcd.print("    "); 
    
    lcd.setCursor(0, 1);
    if (currentState == 5) {
      if (deltaA0 >= deviationThreshold || (rejectionStartTime > 0 && millis() - rejectionStartTime < 3000)) {
        lcd.print("no, later       "); 
      } else {
        lcd.print("zzz...          "); 
      }
    }
    else if (currentState == 2 || (messageStartTime > 0 && millis() - messageStartTime < 3000)) {
       lcd.print(" woo~, ah~       "); 
    } 
    else {
      if (currentState == 0) lcd.print("Status: Idle     ");
      else if (currentState == 1) lcd.print("Status: Pulling  ");
      else if (currentState == 3) lcd.print("Status: Releasing"); 
    }
  }

  // --- State Logic ---

  // State 0: Initial Touch
  if (currentState == 0 && deltaA0 >= deviationThreshold) {
    Serial.println("\n[EVENT] START TRIGGER (Leaf 1 Touched)");
    Serial.print("Cycle: "); Serial.println(cycleCount + 1);
    Serial.print("Leaf 1 -> Raw: "); Serial.print(valA0); Serial.print(" | Delta: "); Serial.println(deltaA0);
    Serial.print("Leaf 2 -> Raw: "); Serial.print(valA1); Serial.print(" | Delta: "); Serial.println(deltaA1);
    
    cycleCount = 0; 
    currentState = 1;
    pullStartTime = millis();
    myServo.writeMicroseconds(pullSpeed);
  }

  // State 1: Pulling
  if (currentState == 1) {
    if (deltaA1 >= contactThreshold) {
      Serial.println("[EVENT] CONTACT MADE (Leaf 2 Touched)");
      Serial.print("Cycle: "); Serial.println(cycleCount + 1);
      Serial.print("Leaf 1 -> Raw: "); Serial.print(valA0); Serial.print(" | Delta: "); Serial.println(deltaA0);
      Serial.print("Leaf 2 -> Raw: "); Serial.print(valA1); Serial.print(" | Delta: "); Serial.println(deltaA1);
      
      pullDuration = millis() - pullStartTime; 
      myServo.writeMicroseconds(stopUs);
      messageStartTime = millis(); 
      currentState = 2;
    }
  }

  // State 2: Wait for Leaf 2 Release
  if (currentState == 2 && deltaA1 < (contactThreshold - 15)) {
    currentState = 3; 
    pullStartTime = millis(); 
    myServo.writeMicroseconds(releaseSpeed);
  }

  // State 3: Releasing & Auto-Cycling
  if (currentState == 3) {
    if (millis() - pullStartTime >= pullDuration) {
      myServo.writeMicroseconds(stopUs);
      cycleCount++; 
      
      if (cycleCount >= maxCycles) {
        Serial.println("\n[STATE] CYCLES COMPLETE -> Sleep Mode");
        Serial.print("Final Leaf 1 -> Raw: "); Serial.print(valA0); Serial.print(" | Delta: "); Serial.println(deltaA0);
        Serial.print("Final Leaf 2 -> Raw: "); Serial.print(valA1); Serial.print(" | Delta: "); Serial.println(deltaA1);
        
        currentState = 5; 
        sleepStartTime = millis();
        rejectionStartTime = 0;
      } else {
        messageStartTime = 0; 
        delay(500); 
        currentState = 1; 
        pullStartTime = millis();
        myServo.writeMicroseconds(pullSpeed);
      }
    }
  }

  // State 5: Tired / Sleep
  if (currentState == 5) {
    if (deltaA0 >= deviationThreshold) {
      Serial.println("[REJECTION] Interruption Attempted");
      Serial.print("Leaf 1 -> Raw: "); Serial.print(valA0); Serial.print(" | Delta: "); Serial.println(deltaA0);
      Serial.print("Leaf 2 -> Raw: "); Serial.print(valA1); Serial.print(" | Delta: "); Serial.println(deltaA1);
      
      sleepStartTime = millis();
      rejectionStartTime = millis();
    }
    if (millis() - sleepStartTime >= 15000) {
      Serial.println("[STATE] SLEEP FINISHED -> Ready for Idle");
      currentState = 0; 
      messageStartTime = 0;
      rejectionStartTime = 0;
      cycleCount = 0; 
    }
  }
}