#include "api.h"
#include "mms.h"
#include <queue>
#include <string>
#include <sys/types.h>

#define W 15 /* ancho y alto del laberinto -1 */

struct maze { /* Distancias de Manhattan */
    int vwalls[15][15];
    int hwalls[15][15];
    int mhtn[16][16];
} maze;

struct fill {
    int x, y, z;
};

void flood(struct maze *, struct mmstats *, int coordinates[2]);
void follow(struct maze maze, class mmstats mms);
int *nextcell(int i, int j, int dir, int ndir[2]);
void updatepos(class mmstats *mms);
void locateWall(struct maze &maze);

void flood(struct maze *maze, struct mmstats *mmstats, int coordinates[2]) {
    int i, j;
    int prevNum;
    int marked[16][16] = {0};
    std::queue<fill> myqueue;

    myqueue.push({coordinates[0], coordinates[1], -1});

    while (myqueue.size()) {
        i = myqueue.front().x;
        j = myqueue.front().y;
        prevNum = myqueue.front().z;
        myqueue.pop();

        if (i < 0 || i > W || j < 0 || j > W || marked[i][j] == 1) {
            continue;
        } else {
            API::setText(i, j, prevNum + 1);
            maze->mhtn[i][j] = prevNum + 1;
            marked[i][j] = 1;
            if (!maze->vwalls[i][j])
                myqueue.push({i + 1, j, prevNum + 1});

            if (!maze->vwalls[i - 1][j])
                myqueue.push({i - 1, j, prevNum + 1});

            if (!maze->hwalls[i][j])
                myqueue.push({i, j + 1, prevNum + 1});

            if (!maze->hwalls[i][j - 1])
                myqueue.push({i, j - 1, prevNum + 1});
        }
    }
}

void follow(struct maze maze, class mmstats *mms) {
    /* para que el micromouse siga el camino descendente
     * se tienen que dar 16 estados, esta funcion lo reduce a 4
     */
    int mht = maze.mhtn[mms->x][mms->y];
    int nextmht = mht - 1;
    int next[2];
    int next_L[2];
    int next_R[2];
    int next_B[2];
    nextcell(mms->x, mms->y, mms->dir, next);
    nextcell(mms->x, mms->y, mms->head->prev->value, next_L);
    nextcell(mms->x, mms->y, mms->head->next->value, next_R);
    nextcell(mms->x, mms->y, mms->head->next->next->value, next_B);
    if (maze.mhtn[next[0]][next[1]] == nextmht && !API::wallFront() ||
        !API::wallFront() && API::wallLeft() && API::wallRight()) {
        if (maze.mhtn[next_L[0]][next_L[1]] <= nextmht &&
            !API::wallLeft()) { /* Detectar si hay atajos */
            API::turnLeft();
            mms->turn(left);
            log("left");
            return;
        } else if (maze.mhtn[next_R[0]][next_R[1]] <= nextmht &&
                   !API::wallRight()) {
            API::turnRight();
            mms->turn(right);
            log("right");
            return;
        } else {
            log("forward");
            return;
        }
    } else if (maze.mhtn[next_L[0]][next_L[1]] <= nextmht && !API::wallLeft()) {
        API::turnLeft();
        mms->turn(left);
        log("left");
        return;
    } else if (maze.mhtn[next_R[0]][next_R[1]] <= nextmht &&
               !API::wallRight()) {
        API::turnRight();
        mms->turn(right);
        log("right");
        return;
    } else if (maze.mhtn[next_B[0]][next_B[1]] == nextmht ||
               API::wallFront() && API::wallLeft() && API::wallRight()) {
        API::turnRight();
        mms->turn(right);
        API::turnRight();
        mms->turn(right);
        log("180");
        return;
    } else if (maze.mhtn[next_R[0]][next_R[1]] <=
                   maze.mhtn[next_B[0]][next_B[1]] &&
               !API::wallRight()) {
        API::turnRight();
        mms->turn(right);
        log("bRight");
    } else if (maze.mhtn[next_L[0]][next_L[1]] <=
                   maze.mhtn[next_B[0]][next_B[1]] &&
               !API::wallLeft()) {
        API::turnLeft();
        mms->turn(left);
        log("bLeft");
    }
    // log("error en la matri");
}

