// =============================================
//   SEMAFOR SUSTAV - Arduino MEGA
// =============================================

// --- glavni semafor ---
const int gl_zeleno   = 2;
const int gl_zuto     = 3;
const int gl_crveno   = 4;
const int gl_desno    = 5;
const int gl_lijevo   = 6;

// --- prvi pjesacki (crveno dok glavna zelena, zeleno dok sporedna zelena) ---
const int pj1a_crveno = 7;
const int pj1a_zeleno = 8;
const int pj1b_crveno = 9;
const int pj1b_zeleno = 10;

// --- sporedni semafor ---
const int sp_zeleno   = 11;
const int sp_zuto     = 12;
const int sp_crveno   = 13;

// --- drugi pjesacki (zeleno dok glavna zelena, crveno inace) ---
const int pj2a_crveno = 14;
const int pj2a_zeleno = 15;
const int pj2b_crveno = 16;
const int pj2b_zeleno = 17;

// --- tipkala ---
const int tipkalo_pj1 = 22;  // ubrzava prijelaz iz faze 1 -> 2
const int tipkalo_pj2 = 23;  // ubrzava prijelaz iz faze 4 -> 5

// --- buzzeri ---
const int buzzer1 = A0;  // za pj1
const int buzzer2 = A1;  // za pj2

// --- prvi ultrazvucni (za tipkalo_pj1) ---
const int trig1 = 25;
const int echo1 = 24;

// --- drugi ultrazvucni (za tipkalo_pj2) ---
const int trig2 = 27;
const int echo2 = 26;

// =============================================
//   TRAJANJA
// =============================================
const unsigned long T_GL_ZELENO      = 10000; // faza 1
const unsigned long T_GL_ZUTO        = 2000;  // faza 2
const unsigned long T_SP_NAJAVA      = 2000;  // faza 3 - sporedna crveno+zuto
const unsigned long T_SP_ZELENO      = 10000; // faza 4
const unsigned long T_SP_ZUTO        = 2000;  // faza 5
const unsigned long T_GL_NAJAVA      = 2000;  // faza 6 - glavna crveno+zuto

// =============================================
//   BUZZERI
// =============================================
const int buzz_sporo = 1000;
const int buzz_brzo  = 200;

unsigned long buzz1_timer  = 0;
unsigned long buzz2_timer  = 0;
bool          buzz1_stanje = false;
bool          buzz2_stanje = false;

// =============================================
//   TIPKALA
// =============================================
const unsigned long debounce = 500;

bool          zahtjev_pj1   = false;
bool          zahtjev_pj2   = false;
unsigned long zadnje_tip1   = 0;
unsigned long zadnje_tip2   = 0;

// =============================================
//   FAZE
// =============================================
enum faza_t {
  FAZA_1_GL_ZELENO,   // glavna zelena, pj2 zeleno, ostalo crveno
  FAZA_2_GL_ZUTO,     // glavna zuta, svi crveno
  FAZA_3_SP_NAJAVA,   // glavna crveno, sporedna crveno+zuto, svi ostali crveno
  FAZA_4_SP_ZELENO,   // sporedna zeleno, pj1 zeleno, ostalo crveno
  FAZA_5_SP_ZUTO,     // sporedna zuto, svi crveno
  FAZA_6_GL_NAJAVA    // glavna crveno+zuto, svi crveno -> najava zelene
};

faza_t        faza       = FAZA_1_GL_ZELENO;
unsigned long faza_start = 0;

