//s153122
//s148989

#include <LCD_C0220BiZ.h>
#include <LCD_kruispunt_overweg.h>
#include <ST7036.h>
#include "Servo.h"


//Pins zijn niet juist omdat we niet weten hoe deze geschakeld moesten worden
const int knop1 = 2;
const int knop2 = 0;
const int knop3 = 4;
const int knop4 = 5;
const int oranjeLED = 13;
const int rodeLED1 = 12;
const int rodeLED2 = 14;
const int slagboomLinksPIN = 27;
const int slagboomRechtsPIN = 26;
const int LCD_Resetpin = 11;
uint8_t LCD_Backlight = 20;

uint8_t LCD_rij = 2;
uint8_t LCD_kolom = 20;

ST7036 lcd = ST7036 (LCD_rij, LCD_kolom, 0X78, LCD_Backlight);

Servo slagboomLinks;
Servo slagboomRechts;

unsigned long previousMillis = 0;
bool slagboomOmlaag = false;
static bool oranjeLEDState = HIGH;
bool treinLinks = false;
bool treinRechts = false;
static int slagboomHoek = 90;
unsigned long omlaagMillis = 0;
unsigned long omhoogMillis = 0;
unsigned long roodMillis = 0;
static bool roodLEDState = LOW;
unsigned long oranjeMillis = 0;
bool oranje = true;
unsigned long LCDMillis = 0;
unsigned long delayMillis = 999999999999;


void setup() {
  // put your setup code here, to run once:
  pinMode(knop1, INPUT);
  pinMode(knop2, INPUT);
  pinMode(knop3, INPUT);
  pinMode(knop4, INPUT);
  pinMode(oranjeLED, OUTPUT);
  pinMode(rodeLED1, OUTPUT);
  pinMode(rodeLED2, OUTPUT);
  pinMode (LCD_Resetpin, OUTPUT);
  digitalWrite (LCD_Resetpin, HIGH);

  pinMode (LCD_Backlight, OUTPUT);
  digitalWrite (LCD_Backlight, HIGH);

  lcd.init();
  lcd.clear();

  slagboomLinks.attach(slagboomLinksPIN);
  slagboomRechts.attach(slagboomRechtsPIN);

  slagboomLinks.write(90);
  slagboomRechts.write(90);
  digitalWrite(oranjeLED, HIGH);
}

void loop() {
  // put your main code here, to run repeatedly:
  
  unsigned long currentMillis = millis();

  //Print groep op LCD
  lcd.setCursor (0,0);
  lcd.print ("1IT9a-groep 8");


  //Oranje knipperlicht als slagbomen omhoog zijn
  if (!slagboomOmlaag && !treinLinks && !treinRechts && oranje) {
    if (currentMillis - oranjeMillis >= 500) {
      digitalWrite(oranjeLED, oranjeLEDState);
      oranjeLEDState = !oranjeLEDState;
      oranjeMillis = currentMillis;
    }
  }


  //Knoppen
  if (digitalRead(knop1) == HIGH) {
    treinLinks = true;
    oranje = false;
  }

  if (digitalRead(knop3) == HIGH) {
    treinRechts = true;
    oranje = false;
  }

  if (digitalRead(knop2) == HIGH) {
    treinLinks = false;
  }

  if (digitalRead(knop4) == HIGH) {
    treinRechts = false;
  }


  //Slagbomen omlaag als er minstens 1 trein aan komt
  if (treinLinks || treinRechts) {
    digitalWrite(oranjeLED, LOW);
    if (!slagboomOmlaag && currentMillis - previousMillis >= 8000) {
      if (currentMillis - omlaagMillis >= 100) {
        slagboomLinks.write(slagboomHoek);
        slagboomRechts.write(slagboomHoek);
        slagboomHoek -= 5;
        omlaagMillis = 0;
      } 
      if (slagboomHoek == 0) {
        slagboomOmlaag = true;
      }
      
    }
    if (currentMillis - roodMillis >= 250) {
      roodLEDState = !roodLEDState;
      digitalWrite(rodeLED1, roodLEDState);
      digitalWrite(rodeLED2, !roodLEDState);
      roodMillis = currentMillis;
    }
  }


  //Slagbomen terug omhoog als treinen weg zijn
  if (!treinLinks && !treinRechts) {
    if (slagboomOmlaag) {
      if (currentMillis - omhoogMillis >= 100) {
        slagboomLinks.write(slagboomHoek);
        slagboomRechts.write(slagboomHoek);
        slagboomHoek += 5;
        omhoogMillis = 0;
      } 
      if (slagboomHoek == 90) {
        slagboomOmlaag = false;
        previousMillis = currentMillis;
      }
    }
    if (currentMillis - roodMillis >= 250 && slagboomOmlaag) {
      roodLEDState = !roodLEDState;
      digitalWrite(rodeLED1, roodLEDState);
      digitalWrite(rodeLED2, !roodLEDState);
      roodMillis = currentMillis;
      delayMillis = currentMillis;
    }
    if (currentMillis - delayMillis <= 2000 && currentMillis - roodMillis >= 250 && !slagboomOmlaag) {
      roodLEDState = !roodLEDState;
      digitalWrite(rodeLED1, roodLEDState);
      digitalWrite(rodeLED2, !roodLEDState);
      roodMillis = currentMillis;
    } else if (currentMillis - delayMillis >= 2000) {
      oranje = true;
    }
  }


  //Print namen op LCD
  if (currentMillis - LCDMillis <= 2000) {
    lcd.setCursor (1,0);
    lcd.print ("Mauro De Mey");
  } else if (currentMillis - LCDMillis <= 4000) {
    lcd.setCursor (1,0);
    lcd.print ("Kobe Verelst");
  } else {
    LCDMillis = currentMillis;
  }
}