#include <Stepper.h>
#include <Preferences.h>
#include <esp_sleep.h>

// --- Pin Definitions ---
#define PIN_IN1 0
#define PIN_IN2 1
#define PIN_IN3 3
#define PIN_IN4 10
#define PIN_LED 4
#define PIN_IR_BEAM 5
#define PIN_HOME_SENSOR 6
#define PIN_MOTOR_GATE 7

const int STEPS_PER_REV = 2038; 
Stepper carousel(STEPS_PER_REV, PIN_IN1, PIN_IN3, PIN_IN2, PIN_IN4);

Preferences nvs;
int currentSlot = 1;
bool isMoving = false;

// Time to sleep between doses (5 seconds for simulation)
#define TIME_TO_SLEEP_SECONDS 5 
#define uS_TO_S_FACTOR 1000000ULL 

void setup() {
  Serial.begin(115200);
  delay(2000); // Wait for USB CDC to connect
  
  Serial.println("\n--- SYSTEM WAKING UP ---");

  // Configure Pins
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_MOTOR_GATE, OUTPUT);
  pinMode(PIN_IR_BEAM, INPUT_PULLUP);
  pinMode(PIN_HOME_SENSOR, INPUT_PULLUP);

  digitalWrite(PIN_LED, LOW);
  digitalWrite(PIN_MOTOR_GATE, LOW); 

  nvs.begin("carousel", false); 
  isMoving = nvs.getBool("moving_flag", false); 
  currentSlot = nvs.getInt("current_slot", 1);  

  // --- WAKEUP LOGIC DIAGNOSTICS ---
  esp_sleep_wakeup_cause_t wakeup_reason = esp_sleep_get_wakeup_cause();

  if (wakeup_reason == ESP_SLEEP_WAKEUP_TIMER) {
    Serial.println("Wake Reason: TIMER. Initiating scheduled dispense.");
    
    if (isMoving) {
       Serial.println("Warning: Mid-move crash detected during last wake. Homing first.");
       homeCarousel();
    }
    dispenseNextDose();
    
  } else {
    Serial.println("Wake Reason: POWER ON / RESET. Running baseline setup.");
    if (isMoving || currentSlot == 1) {
      Serial.println("Forcing safe homing cycle.");
      homeCarousel();
    }
  }

  // --- GO TO SLEEP ---
  Serial.printf("Task complete. Going to Deep Sleep for %d seconds...\n", TIME_TO_SLEEP_SECONDS);
  
  Serial.flush(); 
  delay(100); 
  
  esp_sleep_enable_timer_wakeup(TIME_TO_SLEEP_SECONDS * uS_TO_S_FACTOR);
  esp_deep_sleep_start();
}

void loop() {
  // Deep sleep architecture abandons the standard loop!
}

void homeCarousel() {
  Serial.println("Energizing motor gate...");
  digitalWrite(PIN_MOTOR_GATE, HIGH);
  carousel.setSpeed(10); 
  
  Serial.println("Homing: Searching for Slot 1...");
  
  while(digitalRead(PIN_HOME_SENSOR) == HIGH) {
    carousel.step(1);
    delay(2); 
  }
  
  Serial.println("Homing Complete. Slot 1 established.");
  digitalWrite(PIN_MOTOR_GATE, LOW);
  
  nvs.putBool("moving_flag", false);
  nvs.putInt("current_slot", 1);
  currentSlot = 1;
}

void dispenseNextDose() {
  Serial.println("\n--- Scheduled Dispense Triggered ---");
  nvs.putBool("moving_flag", true);
  
  digitalWrite(PIN_MOTOR_GATE, HIGH);
  carousel.setSpeed(10);
  
  Serial.println("Motor: Rotating to next compartment...");
  for(int i = 0; i < 194; i++) {
    carousel.step(1);
    delay(2);
  }
  digitalWrite(PIN_MOTOR_GATE, LOW); 
  
  currentSlot++;
  if (currentSlot > 21) currentSlot = 1; 
  
  nvs.putInt("current_slot", currentSlot);
  nvs.putBool("moving_flag", false);
  
  waitForPatient(); 
}

void waitForPatient() {
  Serial.println("Waiting for patient... (Press & hold GREEN button for >500ms)");
  
  unsigned long waitStart = millis(); 
  unsigned long pulseStart = millis();
  int ledBrightness = 0;
  int fadeAmount = 5;
  
  unsigned long beamBrokenTime = 0;
  bool isBroken = false;

  const unsigned long ESCALATION_TIME = 10000; 
  const unsigned long TIMEOUT_TIME = 20000;    

  while (true) {
    unsigned long elapsedWait = millis() - waitStart;

    if (elapsedWait >= TIMEOUT_TIME) {
      Serial.println("TIMEOUT: Patient did not retrieve medication. Logging 'Missed Dose'.");
      analogWrite(PIN_LED, 0); 
      break; 
    }

    if (elapsedWait >= ESCALATION_TIME) {
      if (millis() - pulseStart > 150) { 
        ledBrightness = (ledBrightness == 0) ? 255 : 0; 
        analogWrite(PIN_LED, ledBrightness);
        pulseStart = millis();
      }
    } else {
      if (millis() - pulseStart > 30) {
        analogWrite(PIN_LED, ledBrightness);
        ledBrightness += fadeAmount;
        if (ledBrightness <= 0 || ledBrightness >= 255) {
          fadeAmount = -fadeAmount; 
        }
        pulseStart = millis();
      }
    }

    if (digitalRead(PIN_IR_BEAM) == LOW) {
      if (!isBroken) {
        isBroken = true;
        beamBrokenTime = millis(); 
      } else if (millis() - beamBrokenTime > 500) {
        Serial.println("SUCCESS: Medication retrieved!");
        analogWrite(PIN_LED, 0); 
        break; 
      }
    } else {
      isBroken = false; 
    }
    
    delay(10); 
  }
}