#include <TM1637.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Arduino.h>


// #define NOTE_B0  31
// #define NOTE_C1  33
// #define NOTE_CS1 35
// #define NOTE_D1  37
// #define NOTE_DS1 39
// #define NOTE_E1  41
// #define NOTE_F1  44
// #define NOTE_FS1 46
// #define NOTE_G1  49
// #define NOTE_GS1 52
// #define NOTE_A1  55
// #define NOTE_AS1 58
// #define NOTE_B1  62
// #define NOTE_C2  65
// #define NOTE_CS2 69
// #define NOTE_D2  73
// #define NOTE_DS2 78
// #define NOTE_E2  82
// #define NOTE_F2  87
// #define NOTE_FS2 93
// #define NOTE_G2  98
// #define NOTE_GS2 104
// #define NOTE_A2  110
// #define NOTE_AS2 117
// #define NOTE_B2  123
// #define NOTE_C3  131
// #define NOTE_CS3 139
// #define NOTE_D3  147
// #define NOTE_DS3 156
// #define NOTE_E3  165
// #define NOTE_F3  175
// #define NOTE_FS3 185
#define NOTE_G3  196
// #define NOTE_GS3 208
// #define NOTE_A3  220
// #define NOTE_AS3 233
// #define NOTE_B3  247
#define NOTE_C4  262
// #define NOTE_CS4 277
// #define NOTE_D4  294
// #define NOTE_DS4 311
#define NOTE_E4  330
// #define NOTE_F4  349
// #define NOTE_FS4 370
#define NOTE_G4  392
// #define NOTE_GS4 415
// #define NOTE_A4  440
// #define NOTE_AS4 466
// #define NOTE_B4  494
#define NOTE_C5  523
#define NOTE_CS5 554
#define NOTE_D5  587
#define NOTE_DS5 622
#define NOTE_E5  659
// #define NOTE_F5  698
// #define NOTE_FS5 740
#define NOTE_G5  784
// #define NOTE_GS5 831
// #define NOTE_A5  880
// #define NOTE_AS5 932
// #define NOTE_B5  988
// #define NOTE_C6  1047
// #define NOTE_CS6 1109
// #define NOTE_D6  1175
// #define NOTE_DS6 1245
// #define NOTE_E6  1319
// #define NOTE_F6  1397
// #define NOTE_FS6 1480
// #define NOTE_G6  1568
// #define NOTE_GS6 1661
// #define NOTE_A6  1760
// #define NOTE_AS6 1865
// #define NOTE_B6  1976
// #define NOTE_C7  2093
// #define NOTE_CS7 2217
// #define NOTE_D7  2349
// #define NOTE_DS7 2489
// #define NOTE_E7  2637
// #define NOTE_F7  2794
// #define NOTE_FS7 2960
// #define NOTE_G7  3136
// #define NOTE_GS7 3322
// #define NOTE_A7  3520
// #define NOTE_AS7 3729
// #define NOTE_B7  3951
// #define NOTE_C8  4186
// #define NOTE_CS8 4435
// #define NOTE_D8  4699
// #define NOTE_DS8 4978
#define MAX_GAME_LENGTH 100


// PinNumbers for buttons - PinNumbers for DigitDisplay - PinNumbers for EnergyLeds - PinNumbers for PotentioMeters - LCD I2c Adres + Setup
const int8_t whiteButton = 0, blueButton = 1, numberLocker = 2,codeChecker = 3, redButton = 4, yellowButton = 5, greenButton = 6,CLK = 7,DIO = 8, speakerPin = 10, multiPlex1 = 11,multiPlex2 = 12,multiPlex3 = 13, PositionSelector = A0, ValueSelector = A1,  bluePin = A3, greenPin = A4, redPin = A5,lcdAddress = 0x27,cols = 16,rows = 2, firstRow = 0, secondRow = 1, maxObstaclesInPlay = 10, lastCollumn = 15;
int8_t pos = 0, value = 0, CurrentAssignment = 0,methodCounter = 0, gameIndex = 0;

const int8_t gameTones[] = { NOTE_G3, NOTE_C4, NOTE_E4, NOTE_G5};
const int8_t Buttons[] = {redButton,yellowButton,greenButton,blueButton,whiteButton};
const int8_t Leds[] = {multiPlex1,multiPlex2,multiPlex3};

const int8_t numLanes = 2;
int8_t playerLane = 0; // Start in the first lane
bool LockedOut = true, FailedAttempt = false;

const int8_t MAZE_HEIGHT = 16;
const int8_t MAZE_WIDTH = 20;

