// Semáforo veicular e pedestre unificado com fila de comandos via Serial

// Pinos dos semáforos dos carros
const byte sgreen[]  = {23, 29, 35, 41};
const byte syellow[] = {25, 31, 37, 43};
const byte sred[]    = {27, 33, 39, 45};
// Pinos dos botões de pedestre
const byte botao[]   = {3, 2, 20, 21};
// Pinos dos semáforos dos pedestres
int pgreen[] = {A8, A10, A12, A14};
int pred[]   = {A9, A11, A13, A15};

// Fila de comandos via Serial
const int MAX_QUEUE = 8;
int queueCmd[MAX_QUEUE];
int queueStart = 0;
int queueEnd   = 0;
int queueSize  = 0;

// Estados de pedestre
typedef enum { IDLE, REQUESTED, RUNNING } PedState;
volatile PedState pedState = IDLE;
volatile int ledIndex      = -1;
unsigned long ledStartTime = 0;
const unsigned long tempoPedestre = 3000; // ms de travessia

// Controle do semáforo veicular
int lastsemaforo;
#define norte  1
#define leste  2
#define sul    3
#define oeste  4
int direcaoAtual   = norte;
unsigned long ultimoTroca    = 0;
unsigned long intervalo      = 10000; // 10s por direção
bool manual           = false;

// Debounce para pedestre
volatile unsigned long lastInterruptTime[4] = {0,0,0,0};
const unsigned long debounceDelay = 200;

// Funções de interrupção de pedestre
void pedestre0() {
  unsigned long now = millis();
  if (now - lastInterruptTime[0] > debounceDelay) {
    pedState = REQUESTED;
    ledIndex = 0;
    Serial.println("Pedestre botao 0 requisitado");
    lastInterruptTime[0] = now;
  }
}
void pedestre1() {
  unsigned long now = millis();
  if (now - lastInterruptTime[1] > debounceDelay) {
    pedState = REQUESTED;
    ledIndex = 1;
    Serial.println("Pedestre botao 1 requisitado");
    lastInterruptTime[1] = now;
  }
}
void pedestre2() {
  unsigned long now = millis();
  if (now - lastInterruptTime[2] > debounceDelay) {
    pedState = REQUESTED;
    ledIndex = 2;
    Serial.println("Pedestre botao 2 requisitado");
    lastInterruptTime[2] = now;
  }
}
void pedestre3() {
  unsigned long now = millis();
  if (now - lastInterruptTime[3] > debounceDelay) {
    pedState = REQUESTED;
    ledIndex = 3;
    Serial.println("Pedestre botao 3 requisitado");
    lastInterruptTime[3] = now;
  }
}

// Configura todos os semáforos veiculares em vermelho
void todosSemaforosVermelhos() {
  semaforo(5);
}

// Restaura o semáforo veicular à direção atual
void restaurarSemaforoAnterior() {
  semaforo(direcaoAtual);
}

// Ajusta semáforo veicular para a direção slig
void semaforo(int slig) {
  if (slig == 5) {
    int lastIndex = lastsemaforo - 1;
    if (lastIndex >= 0 && lastIndex < 4) {
      digitalWrite(sgreen[lastIndex], LOW);
      digitalWrite(syellow[lastIndex], HIGH);
      delay(2000);
      digitalWrite(syellow[lastIndex], LOW);
      digitalWrite(sred[lastIndex], HIGH);
    }
    for (int i = 0; i < 4; i++) {
      digitalWrite(sgreen[i], LOW);
      digitalWrite(syellow[i], LOW);
      digitalWrite(sred[i], LOW);
    }
    lastsemaforo = 0;
    return;
  }

  if (lastsemaforo != slig) {
    int lastIndex = lastsemaforo - 1;
    if (lastIndex >= 0 && lastIndex < 4) {
      digitalWrite(sgreen[lastIndex], LOW);
      digitalWrite(syellow[lastIndex], HIGH);
      delay(2000);
      digitalWrite(syellow[lastIndex], LOW);
      digitalWrite(sred[lastIndex], HIGH);
    }
  }
  for (int i = 0; i < 4; i++) {
    if (i != (slig - 1)) {
      digitalWrite(sgreen[i], LOW);
      digitalWrite(syellow[i], LOW);
      digitalWrite(sred[i], HIGH);
    }
  }
  int idx = slig - 1;
  digitalWrite(sred[idx], LOW);
  digitalWrite(syellow[idx], LOW);
  digitalWrite(sgreen[idx], HIGH);
  lastsemaforo = slig;
}

