#include <Servo.h>

// --- PIN DEFINITIES ---
const int pinRelais = 13;

// Overweg 1 (Spoor Rood)
const int ow1_dicht = 2; const int ow1_open = 3;
const int ow1_led1 = 5;  const int ow1_led2 = 6;
const int ow1_s1_pin = 9; const int ow1_s2_pin = 10;

// Overweg 2
const int ow2_sensor_D4 = 4;   // Reedcontact (sluit OW2)
const int ow2_paneelknop = 7; // NIEUW: Handknop op paneel (sluit OW2)
const int ow2_led1 = A0; const int ow2_led2 = A1;
const int ow2_s1_pin = 11; const int ow2_s2_pin = 12;

// Master (Blauw)
const int master_dicht = 8; const int master_open = A2;

// --- INSTELLINGEN ---
const int openPos = 90; const int dichtPos = 0;
const int wachttijd = 2000; 
const int snelheid = 25;

Servo s1, s2, s3, s4;
int pos1 = openPos; int pos2 = openPos;
bool ow1_actief = false; bool ow2_actief = false;
unsigned long t1 = 0; unsigned long t2 = 0;

void setup() {
  pinMode(pinRelais, OUTPUT);
  pinMode(ow1_led1, OUTPUT); pinMode(ow1_led2, OUTPUT);
  pinMode(ow2_led1, OUTPUT); pinMode(ow2_led2, OUTPUT);
  
  pinMode(ow1_dicht, INPUT_PULLUP); pinMode(ow1_open, INPUT_PULLUP);
  pinMode(ow2_sensor_D4, INPUT_PULLUP); 
  pinMode(ow2_paneelknop, INPUT_PULLUP); // De extra knop
  pinMode(master_dicht, INPUT_PULLUP); pinMode(master_open, INPUT_PULLUP);

  s1.attach(ow1_s1_pin); s2.attach(ow1_s2_pin);
  s3.attach(ow2_s1_pin); s4.attach(ow2_s2_pin);
  s1.write(openPos); s2.write(openPos); s3.write(openPos); s4.write(openPos);
}

void loop() {
  // --- OW1 LOGICA ---
  if (digitalRead(ow1_dicht) == LOW && !ow1_actief) { ow1_actief = true; t1 = millis(); }
  if (digitalRead(ow1_open) == LOW) { ow1_actief = false; }

  // --- MASTER LOGICA (Alles dicht of alles open) ---
  if (digitalRead(master_dicht) == LOW) {
    if(!ow1_actief) { ow1_actief = true; t1 = millis(); }
    if(!ow2_actief) { ow2_actief = true; t2 = millis(); }
  }
  if (digitalRead(master_open) == LOW) { ow1_actief = false; ow2_actief = false; }

  // --- OW2 SPECIFIEKE LOGICA (D4 reedcontact OF Paneelknop D7) ---
  if ((digitalRead(ow2_sensor_D4) == LOW || digitalRead(ow2_paneelknop) == LOW) && !ow2_actief) {
    ow2_actief = true;
    t2 = millis();
  }

  // --- UITVOERING ---
  digitalWrite(pinRelais, (ow1_actief || ow2_actief || pos1 < openPos || pos2 < openPos));
  handleOverweg(ow1_actief, t1, pos1, s1, s2, ow1_led1, ow1_led2);
  handleOverweg(ow2_actief, t2, pos2, s3, s4, ow2_led1, ow2_led2);
  delay(snelheid);
}

void handleOverweg(bool actief, unsigned long startT, int &p, Servo &srv1, Servo &srv2, int l1, int l2) {
  if (actief || p < openPos) {
    bool flash = (millis() / 500) % 2;
    digitalWrite(l1, flash); digitalWrite(l2, !flash);
  } else {
    digitalWrite(l1, LOW); digitalWrite(l2, LOW);
  }
  if (actief && (millis() - startT > wachttijd)) {
    if (p > dichtPos) { p--; srv1.write(p); srv2.write(p); }
  } else if (!actief) {
    if (p < openPos) { p++; srv1.write(p); srv2.write(p); }
  }
}