const int8_t ESCAPEPATH_HEIGHT = 3;
const int8_t ESCAPEPATH_WIDTH = 16;

int ledStates[6][3] = {
    {1, 0, -1},  // redLed
    {0, 1, -1},  // yellowLed
    {1, -1, 0},   // greenLed
    {0, -1, 1},  // blueLed
    {-1, 1, 0},  // whiteLed
    {-1, -1,-1} // All leds off
};
unsigned long lastDebounceTime = 0 ,debounceDelay = 250, previousMillis = 0;
const unsigned long interval = 1000,selectBlinkIndication = 500; // Blink interval in milliseconds

const int8_t correctCode[] = {0,4,5,1};
int8_t LockedInCode[]  ={0,4,5,1}; 

// Define a struct to hold potentiometer and display values
struct SensorValues {
  int16_t potValue;
  int16_t displayValue;
};

struct Position {
    int8_t x;
    int8_t y;
};

enum Direction {
    UP,
    DOWN,
    LEFT,
    RIGHT,
    NONE
};

enum Roadblock {
    Flag = 1,
    STRAIGHT = 3,
    BEND_DOWN_RIGHT = 4,
    BEND_UP_RIGHT = 5,
    BEND_UP_LEFT = 6,
    BEND_DOWN_LEFT = 7
};

TM1637 tm(CLK, DIO);

// Maak een LiquidCrystal_I2C object aan
LiquidCrystal_I2C lcd(lcdAddress, cols, rows);

void buttonInterrupt(); // Function prototype
void PrintOnLCDScreen(String text, int8_t row, int8_t collumn = 0, bool clearSpace = true);
void PrintOnLCDScreen(int8_t specialChar, int8_t row, int8_t collumn = 0, bool clearSpace = true);
void PrintOnDigitDisplay(TM1637 tm, int time);
void IndicatePositionByBlinking(int8_t pos, int8_t value, unsigned long currentMillis, unsigned long selectBlinkIndication);
void ShowCode(TM1637 tm);
void checkCode();
void lockInAnswer(int8_t pos);
void setNumber(int8_t position, int8_t number);
SensorValues readSensorValues(int PositionSelector, int ValueSelector);
bool arraysEqual(int array1[], int array2[], int size);
void displayMaze();
void Assignment1(SensorValues values);
void Assignment2(SensorValues values);
void Assignment3();
void Assignment4();
void Assignment5();
void updateLEDs(int8_t ledIndex);
void simonSays();
template <typename T, int8_t N>
int8_t sizeOfArray(T (&array)[N]);
void UpdatePositionsOfObstacles(int8_t numObstacles);
void templeRun();
Position mappingValues(SensorValues values, int8_t posMax, int8_t displayMax);
int8_t readButtons();


void setup() {
  // Define custom characters for the robot and flag
const byte robotChar[8] = {
  B00000,
  B00000,
  B00000,
  B00011,
  B01111,
  B01010,
  B11011,
  B11111
};

const byte flagChar[8] = {
  B00110,
  B01110,
  B11110,
  B01110,
  B00110,
  B00010,
  B00010,
  B00010
};

const byte filledBarSection[8] = {
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111,
  B11111
};

const byte straight[8] = {
  B01100,
  B01100,
  B01100,
  B01100,
  B01100,
  B01100,
  B01100,
  B01100
};

const byte bendDownRight[8] = {
  B00000,
  B00000,
  B00000,
  B01111,
  B01111,
  B01100,
  B01100,
  B01100
};

const byte bendUpRight[8] = {
  B01110,
  B01110,
  B01110,
  B01111,
  B01111,
  B00000,
  B00000,
  B00000
};

const byte bendUpLeft[8] = {
  B01100,
  B01100,
  B01100,
  B11100,
  B11100,
  B00000,
  B00000,
  B00000
};


const byte bendDownLeft[8] = {
  B00000,
  B00000,
  B00000,
  B11100,
  B11100,
  B01100,
  B01100,
  B01100
};
  // Initialiseer de communicatie via de I2C-bus
  // The following line primes the random number generator.
  // It assumes pin A5 is floating (disconnected):
  randomSeed(analogRead(A5));
  Wire.begin();
  tm.init();
  tm.set(BRIGHT_TYPICAL);
  pinMode(speakerPin, OUTPUT);
  pinMode(PositionSelector, INPUT);
  pinMode(ValueSelector, INPUT);
  pinMode(numberLocker, INPUT_PULLUP);
  pinMode(codeChecker, INPUT_PULLUP);

  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(bluePin, OUTPUT);
  for (int8_t i = 0; i < 5; i++) 
  {
    pinMode(Buttons[i], INPUT_PULLUP);
  }
  attachInterrupt(digitalPinToInterrupt(numberLocker), buttonInterrupt, FALLING); // Attach interrupt to the button pin
  attachInterrupt(digitalPinToInterrupt(codeChecker), checkCode, FALLING); // Attach interrupt to the button pin
  Serial.begin(115200);
  lcd.init();   
  lcd.backlight();
  lcd.setCursor(0,0);
  int8_t robotCharIndex = 0, flagCharIndex = 1, progressBarIndex = 2, straightIndex = 3, bendDownRightIndex = 4, bendUpRightIndex = 5, bendUpLeftIndex = 6, bendDownLeftIndex = 7;
  lcd.createChar(robotCharIndex,robotChar);
  lcd.createChar(flagCharIndex, flagChar);
  lcd.createChar(progressBarIndex, filledBarSection);
  lcd.createChar(straightIndex, straight);
  lcd.createChar(bendDownRightIndex, bendDownRight);
  lcd.createChar(bendUpRightIndex, bendUpRight);
  lcd.createChar(bendUpLeftIndex, bendUpLeft);
  lcd.createChar(bendDownLeftIndex, bendDownLeft);
}

