Funcion floodfill implementada junto con otras funciones

Reviewed-on: https://git.fisionx.mooo.com/FisionX/mms-sim/pulls/2

Main.cpp

@@ -1,23 +0,0 @@
-#include <iostream>
-#include <string>
-
-#include "API.h"
-
-void log(const std::string& text) {
-    std::cerr << text << std::endl;
-}
-
-int main(int argc, char* argv[]) {
-    log("Running...");
-    API::setColor(0, 0, 'G');
-    API::setText(0, 0, "abc");
-    while (true) {
-        if (!API::wallLeft()) {
-            API::turnLeft();
-        }
-        while (API::wallFront()) {
-            API::turnRight();
-        }
-        API::moveForward();
-    }
-}

api.cpp

@@ -1,4 +1,4 @@
-#include "API.h"
+#include "api.h"
 
 #include <cstdlib>
 #include <iostream>
@@ -89,6 +89,9 @@ void API::clearAllColor() {
 void API::setText(int x, int y, const std::string& text) {
     std::cout << "setText " << x << " " << y << " " << text << std::endl;
 }
+void API::setText(int x, int y, const int& text) {
+    std::cout << "setText " << x << " " << y << " " << text << std::endl;
+}
 
 void API::clearText(int x, int y) {
     std::cout << "clearText " << x << " " << y << std::endl;
@@ -110,3 +113,12 @@ void API::ackReset() {
     std::string ack;
     std::cin >> ack;
 }
+void log(const std::string& text) {
+    std::cerr << text << std::endl;
+}
+void log(const char& text) {
+    std::cerr << text << std::endl;
+}
+void log(const int& num) {
+    std::cerr << num << std::endl;
+}

api.h

@@ -25,6 +25,7 @@ public:
     static void clearAllColor();
 
     static void setText(int x, int y, const std::string& text);
+    static void setText(int x, int y, const int& text);
     static void clearText(int x, int y);
     static void clearAllText();
 
@@ -32,3 +33,6 @@ public:
     static void ackReset();
 
 };
+
+void log(const std::string&);
+void log(const int&);

main.cpp

@@ -0,0 +1,273 @@
+#include <string>
+#include <sys/types.h>
+#include <queue>
+#include "api.h"
+#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;
+
+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});
+    */
+
+    myqueue.push({7,7,-1});
+    myqueue.push({7,8,-1});
+    myqueue.push({8,7,-1});
+    myqueue.push({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 (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()) {
+        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');
+//  API::setText(0, 0, "abc");
+    mmstats stats;
+    stats.x = 0;
+    stats.y = 0;
+    
+    while (true) {
+        locateWall(&maze, &stats);
+        flood(&maze, &stats);
+        follow(maze, &stats);
+        if(maze.mhtn[stats.x][stats.y] == 0){
+            log("Finish");
+            return 0;
+        }
+        //log(stats.x);
+        //log(stats.y);
+        API::moveForward();
+        updatepos(&stats);
+        //log(maze.hwalls[stats.x][stats.y]);
+    }
+    /*
+    while (true) {
+        if (!API::wallLeft()) {
+            locateWall(&maze, &stats);
+            API::turnLeft();
+            stats.turn(left);
+            flood(&maze, &stats);
+        }
+        while (API::wallFront()) {
+            locateWall(&maze, &stats);
+            API::turnRight();
+            stats.turn(right);
+            flood(&maze, &stats);
+        }
+        locateWall(&maze, &stats);
+        API::moveForward();
+        updatepos(&stats);
+        flood(&maze, &stats);
+        log(stats.x);
+        log(stats.y);
+        log("");
+   }
+   */
+}

makefile

@@ -0,0 +1,10 @@
+SRC = *.cpp
+CC = g++
+#ARGS = -lstdc++ -g -Wall
+ARGS = -g -Wall
+
+compile: ${SRC}
+	${CC} ${ARGS} $^ -o a.out
+
+run: compile
+	@./a.out

mms.h

@@ -0,0 +1,59 @@
+#include "api.h"
+enum turn{left, right};
+enum compass{norte, sur, este, oeste};
+struct dirnode {
+    int value;
+    dirnode* next;
+    dirnode* prev;
+    dirnode(int val): value(val){};
+};
+class mmstats {
+private:
+    dirnode* tail;
+    void addnode(int val) {
+        dirnode* temp = new dirnode(val);
+        if (head != nullptr) {
+            head->next = temp;
+            temp->prev = head;
+            temp->next = tail;
+            head = temp;
+            tail->prev = head;
+        }
+
+        if (head == nullptr) {
+            head = temp;
+            tail = temp;
+        }
+    }
+public:
+    dirnode* head;
+    int y;
+    int x;
+    char dir;
+    mmstats(): tail(nullptr), head(nullptr) {
+        addnode(este);
+        addnode(sur);
+        addnode(oeste);
+        addnode(norte);
+        dir = norte;
+    }
+    void turn(int opt) {
+        if (opt)
+            head = head->next;
+        else head = head->prev;
+        dir = head->value;
+        /*
+        switch (dir) {
+        case norte: log("Norte");
+                  break;
+        case este: log("Este");
+                  break;
+        case sur: log("Sur");
+                  break;
+        case oeste: log("Oeste");
+                  break;
+        }
+        */
+    }
+
+};