#include <WiFi.h>
#include <PubSubClient.h>

int d_red = 2;
int d_yellow = 0;
int d_green = 4;
int r_red = 16;
int r_yellow = 17;
int r_green = 5;
int l_red = 12;
int l_yellow = 14;
int l_green = 27;
int u_red = 18;
int u_yellow = 19;
int u_green = 21;

int low = 10000;
int medium = 15000;
int high = 20000;
int none = 0;

int roundNumber = 0;

String p_up = "1";
String p_right = "1";
String p_down = "1";
String p_left = "1";

const char *mqtt_server = "broker.emqx.io";

WiFiClient espClient;
PubSubClient client(espClient);

int getPriority(String p)
{
  if (p == "2")
  {
    return 10000;
  }
  else if (p == "1")
  {
    return 15000;
  }
  else if (p == "0")
  {
    return 20000;
  }
  else if (p == "3")
  {
    return 0;
  }
  else
    return 10000;
}

void changeLights_up(String priority)
{
  // Start (all yellow)

  int p = getPriority(priority);
  Serial.println("Upper lane green");
  digitalWrite(u_red, LOW);
  digitalWrite(d_red, LOW);
  digitalWrite(r_red, LOW);
  digitalWrite(l_green, LOW);
  digitalWrite(u_yellow, HIGH);
  digitalWrite(d_yellow, HIGH);
  digitalWrite(r_yellow, HIGH);
  digitalWrite(l_yellow, HIGH);
  delay(1000);

  // upper lane go
  digitalWrite(u_yellow, LOW);
  digitalWrite(d_yellow, LOW);
  digitalWrite(r_yellow, LOW);
  digitalWrite(l_yellow, LOW);
  digitalWrite(u_green, HIGH);
  digitalWrite(r_red, HIGH);
  digitalWrite(l_red, HIGH);
  digitalWrite(d_red, HIGH);
  delay(p);
}

// ALL YELLOW
void changeLights_right(String priority)
{

  int p = getPriority(priority);
  Serial.println("Right lane green");
  digitalWrite(u_yellow, HIGH);
  digitalWrite(d_yellow, HIGH);
  digitalWrite(r_yellow, HIGH);
  digitalWrite(l_yellow, HIGH);
  digitalWrite(u_green, LOW);
  digitalWrite(r_red, LOW);
  digitalWrite(l_red, LOW);
  digitalWrite(d_red, LOW);
  delay(1000);

  // RIGHT LANE GO
  digitalWrite(u_yellow, LOW);
  digitalWrite(d_yellow, LOW);
  digitalWrite(r_yellow, LOW);
  digitalWrite(l_yellow, LOW);
  digitalWrite(u_red, HIGH);
  digitalWrite(l_red, HIGH);
  digitalWrite(d_red, HIGH);
  digitalWrite(r_green, HIGH);
  delay(p);
}

void changeLights_down(String priority)
{
  // ALL YELLOW ON
  int p = getPriority(priority);
  Serial.println("Down lane green");
  digitalWrite(u_yellow, HIGH);
  digitalWrite(d_yellow, HIGH);
  digitalWrite(r_yellow, HIGH);
  digitalWrite(l_yellow, HIGH);
  digitalWrite(u_red, LOW);
  digitalWrite(l_red, LOW);
  digitalWrite(d_red, LOW);
  digitalWrite(r_green, LOW);
  delay(1000);

  // DOWN LANE GO
  digitalWrite(u_yellow, LOW);
  digitalWrite(d_yellow, LOW);
  digitalWrite(r_yellow, LOW);
  digitalWrite(l_yellow, LOW);
  digitalWrite(u_red, HIGH);
  digitalWrite(l_red, HIGH);
  digitalWrite(r_red, HIGH);
  digitalWrite(d_green, HIGH);
  delay(p);
}

void changeLights_left(String priority)
{
  // ALL YELLOW
  int p = getPriority(priority);
  Serial.println("Left lane green");
  digitalWrite(u_yellow, HIGH);
  digitalWrite(d_yellow, HIGH);
  digitalWrite(r_yellow, HIGH);
  digitalWrite(l_yellow, HIGH);
  digitalWrite(u_red, LOW);
  digitalWrite(l_red, LOW);
  digitalWrite(r_red, LOW);
  digitalWrite(d_green, LOW);
  delay(1000);

  // LEFT LANE GO
  digitalWrite(u_yellow, LOW);
  digitalWrite(d_yellow, LOW);
  digitalWrite(r_yellow, LOW);
  digitalWrite(l_yellow, LOW);
  digitalWrite(u_red, HIGH);
  digitalWrite(d_red, HIGH);
  digitalWrite(r_red, HIGH);
  digitalWrite(l_green, HIGH);
  delay(p);
}

void callback(char *topic, byte *message, unsigned int length)
{
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  String stMessage;
  for (int i = 0; i < length; i++) {
    Serial.print((char)message[i]);
    stMessage += (char)message[i];
  }
  Serial.println();

  // if (String(topic) == "gettraffic/upper") {
  //   Serial.print("Setting upper lane priority ");
  //   p_up = stMessage;
  // }else if (String(topic) == "gettraffic/right") {
  //   Serial.print("Setting right lane priority ");
  //   p_right = stMessage;
  // }else if (String(topic) == "gettraffic/down") {
  //   Serial.print("Setting down lane priority ");
  //   p_down = stMessage;
  // }else  if (String(topic) == "gettraffic/left") {
  //   Serial.print("Setting left lane priority ");
  //   p_left = stMessage;
  // }

  if (String(topic) == "gettraffic/priorities") {
    //Serial.print("Setting left lane priority ");
    p_up = stMessage[0];
    p_right = stMessage[1];
    p_down = stMessage[2];
    p_left = stMessage[3];
  }
  roundFn();

}

void reconnect()
{
  // Loop until we're reconnected
  while (!client.connected())
  {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("publisher"))
    {
      Serial.println("connected");
      client.subscribe("gettraffic/priorities");
      // client.subscribe("gettraffic/upper");
      // client.subscribe("gettraffic/right");
      // client.subscribe("gettraffic/down");
      // client.subscribe("gettraffic/left");
    }
    else
    {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void roundFn(){
  if (roundNumber==0){
    changeLights_up(p_up);
    roundNumber++;
  }else if(roundNumber==1){
    changeLights_right(p_right);
    roundNumber++;
  }else if(roundNumber==2){
    changeLights_down(p_down);
    roundNumber++;
  }else{
    changeLights_left(p_left);
    roundNumber==0;
  }
}

void setup()
{
  Serial.begin(9600);
  pinMode(d_red, OUTPUT);
  pinMode(d_yellow, OUTPUT);
  pinMode(d_green, OUTPUT);
  pinMode(r_red, OUTPUT);
  pinMode(r_yellow, OUTPUT);
  pinMode(r_green, OUTPUT);
  pinMode(l_red, OUTPUT);
  pinMode(l_yellow, OUTPUT);
  pinMode(l_green, OUTPUT);
  pinMode(u_red, OUTPUT);
  pinMode(u_yellow, OUTPUT);
  pinMode(u_green, OUTPUT);

  if (WiFi.status() != WL_CONNECTED)
  {
    WiFi.begin("Wokwi-GUEST", "", 6);
    while (WiFi.status() != WL_CONNECTED)
    {
      delay(250);
      Serial.println("Connecting to WIFI..");
    }
  }

  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
}
void loop()
{
  if (!client.connected())
  {
    reconnect();
  }
  client.loop();
}