#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET    -1 // Reset pin not used
#define SCREEN_ADDRESS 0x3C // I2C address for the OLED display

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define UP_BUTTON 4
#define DOWN_BUTTON 5
#define LEFT_BUTTON 2
#define RIGHT_BUTTON 3
#define A_BUTTON 6

#define GRID_SIZE_X 16
#define GRID_SIZE_Y 8
#define CELL_SIZE 8


#define START 2
#define FINISH 3
#define WALL 1

int cursorX = 0;
int cursorY = 0;
bool grid[GRID_SIZE_X][GRID_SIZE_Y];

int mapLevel1[8][16] = {
  {0, 0, 0, 0, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL,0, 0, 0, 0},
  {0, 0, 0, 0, WALL, START, WALL, 0, 0, 0, 0, WALL,0, 0, 0, 0},
  {0, 0, 0, 0, WALL, 0, 0, 0, WALL, WALL, 0, WALL ,0, 0, 0 ,0},
  {0, 0, 0, 0, WALL, 0, WALL, 0, WALL, 0, 0, WALL ,0, 0, 0, 0},
  {0, 0, 0, 0, WALL, 0, WALL, 0, WALL, WALL, 0, WALL, 0, 0, 0, 0},
  {0, 0, 0, 0, WALL, 0, WALL, 0, 0, 0, 0, WALL, 0, 0, 0, 0},
  {0, 0, 0, 0, WALL, 0, WALL, WALL, WALL, WALL, FINISH, WALL, 0, 0, 0, 0},
  {0, 0, 0, 0, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, 0 ,0, 0 ,0}
};

int mapLevel2[8][16] = {
  {WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL},
  {WALL, START, WALL, 0, 0, 0, 0, WALL, WALL, 0, 0, WALL, 0, 0, 0, WALL},
  {WALL, 0, 0, 0, WALL, WALL, 0, WALL, WALL, 0, 0, 0, WALL, WALL, 0, WALL},
  {WALL, 0, WALL, 0, WALL, 0, 0, WALL, WALL, 0, WALL, 0, WALL, 0, 0, WALL},
  {WALL, 0, WALL, 0, WALL, WALL, 0, WALL, WALL, 0, WALL, 0, WALL, WALL, 0, WALL},
  {WALL, 0, WALL, 0, 0, 0, 0, WALL, WALL, 0, WALL, 0, 0, 0, 0, WALL},
  {WALL, 0, WALL, WALL, WALL, WALL, 0, 0, 0, 0, WALL, WALL, WALL, 0, 0, FINISH},
  {WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL, WALL}
};

int (*currentMap)[16];

void setup() {
  currentMap = mapLevel1;
  pinMode(UP_BUTTON, INPUT_PULLUP);
  pinMode(DOWN_BUTTON, INPUT_PULLUP);
  pinMode(LEFT_BUTTON, INPUT_PULLUP);
  pinMode(RIGHT_BUTTON, INPUT_PULLUP);
  pinMode(A_BUTTON, INPUT_PULLUP);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  
  // Initialize grid
  /*
  for (int y = 0; y < GRID_SIZE_Y; y++) {
    for (int x = 0; x < GRID_SIZE_X; x++) {
      grid[x][y] = false;
  }
  */
  for (int y = 0; y < GRID_SIZE_Y; y++) {
    for (int x = 0; x < GRID_SIZE_X; x++) {
      grid[x][y] = false; // Start with all cells as empty
    }
  }

  // Define walls in the maze (true represents a wall)
  /*
  grid[1][1] = true;
  grid[1][2] = true;
  grid[2][1] = true;
  grid[3][1] = true;
  */
  for (int y = 0; y < 8; y++) {
    for (int x = 0; x < 8; x++) {
      if (currentMap[y][x] == START) {
        cursorX = x;
        cursorY = y;
      }
    }
  }


  // Ensure the starting cell and a path are clear
  // grid[0][0] = false;
  // grid[1][0] = false;
  // grid[0][1] = false;
}

