From f4e82b4be43707942b98386424972f6ae4bcdda9 Mon Sep 17 00:00:00 2001
From: Fernando R Jacobo <fherpato8@gmail.com>
Date: Wed, 13 Nov 2024 13:53:41 -0600
Subject: [PATCH] Added ifdef for debuggin mms.h

---
 main.cpp | 96 +++++++++++++++++++++++++++++++++++++++++---------------
 makefile |  5 ++-
 mms.h    |  4 +--
 3 files changed, 77 insertions(+), 28 deletions(-)

diff --git a/main.cpp b/main.cpp
index edb5773..81f5c68 100644
--- a/main.cpp
+++ b/main.cpp
@@ -17,6 +17,7 @@ struct fill {
 };
 
 void flood(struct maze*, struct mmstats*);
+void flood_start(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);
@@ -79,6 +80,43 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
     }
 }
 
+void flood_start(struct maze* maze, struct mmstats* mmstats) {
+    int i, j;
+    int prevNum;
+    int marked[16][16];
+    std::queue<fill> myqueue;
+
+    myqueue.push({0,0,-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 
@@ -93,9 +131,24 @@ void follow(struct maze maze, class mmstats* mms) {
     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;
+    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();
@@ -233,13 +286,13 @@ int main(int argc, char* argv[]) {
     stats.x = 0;
     stats.y = 0;
     
-    while (true) {
+    for(;;) {
         locateWall(&maze, &stats);
         flood(&maze, &stats);
         follow(maze, &stats);
         if(maze.mhtn[stats.x][stats.y] == 0){
             log("Finish");
-            return 0;
+            break;
         }
         //log(stats.x);
         //log(stats.y);
@@ -247,27 +300,20 @@ int main(int argc, char* argv[]) {
         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);
-        }
+    for(;;){
+        log("returning start");
+        flood_start(&maze, &stats);
         locateWall(&maze, &stats);
+        follow(maze, &stats);
+        if(maze.mhtn[stats.x][stats.y] == 0){
+            log("start");
+            break;
+        }
+        //log(stats.x);
+        //log(stats.y);
         API::moveForward();
         updatepos(&stats);
-        flood(&maze, &stats);
-        log(stats.x);
-        log(stats.y);
-        log("");
-   }
-   */
+        //log(maze.hwalls[stats.x][stats.y]);
+    }
+   return 0;
 }
diff --git a/makefile b/makefile
index e26bda9..f9af9c7 100644
--- a/makefile
+++ b/makefile
@@ -6,5 +6,8 @@ ARGS = -g -Wall
 compile: ${SRC}
 	${CC} ${ARGS} $^ -o a.out
 
-run: compile
+dir: ${SRC}
+	${CC} ${ARGS} -DDIR $^ -o a.out
+
+run: a.out
 	@./a.out
diff --git a/mms.h b/mms.h
index b445127..7d2341a 100644
--- a/mms.h
+++ b/mms.h
@@ -42,7 +42,7 @@ public:
             head = head->next;
         else head = head->prev;
         dir = head->value;
-        /*
+#ifdef DIR
         switch (dir) {
         case norte: log("Norte");
                   break;
@@ -53,7 +53,7 @@ public:
         case oeste: log("Oeste");
                   break;
         }
-        */
+#endif
     }
 
 };