#include "classes.cpp"

enum States
{
  G1T,
  G2T,
  NoT,
  YesP
};

class TrafficSystem {
public:
  static TrafficLight g1TrafficLight;

  static TrafficLight g2TrafficLight;
  static PedestrianLight g2PedestrianLight;

  static byte g1EmergencyButton;
  static byte g1PedestrianButton;

  static States lastTrafficState;
  static States currentState;
  volatile static States nextState;

  static unsigned long long int waitTime;
  static unsigned long long int lastActionTime;

  TrafficSystem() {
    pinMode(g1TrafficLight.getRedLight(), OUTPUT);
    pinMode(g1TrafficLight.getAmberLight(), OUTPUT);
    pinMode(g1TrafficLight.getGreenLight(), OUTPUT);

    pinMode(g2TrafficLight.getRedLight(), OUTPUT);
    pinMode(g2TrafficLight.getAmberLight(), OUTPUT);
    pinMode(g2TrafficLight.getGreenLight(), OUTPUT);

    pinMode(g2PedestrianLight.getRedLight(), OUTPUT);
    pinMode(g2PedestrianLight.getGreenLight(), OUTPUT);

    pinMode(g1EmergencyButton, INPUT_PULLUP);
    pinMode(g1PedestrianButton, INPUT_PULLUP);

    //initial light setup
    digitalWrite(g1TrafficLight.getRedLight(), LOW);
    digitalWrite(g1TrafficLight.getAmberLight(), LOW);
    digitalWrite(g1TrafficLight.getGreenLight(), HIGH);

    digitalWrite(g2PedestrianLight.getRedLight(), HIGH);
    digitalWrite(g2PedestrianLight.getGreenLight(), LOW);

    digitalWrite(g2TrafficLight.getRedLight(), HIGH);
    digitalWrite(g2TrafficLight.getAmberLight(), LOW);
    digitalWrite(g2TrafficLight.getGreenLight(), LOW);
  }

  static void G1T_to_NoT() {
    digitalWrite(g1TrafficLight.getRedLight(), LOW);
    digitalWrite(g1TrafficLight.getAmberLight(), HIGH);
    digitalWrite(g1TrafficLight.getGreenLight(), LOW);
    delay(1000);
    digitalWrite(g1TrafficLight.getRedLight(), HIGH);
    digitalWrite(g1TrafficLight.getAmberLight(), LOW);
    digitalWrite(g1TrafficLight.getGreenLight(), LOW);
    delay(1000);
    currentState = NoT;
    nextState = G2T;
    waitTime = 1000;
    lastActionTime = millis();
  }

  static void G2T_to_NoT() {
    digitalWrite(g2TrafficLight.getRedLight(), LOW);
    digitalWrite(g2TrafficLight.getAmberLight(), HIGH);
    digitalWrite(g2TrafficLight.getGreenLight(), LOW);
    delay(1000);
    digitalWrite(g2TrafficLight.getRedLight(), HIGH);
    digitalWrite(g2TrafficLight.getAmberLight(), LOW);
    digitalWrite(g2TrafficLight.getGreenLight(), LOW);
    delay(1000);
    currentState = NoT;
    nextState = G1T;
    waitTime = 1000;
    lastActionTime = millis();
  }

  static void NoT_to_G2T() {
    digitalWrite(g2TrafficLight.getRedLight(), HIGH);
    digitalWrite(g2TrafficLight.getAmberLight(), HIGH);
    digitalWrite(g2TrafficLight.getGreenLight(), LOW);
    delay(1000);
    digitalWrite(g2TrafficLight.getRedLight(), LOW);
    digitalWrite(g2TrafficLight.getAmberLight(), LOW);
    digitalWrite(g2TrafficLight.getGreenLight(), HIGH);
    delay(1000);
    currentState = G2T;
    nextState = NoT;
    waitTime = 7000;
    lastActionTime = millis();
  }

  static void NoT_to_G1T() {
    digitalWrite(g1TrafficLight.getRedLight(), HIGH);
    digitalWrite(g1TrafficLight.getAmberLight(), HIGH);
    digitalWrite(g1TrafficLight.getGreenLight(), LOW);
    delay(1000);
    digitalWrite(g1TrafficLight.getRedLight(), LOW);
    digitalWrite(g1TrafficLight.getAmberLight(), LOW);
    digitalWrite(g1TrafficLight.getGreenLight(), HIGH);
    delay(1000);
    currentState = G1T;
    nextState = NoT;
    waitTime = 7000;
    lastActionTime = millis();
  }