void loopThroughLeds(){
    for (int i = 0; i <= 5; i++) {
    updateLEDs(i);
    delay(500);
  }
}

void loop() 
{
   switch (CurrentAssignment) 
   {
    case 0:
      Assignment1();
    break;
    case 1:
      Assignment2();
      playLevelUpSound();
    break;
    case 2:
       Assignment3();
       playLevelUpSound();
    break;
    case 3:
      Assignment4(); 
      playLevelUpSound();
    break;
    case 4:
      Assignment5();
      playLevelUpSound();
    break;
    case 5:
      summarySection();
    break;
    default:
     summarySection();
    break;
  }
}

void Assignment1() {
  unsigned long beginTime = millis();
  while (LockedOut)
  {
    SensorValues values = readSensorValues(PositionSelector, ValueSelector);
    showCode(tm);
    Position displayControl = mappingValues(values,3,9);
    unsigned long currentMillis = millis();
    IndicatePositionByBlinking(tm,displayControl.x,displayControl.y, currentMillis, selectBlinkIndication);
    if (FailedAttempt){
      gameOver();
      FailedAttempt = false;
    }
  }
  unsigned long endTime = millis();
  CurrentAssignment++; 
  playLevelUpSound();
  gameIndex += endTime - beginTime;
}

void Assignment2() 
{
  simonSays();
  CurrentAssignment++;
}

void Assignment3(){
  templeRun(whiteButton,redButton);
  CurrentAssignment++;
}

void Assignment4(){
    displayMaze();
}

void Assignment5(){
    escapeSystem(gameIndex*10, whiteButton,redButton);
    CurrentAssignment++;
}
// Function to generate tones with the buzzer
void playTone(int frequency, int duration) {
    tone(speakerPin, frequency, duration);
    delay(duration);
}

// Summary section method
void summarySection() {
    // Count up from zero to points
    for (int i = 0; i <= gameIndex; i++) {
        // Display the current number on the digital display
        PrintOnDigitDisplay(tm,i);
        
        // Play a tone with increasing frequency
        playTone(1000 + i * 100, 100); // Adjust the frequency increment as needed
    }
}


void playBuzzer(int8_t Hz, int8_t time){
  tone(speakerPin, Hz, time); 
}

void burnLed(int8_t Led){
  digitalWrite(Led, LOW);
}

void clearLCDSpace(int8_t row, int8_t column = 0, int8_t spaces = 16){
  lcd.setCursor(column, row); 
  for (int i = 0; i < spaces; i++) {
    lcd.print(" "); // Print spaces to clear part of the screen
  }
}

void PrintOnDigitDisplay(TM1637 tm, int number) {
  // If the number is greater than 9999, limit it to 9999
  if (number > 9999) {
    number = 9999;
  }
   if (number < 0) {
    number = 0000;
  }
  
  // Display each digit on the digital display
  tm.display(0, (number / 1000) % 10);
  tm.display(1, (number / 100) % 10);
  tm.display(2, (number / 10) % 10);
  tm.display(3, number % 10);
}

