/*
 * Flower Timer Vase - Continuous Rotation Servo
 * 
 * FeeTech FS5103R Control using Servo.write():
 * - write(90)  = STOP
 * - write(180) = Full speed one direction
 * - write(0)   = Full speed other direction
 * 
 * WIRING:
 * - Servo: Orange->D9, Red->VBUS(5V), Brown->GND
 * - Ultrasonic: VCC->3V3, TRIG->D10, ECHO->D11, GND->GND
 * - Switch: Pin1->GND, Pin2->D7
 * - Buzzer: Leg1->D8, Leg2->GND
 */

#include <ESP32Servo.h>

// ============== PINS ==============
const int SERVO_PIN = 9;
const int ULTRASONIC_TRIG_PIN = 10;
const int ULTRASONIC_ECHO_PIN = 11;
const int TOGGLE_SWITCH_PIN = 7;
const int BUZZER_PIN = 8;

// ============== MOTOR SPEEDS (0-180 scale, 90=stop) ==============
const int MOTOR_STOP = 90;
const int MOTOR_EXTEND_SLOW = 100;    // Slow extend (closer to 90 = slower)
const int MOTOR_EXTEND_FAST = 130;    // Fast extend
const int MOTOR_RETRACT_FAST = 50;    // Fast retract (below 90)

// Set to false if motor direction is reversed
const bool EXTEND_IS_FORWARD = true;

// ============== STEP TRACKING ==============
const int TOTAL_STEPS = 100;              // 100 steps for full 5 circles
const int STEP_DURATION_SLOW_MS = 50;     // 50ms per step slow = 5 sec total
const int STEP_DURATION_FAST_MS = 40;     // 40ms per step fast = 4 sec total

int currentStep = 0;  // 0=retracted, TOTAL_STEPS=extended

// ============== ULTRASONIC ==============
const float LIFT_THRESHOLD_CM = 3.0;
const int SENSOR_TIMEOUT_US = 30000;

// ============== SWITCH ==============
const bool DISPLAY_MODE_SWITCH_STATE = HIGH;

// ============== STATE ==============
Servo myServo;

enum Mode { TIMER_MODE, DISPLAY_MODE };
enum TimerState { WAITING_FOR_SURFACE, TIMER_RUNNING, TIMER_COMPLETE };

Mode currentMode = TIMER_MODE;
TimerState timerState = WAITING_FOR_SURFACE;
unsigned long lastStatusTime = 0;

// ============== FUNCTION PROTOTYPES ==============
void motorStop();
void motorExtend(bool fast);
void motorRetract();
void moveOneStepForward(bool fast);
void moveOneStepBackward();
bool checkIfLifted();
Mode readSwitchMode();
void playCompletionSound();
void playTimerModeSound();
void playDisplayModeSound();

// ============== SETUP ==============
void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println();
  Serial.println("================================");
  Serial.println("   Flower Timer Vase Starting   ");
  Serial.println("================================");
  
  // Init pins
  pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
  pinMode(ULTRASONIC_ECHO_PIN, INPUT);
  pinMode(TOGGLE_SWITCH_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);
  
  // Init servo
  myServo.attach(SERVO_PIN);
  motorStop();
  delay(200);
  
  // ===== MOTOR TEST AT STARTUP =====
  Serial.println();
  Serial.println(">> MOTOR TEST: Forward 1 second...");
  motorExtend(false);
  delay(1000);
  motorStop();
  delay(500);
  
  Serial.println(">> MOTOR TEST: Backward 1 second...");
  motorRetract();
  delay(1000);
  motorStop();
  delay(500);
  
  Serial.println(">> MOTOR TEST: Complete!");
  Serial.println();
  // ===== END MOTOR TEST =====
  
  // Test beep
  tone(BUZZER_PIN, 1000, 200);
  delay(250);
  noTone(BUZZER_PIN);
  
  // Start at step 0 (fully retracted)
  currentStep = 0;
  
  // Read initial mode
  currentMode = readSwitchMode();
  Serial.print("Initial mode: ");
  Serial.println(currentMode == DISPLAY_MODE ? "DISPLAY" : "TIMER");
  
  // Initialize based on mode
  if (currentMode == DISPLAY_MODE) {
    // Display mode on startup - play upbeat sound first
    Serial.println("Playing display mode sound...");
    playDisplayModeSound();
    
    // Then extend fully fast
    Serial.println("Display mode - extending fully...");
    while (currentStep < TOTAL_STEPS) {
      moveOneStepForward(true);
    }
    motorStop();
    Serial.println("Fully extended for display.");
    
  } else {
    // Timer mode - play alternating sound first
    Serial.println("Playing timer mode sound...");
    playTimerModeSound();
    
    // Then check if on surface
    bool lifted = checkIfLifted();
    Serial.print("Lifted: ");
    Serial.println(lifted ? "YES" : "NO");
    
    if (lifted) {
      timerState = WAITING_FOR_SURFACE;
      Serial.println("Waiting for surface...");
    } else {
      timerState = TIMER_RUNNING;
      Serial.println("Starting timer!");
    }
  }
  
  motorStop();
  Serial.println();
  Serial.println("Setup complete. Entering main loop.");
  Serial.println("================================");
}

