#include <string>
#include <sys/types.h>
#include "api.h"
#include <queue>
#include "mms.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(){
        mhtn[7][7] = 0; /* las celdas del centro tienen */
        mhtn[7][8] = 0; /* un valor de cero porque son  */
        mhtn[8][7] = 0; /* la meta.                     */
        mhtn[8][8] = 0;
    }
}maze;

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

void flood(struct maze*, struct mmstats*);
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 i, j;
    int prevNum;
    int marked[16][16];
    std::queue<fill> myqueue;

    myqueue.push({6,7,0});
    myqueue.push({6,8,0});
    myqueue.push({7,6,0});
    myqueue.push({8,6,0});

    myqueue.push({9,7,0});
    myqueue.push({9,8,0});
    myqueue.push({7,9,0});
    myqueue.push({8,9,0});
    
    marked[7][7] = 1; /* las celdas del centro tienen */
    marked[7][8] = 1; /* un valor de cero porque son  */
    marked[8][7] = 1; /* la meta.                     */
    marked[8][8] = 1;

    API::setText(7, 7, 0);
    API::setText(7, 8, 0);
    API::setText(8, 7, 0);
    API::setText(8, 8, 0);

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

        if (mmstats->x == i && mmstats->y == j) {
            log("flood end");
            API::setText(i, j, prevNum+1);
            maze->mhtn[i][j] = prevNum+1;
            break;
        }

        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;
            myqueue.push({i+1, j, prevNum +1});
            myqueue.push({i-1, j, prevNum +1});
            myqueue.push({i, j+1, prevNum +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];
    nextcell(mms->x, mms->y, mms->dir, next);
    if(maze.mhtn[next[0]][next[1]] == nextmht) {
        log("forward");
        return;
    }
    nextcell(mms->x, mms->y, mms->head->prev->value, next);
    if(maze.mhtn[next[0]][next[1]] == nextmht) {
        API::turnLeft();
        mms->turn(left);
        log("left");
        return;
    }
    nextcell(mms->x, mms->y, mms->head->next->value, next);
    if(maze.mhtn[next[0]][next[1]] == nextmht) {
        API::turnRight();
        mms->turn(right);
        log("right");
        return;
    }
    API::turnRight();
    mms->turn(right);
    API::turnRight();
    mms->turn(right);
    log("180");
}

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->vwalls[mms->x][mms->y-1] = 1;
            API::setWall(mms->x, mms->y-1, 'n');
        }
        if(API::wallLeft() && mms->x != 15){
            maze->hwalls[mms->x][mms->y] = 1;
            API::setWall(mms->x, mms->y, 'e');
        }
        if(API::wallRight() && mms->x != 0){
            maze->hwalls[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 != 0){
            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');
//  API::setText(0, 0, "abc");
    mmstats stats;
    stats.x = 0;
    stats.y = 0;

    flood(&maze, &stats);
    /*
    while (true) {
        follow(maze, &stats);
        API::moveForward();
        updatepos(&stats);
        if(maze.mhtn[stats.x][stats.y] == 0){
            log("Finish");
            return 0;
        }
        log(stats.x);
        log(stats.y);
        log("");
    }
    */
    while (true) {
        if (!API::wallLeft()) {
            locateWall(&maze, &stats);
            API::turnLeft();
            stats.turn(left);
        }
        while (API::wallFront()) {
            locateWall(&maze, &stats);
            API::turnRight();
            stats.turn(right);
        }
        locateWall(&maze, &stats);
        API::moveForward();
        updatepos(&stats);
        log(stats.x);
        log(stats.y);
        log("");
   }
}