#include <ESP32Servo.h>

#define SERVO_PIN 26  // Pin connected to the servo
#define STAGGER_PIN 32  // Pin connected to the button to control staggering
#define LED_PIN 22  // Pin connected to the button to control oscillation
#define LEVEL_PIN 4  // Pin connected to the button to change LED light level

Servo myServo;  // Create a servo object

int servoPosition = 0;  // Initial position of the servo
bool isStaggering = false;  // Flag to indicate whether the servo is staggering or not
bool isOscillating = false;  // Flag to indicate whether the servo is oscillating or not
int fanSpeed = 0; //Initial fan speed
const int fanSpeedLevels[] = {51, 102, 153, 204, 255}; // PWM values for each fan speed level
int currentSpeedLevel = 0; // Index of the current fan speed level

// Interrupt service routine to toggle staggering
void IRAM_ATTR toggleStaggering() {
  isStaggering = !isStaggering;  // Toggle staggering
  isOscillating = !isOscillating;  // Toggle oscillation
}

// // Interrupt service routine to toggle oscillation
// void IRAM_ATTR toggleOscillation() {
//   isOscillating = !isOscillating;  // Toggle oscillation
// }

// Interrupt service routine to change fan speed level
void IRAM_ATTR changeSpeedLevel() {
  currentSpeedLevel = (currentSpeedLevel + 1) % 5; // Change to the next speed level
  fanSpeed = fanSpeedLevels[currentSpeedLevel];
}

void setup() {
  pinMode(STAGGER_PIN, INPUT);
  pinMode(LEVEL_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(SERVO_PIN, OUTPUT);//
  attachInterrupt(digitalPinToInterrupt(STAGGER_PIN), toggleStaggering, FALLING); // Attach interrupt to toggle staggering
  //attachInterrupt(digitalPinToInterrupt(OSCILLATE_PIN), toggleOscillation, RISING); // Attach interrupt to toggle oscillation
  attachInterrupt(digitalPinToInterrupt(LEVEL_PIN), changeSpeedLevel, RISING); // Attach interrupt to change speed level
  myServo.attach(SERVO_PIN);  // Attach the servo to the pin
}

void loop() {
  // Update servo position if it's staggering
  if (isStaggering) {
    servoPosition += 1;  // Increase position
    if (servoPosition >= 180) {
      isStaggering = false;  // Stop staggering at max position
    }
  }
  // Update servo position if it's oscillating
  else if (isOscillating) {
    servoPosition += 1;  // Increase position
    if (servoPosition >= 180) {
      servoPosition = 0;  // Reset position at max position
    }
  }
  // Set servo position if it's not staggering or oscillating
  myServo.write(servoPosition);  // Set servo position
  // Set LED brightness according to current fan speed level
  analogWrite(LED_PIN, fanSpeedLevels[currentSpeedLevel]);
  // Wait for a short period before looping again
  delay(10);
}