// ============== MAIN LOOP ==============
void loop() {
  Mode switchMode = readSwitchMode();
  bool lifted = checkIfLifted();
  
  // Status print every second
  if (millis() - lastStatusTime >= 1000) {
    lastStatusTime = millis();
    Serial.print("[");
    Serial.print(currentMode == DISPLAY_MODE ? "DISPLAY" : "TIMER");
    if (currentMode == TIMER_MODE) {
      Serial.print("/");
      if (timerState == WAITING_FOR_SURFACE) Serial.print("WAIT");
      else if (timerState == TIMER_RUNNING) Serial.print("RUN");
      else Serial.print("DONE");
    }
    Serial.print("] Step:");
    Serial.print(currentStep);
    Serial.print("/");
    Serial.print(TOTAL_STEPS);
    Serial.print(" Lift:");
    Serial.println(lifted ? "Y" : "N");
  }
  
  // === MODE SWITCH (highest priority) ===
  if (switchMode != currentMode) {
    Serial.println();
    Serial.print(">>> MODE CHANGE: ");
    Serial.print(currentMode == DISPLAY_MODE ? "DISPLAY" : "TIMER");
    Serial.print(" -> ");
    Serial.println(switchMode == DISPLAY_MODE ? "DISPLAY" : "TIMER");
    
    motorStop();
    
    if (switchMode == DISPLAY_MODE) {
      // Switch to DISPLAY - play upbeat sound first
      Serial.println("Playing display mode sound...");
      playDisplayModeSound();
      
      // Then extend to full
      Serial.println("Extending to full...");
      while (currentStep < TOTAL_STEPS) {
        moveOneStepForward(true);
      }
      motorStop();
      Serial.println("Extended.");
      
    } else {
      // Switch to TIMER - play alternating sound first
      Serial.println("Playing timer mode sound...");
      playTimerModeSound();
      
      // Then retract fully
      Serial.println("Retracting to zero...");
      while (currentStep > 0) {
        moveOneStepBackward();
      }
      motorStop();
      Serial.println("Retracted.");
      
      // Check surface
      lifted = checkIfLifted();
      if (lifted) {
        timerState = WAITING_FOR_SURFACE;
        Serial.println("In air - waiting for surface...");
      } else {
        timerState = TIMER_RUNNING;
        Serial.println("On surface - starting timer!");
      }
    }
    
    currentMode = switchMode;
    return;
  }
  
  // === DISPLAY MODE ===
  if (currentMode == DISPLAY_MODE) {
    motorStop();
    // Do nothing else
    delay(50);
    return;
  }
  
  // === TIMER MODE ===
  switch (timerState) {
    
    case WAITING_FOR_SURFACE:
      motorStop();
      if (!lifted) {
        Serial.println(">>> Placed on surface - STARTING TIMER!");
        timerState = TIMER_RUNNING;
        currentStep = 0;
      }
      break;
      
    case TIMER_RUNNING:
      // Check if lifted - RESET
      if (lifted) {
        Serial.println(">>> LIFTED - RESETTING!");
        motorStop();
        
        Serial.print("Retracting ");
        Serial.print(currentStep);
        Serial.println(" steps...");
        
        while (currentStep > 0) {
          moveOneStepBackward();
          // Check mode switch during retract
          if (readSwitchMode() != currentMode) {
            return;  // Mode changed, exit and let loop handle it
          }
        }
        motorStop();
        
        timerState = WAITING_FOR_SURFACE;
        Serial.println("Reset complete. Waiting for surface...");
        break;
      }
      
      // Check if complete
      if (currentStep >= TOTAL_STEPS) {
        motorStop();
        Serial.println(">>> TIMER COMPLETE!");
        playCompletionSound();
        timerState = TIMER_COMPLETE;
        Serial.println("Waiting for lift to reset...");
        break;
      }
      
      // Move one step forward (slow)
      moveOneStepForward(false);
      break;
      
    case TIMER_COMPLETE:
      motorStop();
      if (lifted) {
        Serial.println(">>> Lifted after complete - RESETTING!");
        
        while (currentStep > 0) {
          moveOneStepBackward();
          if (readSwitchMode() != currentMode) {
            return;
          }
        }
        motorStop();
        
        timerState = WAITING_FOR_SURFACE;
        Serial.println("Reset. Waiting for surface...");
      }
      break;
  }
  
  delay(10);
}

