#define RedNorth 22
#define RedWest 23
#define RedSouth 2
#define RedEast 4
#define GreenNorth 19
#define GreenWest 21
#define GreenSouth 5
#define GreenEast 18
#define NorthButton 12
#define WestButton 13
#define SouthButton 14
#define EastButton 27
#define OrangeLed 32

int i;
int j;
int North;
int West;
int South;
int East;
int CurrentCollumn=11;
int CurrentRow=1;
int Counter=0;
bool NorthCurrent=0;
bool NorthPrev=0;
bool WestCurrent=0;
bool WestPrev=0;
bool SouthCurrent=0;
bool SouthPrev=0;
bool EastCurrent=0;
bool EastPrev=0;
/*
int maze[19][19]={
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0},
{0, 1, 0, 2, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 2, 1, 1, 0},
{0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0},
{0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 2, 0, 0, 0, 1, 0, 1, 0},
{0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0},
{0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0},
{0, 1, 1, 1, 1, 1, 0, 1, 1, 2, 1, 1, 1, 1, 0, 1, 0, 2, 0},
{0, 2, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
{0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0},
{0, 1, 1, 2, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0},
{0, 1, 0, 1, 2, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 2, 1, 1, 0},
{0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0},
{0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 2, 1, 1, 1, 1, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};*/

int maze[25][25]={
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 1, 1, 1, 1, 0, 2, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
{0, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0},
{0, 1, 0, 1, 1, 1, 1, 1, 0, 2, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0},
{0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0},
{0, 2, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0},
{0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0},
{0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0},
{0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0},
{0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 2, 0, 1, 1, 1, 0, 2, 1, 1, 1, 1, 0, 1, 0},
{0, 2, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0},
{0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 2, 1, 1, 0, 1, 0},
{0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0},
{0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 0, 2, 0, 1, 1, 1, 0},
{0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
{0, 1, 1, 1, 0, 1, 0, 2, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0},
{0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0},
{0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0},
{0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0},
{0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0},
{0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0},
{0, 1, 1, 2, 0, 1, 1, 1, 0, 1, 1, 2, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};

void GoalReached(){
  digitalWrite(RedNorth, 0);
  digitalWrite(RedWest, 0);
  digitalWrite(RedSouth, 0);
  digitalWrite(RedEast, 0);
  digitalWrite(GreenNorth, 0);
  digitalWrite(GreenWest, 0);
  digitalWrite(GreenSouth, 0);
  digitalWrite(GreenEast, 0);
  delay(500);
  digitalWrite(RedNorth, 1);
  digitalWrite(RedWest, 1);
  digitalWrite(RedSouth, 1);
  digitalWrite(RedEast, 1);
  digitalWrite(GreenNorth, 1);
  digitalWrite(GreenWest, 1);
  digitalWrite(GreenSouth, 1);
  digitalWrite(GreenEast, 1);
  delay(500);
}

void LEDClear(){
  digitalWrite(RedNorth, 0);
  digitalWrite(RedWest, 0);
  digitalWrite(RedSouth, 0);
  digitalWrite(RedEast, 0);
  digitalWrite(GreenNorth, 0);
  digitalWrite(GreenWest, 0);
  digitalWrite(GreenSouth, 0);
  digitalWrite(GreenEast, 0);
  digitalWrite(OrangeLed, 0);
  delay(500);
}

void setup() {
  Serial.begin(9600);
  pinMode(OrangeLed, OUTPUT);
  pinMode(RedNorth, OUTPUT);
  pinMode(RedWest, OUTPUT);
  pinMode(RedSouth, OUTPUT);
  pinMode(RedEast, OUTPUT);
  pinMode(GreenNorth, OUTPUT);
  pinMode(GreenWest, OUTPUT);
  pinMode(GreenSouth, OUTPUT);
  pinMode(GreenEast, OUTPUT);
  pinMode(NorthButton, INPUT);
  pinMode(WestButton, INPUT);
  pinMode(SouthButton, INPUT);
  pinMode(EastButton, INPUT);
  pinMode(OrangeLed, OUTPUT);
  /*for(i=0; i<21; i++){
    Serial.println();
    for(j=0; j<21; j++){
      Serial.print(maze[i][j]);
    }
  }*/
 
}

void loop() {
  if(maze[CurrentRow][CurrentCollumn]==2){
    digitalWrite(OrangeLed, HIGH);
  }
  if(maze[CurrentRow][CurrentCollumn]!=3){
    North=maze[CurrentRow-1][CurrentCollumn];
    West=maze[CurrentRow][CurrentCollumn-1];
    South=maze[CurrentRow+1][CurrentCollumn];
    East=maze[CurrentRow][CurrentCollumn+1];

    digitalWrite(RedNorth, !North);
    digitalWrite(RedWest, !West);
    digitalWrite(RedSouth, !South);
    digitalWrite(RedEast, !East);
    digitalWrite(GreenNorth, North);
    digitalWrite(GreenWest, West);
    digitalWrite(GreenSouth, South);
    digitalWrite(GreenEast, East);

    NorthCurrent=digitalRead(NorthButton);
    WestCurrent=digitalRead(WestButton);
    SouthCurrent=digitalRead(SouthButton);
    EastCurrent=digitalRead(EastButton);

    if(NorthCurrent!=NorthPrev && North!=0){
      NorthPrev=NorthCurrent;
      if(NorthCurrent==1){
        maze[CurrentRow][CurrentCollumn]=1;
        CurrentRow--;
        //Serial.println("Észak");
        LEDClear();
      }
    }
    if(WestCurrent!=WestPrev && West!=0){
      WestPrev=WestCurrent;
      if(WestCurrent==1){
        maze[CurrentRow][CurrentCollumn]=1;
        CurrentCollumn--;
        //Serial.println("Nyugat");
        LEDClear();
      }
    }
    if(SouthCurrent!=SouthPrev && South!=0){
      SouthPrev=SouthCurrent;
      if(SouthCurrent==1){
        maze[CurrentRow][CurrentCollumn]=1;
        CurrentRow++;
        //Serial.println("Dél");
        LEDClear();
      }
    }
    if(EastCurrent!=EastPrev && East!=0){
      EastPrev=EastCurrent;
      if(EastCurrent==1){
      maze[CurrentRow][CurrentCollumn]=1;
      CurrentCollumn++;
      //Serial.println("Kelet");
      LEDClear();
      }
    }
  }
  else{
    GoalReached();
  }
}