void PrintOnLCDScreen(String text, int8_t row, int8_t collum = 0, bool clearSpace = true) {
  if (clearSpace)
    clearLCDSpace(row,collum);
  lcd.setCursor(collum, row); // Stel de cursorpositie in op de tweede regel
  lcd.print(text); // Print de tekst naar het LCD-scherm
}

void PrintOnLCDScreen(int8_t specialChar, int8_t row, int8_t collum = 0, bool clearSpace = true) {
  if (clearSpace)
    clearLCDSpace(row,collum);
  lcd.setCursor(collum, row); // Stel de cursorpositie in op de tweede regel
  lcd.write(specialChar); // display special char naar het LCD-scherm
}

void IndicatePositionByBlinking(TM1637 tm, int8_t pos, int8_t value, unsigned long currentMillis, unsigned long selectBlinkIndication)
{
    if ((currentMillis / selectBlinkIndication) % 2 == 0) {
    tm.display(pos, value); // Display the digit
  } else {
    tm.display(pos, 0x7F); // Clear the digit (0x7F is used to turn off the segment)
  }
}

void lockInAnswer(int8_t pos, int8_t value) {
  setNumber(pos,value);
}

// Function to set a number at a specific position in the array
void setNumber(int8_t position, int8_t number) {
  if (position >= 0 && position < 4) { // Check if the position is valid
    LockedInCode[position] = number; // Set the number at the specified position
  } else {
  //  Serial.println("Invalid position!"); replace with feedback
  }
}

void showCode(TM1637 tm){
  for (int i = 0; i < sizeof(LockedInCode) / sizeof(LockedInCode[0]); i++) {
    if (i != pos) 
    {
      tm.display(i,LockedInCode[i]);
    }
  }
}

void checkCode() {
 // Read the current time
  unsigned long currentMillis = millis();

  // Check if enough time has passed since the last button press
  if (currentMillis - lastDebounceTime > debounceDelay) {
    lastDebounceTime = currentMillis;
    if (arraysEqual(LockedInCode,correctCode,4))
    {
    //  Serial.println("Granted Access!");
      LockedOut = false;
    }
    else {
        //Serial.println("Access Denied!"); replace with feedback with leds and buzzer
        FailedAttempt = true;
    }
  }
}

void buttonInterrupt() {

   // Read the current time
  unsigned long currentMillis = millis();

  // Check if enough time has passed since the last button press
  if (currentMillis - lastDebounceTime > debounceDelay) 
  {
  lastDebounceTime = currentMillis;
  lockInAnswer(pos,value);
  }
}

bool arraysEqual(int8_t array1[], int8_t array2[], int8_t size) {
  for (int i = 0; i < size; i++) {
    if (array1[i] != array2[i]) {
      return false; // Arrays are different
    }
  }
  return true; // Arrays are identical
}


// Function to read potentiometer and display values and return them in a struct
SensorValues readSensorValues(int PositionSelector, int ValueSelector) {
  SensorValues values;
  values.potValue = analogRead(PositionSelector);
  values.displayValue = analogRead(ValueSelector);
 // Serial.println("Gathering values: " + String(values.potValue) + " " + String(values.displayValue));
  return values;
}

Position mappingValues(SensorValues values, int8_t posLimit, int8_t valueLimit){
    Position position;
 // Serial.println("Mapping values: " + String(values.potValue) + " " + String(values.displayValue));
    pos = map(values.potValue, 0, 1023, 0, posLimit);
    value = map(values.displayValue, 0, 1023, 0, valueLimit);

    position.x = pos;
    position.y = value;
    return position;
}


bool operator==(const Position& p1, const Position& p2) {
    return p1.x == p2.x && p1.y == p2.y;
}

bool operator!=(const Position& p1, const Position& p2) {
    return p1.x != p2.x || p1.y != p2.y;
}