// ============== MOTOR CONTROL ==============
void motorStop() {
  myServo.write(MOTOR_STOP);
}

void motorExtend(bool fast) {
  int speed = fast ? MOTOR_EXTEND_FAST : MOTOR_EXTEND_SLOW;
  if (!EXTEND_IS_FORWARD) {
    speed = 180 - speed;  // Reverse direction
  }
  myServo.write(speed);
}

void motorRetract() {
  int speed = MOTOR_RETRACT_FAST;
  if (!EXTEND_IS_FORWARD) {
    speed = 180 - speed;  // Reverse direction
  }
  myServo.write(speed);
}

void moveOneStepForward(bool fast) {
  if (currentStep >= TOTAL_STEPS) {
    motorStop();
    return;
  }
  
  motorExtend(fast);
  delay(fast ? STEP_DURATION_FAST_MS : STEP_DURATION_SLOW_MS);
  motorStop();
  currentStep++;
  delay(5);  // Brief pause between steps
}

void moveOneStepBackward() {
  if (currentStep <= 0) {
    motorStop();
    return;
  }
  
  motorRetract();
  delay(STEP_DURATION_FAST_MS);
  motorStop();
  currentStep--;
  delay(5);
}

// ============== SOUND ==============
void playCompletionSound() {
  noTone(BUZZER_PIN);
  delay(50);
  
  tone(BUZZER_PIN, 523, 150);  // C5
  delay(170);
  tone(BUZZER_PIN, 587, 150);  // D5
  delay(170);
  tone(BUZZER_PIN, 659, 150);  // E5
  delay(170);
  tone(BUZZER_PIN, 784, 150);  // G5
  delay(170);
  tone(BUZZER_PIN, 880, 200);  // A5
  delay(220);
  tone(BUZZER_PIN, 1047, 400); // C6
  delay(450);
  
  noTone(BUZZER_PIN);
}

void playTimerModeSound() {
  // Alternating two tones for 1 second
  // tone1 -> tone2 -> tone1 -> tone2 -> tone1 -> tone2 -> tone1 -> tone2
  // 8 tones x 125ms = 1000ms = 1 second
  noTone(BUZZER_PIN);
  delay(20);
  
  for (int i = 0; i < 8; i++) {
    if (i % 2 == 0) {
      tone(BUZZER_PIN, 600, 115);   // First tone
    } else {
      tone(BUZZER_PIN, 800, 115);   // Second tone
    }
    delay(125);
  }
  
  noTone(BUZZER_PIN);
}

void playDisplayModeSound() {
  // Upbeat ascending sound for 1 second
  noTone(BUZZER_PIN);
  delay(20);
  
  // Quick ascending notes - cheerful and upbeat
  tone(BUZZER_PIN, 523, 80);   // C5
  delay(100);
  tone(BUZZER_PIN, 659, 80);   // E5
  delay(100);
  tone(BUZZER_PIN, 784, 80);   // G5
  delay(100);
  tone(BUZZER_PIN, 1047, 80);  // C6
  delay(100);
  tone(BUZZER_PIN, 784, 80);   // G5
  delay(100);
  tone(BUZZER_PIN, 1047, 80);  // C6
  delay(100);
  tone(BUZZER_PIN, 1319, 80);  // E6
  delay(100);
  tone(BUZZER_PIN, 1568, 200); // G6 (longer final note)
  delay(220);
  
  noTone(BUZZER_PIN);
}

// ============== ULTRASONIC ==============
float readDistance() {
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  
  long duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH, SENSOR_TIMEOUT_US);
  
  if (duration == 0) {
    return -1.0;
  }
  
  return (duration * 0.0343) / 2.0;
}

bool checkIfLifted() {
  int validCount = 0;
  int liftedCount = 0;
  
  for (int i = 0; i < 3; i++) {
    float dist = readDistance();
    if (dist > 0) {
      validCount++;
      if (dist > LIFT_THRESHOLD_CM) {
        liftedCount++;
      }
    }
    delay(5);
  }
  
  if (validCount == 0) {
    return false;  // No valid = assume on surface
  }
  
  return (liftedCount > validCount / 2);
}

// ============== SWITCH ==============
Mode readSwitchMode() {
  bool state = digitalRead(TOGGLE_SWITCH_PIN);
  return (state == DISPLAY_MODE_SWITCH_STATE) ? DISPLAY_MODE : TIMER_MODE;
}