// =============================================
void setup() {
  pinMode(gl_zeleno,   OUTPUT);
  pinMode(gl_zuto,     OUTPUT);
  pinMode(gl_crveno,   OUTPUT);
  pinMode(gl_desno,    OUTPUT);
  pinMode(gl_lijevo,   OUTPUT);
  pinMode(pj1a_crveno, OUTPUT);
  pinMode(pj1a_zeleno, OUTPUT);
  pinMode(pj1b_crveno, OUTPUT);
  pinMode(pj1b_zeleno, OUTPUT);
  pinMode(sp_zeleno,   OUTPUT);
  pinMode(sp_zuto,     OUTPUT);
  pinMode(sp_crveno,   OUTPUT);
  pinMode(pj2a_crveno, OUTPUT);
  pinMode(pj2a_zeleno, OUTPUT);
  pinMode(pj2b_crveno, OUTPUT);
  pinMode(pj2b_zeleno, OUTPUT);
  pinMode(buzzer1,     OUTPUT);
  pinMode(buzzer2,     OUTPUT);
  pinMode(tipkalo_pj1, INPUT_PULLUP);
  pinMode(tipkalo_pj2, INPUT_PULLUP);
  pinMode(trig1,       OUTPUT);
  pinMode(echo1,       INPUT);
  pinMode(trig2,       OUTPUT);
  pinMode(echo2,       INPUT);

  ugasi_sve();
  postavi_fazu_1();
  faza_start = millis();
}

// =============================================
void loop() {
  unsigned long sad = millis();

  provjeri_tipkala(sad);
  upravljaj_buzzerima(sad);

  switch (faza) {

    // -----------------------------------------
    // FAZA 1: glavna zelena (10s)
    // glavna:  zelena ON
    // pj1:     crveno ON
    // pj2:     zeleno ON
    // sporedna: crveno ON
    // -----------------------------------------
    case FAZA_1_GL_ZELENO: {
      bool isteklo = (sad - faza_start >= T_GL_ZELENO);
      bool tip_aktivan = zahtjev_pj1 && !izmjeri_guzvu(trig1, echo1);

      if (isteklo || tip_aktivan) {
        zahtjev_pj1 = false;
        faza = FAZA_2_GL_ZUTO;
        faza_start = sad;
        postavi_fazu_2();
      }
      break;
    }

    // -----------------------------------------
    // FAZA 2: glavna zuta (2s)
    // glavna:  zuta ON
    // pj1:     crveno ON
    // pj2:     crveno ON  <- prelazi s zelene na crveno
    // sporedna: crveno ON
    // -----------------------------------------
    case FAZA_2_GL_ZUTO: {
      if (sad - faza_start >= T_GL_ZUTO) {
        faza = FAZA_3_SP_NAJAVA;
        faza_start = sad;
        postavi_fazu_3();
      }
      break;
    }

    // -----------------------------------------
    // FAZA 3: sporedna najava (2s)
    // glavna:  crveno ON
    // pj1:     crveno ON
    // pj2:     crveno ON
    // sporedna: crveno ON + zuto ON zajedno
    // -----------------------------------------
    case FAZA_3_SP_NAJAVA: {
      if (sad - faza_start >= T_SP_NAJAVA) {
        faza = FAZA_4_SP_ZELENO;
        faza_start = sad;
        postavi_fazu_4();
      }
      break;
    }

    // -----------------------------------------
    // FAZA 4: sporedna zelena (10s)
    // glavna:  crveno ON
    // pj1:     zeleno ON  <- prelazi s crvene na zeleno
    // pj2:     crveno ON
    // sporedna: zeleno ON  <- crveno i zuto se gase
    // -----------------------------------------
    case FAZA_4_SP_ZELENO: {
      bool isteklo = (sad - faza_start >= T_SP_ZELENO);
      bool tip_aktivan = zahtjev_pj2 && !izmjeri_guzvu(trig2, echo2);

      if (isteklo || tip_aktivan) {
        zahtjev_pj2 = false;
        faza = FAZA_5_SP_ZUTO;
        faza_start = sad;
        postavi_fazu_5();
      }
      break;
    }

    // -----------------------------------------
    // FAZA 5: sporedna zuta (2s)
    // glavna:  crveno ON
    // pj1:     crveno ON  <- prelazi s zelene na crveno
    // pj2:     crveno ON
    // sporedna: zuto ON  <- zeleno se gasi
    // -----------------------------------------
    case FAZA_5_SP_ZUTO: {
      if (sad - faza_start >= T_SP_ZUTO) {
        faza = FAZA_6_GL_NAJAVA;
        faza_start = sad;
        postavi_fazu_6();
      }
      break;
    }

    // -----------------------------------------
    // FAZA 6: glavna najava (2s)
    // glavna:  crveno ON + zuto ON zajedno
    // pj1:     crveno ON
    // pj2:     crveno ON
    // sporedna: crveno ON  <- zuto se gasi
    // -----------------------------------------
    case FAZA_6_GL_NAJAVA: {
      if (sad - faza_start >= T_GL_NAJAVA) {
        faza = FAZA_1_GL_ZELENO;
        faza_start = sad;
        postavi_fazu_1();
      }
      break;
    }
  }
}

