arduino_floodfill.cpp

//
// Flood fill algorithm implementation for Arduino
// Created by Lakshith on 8/7/22.
//

struct Coord {
    int x;
    int y;
};

struct WallMemory {
    bool top;
    bool left;
    bool bottom;
    bool right;
};

const int LENGTH = 14;
const int AREA = LENGTH * LENGTH;
const Coord TARGET = {LENGTH / 2 - 1, LENGTH / 2 - 1};

int floodFillArr[LENGTH][LENGTH];
WallMemory wallMemory[LENGTH][LENGTH];
Coord neighbours[4];

void updateWalls(Coord);
void initializeFloodFill();
void floodFill(Coord currentCoordinate);

void enqueue(Coord c);
Coord peak();
Coord dequeue();
void resetQueue();

inline int min(int a, int b) {
    return a > b ? b : a;
}

inline int max(int a, int b) {
    return a > b ? a : b;
}

inline int absolute(int a) {
    return a > 0 ? a : a * -1;
}

inline int index(Coord coordinate) { return LENGTH * coordinate.y + coordinate.x; }

inline int manhattanDistance(Coord currentCoordinate) {
    return absolute(currentCoordinate.x - TARGET.x) + absolute(currentCoordinate.y = TARGET.y);
}

// flood fill from the destination assuming that there are no walls
// mark that we don't know about any walls
void initializeFloodFill() {
    for (int i = 0; i < LENGTH; i++)
        for (int j = 0; j < LENGTH; j++) {
            floodFillArr[i][j] = manhattanDistance({i, j});
            wallMemory[i][j] = {false, false, false, false};
        }
}

Coord queue[AREA + 10];
int front = 0, rear = 0, size = 0;

void enqueue(Coord c) {
    if (size == AREA) {
        return;
    }

    size ++;

    front += 1;
    front %= AREA;

    queue[front] = c;
}

Coord peak() {
    return queue[rear];
}

Coord dequeue() {
    Coord r = queue[rear];
    size--;
    rear += 1;
    rear %= AREA;

    return r;
}

void resetQueue() {
    front = 0, rear = 0, size = 0;
}

void floodFill(Coord currentCoordinate) {
    updateWalls(currentCoordinate);

    resetQueue();
    enqueue(currentCoordinate);

    while (size != 0) {
        Coord takenCoordinate = dequeue();

        //get accessible neighbours
        int numNeighbours = 0;
        if (takenCoordinate.y != LENGTH - 1 && !wallMemory[takenCoordinate.x][takenCoordinate.y].top) {
            neighbours[numNeighbours++] = {takenCoordinate.x, takenCoordinate.y + 1};
        }
        if (takenCoordinate.y != 0 && !wallMemory[takenCoordinate.x][takenCoordinate.y].bottom) {
            neighbours[numNeighbours++] = {takenCoordinate.x, takenCoordinate.y - 1};
        }
        if (takenCoordinate.x != 0 && !wallMemory[takenCoordinate.x][takenCoordinate.y].left) {
            neighbours[numNeighbours++] = {takenCoordinate.x - 1, takenCoordinate.y};
        }
        if (takenCoordinate.x != LENGTH - 1 && !wallMemory[takenCoordinate.x][takenCoordinate.y].right) {
            neighbours[numNeighbours++] = {takenCoordinate.x + 1, takenCoordinate.y};
        }

        //get minimum value
        int minimum = 1e4;
        for (int i = 0; i < numNeighbours; i++) {
            minimum = min(minimum, floodFillArr[neighbours[i].x][neighbours[i].y]);
        }

        //update flood fill array
        if (minimum + 1 == floodFillArr[takenCoordinate.x][takenCoordinate.y]) {
            //do nothing. array is all good
        } else {
            floodFillArr[takenCoordinate.x][takenCoordinate.y] = minimum + 1;
            for (int i = 0; i < numNeighbours; i++) {
                enqueue(neighbours[i]);
            }
        }
    }
}

void updateWalls(Coord coordinate) {
    //check sensors and update walls
}

int main() {
    initializeFloodFill();
    floodFill({0, 0});
    return 0;
}