// =======================================================
// ROBOT LABYRINTHE PRO - FLOODFILL MICROMOUSE
// Arduino + L298N + capteurs distance
// =======================================================

#define SIZE 16

// -----------------------------
// MOTEURS
// -----------------------------

const int ENA = 5;
const int IN1 = 6;
const int IN2 = 7;

const int ENB = 9;
const int IN3 = 10;
const int IN4 = 11;

// -----------------------------
// CAPTEURS
// -----------------------------

const int trigFront = 2;
const int echoFront = 3;

const int trigLeft = 4;
const int echoLeft = 8;

const int trigRight = 12;
const int echoRight = 13;

// -----------------------------
// STRUCTURE LABYRINTHE
// -----------------------------

struct Cell
{
  bool wallN;
  bool wallE;
  bool wallS;
  bool wallW;

  int distance;
  bool visited;
};

Cell maze[SIZE][SIZE];

// position robot

int posX = 0;
int posY = 0;

// orientation
// 0 = NORD
// 1 = EST
// 2 = SUD
// 3 = OUEST

int direction = 0;

// centre objectif

int goalX = 7;
int goalY = 7;

// -----------------------------
// SETUP
// -----------------------------

void setup()
{

  Serial.begin(9600);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(trigFront, OUTPUT);
  pinMode(echoFront, INPUT);

  pinMode(trigLeft, OUTPUT);
  pinMode(echoLeft, INPUT);

  pinMode(trigRight, OUTPUT);
  pinMode(echoRight, INPUT);

  analogWrite(ENA, 160);
  analogWrite(ENB, 160);

  initMaze();
  floodFill();
}

// -----------------------------
// LOOP PRINCIPAL
// -----------------------------

void loop()
{

  scanWalls();

  maze[posX][posY].visited = true;

  floodFill();

  int bestDir = bestDirection();

  moveRobot(bestDir);
}

// =======================================================
// INITIALISATION LABYRINTHE
// =======================================================

void initMaze()
{

  for (int x = 0; x < SIZE; x++)
  {
    for (int y = 0; y < SIZE; y++)
    {

      maze[x][y].wallN = false;
      maze[x][y].wallE = false;
      maze[x][y].wallS = false;
      maze[x][y].wallW = false;

      maze[x][y].visited = false;

      maze[x][y].distance = abs(goalX - x) + abs(goalY - y);
    }
  }
}

// =======================================================
// FLOODFILL
// =======================================================

void floodFill()
{

  bool change = true;

  while (change)
  {

    change = false;

    for (int x = 0; x < SIZE; x++)
    {
      for (int y = 0; y < SIZE; y++)
      {

        int minNeighbour = 1000;

        if (!maze[x][y].wallN && y < SIZE - 1)
          minNeighbour = min(minNeighbour, maze[x][y + 1].distance);

        if (!maze[x][y].wallE && x < SIZE - 1)
          minNeighbour = min(minNeighbour, maze[x + 1][y].distance);

        if (!maze[x][y].wallS && y > 0)
          minNeighbour = min(minNeighbour, maze[x][y - 1].distance);

        if (!maze[x][y].wallW && x > 0)
          minNeighbour = min(minNeighbour, maze[x - 1][y].distance);

        if (maze[x][y].distance != minNeighbour + 1)
        {
          maze[x][y].distance = minNeighbour + 1;
          change = true;
        }
      }
    }
  }
}

// =======================================================
// CHOIX DIRECTION OPTIMALE
// =======================================================

int bestDirection()
{

  int best = 1000;
  int bestDir = direction;

  int nx, ny;

  for (int d = 0; d < 4; d++)
  {

    nx = posX;
    ny = posY;

    if (d == 0)
      ny++;
    if (d == 1)
      nx++;
    if (d == 2)
      ny--;
    if (d == 3)
      nx--;

    if (nx < 0 || ny < 0 || nx >= SIZE || ny >= SIZE)
      continue;

    if (isWall(d))
      continue;

    if (maze[nx][ny].distance < best)
    {
      best = maze[nx][ny].distance;
      bestDir = d;
    }
  }

  return bestDir;
}

// =======================================================
// MOUVEMENT ROBOT
// =======================================================

void moveRobot(int dir)
{

  int turn = dir - direction;

  if (turn == -3)
    turn = 1;
  if (turn == 3)
    turn = -1;

  if (turn == 1)
    turnRight();

  if (turn == -1)
    turnLeft();

  if (turn == 2 || turn == -2)
  {
    turnRight();
    turnRight();
  }

  forward();

  direction = dir;

  updatePosition();
}

// =======================================================
// POSITION
// =======================================================

void updatePosition()
{

  if (direction == 0)
    posY++;

  if (direction == 1)
    posX++;

  if (direction == 2)
    posY--;

  if (direction == 3)
    posX--;
}

// =======================================================
// DETECTION MURS
// =======================================================

void scanWalls()
{

  if (wallFront())
    setWall(direction);

  if (wallLeft())
    setWall((direction + 3) % 4);

  if (wallRight())
    setWall((direction + 1) % 4);
}

void setWall(int dir)
{

  if (dir == 0)
    maze[posX][posY].wallN = true;

  if (dir == 1)
    maze[posX][posY].wallE = true;

  if (dir == 2)
    maze[posX][posY].wallS = true;

  if (dir == 3)
    maze[posX][posY].wallW = true;
}

bool isWall(int dir)
{

  if (dir == 0)
    return maze[posX][posY].wallN;

  if (dir == 1)
    return maze[posX][posY].wallE;

  if (dir == 2)
    return maze[posX][posY].wallS;

  if (dir == 3)
    return maze[posX][posY].wallW;

  return false;
}

// =======================================================
// CAPTEURS DISTANCE
// =======================================================

bool wallFront()
{
  return distance(trigFront, echoFront) < 15;
}

bool wallLeft()
{
  return distance(trigLeft, echoLeft) < 15;
}

bool wallRight()
{
  return distance(trigRight, echoRight) < 15;
}

int distance(int trigPin, int echoPin)
{

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);

  int d = duration * 0.034 / 2;

  return d;
}

// =======================================================
// MOTEURS
// =======================================================

void forward()
{

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  delay(300);

  stopMotors();
}

void turnLeft()
{

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  delay(350);

  stopMotors();
}

void turnRight()
{

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);

  delay(350);

  stopMotors();
}

void stopMotors()
{

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);

  delay(100);
}

// =======================================================
// DEBUG
// =======================================================

void printMaze()
{

  for (int y = SIZE - 1; y >= 0; y--)
  {

    for (int x = 0; x < SIZE; x++)
    {

      Serial.print(maze[x][y].distance);
      Serial.print("\t");
    }

    Serial.println();
  }

  Serial.println();
}