// =============================================
//   POSTAVLJANJE FAZA
// =============================================

void ugasi_sve() {
  digitalWrite(gl_zeleno,   LOW);
  digitalWrite(gl_zuto,     LOW);
  digitalWrite(gl_crveno,   LOW);
  digitalWrite(gl_desno,    LOW);
  digitalWrite(gl_lijevo,   LOW);
  digitalWrite(pj1a_crveno, LOW);
  digitalWrite(pj1a_zeleno, LOW);
  digitalWrite(pj1b_crveno, LOW);
  digitalWrite(pj1b_zeleno, LOW);
  digitalWrite(sp_zeleno,   LOW);
  digitalWrite(sp_zuto,     LOW);
  digitalWrite(sp_crveno,   LOW);
  digitalWrite(pj2a_crveno, LOW);
  digitalWrite(pj2a_zeleno, LOW);
  digitalWrite(pj2b_crveno, LOW);
  digitalWrite(pj2b_zeleno, LOW);
  digitalWrite(buzzer1,     LOW);
  digitalWrite(buzzer2,     LOW);
}

// FAZA 1 - glavna zelena
void postavi_fazu_1() {
  ugasi_sve();
  digitalWrite(gl_zeleno,   HIGH); // glavna zelena
  digitalWrite(gl_desno,    HIGH); // strelica desno
  digitalWrite(pj1a_crveno, HIGH); // pj1 strana a - crveno
  digitalWrite(pj1b_crveno, HIGH); // pj1 strana b - crveno
  digitalWrite(pj2a_zeleno, HIGH); // pj2 strana a - zeleno
  digitalWrite(pj2b_zeleno, HIGH); // pj2 strana b - zeleno
  digitalWrite(sp_crveno,   HIGH); // sporedna - crveno
}

// FAZA 2 - glavna zuta, pj2 prelazi na crveno
void postavi_fazu_2() {
  ugasi_sve();
  digitalWrite(gl_zuto,     HIGH); // glavna zuta
  digitalWrite(pj1a_crveno, HIGH); // pj1 - crveno
  digitalWrite(pj1b_crveno, HIGH);
  digitalWrite(pj2a_crveno, HIGH); // pj2 - prelazi na crveno
  digitalWrite(pj2b_crveno, HIGH);
  digitalWrite(sp_crveno,   HIGH); // sporedna - crveno
}

// FAZA 3 - sporedna najava (crveno+zuto zajedno)
void postavi_fazu_3() {
  ugasi_sve();
  digitalWrite(gl_crveno,   HIGH); // glavna - crveno
  digitalWrite(pj1a_crveno, HIGH); // pj1 - crveno
  digitalWrite(pj1b_crveno, HIGH);
  digitalWrite(pj2a_crveno, HIGH); // pj2 - crveno
  digitalWrite(pj2b_crveno, HIGH);
  digitalWrite(sp_crveno,   HIGH); // sporedna - crveno + zuto zajedno
  digitalWrite(sp_zuto,     HIGH);
  digitalWrite(gl_lijevo,   HIGH);  
}

// FAZA 4 - sporedna zelena, pj1 zeleno
void postavi_fazu_4() {
  ugasi_sve();
  digitalWrite(gl_crveno,   HIGH); // glavna - crveno
  digitalWrite(pj1a_zeleno, HIGH); // pj1 - zeleno
  digitalWrite(pj1b_zeleno, HIGH);
  digitalWrite(pj2a_crveno, HIGH); // pj2 - crveno
  digitalWrite(pj2b_crveno, HIGH);
  digitalWrite(sp_zeleno,   HIGH); // sporedna - zeleno (crveno i zuto ugaseni)
}