void displayMaze() {

  // Define the maze layout using multiple arrays
  char maze[MAZE_HEIGHT][MAZE_WIDTH + 1] = {
      "####################", // Row 1
      "#                  #", // Row 2
      "# #######   ########", // Row 3
      "# #     #   #     ##", // Row 4
      "# # ####### ###### #", // Row 5
      "# #                #", // Row 6
      "# # ### #### ### # #", // Row 7
      "# #     #  #     # #", // Row 8
      "# #######  # #######", // Row 9
      "#         ##       ", // Row 10
      "####### # ## #######", // Row 11
      "#    #  # ## #     #", // Row 12
      "# ## #    ## #######", // Row 13
      "# ## ######  #     #", // Row 14
      "#                 ##", // Row 15
      "####################"  // Row 16
  };
  bool inMaze = true;
  Position robotPosition = Position{13,13};
  Position flagPosition = Position{14,13};
  Position previousRobotPosition = Position{0,0};
  maze[robotPosition.x][robotPosition.y] = 'R';
  maze[flagPosition.x][flagPosition.y] = 'F';
  int8_t previousValue = 0;
  int8_t previousPos = 0;
  unsigned long lastMovementTime, movementSpeed = 500;

  while(inMaze)
  {  // Display the maze layout
    SensorValues values = readSensorValues(PositionSelector, ValueSelector);
    Position mazeControl = mappingValues(values, 14,4);
    if (previousPos != mazeControl.x || robotPosition != previousRobotPosition)
    {
      lcd.setCursor(0, 0);
      lcd.print(maze[mazeControl.x]);

      lcd.setCursor(0, 1);
      lcd.print(maze[mazeControl.x + 1]);
    }
    // Scroll the display to the right
    int8_t deltaValue = previousValue - mazeControl.y;
    if (deltaValue != 0)
    {
      if (deltaValue > 0)
        for (int i = 0; i < deltaValue; i++) 
          lcd.scrollDisplayRight();
      else 
        for (int i = 0; i > deltaValue; i--) 
          lcd.scrollDisplayLeft();
    }
    previousValue = mazeControl.y;
    previousPos = mazeControl.x;
    previousRobotPosition = robotPosition;

    unsigned long currentTime = millis();
    if (currentTime - lastMovementTime >= movementSpeed) {
      int8_t button = readButtons(false);
      Direction direction = getDirection(button);
      if (direction != NONE)
        {
          inMaze = driveInToDirection(maze,direction,robotPosition);
          lastMovementTime = millis();
        }
      if (robotPosition == flagPosition)
        inMaze = false;
    }
  }

  lcd.clear();
  PrintOnLCDScreen("Out of maze",0,0,true);
  
  if (robotPosition == flagPosition)
    CurrentAssignment++;
}

void escapeSystem(int initialTime, int8_t turnLeft, int8_t turnRight) {

  Roadblock escapePath[ESCAPEPATH_HEIGHT][ESCAPEPATH_WIDTH] = {
    {BEND_DOWN_LEFT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT, STRAIGHT}, // Row 1
    {BEND_UP_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, BEND_DOWN_LEFT, BEND_DOWN_RIGHT, Flag}, // Row 2
    {STRAIGHT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, BEND_UP_RIGHT, BEND_UP_LEFT, STRAIGHT}, // Row 3
  };
    // Start timer at 4526 seconds
    int16_t remainingTime = initialTime;
    bool escapedSystem = false;
    bool blinkState = false;
    Position flagPosition = Position{15,1};

    PrintOnLCDScreen("Welcome to the end the solve the path and escape the system",0,0, true);
    delay(1000);
    while (remainingTime > 0 && !escapedSystem) 
    {
      // Print remaining time on digit display
      PrintOnDigitDisplay(tm, remainingTime);

      // Sound buzzer
    
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillis >= selectBlinkIndication) {
      // Save the last time the blink state was toggled
      previousMillis = currentMillis;

      // Toggle the blink state
        blinkState = !blinkState;
      }
      // Timer
      // Buzzer also on each passing second
      // Leds Counting down as wel / Going nuts
      // Something on LCD SCreen to defuse bomb / show bomb

      // Success / Fail state

      // Update LEDs to indicate countdown
      lightLedAndPlayTone(map(remainingTime / 1000,0,9,0,4));

      SensorValues values = readSensorValues(PositionSelector, ValueSelector);
      Position escapePathControl = mappingValues(values, 2,14);
      // Decrement remaining time
      remainingTime--;

      buildRoad(escapePathControl.x,escapePathControl.y,turnLeft,turnRight,blinkState,escapePath);
      escapedSystem = checkConnection(escapePath,flagPosition);
  }
  clearLCDSpace(0,0);
  clearLCDSpace(1,0);

  if (escapedSystem) {
    PrintOnLCDScreen("Congratulations you have made it out of the system alive",0,0, true);
  //  Serial.println("Maze solved");
  } 
  else {
    PrintOnLCDScreen("BOOOM!!, The system has self destructed all your hard work has been for nothing better luck next time",1,0, true);
  }
  delay(1000);
  // CurrentAssignment++;
}
// Function to display the road on the LCD screen
void displayRoad(int8_t startPosition, int8_t selectedCollumn, bool blink, Roadblock Path[ESCAPEPATH_HEIGHT][ESCAPEPATH_WIDTH]) {
  if (startPosition == 2)
    clearLCDSpace(1,0);
   for (int8_t row = startPosition; row < ESCAPEPATH_HEIGHT; row++ )
    for(int8_t collumn = 0; collumn < ESCAPEPATH_WIDTH; collumn++) {
      if (row - startPosition == 0 && collumn == selectedCollumn && blink)
      {
        clearLCDSpace(row - startPosition,collumn,1);
      }
      else
        PrintOnLCDScreen((int8_t)Path[row][collumn],row - startPosition,collumn,false);
    }
}