  static void NoT_to_YesP() {
    digitalWrite(g2PedestrianLight.getRedLight(), LOW);
    digitalWrite(g2PedestrianLight.getGreenLight(), HIGH);
    currentState = YesP;
    nextState = NoT;
    waitTime = 7000;
    lastActionTime = millis();
  }

  static void YesP_to_NoT() {
    digitalWrite(g2PedestrianLight.getRedLight(), HIGH);
    digitalWrite(g2PedestrianLight.getGreenLight(), LOW);
    currentState = NoT;
    if (lastTrafficState == G1T) {
      nextState = G2T;
    } else {
      nextState = G1T;
    }
    waitTime = 1000;
    lastActionTime = millis();
  }


};


TrafficLight TrafficSystem::g1TrafficLight = TrafficLight(7, 6, 5);

TrafficLight TrafficSystem::g2TrafficLight = TrafficLight(12, 11, 10);
PedestrianLight TrafficSystem::g2PedestrianLight = PedestrianLight(9, 8);

byte TrafficSystem::g1EmergencyButton = 4;
byte TrafficSystem::g1PedestrianButton = 3;

States TrafficSystem::lastTrafficState = G1T;
States TrafficSystem::currentState = G1T;
volatile States TrafficSystem::nextState = G2T;

unsigned long long int TrafficSystem::waitTime = 0;
unsigned long long int TrafficSystem::lastActionTime = millis();

void setup() {
  // put your setup code here, to run once:
  TrafficSystem mainSystem = TrafficSystem();
  attachInterrupt(digitalPinToInterrupt(TrafficSystem::g1PedestrianButton), pedestrianInter, FALLING);
  attachInterrupt(digitalPinToInterrupt(TrafficSystem::g1EmergencyButton), emergencyInter, FALLING);

}

void loop() {
  // put your main code here, to run repeatedly:
    if (TrafficSystem::currentState == G2T && TrafficSystem::nextState == G1T) { //Emergency interrupt
      TrafficSystem::G2T_to_NoT();
      TrafficSystem::NoT_to_G1T();
      TrafficSystem::waitTime = 10000;
    }
    else if (TrafficSystem::currentState == YesP && TrafficSystem::nextState == G1T) { //Emergency interrupt
      TrafficSystem::YesP_to_NoT();
      TrafficSystem::NoT_to_G1T();
      TrafficSystem::waitTime = 10000;
    }
    else if (TrafficSystem::currentState == G1T && TrafficSystem::nextState == G1T) { //Emergency interrupt
      TrafficSystem::waitTime = 10000;
      TrafficSystem::nextState = NoT;
    }
  
  if (millis() > TrafficSystem::waitTime + TrafficSystem::lastActionTime) {
    if (TrafficSystem::currentState == G1T && TrafficSystem::nextState == NoT)
      TrafficSystem::G1T_to_NoT();
    else if (TrafficSystem::currentState == G2T && TrafficSystem::nextState == NoT)
      TrafficSystem::G2T_to_NoT();
    else if (TrafficSystem::currentState == NoT && TrafficSystem::nextState == G2T)
      TrafficSystem::NoT_to_G2T();
    else if (TrafficSystem::currentState == NoT && TrafficSystem::nextState == G1T)
      TrafficSystem::NoT_to_G1T();
    else if (TrafficSystem::currentState == NoT && TrafficSystem::nextState == YesP)
      TrafficSystem::NoT_to_YesP();
    else if (TrafficSystem::currentState == YesP && TrafficSystem::nextState == NoT)
      TrafficSystem::YesP_to_NoT();
    else if (TrafficSystem::currentState == G1T && TrafficSystem::nextState == YesP) { //Pedestrian interrupt
      TrafficSystem::G1T_to_NoT();
      TrafficSystem::NoT_to_YesP();
    }
    else if (TrafficSystem::currentState == G2T && TrafficSystem::nextState == YesP) { //Pedestrian interrupt
      TrafficSystem::G2T_to_NoT();
      TrafficSystem::NoT_to_YesP();
    }
      
  }
}

void pedestrianInter() {
  TrafficSystem::nextState = YesP;
}

void emergencyInter() {
  TrafficSystem::nextState = G1T;
}