mms-sim/main.cpp

#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 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");
            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 x = mms.x;
    int y = mms.y;
    int mht = maze.mhtn[x][y];
    int next[2];
    if (mht > maze.mhtn[x][y+1]) {
        next[0] = x;
    }
}

void nextcell(int i, int j, int dir, int& nextx, int& nexty) {
    switch (dir) {
    case norte: j++; 
                break;
    case este: i++; 
                break;
    case sur: j--; 
                break;
    case oeste: i--; 
                break;
    }
    nextx = i;
    nexty = j;
}

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;
    }
}


int main(int argc, char* argv[]) {
    log("Running...");
    API::setColor(0, 0, 'G');
//  API::setText(0, 0, "abc");
    API::setWall(1, 0, 'e');
    mmstats stats;
    stats.x = 0;
    stats.y = 0;

    flood(&maze, &stats);

    while (true) {
        if (!API::wallLeft()) {
            API::turnLeft();
            stats.turn(left);
        }
        while (API::wallFront()) {
            API::turnRight();
            stats.turn(right);
        }
        API::moveForward();
        updatepos(&stats);
   }
}