#include <Servo.h>

Servo myServo;

const int modeSwitchPin = 2; // Switch to select mode
const int redLed = 13;
const int greenLed = 12;

// --- SERVO VARIABLES ---
int angle = 0;
int step = 1;                 // Step size (Lower = Smoother)
unsigned long lastServoTime = 0;
const int servoInterval = 15; // Update every 15ms (approx 60 times per second)

// --- BLINK VARIABLES ---
int ledState = LOW;
unsigned long lastBlinkTime = 0;
const int blinkInterval = 200; // Blink speed for LEDs

void setup() {
  myServo.attach(9);
  pinMode(modeSwitchPin, INPUT);
  pinMode(redLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
  Serial.begin(9600);
}

void loop() {
  int mode = digitalRead(modeSwitchPin);

  if (mode == HIGH) {
    // === MODE 1: BLOCKING (The "Jerky" Mode) ===
    // This mode uses delay(), so the servo stops moving while the LED blinks.
    runBlockingMode();
  } 
  else {
    // === MODE 2: NON-BLOCKING (The "Smooth" Mode) ===
    // This mode uses millis(), so the servo moves fluidly while LEDs blink.
    runNonBlockingMode();
  }
}

// ---------------------------------------------------------
// MODE 1: The "Bad" Way (Jerky Movement)
// ---------------------------------------------------------
void runBlockingMode() {
  // 1. Move the servo slightly
  angle = angle + step;
  if (angle >= 180 || angle <= 0) step = -step;
  myServo.write(angle);

  // 2. Blink logic WITH DELAY
  // The servo has to WAIT for these delays to finish before it can move again.
  if (angle < 90) {
    digitalWrite(greenLed, LOW);
    digitalWrite(redLed, HIGH);
    delay(50);  // WAIT...
    digitalWrite(redLed, LOW);
    delay(50);  // WAIT...
  } else {
    digitalWrite(redLed, LOW);
    digitalWrite(greenLed, HIGH);
    delay(50);  // WAIT...
    digitalWrite(greenLed, LOW);
    delay(50);  // WAIT...
  }
}

// ---------------------------------------------------------
// MODE 2: The "Good" Way (Smooth Movement)
// ---------------------------------------------------------
void runNonBlockingMode() {
  unsigned long currentMillis = millis();

  // 1. SERVO TASK: Runs frequently (every 15ms)
  // This ensures the movement looks like a continuous sweep.
  if (currentMillis - lastServoTime >= servoInterval) {
    lastServoTime = currentMillis;
    
    angle = angle + step;
    if (angle >= 180 || angle <= 0) step = -step;
    myServo.write(angle);
  }

  // 2. LED BLINK TASK: Runs slowly (every 200ms)
  // This happens independently and does NOT stop the servo.
  if (currentMillis - lastBlinkTime >= blinkInterval) {
    lastBlinkTime = currentMillis;
    
    // Toggle the LED state (On -> Off, or Off -> On)
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }
  }

  // 3. Apply the state to the correct LED
  if (angle < 90) {
    digitalWrite(greenLed, LOW);     // Turn off Green
    digitalWrite(redLed, ledState);  // Blink Red
  } else {
    digitalWrite(redLed, LOW);       // Turn off Red
    digitalWrite(greenLed, ledState);// Blink Green
  }
}