void loop() {
  // Read button states and move cursor
  /*
  if (!digitalRead(UP_BUTTON) && cursorY > 0) {
    cursorY = cursorY - 1;
    delay(250);
  }
  if (!digitalRead(DOWN_BUTTON) && cursorY < GRID_SIZE_Y - 1){
     cursorY = cursorY + 1;
    delay(250);
  }
  if (!digitalRead(LEFT_BUTTON) && cursorX > 0){
    cursorX = cursorX - 1;
    delay(250);
  }
  if (!digitalRead(RIGHT_BUTTON) && cursorX < GRID_SIZE_X - 1){
    cursorX = cursorX +1; 
    delay(250);
  }*/
   if (!digitalRead(UP_BUTTON) && cursorY > 0){
    if (currentMap[cursorY - 1][cursorX] != WALL) {
      cursorY--;}
    delay(250);
  }
  if (!digitalRead(DOWN_BUTTON) && cursorY < GRID_SIZE_Y - 1){
    if (currentMap[cursorY + 1][cursorX] != WALL) {
      cursorY++;}
    delay(250);
  }
  if (!digitalRead(LEFT_BUTTON) && cursorX > 0){
    if (currentMap[cursorY][cursorX - 1] != WALL) {
      cursorX--;}
    delay(250);
  }
  if (!digitalRead(RIGHT_BUTTON) && cursorX < GRID_SIZE_X - 1){
    if (currentMap[cursorY][cursorX + 1] != WALL) {
      cursorX++;}
    delay(250);
  }

  // Select grid cell
  if (!digitalRead(A_BUTTON)) {
    grid[cursorX][cursorY] = !grid[cursorX][cursorY]; // Toggle cell state
    delay(200); // Debounce delay
  }

  if (currentMap[cursorY][cursorX] == FINISH) {
    // Display congratulations message
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0,0);
    display.println("Congratulations!");
    display.display();
    delay(1000); // Show the message for 3 seconds
    // if (currentMap == mapLevel1) {
    //   currentMap = mapLevel2;
    // }

    // Find the start position for the next level
    for (int y = 0; y < GRID_SIZE_Y; y++) {
      for (int x = 0; x < GRID_SIZE_X; x++) {
        if (currentMap[y][x] == START) {
          cursorX = x;
          cursorY = y;
        }
      }
    }  
    // Redraw the screen with the new level
    display.clearDisplay();
    for (int y = 0; y < GRID_SIZE_Y; y++) {
      for (int x = 0; x < GRID_SIZE_X; x++) {
        int cellX = x * CELL_SIZE;
        int cellY = y * CELL_SIZE;
        if (currentMap[y][x] == WALL) {
          display.fillRect(cellX, cellY, CELL_SIZE, CELL_SIZE, WHITE);
        }
      }
    }
    display.display();
    return;
  }

  display.clearDisplay();
  for (int y = 0; y < 8; y++) {
    for (int x = 0; x < 16; x++) {
      int cellX = x * CELL_SIZE;
      int cellY = y * CELL_SIZE;
      // Draw walls
      if (currentMap[y][x] == 1) {
        display.fillRect(cellX, cellY, CELL_SIZE, CELL_SIZE, WHITE);
      }
    }
  }
  /*
  // Draw the grid
  display.clearDisplay();
  for (int y = 0; y < GRID_SIZE_Y; y++) {
    for (int x = 0; x < GRID_SIZE_X; x++) {
      int cellX = x * CELL_SIZE;
      int cellY = y * CELL_SIZE;
      if (grid[x][y]) {
        display.fillRect(cellX, cellY, CELL_SIZE, CELL_SIZE, WHITE);
      } 
      // else {
      //   display.drawRect(cellX, cellY, CELL_SIZE, CELL_SIZE, WHITE);
      // }
      // Highlight the cursor position
      
    }
  }*/
  // Draw the cursor as a filled circle
    int cursorCenterX = cursorX * CELL_SIZE + CELL_SIZE / 2;
    int cursorCenterY = cursorY * CELL_SIZE + CELL_SIZE / 2;
    int cursorRadius = CELL_SIZE / 4; // Adjust the radius as needed
    display.fillCircle(cursorCenterX, cursorCenterY, cursorRadius, WHITE);
      
  display.display();
}