// =====================================
// TEST VOIE D'EVITEMENT ISOLEE
// =====================================

#define ARRIVE_BTN 22
#define LIB_DIR_BTN 23
#define LIB_DEV_BTN 24
#define B2_OCC_BTN 25
#define B2_LIB_BTN 26

#define DIR_LED 30
#define DEV_LED 31

#define AIG_ENT_DIR 40
#define AIG_ENT_DEV 41
#define AIG_SORT_DIR 42
#define AIG_SORT_DEV 43

#define B2_LED 44

bool voieDir = false;
bool voieDev = false;
bool b2Occ = false;

bool prevArrive = HIGH;

enum Pref {DIRECTE, DEVIEE};
Pref prochainePref = DIRECTE;

void setup() {

  Serial.begin(115200);
  Serial.println("=== EVITEMENT ISOLE PRET ===");

  pinMode(ARRIVE_BTN, INPUT_PULLUP);
  pinMode(LIB_DIR_BTN, INPUT_PULLUP);
  pinMode(LIB_DEV_BTN, INPUT_PULLUP);
  pinMode(B2_OCC_BTN, INPUT_PULLUP);
  pinMode(B2_LIB_BTN, INPUT_PULLUP);

  pinMode(DIR_LED, OUTPUT);
  pinMode(DEV_LED, OUTPUT);

  pinMode(AIG_ENT_DIR, OUTPUT);
  pinMode(AIG_ENT_DEV, OUTPUT);
  pinMode(AIG_SORT_DIR, OUTPUT);
  pinMode(AIG_SORT_DEV, OUTPUT);

  pinMode(B2_LED, OUTPUT);
}

void loop() {

  readInputs();
  updateOutputs();
  delay(40);
}

// =========================

void readInputs() {

  bool arrive = digitalRead(ARRIVE_BTN);

  if(prevArrive == HIGH && arrive == LOW) {
    trainArrive();
  }

  prevArrive = arrive;

  if(!digitalRead(B2_OCC_BTN)) {
    b2Occ = true;
    Serial.println("B2 occupe");
  }

  if(!digitalRead(B2_LIB_BTN)) {
    b2Occ = false;
    Serial.println("B2 libre");
  }

  // Demande départ voie directe
  if(!digitalRead(LIB_DIR_BTN)) {
    if(!b2Occ && voieDir) {
      Serial.println("Depart voie directe autorise");
      voieDir = false;
    } else {
      Serial.println("Depart voie directe REFUSE (B2 occupe)");
    }
  }

  // Demande départ voie deviee
  if(!digitalRead(LIB_DEV_BTN)) {
    if(!b2Occ && voieDev) {
      Serial.println("Depart voie deviee autorise");
      voieDev = false;
    } else {
      Serial.println("Depart voie deviee REFUSE (B2 occupe)");
    }
  }
}

// =========================

void trainArrive() {

  Serial.println("Train arrive");

  if(!voieDir && !voieDev) {
    if(prochainePref == DIRECTE) {
      occuperDirecte();
      prochainePref = DEVIEE;
    } else {
      occuperDeviee();
      prochainePref = DIRECTE;
    }
  }
  else if(!voieDir) {
    occuperDirecte();
  }
  else if(!voieDev) {
    occuperDeviee();
  }
  else {
    Serial.println("Les deux voies sont occupees");
  }
}

// =========================

void departTrain() {

  if(b2Occ) return;

  if(voieDir) {
    Serial.println("Depart voie directe");
    voieDir = false;
  }
  else if(voieDev) {
    Serial.println("Depart voie deviee");
    voieDev = false;
  }
}

// =========================

void occuperDirecte() {
  voieDir = true;
  Serial.println("Train dirige vers voie DIRECTE");
}

void occuperDeviee() {
  voieDev = true;
  Serial.println("Train dirige vers voie DEVIEE");
}

// =========================

void updateOutputs() {

  digitalWrite(DIR_LED, voieDir);
  digitalWrite(DEV_LED, voieDev);

  digitalWrite(AIG_ENT_DIR, !voieDev);
  digitalWrite(AIG_ENT_DEV, voieDev);

  digitalWrite(AIG_SORT_DIR, !voieDev);
  digitalWrite(AIG_SORT_DEV, voieDev);

  digitalWrite(B2_LED, b2Occ);
}