#include "Maze.h"
#include "HardwareSetup.h"
#include "mouse.h"
//const int maze_viseted = 8;   //0x08
//const int maze_west    = 16;  //0x0F
//const int maze_north   = 32;  //0x20
//const int maze_east    = 64;  //0x40
//const int maze_south   = 128; //0x80

bool mazeClass::atTarget(int pos) {
  return pos == target;
}

//return the number of cells in a line to the next unexplored cell or direction change - 1
int mazeClass::cellsToMoveForward(int pos, byte direc) {
  int answer = 0;
  while (direction_to_go(pos) == direc) {
    if (mazemap[pos].viseted) {
      answer++;
      pos = mouse.movePosForward(pos);  // calculate position forwad 1 square
    } else {
      break;
    }
  }
  return answer;
}

//return best direction to go from maze map if mouse was in position pos
byte mazeClass::direction_to_go(int pos) {

  byte low = 255;
  byte direction = 0;

  if (!(mazemap[pos].north)) {
    if (mazework[pos - Maze_Width] < low) {
      low = mazework[pos - Maze_Width];
      direction = north;
    }
  }
  if (!(mazemap[pos].east)) {
    if (mazework[pos + 1] < low) {
      low = mazework[pos + 1];
      direction = east;
    }
  }
  if (!(mazemap[pos].west)) {
    if (mazework[pos - 1] < low) {
      low = mazework[pos - 1];
      direction = west;
    }
  }
  if (!(mazemap[pos].south)) {
    if (mazework[pos + Maze_Width] < low) {
      low = mazework[pos + Maze_Width];
      direction = south;
    }
  }

  return direction;
}

void mazeClass::clear() {  //clear the maze for a new run
  //  for (int k = 0; k < Maze_Width*MazeDepth; k++) {
  //    mazemap[k].cell = pgm_read_byte(blank_maze + k);
  //    mazework[k] = 0;
  //  }
  int t = 0;
  for (int k = 0; k < Maze_Width * Maze_Depth; k++) {
    mazemap[k].cell = 0;
    if (k < Maze_Width) {  //add north wall at top of maze
      mazemap[k].north = 1;
    }
    if (k >= (start)) {  //add south wall at bottom of maze
      mazemap[k].south = 1;
    }
    if (k % Maze_Width == 0) {
      mazemap[k].west = 1;  //add west wall down the left hand side
      t = k + Maze_Width - 1;
    }
    if (k == t) {
      mazemap[k].east = 1;  //add east wall down the right hand side
    }
    //Serial.println(mazemap[k].cell,HEX);
  }
  mazemap[start].east = 1;  //add east wall to start
  mazemap[start].viseted = 1;
  mazemap[start + 1].west = 1;  //and the other side of the wall
  solve();
}

//block unknown paths
void mazeClass::block() {
  for (int k = 0; k < sizeof(mazemap); k++) {
    if (!(mazemap[k].viseted))
      mazemap[k].cell = 255;  //set all walls to 1
  }
}

void mazeClass::solve(void) {
  for (byte k = 0; k < 255; k++) {
    if (solvePart()) {
      break;
    }
  }
}

bool mazeClass::solvePart(void) {
  byte high;
  bool solved = true;
  for (int i = 0; i < sizeof(mazemap); i++) {
    if (i == target) {
      mazework[i] = 0;
    } else {
      high = 254;
      if (!(mazemap[i].north)) {
        if (mazework[(byte)i - Maze_Width] < high) {
          high = mazework[(byte)i - Maze_Width];
        }
      }

      if (!(mazemap[i].east)) {
        if (mazework[(byte)i + 1] < high) {
          high = mazework[(byte)i + 1];
        }
      }

      if (!(mazemap[i].south)) {
        if (mazework[(byte)i + Maze_Width] < high) {
          high = mazework[(byte)i + Maze_Width];
        }
      }

      if (!(mazemap[i].west)) {
        if (mazework[(byte)i - 1] < high) {
          high = mazework[(byte)i - 1];
        }
      }
      high++;
      if (mazework[(byte)i] != high) {
        mazework[(byte)i] = high;
        solved = false;
      }
    }
  }
  return solved;
}


//add walls to the maze, pos is the position of the mouse in the maze
void mazeClass::addWalls(int pos, bool northWall, bool eastWall, bool southWall, bool westWall) {

  if (!mazemap[pos].viseted) {
    if (northWall) {
      mazemap[pos].north = 1;
      if (pos > Maze_Width) mazemap[pos - Maze_Width].south = 1;
    }
    if (eastWall) {
      mazemap[pos].east = 1;
      if (pos < sizeof(mazemap)) mazemap[pos + 1].west = 1;
    }
    if (southWall) {
      mazemap[pos].south = 1;
      if (pos < start) mazemap[pos + Maze_Width].north = 1;
    }
    if (westWall) {
      mazemap[pos].west = 1;
      if (pos > 0) mazemap[pos - 1].east = 1;
    }
    mazemap[pos].viseted = 1;
  }
}


void mazeClass::list(int pos, byte direction) {  //list maze to debug window using text
  const char body[] = { '^', '>', 'v', '<' };
  int x, y;
  Serial.println();
  Serial.print(' ');
  for (x = 0; x < Maze_Width; x++) {  //list top of maze
    if (x <= 0xf) {
      Serial.print(' ');
    }
    Serial.print("  ");
    Serial.print(x, HEX);
  }
  Serial.println();

  for (y = 0; y < sizeof(mazemap); y += Maze_Width) {  //list most of the maze
    char centre = ' ';
    Serial.print("  ");
    for (x = y; x < y + Maze_Width; x++) {

      if (mazemap[x].north) {
        Serial.print("+---");
      } else {
        Serial.print("+   ");
      }
    }
    Serial.println('+');

    if (y / Maze_Width <= 0xf) {
      Serial.print(' ');
    }

    Serial.print(y / Maze_Width, HEX);
    for (x = y; x < y + Maze_Width; x++) {
      centre = ' ';
      if (mazemap[x].west) {
        centre = '|';
      }
      Serial.print(centre);
      centre = ' ';
      Serial.print(centre);

      if (x == pos) {
        centre = body[direction];
      } else if (x == target) {
        centre = 'X';
      } else if (mazemap[x].viseted) {
        centre = '.';
      }
      Serial.print(centre);

      centre = ' ';
      Serial.print(centre);
    }
    if (mazemap[x - 1].east) {
      centre = '|';
    }
    Serial.println(centre);
  }

  Serial.print("  ");
  for (x = start; x < sizeof(mazemap); x++) {  //list bottem of the maze
    if (mazemap[x].south) {
      Serial.print("+---");
    } else {
      Serial.print("+   ");
    }
  }
  Serial.println("+\n");
}

void mazeClass::logCell(int pos) {
  Serial.print("/c=");
  if (pos < 16) {
    Serial.print('0');
  }
  if (pos < 256) {
    Serial.print('0');
  }
  Serial.print(pos, HEX);
  Serial.print(',');
  if (mazemap[pos].cell < 16) {
    Serial.print('0');
  }
  Serial.println(mazemap[pos].cell, HEX);
}

void mazeClass::logMaze() {
  for (int pos = 0; pos < Maze_Width * Maze_Depth; pos++) {
    logCell(pos);
  }
}