// =====================================
// BLOCK 5 CANTONS - SIGNAL PROTEGE SON PROPRE CANTON
// =====================================

#define NB 5

int pinZS[NB]  = {22,24,26,28,30};
int pinLIB[NB] = {23,25,27,29,31};

int pinPWR[NB] = {32,33,34,35,36};

int pinR[NB] = {40,43,46,49,52};
int pinY[NB] = {41,44,47,50,53};
int pinG[NB] = {42,45,48,51,37};

// ====== Etats ======

bool occ[NB] = {false};
bool prevZS[NB] = {HIGH};
bool powerState[NB] = {true};

enum Signal {VERT, JAUNE, ROUGE};
Signal sig[NB];

void setup() {

  Serial.begin(115200);
  Serial.println("=== BLOCK 5 CANTONS PRET ===");

  for(int i=0;i<NB;i++){
    pinMode(pinZS[i], INPUT_PULLUP);
    pinMode(pinLIB[i], INPUT_PULLUP);
    pinMode(pinPWR[i], OUTPUT);
    pinMode(pinR[i], OUTPUT);
    pinMode(pinY[i], OUTPUT);
    pinMode(pinG[i], OUTPUT);
  }
}

void loop() {

  readInputs();
  computeSignals();
  computePower();
  updateOutputs();

  delay(40);
}

// ===============================
// Lecture ZS + LIB
// ===============================

void readInputs(){

  for(int i=0;i<NB;i++){

    bool zs = digitalRead(pinZS[i]);

    // Front montant ZS (libre -> occupé)
    if(prevZS[i] == HIGH && zs == LOW){
      occ[i] = true;
      Serial.print("Train entre B");
      Serial.println(i+1);
    }

    prevZS[i] = zs;

    if(!digitalRead(pinLIB[i])){
      occ[i] = false;
      Serial.print("B");
      Serial.print(i+1);
      Serial.println(" libere");
    }
  }
}

// ===============================
// Calcul signaux (protege lui-meme)
// ===============================

void computeSignals(){

  for(int i=0;i<NB;i++){

    int next = (i+1) % NB;

    if(occ[i])
      sig[i] = ROUGE;
    else if(occ[next])
      sig[i] = JAUNE;
    else
      sig[i] = VERT;
  }
}

// ===============================
// Gestion coupure réaliste
// ===============================

void computePower(){

  for(int i=0;i<NB;i++){

    int next = (i+1) % NB;

    // Coupure uniquement sur front ZS et si canton suivant occupé
    if(prevZS[i] == LOW && occ[next] && powerState[i]){
      powerState[i] = false;
      Serial.print(">>> Coupure B");
      Serial.println(i+1);
    }

    // Réalimentation si canton suivant libre
    if(!occ[next] && !powerState[i]){
      powerState[i] = true;
      Serial.print(">>> Realimentation B");
      Serial.println(i+1);
    }
  }
}

// ===============================
// Sorties
// ===============================

void updateSignal(int i){
  digitalWrite(pinR[i], sig[i] == ROUGE);
  digitalWrite(pinY[i], sig[i] == JAUNE);
  digitalWrite(pinG[i], sig[i] == VERT);
}

void updateOutputs(){

  for(int i=0;i<NB;i++){
    digitalWrite(pinPWR[i], powerState[i]);
    updateSignal(i);
  }
}