const char* directionToString(Direction dir) {
    switch (dir) {
        case UP: return "UP";
        case DOWN: return "DOWN";
        case LEFT: return "LEFT";
        case RIGHT: return "RIGHT";
        case NONE: return "NONE";
        default: return "INVALID";
    }
}
// make first static a turn from Left to Down
bool checkConnection(Roadblock Path[ESCAPEPATH_HEIGHT][ESCAPEPATH_WIDTH], Position flagLocation)
{
  bool connected = true;
  Position roadPosition{0,0};
  Roadblock endPosition = Path[roadPosition.y][roadPosition.x];
  int8_t adjacentPosition;
  Direction outputPrevious = RIGHT;
  int8_t roadLength = 0;
  while (connected)
  {
    Direction outputDirection =  getDirection(outputPrevious, endPosition);

    // based on direction row++/-- or collumn++/--
    if (outputDirection == UP && roadPosition.y > 0) 
      roadPosition.y--; // Move up
    else if (outputDirection == DOWN && roadPosition.y <= 3) 
      roadPosition.y++; // Move down
    else if (outputDirection == LEFT && roadPosition.x > 0) 
      roadPosition.x--; // Move left
    else if (outputDirection == RIGHT && roadPosition.x <= 15) 
      roadPosition.x++; // Move right

    adjacentPosition = Path[roadPosition.y][roadPosition.x];
    outputPrevious = outputDirection;
    Direction inputDirectionFromAdjacentOutput = getDirection(outputPrevious,adjacentPosition);
    connected = inputDirectionFromAdjacentOutput != NONE;
    if (endPosition == adjacentPosition)
      connected = false;
    endPosition = adjacentPosition;
 
    roadLength++;
  }
  if (endPosition == Path[flagLocation.y][flagLocation.x])
    return true;
  else
    return false;
}

bool ConnectedRoad(Direction output,Direction input)
{
  if (input == invertDirection(output))
      return true;
  return false;
}

void buildRoad(int8_t row, int8_t collumn, int8_t turnLeft, int8_t turnRight, bool blink, Roadblock Path[ESCAPEPATH_HEIGHT][ESCAPEPATH_WIDTH]) {

  int8_t roadBlock = Path[row][collumn];
    // Check for input to switch lanes
    if (digitalRead(turnLeft)  == LOW && roadBlock > 3) {
        roadBlock--;
    } else if (digitalRead(turnRight)  == LOW  && roadBlock < 7) {
        roadBlock++;
    }
  Path[row][collumn] = roadBlock;
  displayRoad(row,collumn, blink,Path);
}


bool driveInToDirection(char maze[MAZE_HEIGHT][MAZE_WIDTH + 1],Direction direction, Position &position){
  maze[position.x][position.y] = ' ';
      // Update nextRow and nextColumn based on direction
    if (direction == UP) {
        position.x--; // Move up
    } else if (direction == DOWN) {
        position.x++; // Move down
    } else if (direction == LEFT) {
        position.y--; // Move left
    } else if (direction == RIGHT) {
        position.y++; // Move right
    }

  if (maze[position.x][position.y] == '#'){
      return false;
  }
  else {
    maze[position.x][position.y] = 'R';
    return true;
  }
}

void updateLEDs(int8_t methodCounter) {
    for (int i = 0; i < 3; i++) {
        if (ledStates[methodCounter][i] == -1)
        {
            pinMode(Leds[i], INPUT);
            continue;
        }
        pinMode(Leds[i], OUTPUT);
        digitalWrite(Leds[i],ledStates[methodCounter][i]);
    }
}


void lightLedAndPlayTone(int8_t ledIndex) {
  updateLEDs(ledIndex);
  tone(speakerPin, gameTones[ledIndex]);
  delay(300);
  updateLEDs(5); // all leds off
  noTone(speakerPin);
}