// Adiciona um comando na fila (se houver espaço)
void enqueueCommand(int cmd) {
  if (queueSize < MAX_QUEUE) {
    queueCmd[queueEnd] = cmd;
    queueEnd = (queueEnd + 1) % MAX_QUEUE;
    queueSize++;
    printQueue(); // exibe fila no terminal
  }
}

// Remove e retorna o próximo comando da fila; retorna 0 se vazia
int dequeueCommand() {
  if (queueSize == 0) return 0;
  int cmd = queueCmd[queueStart];
  queueStart = (queueStart + 1) % MAX_QUEUE;
  queueSize--;
  printQueue(); // exibe fila atualizada
  return cmd;
}

// Imprime a sequência de comandos restante na fila
void printQueue() {
  Serial.print("Comandos na fila: ");
  for (int i = 0; i < queueSize; i++) {
    int idx = (queueStart + i) % MAX_QUEUE;
    int cmd = queueCmd[idx];
    char c;
    switch (cmd) {
      case norte: c = 'N'; break;
      case leste: c = 'L'; break;
      case sul:   c = 'S'; break;
      case oeste: c = 'O'; break;
      default:    c = '?'; break;
    }
    Serial.print(c);
    Serial.print(' ');
  }
  Serial.println();
}

void setup() {
  Serial.begin(9600);
  Serial.println("Iniciando sistema de semáforo");
  for (int i = 0; i < 4; i++) {
    pinMode(sgreen[i], OUTPUT);
    pinMode(syellow[i], OUTPUT);
    pinMode(sred[i], OUTPUT);
    pinMode(pgreen[i], OUTPUT);
    pinMode(pred[i], OUTPUT);
    pinMode(botao[i], INPUT_PULLUP);
  }
  semaforo(norte);
  for (int i = 0; i < 4; i++) {
    digitalWrite(pgreen[i], LOW);
    digitalWrite(pred[i], HIGH);
  }
  // Uso de RISING para botão com pull-up ativo
  attachInterrupt(digitalPinToInterrupt(botao[0]), pedestre0, RISING);
  attachInterrupt(digitalPinToInterrupt(botao[1]), pedestre1, RISING);
  attachInterrupt(digitalPinToInterrupt(botao[2]), pedestre2, RISING);
  attachInterrupt(digitalPinToInterrupt(botao[3]), pedestre3, RISING);
  ultimoTroca = millis();
}

void loop() {
  // Captura comandos via Serial e coloca na fila
  while (Serial.available() > 0) {
    char c = tolower(Serial.read());
    int cmd = 0;
    switch (c) {
      case 'n': cmd = norte; break;
      case 'l': cmd = leste; break;
      case 's': cmd = sul;   break;
      case 'o': cmd = oeste; break;
    }
    if (cmd != 0) enqueueCommand(cmd);
  }

  // Tratamento de pedestre via estado
  if (pedState == REQUESTED) {
    pedState = RUNNING;
    Serial.print("Iniciando travessia pedestre ");
    Serial.println(ledIndex);
    todosSemaforosVermelhos();
    digitalWrite(pgreen[ledIndex], HIGH);
    digitalWrite(pred[ledIndex], LOW);
    ledStartTime = millis();
  }

  if (pedState == RUNNING && millis() - ledStartTime >= tempoPedestre) {
    Serial.print("Finalizando travessia pedestre ");
    Serial.println(ledIndex);
    digitalWrite(pgreen[ledIndex], LOW);
    digitalWrite(pred[ledIndex], HIGH);
    restaurarSemaforoAnterior();
    pedState = IDLE;
    ultimoTroca = millis();
  }

  // Operação normal quando não há travessia
  if (pedState == IDLE && !manual && millis() - ultimoTroca >= intervalo) {
    int nextCmd = dequeueCommand();
    if (nextCmd != 0) {
      manual = true;
      semaforo(nextCmd);
      delay(5000); // tempo manual fixo
      int seq = (nextCmd == oeste) ? norte : nextCmd + 1;
      semaforo(seq);
      direcaoAtual = seq;
      ultimoTroca = millis();
      manual = false;
    } else {
      int seq = (direcaoAtual == oeste) ? norte : direcaoAtual + 1;
      semaforo(seq);
      direcaoAtual = seq;
      ultimoTroca = millis();
    }
  }
}