#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>
#include <Keypad.h>
#include <EEPROM.h>



#define LCD_COLUMNS 20
#define LCD_LINES 2
#define I2C_ADDR 0x27
#define INGANG_TRIG 2
#define INGANG_ECHO 3
#define UITGANG_TRIG 4
#define UITGANG_ECHO 5
#define SERVO_PIN 9
#define NUM_LEDS 16
#define PIN_NEO A0
#define BRIGHTNESS 255
#define ROWS 4
#define COLS 3

Servo slagboomServo;
Adafruit_NeoPixel ring(NUM_LEDS, PIN_NEO, NEO_GRB + NEO_KHZ800);

enum SlagboomStatus {GESLOTEN, WACHT_NA_OPEN, KLAAR, WACHT_TOT_VRIJ, WACHTEN};
SlagboomStatus status = GESLOTEN;

long wachtStartTijd = 0;
int afstandAuto = 30;
int wachtnaOpen = 3000; // 3 seconden
int sluitVertraging = 3000;   // 5 seconden
int lcdRefresh = 1000;
int autoTeller = 0;
int ingangAfstand = 0;
int uitgangAfstand = 0;
int line = 7;
String code = "";
byte pos = 0;

// LCD: adres 0x27, 16 karakters, 2 regels
LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

/* Keypad setup */

char keys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};
byte rowPins[ROWS] = {13, 12, 11, 10};
byte colPins[COLS] = {A3, A2, A1};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

void initNeoPixel();
void reset();


void setup() {
  Serial.begin(9600);
  slagboomServo.attach(SERVO_PIN);
  pinMode(INGANG_TRIG, OUTPUT);
  pinMode(INGANG_ECHO, INPUT);
  pinMode(UITGANG_TRIG, OUTPUT);
  pinMode(UITGANG_ECHO, INPUT);
  slagboomServo.write(0); // Startpositie: gesloten
  initNeoPixel();

  lcd.init();           // LCD initialiseren
  lcd.backlight();      // Backlight aan
  //toonStatus();         // Eerste status tonen
}

void loop() {
  int laatsteUpdate = 100;
  ingangAfstand = meetAfstand(INGANG_TRIG, INGANG_ECHO);
  uitgangAfstand = meetAfstand(UITGANG_TRIG, UITGANG_ECHO);

  char key = keypad.getKey();  // lokaal
  if (key != NO_KEY) {
    Serial.println(key);
    toetsInput(key); //key doorgeven als parameter
  }

  switch (status) {
    case GESLOTEN:
      if (ingangAfstand < afstandAuto) {
        changeRingColor(0, 255, 0);
        Serial.println(ingangAfstand);
        slagboomServo.write(90);
        status = WACHT_NA_OPEN;
        wachtStartTijd = millis();
        toonStatus();
      }
      else changeRingColor(255, 0, 0);
      break;

    case WACHT_NA_OPEN:
      if (millis() - wachtStartTijd >= wachtnaOpen) {
        Serial.println("2");
        status = KLAAR;
        toonStatus();
      }
      break;

    case KLAAR:
      if (uitgangAfstand < afstandAuto || ingangAfstand > afstandAuto ) {
        Serial.println("3");
        status = WACHT_TOT_VRIJ;
        toonStatus();
      }
      break;

    case WACHT_TOT_VRIJ:
      if (uitgangAfstand >= afstandAuto) {  // Auto volledig weg
        Serial.println(uitgangAfstand);
        status = WACHTEN;
        wachtStartTijd = millis();
        toonStatus();
      }
      break;

    case WACHTEN:
      if (millis() - wachtStartTijd >= sluitVertraging) {
        Serial.println("4");
        slagboomServo.write(0);
        autoTeller++;
        status = GESLOTEN;
        toonStatus();
      }
      break;
  }
  // LCD om de 500ms verversen
  if (millis() - laatsteUpdate >= lcdRefresh) {
    toonStatus();
    laatsteUpdate = millis();
  }
}

void toonStatus() {
  //lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Slgbm: ");
  if (status == GESLOTEN) lcd.print("1-GESLOTEN");
  else if (status == WACHT_NA_OPEN) lcd.print("2-WACHT");
  else if (status == KLAAR) lcd.print("3-OPEN");
  else lcd.print("WACHT_VRIJ");
  lcd.setCursor(0, 1);
  lcd.print("Autos: ");
  lcd.print(autoTeller);
}


long meetAfstand(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) * 0.034 / 2;
  //return pulseIn(echoPin, HIGH) /58;
}

void initNeoPixel() {
  ring.begin();
  ring.setBrightness(BRIGHTNESS);
  changeRingColor(0, 0, 0); // Initialiseer alle led op zwart (geen licht)
  ring.show();
}

void changeRingColor(byte R, byte G, byte B) {
  for (int i = 0; i < NUM_LEDS; i++) {
    ring.setPixelColor(i, ring.Color(R, G, B));
  }
  ring.show();
}

void clearLCDLine(int line) {
  lcd.setCursor(line, 0);
  lcd.print("       "); // 16 spaces for a 16x2 LCD
}

void toetsInput(char key) {
  switch (key) {
    case '*': // reset display met functie reset()
      reset();
      break;
    case '#': // knop "OK" op het toetsenbord
      controlCode();
      Serial.println("Enter");
      break;
      Case '0': // verander password
      // changePassword()
      break;
    default:
      byte lengteInput = pos % 4;
      if ((lengteInput) == 0) {
        code = key;
      } else {
        code = code + key;
      }
      Serial.print("Toetsen reeds ingedrukt: ");
      Serial.println(code);
      pos++;
      //timerCode = millis();
      break;
  }
}

void controlCode() {
  if (code == "1234") {
    changeRingColor(0, 255, 0);
    ingangAfstand = 10;
    status = GESLOTEN;
  } else {
    tekstOpLCD(0, "Fout", 1, "Code opnieuw");
    ingangAfstand = 100;
    delay(1000);
    lcd.clear();
    status = GESLOTEN;
  }

}

void reset() {
  Serial.println("Reset");
  lcd.clear();
  code = "";
  pos = 0;
  changeRingColor(255, 0, 0);
}

void tekstOpLCD(byte start0, String boodschap0, byte start1, String boodschap1) {
  lcd.clear();
  lcd.setCursor(start0, 0); // 3 is de startpositie die ik koos op de LCD
  lcd.print(boodschap0);
  lcd.setCursor(start1, 1);
  lcd.print(boodschap1);
}

/*void veranderWachtwoord() {
  Serial.println("Voer het nieuwe wachtwoord in:");
  while (!Serial.available()) {}  // Wacht op input
  String nieuwWachtwoord = Serial.readStringUntil('\n');
  nieuwWachtwoord.trim();

  if (nieuwWachtwoord.length() < MAX_PASSWORD_LENGTH) {
    nieuwWachtwoord.toCharArray(wachtwoord, MAX_PASSWORD_LENGTH);
    Serial.println("Wachtwoord gewijzigd.");
  } else {
    Serial.println("Wachtwoord te lang!");
  }
}*/