/**
   Plays the current sequence of notes that the user has to repeat
*/
void playSequence(int8_t gameSequence[MAX_GAME_LENGTH], int8_t gameIndex) {
  for (int i = 0; i < gameIndex; i++) {
    int8_t currentLed = gameSequence[i];
    lightLedAndPlayTone(currentLed);
    delay(500);
  }
}

Direction getDirection(int8_t buttonNumber) {
    switch (buttonNumber) {
        case 0:
            return UP;
        case 1:
            return DOWN;
        case 2:
            return LEFT;
        case 3:
            return RIGHT;
        default:
            return NONE; // Default case if buttonNumber is out of range
    }
}

Direction invertDirection(Direction direction){
  switch (direction) {
        case UP:
            return DOWN;
        case DOWN:
            return UP;
        case LEFT:
            return RIGHT;
        case RIGHT:
            return LEFT;
        default:
            return NONE; // Default case if buttonNumber is out of range
    }
}

Direction getDirection(Direction previousOutput, Roadblock roadblock) {
    Direction neededInputDirection = invertDirection(previousOutput);
    Direction outputDirection = NONE;
    Direction roadblockDirection1;
    Direction roadblockDirection2;
    switch (roadblock) {
        case STRAIGHT:
            roadblockDirection1 = UP;
            roadblockDirection2 = DOWN;
            break;
        case BEND_DOWN_RIGHT:
            roadblockDirection1 = DOWN;
            roadblockDirection2 = RIGHT;
            break;
        case BEND_UP_RIGHT:
            roadblockDirection1 = UP;
            roadblockDirection2 = RIGHT;
            break;
        case BEND_UP_LEFT:
            roadblockDirection1 = UP;
            roadblockDirection2 = LEFT;
            break;
        case BEND_DOWN_LEFT:
            roadblockDirection1 = DOWN;
            roadblockDirection2 = LEFT;
            break;
        default:
            return NONE;
    }
    if (roadblockDirection1 == neededInputDirection || roadblockDirection2 == neededInputDirection) {
        if (ConnectedRoad(previousOutput, roadblockDirection1)) {
            outputDirection = roadblockDirection2;
        } else if (ConnectedRoad(previousOutput, roadblockDirection2)) {
            outputDirection = roadblockDirection1;
        }
    }
    return outputDirection;
}

/**
    Waits until the user pressed one of the buttons,
    and returns the index of that button
    lijst met buttons meegeven om door te loopen
*/
int8_t readButtons(bool waitForInput) {
  if (waitForInput) {
    while (true) 
    {
      for (int8_t i = 0; i < 5; i++) {
        int8_t buttonPin = Buttons[i];
        if (digitalRead(buttonPin) == LOW) {
          return i;
        }
      }
    }
  }
  else{
    for (int8_t i = 0; i < 5; i++) {
        int8_t buttonPin = Buttons[i];
        if (digitalRead(buttonPin) == LOW) {
          return i;
        }
  }
     return 5;
  }
}


/**
  Play the game over sequence, and report the game score
*/
void gameOver() {
  delay(200);
  // Play a Wah-Wah-Wah-Wah sound
  tone(speakerPin, NOTE_DS5);
  delay(300);
  tone(speakerPin, NOTE_D5);
  delay(300);
  tone(speakerPin, NOTE_CS5);
  delay(300);
  for (int8_t i = 0; i < 10; i++) {
    for (int pitch = -10; pitch <= 10; pitch++) {
      tone(speakerPin, NOTE_C5 + pitch);
      delay(5);
    }
  }
  noTone(speakerPin);
  delay(500);
}

/**
   Get the user's input and compare it with the expected sequence.
*/
bool checkUserSequence(int8_t gameSequence[MAX_GAME_LENGTH], int8_t gameIndex) {
  for (int i = 0; i < gameIndex; i++) {
    int8_t expectedButton = gameSequence[i];
    int8_t actualButton = readButtons(true);
    lightLedAndPlayTone(actualButton);
    if (expectedButton != actualButton) {
      return false;
    }
  }
  return true;
}

/**
   Plays a hooray sound whenever the user finishes a level
*/
void playLevelUpSound() {
  tone(speakerPin, NOTE_E4);
  delay(150);
  tone(speakerPin, NOTE_G4);
  delay(150);
  tone(speakerPin, NOTE_E5);
  delay(150);
  tone(speakerPin, NOTE_C5);
  delay(150);
  tone(speakerPin, NOTE_D5);
  delay(150);
  tone(speakerPin, NOTE_G5);
  delay(150);
  noTone(speakerPin);
}


