From 4ddb70286277f44467d1e41ea2f6a416b81186e0 Mon Sep 17 00:00:00 2001
From: fernando <fherpato8@gmail.com>
Date: Fri, 9 Aug 2024 01:03:00 -0600
Subject: [PATCH] Funcion para girar el mouse a la direccion correcta

---
 main.cpp | 63 ++++++++++++++++++++++++++++++++++++++++++++++----------
 mms.h    |  2 +-
 2 files changed, 53 insertions(+), 12 deletions(-)

diff --git a/main.cpp b/main.cpp
index b50c2c8..18000b3 100644
--- a/main.cpp
+++ b/main.cpp
@@ -24,6 +24,9 @@ struct fill {
 };
 
 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 flood(struct maze* maze, struct mmstats* mmstats) {
     int i, j;
     int prevNum;
@@ -58,6 +61,8 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
 
         if (mmstats->x == i && mmstats->y == j) {
             log("flood end");
+            API::setText(i, j, prevNum+1);
+            maze->mhtn[i][j] = prevNum+1;
             break;
         }
 
@@ -77,20 +82,40 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
     }
 }
 
-void follow(struct maze maze, class mmstats mms) {
+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 mht = maze.mhtn[mms->x][mms->y];
+    int nextmht = mht-1;
     int next[2];
-    if (mht > maze.mhtn[x][y+1]) {
-        next[0] = x;
+    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");
 }
 
-void nextcell(int i, int j, int dir, int& nextx, int& nexty) {
+int* nextcell(int i, int j, int dir, int ndir[2]) {
     switch (dir) {
     case norte: j++; 
                 break;
@@ -101,8 +126,9 @@ void nextcell(int i, int j, int dir, int& nextx, int& nexty) {
     case oeste: i--; 
                 break;
     }
-    nextx = i;
-    nexty = j;
+    ndir[0] = i;
+    ndir[1] = j;
+    return ndir;
 }
 
 void updatepos(class mmstats* mms) {
@@ -126,13 +152,24 @@ 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) {
+        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()) {
             API::turnLeft();
@@ -144,5 +181,9 @@ int main(int argc, char* argv[]) {
         }
         API::moveForward();
         updatepos(&stats);
+        log(stats.x);
+        log(stats.y);
+        log("");
    }
+   */
 }
diff --git a/mms.h b/mms.h
index fa18f5c..b445127 100644
--- a/mms.h
+++ b/mms.h
@@ -53,7 +53,7 @@ public:
         case oeste: log("Oeste");
                   break;
         }
-       */
+        */
     }
 
 };