#include <Servo.h>

Servo servo1;
Servo servo2;

// --- Instellingen ---
int standOpen = 0;    
int standDicht = 90;   
int traagheid = 30;    
int wachttijdLeds = 1000; // 3000 milliseconden = 3 seconden vertraging
// --------------------

int huidigePos = standOpen;
bool bomenMoetenDicht = false;
unsigned long startTijdLeds = 0; 

void setup() {
  servo1.attach(9);
  servo2.attach(10);
  pinMode(2, INPUT_PULLUP); // Reed Dicht
  pinMode(3, INPUT_PULLUP); // Reed Open
  pinMode(5, OUTPUT);       
  pinMode(6, OUTPUT);       
  
  servo1.write(standOpen);
  servo2.write(standOpen);
}

void loop() {
  // Check reedcontacten
  if (digitalRead(2) == LOW && !bomenMoetenDicht) {
    bomenMoetenDicht = true;
    startTijdLeds = millis(); // Onthoud wanneer de lichten aangingen
  }
  
  if (digitalRead(3) == LOW) {
    bomenMoetenDicht = false;
  }

  // Knipper-logica (LEDs aan als bomen niet open zijn OF als ze moeten gaan sluiten)
  if (bomenMoetenDicht || huidigePos > standOpen) {
    digitalWrite(5, (millis() / 500) % 2);
    digitalWrite(6, !((millis() / 500) % 2));
  } else {
    digitalWrite(5, LOW);
    digitalWrite(6, LOW);
  }

  // Bewegings-logica met vertraging
  if (bomenMoetenDicht) {
    // Alleen zakken als de wachttijd voorbij is
    if (millis() - startTijdLeds > wachttijdLeds) {
      if (huidigePos < standDicht) {
        huidigePos++;
        stuurServos();
      }
    }
  } else {
    // Omhoog gaan gebeurt altijd direct
    if (huidigePos > standOpen) {
      huidigePos--;
      stuurServos();
    }
  }
}

void stuurServos() {
  servo1.write(huidigePos);
  servo2.write(huidigePos);
  delay(traagheid);
}