void simonSays(){
  /* Global variables - store the game state */
  int8_t gameSequence[MAX_GAME_LENGTH] = {};
  bool inSimonSays = true;
  int8_t scoreSoFar = gameIndex;
  int8_t simonSaysGameScore = 0;
  while (inSimonSays)
  {
    gameSequence[simonSaysGameScore] = random(0, 5);
    simonSaysGameScore++;
    PrintOnDigitDisplay(tm,simonSaysGameScore);
    if (simonSaysGameScore >= MAX_GAME_LENGTH) {
      playLevelUpSound();
      inSimonSays = false;
      break;
    }
    playSequence(gameSequence, simonSaysGameScore);
    if (!checkUserSequence(gameSequence, simonSaysGameScore)) {
      gameOver();
      inSimonSays = false;
      break;
    }
    delay(300);
    playLevelUpSound();
    delay(300);
  }
  gameIndex = scoreSoFar + simonSaysGameScore;
}

void templeRun(int8_t buttonUP, int8_t buttonDown){
  int8_t intervall = 0, obstaclesInPlay = 1;
  bool inTempleRun = true;
  bool switchedLanes = false;
  Position firstObstacle = {firstRow, lastCollumn};
  Position obstacles[maxObstaclesInPlay] = {firstObstacle};
  PrintOnLCDScreen("X",obstacles[0].x, obstacles[0].y,false);
  PrintOnLCDScreen("O",playerLane, 0, false);

  unsigned long lastObstacleTime, obstacleInterval = 1000;
  while (inTempleRun)
  {
    PrintOnDigitDisplay(tm,gameIndex);
    if (switchedLanes) 
    {
      PrintOnLCDScreen("O",playerLane, 0, false);
      switchedLanes = false;
    }
    unsigned long currentTime = millis();
    if (currentTime - lastObstacleTime >= obstacleInterval) {
      lastObstacleTime = millis();
      UpdatePositionsOfObstacles(obstaclesInPlay, obstacles);
      inTempleRun = checkForColissions(obstaclesInPlay,obstacles);
      intervall++;
      if (obstaclesInPlay < 3 && intervall == 4)
      {
        intervall = 0;
        obstaclesInPlay = addNewobstacleToThecourse(obstaclesInPlay, obstacles);
      }
    }
    if (digitalRead(buttonUP) == LOW && playerLane > 0) {
      switchTempleLane(playerLane, false);
      switchedLanes = true;
    } else if (digitalRead(buttonDown) == LOW && playerLane < numLanes - 1) {
      switchTempleLane(playerLane, true);
      switchedLanes = true;
    }
  } 
  lcd.clear();
}

void switchTempleLane(int8_t currentLane, bool up){
    clearLCDSpace(currentLane,0,1);
    if (up)
     playerLane++;
    else
     playerLane--;
}

void UpdatePositionsOfObstacles(int8_t numObstacles, Position obstacles[maxObstaclesInPlay]) {
// Update obstacle positions and display them
    for (int i = 0; i < numObstacles; i++) {
      clearLCDSpace(obstacles[i].x,obstacles[i].y);
      obstacles[i].y--;
        if (obstacles[i].y < 0)
          relocateObstacleToTheEnd(obstacles);
      PrintOnLCDScreen("X", obstacles[i].x, obstacles[i].y, false);
    }
}

int8_t addNewobstacleToThecourse(int8_t numberOfObstaclesInPlay, Position obstacles[maxObstaclesInPlay]) {
  int obstacleLane = random(0, numLanes);
  PrintOnLCDScreen("X",obstacleLane, lastCollumn, false);
  obstacles[numberOfObstaclesInPlay] = Position{obstacleLane,lastCollumn};
  return ++numberOfObstaclesInPlay;
}

void relocateObstacleToTheEnd(Position obstacles[maxObstaclesInPlay]) {
    int obstacleLane = random(0, numLanes);
    obstacles[0] = obstacles[1];
    obstacles[1] = obstacles[2];
    obstacles[2] = {obstacleLane, lastCollumn}; // Assuming default position is (0, 0)
    gameIndex++;
    playLevelUpSound();
}

bool checkForColissions(int8_t numObstacles, Position obstacles[maxObstaclesInPlay]){
      // Check for collisions
    for (int i = 0; i < numObstacles; i++) {
      if (obstacles[i].x == playerLane && obstacles[i].y == 0) {
          gameOver();
        return false;
      }
    }
    return true;
}

template <typename T, int8_t N>
int8_t sizeOfArray(T (&array)[N]) {
    return N;
}