// FAZA 5 - sporedna zuta, pj1 prelazi na crveno
void postavi_fazu_5() {
  ugasi_sve();
  digitalWrite(gl_crveno,   HIGH); // glavna - crveno
  digitalWrite(pj1a_crveno, HIGH); // pj1 - prelazi na crveno
  digitalWrite(pj1b_crveno, HIGH);
  digitalWrite(pj2a_crveno, HIGH); // pj2 - crveno
  digitalWrite(pj2b_crveno, HIGH);
  digitalWrite(sp_zuto,     HIGH); // sporedna - zuto (zeleno ugaseno)
}

// FAZA 6 - glavna najava zelene (crveno+zuto zajedno)
void postavi_fazu_6() {
  ugasi_sve();
  digitalWrite(gl_crveno,   HIGH); // glavna - crveno + zuto zajedno
  digitalWrite(gl_zuto,     HIGH);
  digitalWrite(pj1a_crveno, HIGH); // pj1 - crveno
  digitalWrite(pj1b_crveno, HIGH);
  digitalWrite(pj2a_crveno, HIGH); // pj2 - crveno
  digitalWrite(pj2b_crveno, HIGH);
  digitalWrite(sp_crveno,   HIGH); // sporedna - crveno (zuto ugaseno)
}

// =============================================
//   ULTRAZVUCNI SENZORI
// =============================================
bool izmjeri_guzvu(int trig_pin, int echo_pin) {
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);

  long trajanje = pulseIn(echo_pin, HIGH, 30000);
  if (trajanje == 0) return false;

  long udaljenost = trajanje / 58;
  return (udaljenost > 0 && udaljenost < 30);
}

// =============================================
//   TIPKALA
// =============================================
void provjeri_tipkala(unsigned long sad) {
  // tipkalo_pj1 aktivno samo u fazi 1 (dok je pj1 crven)
  if (faza == FAZA_1_GL_ZELENO) {
    if (digitalRead(tipkalo_pj1) == LOW) {
      if (sad - zadnje_tip1 > debounce) {
        zahtjev_pj1 = true;
        zadnje_tip1 = sad;
      }
    }
  }
  // tipkalo_pj2 aktivno samo u fazi 4 (dok je pj2 crven)
  if (faza == FAZA_4_SP_ZELENO) {
    if (digitalRead(tipkalo_pj2) == LOW) {
      if (sad - zadnje_tip2 > debounce) {
        zahtjev_pj2 = true;
        zadnje_tip2 = sad;
      }
    }
  }
}

// =============================================
//   BUZZERI
// =============================================
void upravljaj_buzzerima(unsigned long sad) {

  // =========================
  // BUZZER 1 (pj1)
  // =========================
  bool pj1_zelen = digitalRead(pj1a_zeleno);
  bool pj1_crven = digitalRead(pj1a_crveno);

  int perioda1 = 0;
  if (pj1_zelen) perioda1 = buzz_brzo;   // brzo kad zeleno
  else if (pj1_crven) perioda1 = buzz_sporo; // sporo kad crveno

  if (perioda1 > 0 && sad - buzz1_timer >= perioda1) {
    buzz1_timer = sad;

    // kratki beep (klik)
    tone(buzzer1, 2000, 50); // frekvencija 2000Hz, trajanje 50ms
  }


  // =========================
  // BUZZER 2 (pj2)
  // =========================
  bool pj2_zelen = digitalRead(pj2a_zeleno);
  bool pj2_crven = digitalRead(pj2a_crveno);

  int perioda2 = 0;
  if (pj2_zelen) perioda2 = buzz_brzo;
  else if (pj2_crven) perioda2 = buzz_sporo;

  if (perioda2 > 0 && sad - buzz2_timer >= perioda2) {
    buzz2_timer = sad;

    tone(buzzer2, 2000, 50); // frekvencija 2000Hz, trajanje 50ms
  }
}