int *nextcell(int i, int j, int dir, int ndir[2]) {
    switch (dir) {
    case norte:
        j++;
        break;
    case este:
        i++;
        break;
    case sur:
        j--;
        break;
    case oeste:
        i--;
        break;
    }
    ndir[0] = i;
    ndir[1] = j;
    return ndir;
}

void updatepos(class mmstats *mms) {
    /* cada que se ejecuta moveForward la funcion actualiza
     * la posicion del micromouse en el valor correspondiente
     */
    switch (mms->dir) {
    case norte:
        mms->y++;
        break;
    case este:
        mms->x++;
        break;
    case sur:
        mms->y--;
        break;
    case oeste:
        mms->x--;
        break;
    }
}

void locateWall(struct maze *maze, class mmstats *mms) {
    switch (mms->dir) {
    case norte:
        if (API::wallFront() && mms->y != 15) {
            maze->hwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'n');
        }
        if (API::wallLeft() && mms->x != 0) {
            maze->vwalls[mms->x - 1][mms->y] = 1;
            API::setWall(mms->x - 1, mms->y, 'e');
        }
        if (API::wallRight() && mms->x != 15) {
            maze->vwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'e');
        }
        break;
    case este:
        if (API::wallFront() && mms->x != 15) {
            maze->vwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'e');
        }
        if (API::wallLeft() && mms->y != 15) {
            maze->hwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'n');
        }
        if (API::wallRight() && mms->y != 0) {
            maze->hwalls[mms->x][mms->y - 1] = 1;
            API::setWall(mms->x, mms->y - 1, 'n');
        }
        break;
    case sur:
        if (API::wallFront() && mms->y != 0) {
            maze->hwalls[mms->x][mms->y - 1] = 1;
            API::setWall(mms->x, mms->y - 1, 'n');
        }
        if (API::wallLeft() && mms->x != 15) {
            maze->vwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'e');
        }
        if (API::wallRight() && mms->x != 0) {
            maze->vwalls[mms->x - 1][mms->y] = 1;
            API::setWall(mms->x - 1, mms->y, 'e');
        }
        break;
    case oeste:
        if (API::wallFront() && mms->x != 0) {
            maze->vwalls[mms->x - 1][mms->y] = 1;
            API::setWall(mms->x - 1, mms->y, 'e');
        }
        if (API::wallLeft() && mms->y != 0) {
            maze->hwalls[mms->x][mms->y - 1] = 1;
            API::setWall(mms->x, mms->y - 1, 'n');
        }
        if (API::wallRight() && mms->y != 15) {
            maze->hwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'n');
        }
        break;
    }
}

int main(int argc, char *argv[]) {
    log("Running...");
    API::setColor(0, 0, 'G');
    mmstats stats;

    stats.x = 0;
    stats.y = 0;

    int next_pos[2] = {7, 7};

    for (;;) {
        locateWall(&maze, &stats);
        flood(&maze, &stats, next_pos);
        follow(maze, &stats);
        if (maze.mhtn[stats.x][stats.y] == 0) {
            log("Finish");
            break;
        }
        // log(stats.x);
        // log(stats.y);
        API::moveForward();
        updatepos(&stats);
        // log(maze.hwalls[stats.x][stats.y]);
    }

    next_pos[0] = 0;
    next_pos[1] = 0;
    for (;;) {
        locateWall(&maze, &stats);
        flood(&maze, &stats, next_pos);
        follow(maze, &stats);
        if (stats.x == 0 && stats.y == 0) {
            log("start");
            break;
        }
        // log(stats.x);
        // log(stats.y);
        API::moveForward();
        updatepos(&stats);
        // log(maze.hwalls[stats.x][stats.y]);
    }

    next_pos[0] = 7;
    next_pos[1] = 7;
    for (;;) {
        locateWall(&maze, &stats);
        flood(&maze, &stats, next_pos);
        follow(maze, &stats);
        if (maze.mhtn[stats.x][stats.y] == 0) {
            log("Finish");
            break;
        }
        // log(stats.x);
        // log(stats.y);
        API::moveForward();
        updatepos(&stats);
    }
    // log(maze.hwalls[stats.x][stats.y]);
    return 0;
}