void setup() {
  Serial.begin(9600);
}

const int dimension = 5;
const int obstacle = 50;
const bool seeProcess = true;

void loop() {
  Serial.println("-----");
  // Setting the goal and initializing the map:
  int map[dimension][dimension];
  int currentx = dimension/2;
  int currenty = dimension/2;
  int goal = createArray(&map[0][0], dimension, obstacle);
  int goalx = goal % dimension;
  int goaly = goal / dimension;

  // Booleans to end the loop when needed:
  bool stop = false;
  bool win = false;

  while (!stop){
    // Print the Current Map:
    if (seeProcess){
      for (int i = 0; i < dimension; i++){
        for (int j = 0; j < dimension; j++){
          Serial.print(map[i][j]);
          Serial.print(" ");
        }
        Serial.println();
      }
    }
    Serial.println("-----");

    // Checking directions:
    int firstDirection = getDirection(currentx, currenty, goalx, goaly);
    int secondDirection = getSecondDirection(currentx, currenty, goalx, goaly);
    int thirdDirection = getThirdDirection(secondDirection);
    int fourthDirection = getLastDirection(firstDirection);
    int firstChoice = checkDirection(firstDirection, &map[0][0], currentx, currenty, dimension);
    int secondChoice = checkDirection(secondDirection, &map[0][0], currentx, currenty, dimension);
    int thirdChoice = checkDirection(thirdDirection, &map[0][0], currentx, currenty, dimension);
    int fourthChoice = checkDirection(fourthDirection, &map[0][0], currentx, currenty, dimension);

    // Deciding movement:
    int newPosition = currenty * dimension + currentx;
    if (firstChoice == 10){
      stop = true;
      win = true;
      newPosition = move(&map[0][0], currentx, currenty, dimension, firstDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (firstChoice == 0){
      newPosition = move(&map[0][0], currentx, currenty, dimension, firstDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (secondChoice == 10){
      stop = true;
      win = true;
      newPosition = move(&map[0][0], currentx, currenty, dimension, secondDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (secondChoice == 0){
      newPosition = move(&map[0][0], currentx, currenty, dimension, secondDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (thirdChoice == 10){
      stop = true;
      win = true;
      newPosition = move(&map[0][0], currentx, currenty, dimension, thirdDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (thirdChoice == 0){
      newPosition = move(&map[0][0], currentx, currenty, dimension, thirdDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (fourthChoice == 10){
      stop = true;
      win = true;
      newPosition = move(&map[0][0], currentx, currenty, dimension, fourthDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (fourthChoice == 0){
      newPosition = move(&map[0][0], currentx, currenty, dimension, fourthDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (firstChoice == 1){
      newPosition = move(&map[0][0], currentx, currenty, dimension, firstDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (secondChoice == 1){
      newPosition = move(&map[0][0], currentx, currenty, dimension, secondDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (thirdChoice == 1){
      newPosition = move(&map[0][0], currentx, currenty, dimension, thirdDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else if (fourthChoice == 1){
      newPosition = move(&map[0][0], currentx, currenty, dimension, fourthDirection, firstChoice, secondChoice, thirdChoice, fourthChoice);
    }
    else{
      stop = true;
    }

    // Updating current position:
    currentx = newPosition % dimension;
    currenty = newPosition / dimension;
  }

  // After the procedure:
  if (win){
    Serial.println("Goal reached!");
  }
  else{
    Serial.println("Goal is blocked.");
  }
  for (int i = 0; i < dimension; i++){
    for (int j = 0; j < dimension; j++){
      Serial.print(map[i][j]);
      Serial.print(" ");
    }
    Serial.println();
  }
  Serial.println("-----");
  delay(20000);
}

int checkDirection(int direction, int* theArray, int currentx, int currenty, int dimensions){
  if (direction == 1){
    if (currenty - 1 < 0){
      return 2;
    }
    return theArray[(currenty-1) * dimensions + currentx];
  }
  else if (direction == 2){
    if (currentx + 1 == dimensions){
      return 2;
    }
    return theArray[currenty * dimensions + currentx+1];
  }
  else if (direction == 3){
    if (currenty + 1 == dimensions){
      return 2;
    }
    return theArray[(currenty+1) * dimensions + currentx];
  }
  else{
    if (currentx - 1 < 0){
      return 2;
    }
    return theArray[currenty * dimensions + currentx-1];
  }
}

int getDirection(int currentx, int currenty, int goalx, int goaly){
  bool north = false;
  bool west = false;
  if (goalx < currentx){
    west = true;
  }
  if (goaly < currenty){
    north = true;
  }
  if (abs(goaly - currenty) < abs(goalx - currentx)){
    if (west){
      return 4;
    }
    return 2;
  }
  else{
    if (north){
      return 1;
    }
    return 3;
  }
}

int getSecondDirection(int currentx, int currenty, int goalx, int goaly){
  int never = getDirection(currentx, currenty, goalx, goaly);
  if (never == 1 || never == 3){
    if (goalx < currentx){
      return 4;
    }
    return 2;
  }
  else{
    if (goaly < currenty){
      return 1;
    }
    return 3;
  }
}

int getThirdDirection(int secondDirection){
  if (secondDirection == 1 || secondDirection == 3){
    return 4 - secondDirection;
  }
  if (secondDirection % 4 == 2){
    return 4;
  }
  return 2;
}

int getLastDirection(int firstDirection){
  if (firstDirection == 1 || firstDirection == 3){
    return 4 - firstDirection;
  }
  if (firstDirection % 4 == 2){
    return 4;
  }
  return 2;
}

int move(int* theArray, int currentx, int currenty, int dimensions, int direction, int side1, int side2, int side3, int side4){
  // Set old position:
  int sum = 6 - (side1 + side2 + side3 + side4);
  if (sum >= 1){
    theArray[currenty * dimensions + currentx] = 1;
  }
  else if (sum <= 0){
    theArray[currenty * dimensions + currentx] = 2;
  }

  // Set new position:
  if (direction == 1){
    int pos = (currenty-1) * dimensions + currentx;
    theArray[pos] = 3;
    return pos;
  }
  else if (direction == 2){
    int pos = currenty * dimensions + currentx+1;
    theArray[pos] = 3;
    return pos;
  }
  else if (direction == 3){
    int pos = (currenty+1) * dimensions + currentx;
    theArray[pos] = 3;
    return pos;
  }
  else{
    int pos = currenty * dimensions + currentx-1;
    theArray[pos] = 3;
    return pos;
  }
}

int createArray(int* theArray, int dimensions, int obstacleChance){
  // Creating the obstacles:
  for (int i = 0; i < dimensions; i++){
    for (int j = 0; j < dimensions; j++){
      if (random(0, 100) < obstacleChance){
        theArray[i * dimensions + j] = 2;
      }
      else{
        theArray[i * dimensions + j] = 0;
      }
    }
  }

  // Initializing the starting position:
  theArray[dimensions/2 * dimensions + dimensions/2] = 3;

  // Setting the position of the goal:
  int goal1 = random(0, dimensions);
  int goal2 = random(0, dimensions);
  while ((goal1 == dimensions/2) && (goal2 == dimensions/2)){
    goal1 = random(0, dimensions);
    goal2 = random(0, dimensions);
  }
  theArray[goal1 * dimensions + goal2] = 10;
  return goal1*dimensions+goal2;
}