#include "Arduino.h"
/*/// to do!!!

- uspostavit povezavo z dvem espom ESP_Now = https://randomnerdtutorials.com/esp-now-esp32-arduino-ide/
- sync za oba semaforaj
- preverjanje stanje samaforja in glede na tega priziganje/ugasanje
- Cas med smaforjama naj dopusca da avto pride na drugo stran (5s delay)

 
 /*/// To do!!!

//#include <esp_now.h>
//#include <WiFi.h>


//Reciver addres
uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

//message ki bo poslan prek ESP now
typedef struct struct_message {
  bool Primary;
  bool Secondary;
} struct_message;



//Trafic Light state
bool Primary = true;
bool Secondary = false;

//debug Status
bool debug = true;
bool color = false;

//led 
int Re = 13;
int Ye = 12;
int Gr = 14;

//senzor
int Sens = 27;


int start_time, curent_time, Time;







void setup() {
  Serial.begin(115200);

  pinMode(Re, OUTPUT);
  pinMode(Ye, OUTPUT);
  pinMode(Gr, OUTPUT);

  pinMode(Sens, INPUT);


}

void loop() {
  //Serial.println(digitalRead(Sens));
  

  if (Secondary  == true){
    digitalWrite(Re, HIGH);
  }
  else{
     traffic_light(/*/Red On time/*/    5   ,
                  /*/Green On time/*/   5   ,
                  /*/Yellow On Time/*/  1
                  );
  }
  delay(10);
  

}



//funkciaja za semafor
void traffic_light(int delay_Re, int delay_Gr, int delay_Ye ){

  
  curent_time = millis();
  Time = curent_time - start_time;



   //kalkulacije za izracun delaya
  delay_Re = (delay_Re*1000);//5000ms
  delay_Ye = (delay_Ye*1000) + delay_Re;//6000ms
  delay_Gr = (delay_Gr*1000) + delay_Ye;//11000ms

  
  Serial.printf("Time: %d State: ",Time);


  if (Time <= delay_Re){
    digitalWrite(Ye, LOW);
    digitalWrite(Re, HIGH);
    if (debug == true){
      if (color == true){
      Serial.println("\e[1;91m Red \e[1;97m");
      }
      else {
        Serial.println("Red");
      } 
    }
  }
  else if (Time <= delay_Ye){
    digitalWrite(Ye, HIGH);
    if (debug == true){
      if (color == true){
      Serial.println("\e[1;91m Red\e[1;97m/\e[1;93mYellow \e[1;97m");
      }
      else {
        Serial.println("Red/Yellow");
      }
    }
  }
  else if (Time <= delay_Gr){
    digitalWrite(Re, LOW);
    digitalWrite(Ye, LOW);
    digitalWrite(Gr, HIGH);
    if (debug == true){
      if (color == true){
        Serial.println("\e[1;92m Green \e[1;97m");
      }
      else {
        Serial.println("Green");
      } 
    } 
  }            
  else if (Time <= delay_Gr +(delay_Ye - delay_Re)){
    digitalWrite(Gr, LOW);//
    digitalWrite(Ye, HIGH);
    if (debug == true){
      if (color == true){
        Serial.println("\e[1;93m Yellow \e[1;97m");
      }
      else {
        Serial.println("Yellow");
      }
      }
    }
    

  

 
  else {
    
    start_time = curent_time;

    Serial.write(27);       // ESC command
    Serial.print("[2J");    // clear screen command

  }
  
}