commit 5d1c5d8107f3c26f2cd388253e0b634cf72882b0
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Sun Apr 7 12:49:46 2013 -0500

    undid overwrite of maze_solve with maze_solve_simple

diff --git a/scout/libscout/src/test_behaviors/maze_solve.cpp b/scout/libscout/src/test_behaviors/maze_solve.cpp
index 511b0b3..99fa3ca 100644
--- a/scout/libscout/src/test_behaviors/maze_solve.cpp
+++ b/scout/libscout/src/test_behaviors/maze_solve.cpp
@@ -25,103 +25,246 @@
 
 #include "maze_solve.h"
 
-#define D_THRESH 600
-#define max(x,y) ((x > y) ? x : y)
-
 using namespace std;
 
-void maze_solve::spin_for(double duration)
-{
-    ros::Rate r(100);     // 100 hz
-    int ticks = int(duration * 100);
-    for (int i = 0; i < ticks; i++)
-    {
-        spinOnce();
-        r.sleep();
-    }
-}
+
+// want to have a minimal working thing, use a big enough 
+// static array and start in the middle
+// we assume we are facing right, that affects where we store
+// wall information
+// -1 for wall, 0 for unseen, 1 for traveled, 2 for critical
+#define WALL -1
+#define UNSEEN 0
+#define SEEN 1
+#define CRITICAL 2
+// facings
+#define UP 0
+#define RIGHT 1
+#define DOWN 2
+#define LEFT 3
+
+// TODO This is bad! It's defined globally across all files. Please put it inside a good scope. -Alex
+Duration sonar_update_time(1.5);
 
 void maze_solve::run()
 {    
+    // TODO:first initialize map to all 0's
     ROS_INFO("Starting to solve the maze");
-
     // Go up to the first line.
     follow_line();
-
     // Turn the sonar on.
     sonar->set_on();
     sonar->set_range(0, 23);
 
-    ROS_INFO("Off we go!");
+    // Wait for the sonar to initialize.
+    while(!look_around(25, 25, RIGHT) && ok())
+    {
+      spinOnce();      
+    }
+
+    // Solve the maze
+    bool finished = solve(25,25, RIGHT);
+
+    // Check and report final condition.
+    if (finished)
+        ROS_INFO("YAY! I have solved the maze");
+    else
+        ROS_INFO("NO! The maze is unsolvable");
+}
+
+bool maze_solve::solve(int row, int col, int dir)
+{
+    int initial_dir = dir;
+
+    ROS_INFO("I am at direction %d", dir);
+
+    // use backtracking to solve the maze
+    if (at_destination())
+        return true;
 
-    while (!at_destination())
+    // Wait for sonar to update.
+    sonar_update_time.sleep();
+
+    // this function should fill the adjacent cells around me with
+    // wall's or paths
+    while(!look_around(row, col, dir) && ok())
     {
-        // Wait for sonar to update.
-        spin_for(1.5);
-        int* readings = sonar->get_sonar_readings();
-        
-        // Wait until the sonar gives us real values.
-        bool readings_ok = true;
-        cout << "[";
-        for (int i = 0; i < 48; i++)
+        spinOnce();
+    }
+
+    /* try go up */
+    if (map[row-1][col] != WALL && initial_dir != UP)
+    {
+    ROS_INFO("GOING UP!");
+        // Turn up.
+        turn_from_to(dir, UP);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row-1, col, DOWN);
+        if (solved)
         {
-            if (i == 24 || i == 36 || i == 0)
-            {
-                cout << "(" << readings[i] << ") ";
-            }
-            else
-            {
-                cout << readings[i] << " ";
-            }
-            if (readings[i] == 0)
-            {
-                ROS_INFO("Waiting. readings[%d] == 0.", i);
-                readings_ok = false;
-                break;
-            }
+            return solved;
         }
-        cout << endl;
-
-        if (!readings_ok)
+        else
         {
-            continue;
+            //Update where we are.
+            dir = UP;
         }
-
-        int r_read = max(readings[23], max(readings[24], readings[25]));
-        int s_read = max(readings[35], max(readings[36], readings[37]));
-        int l_read = max(readings[47], max(readings[0],  readings[1]));
-
-        ROS_INFO("Left: %d. Straight: %d. Right: %d.", l_read, s_read, r_read);
-
-        if (r_read > D_THRESH)      // Right
+    }
+    /* try right */
+    if (map[row][col+1] != WALL && initial_dir != RIGHT)
+    {
+    ROS_INFO("GOING RIGHT!");
+        // Turn right.
+        turn_from_to(dir, RIGHT);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row, col+1, LEFT);
+        if (solved)
         {
-            ROS_INFO("Right.");
-            turn_right();
+            return solved;
         }
-        else if (s_read > D_THRESH) // Straight
+        else
         {
-            ROS_INFO("Straight.");
-            turn_straight();
+            //Update where we are.
+            dir = RIGHT;
         }
-        else if (l_read > D_THRESH)   // Left
+    }
+    /* try down */
+    if (map[row+1][col] != WALL && initial_dir != DOWN)
+    {
+    ROS_INFO("GOING DOWN!");
+        // Turn down.
+        turn_from_to(dir, DOWN);
+        follow_line();
+        // Solve recursively.
+        bool solved = solve(row+1, col, UP);
+        if (solved)
         {
-            ROS_INFO("Left.");
-            turn_left();
+            return solved;
         }
-        else                                                        // Deadend
+        else
         {
-            ROS_INFO("Dead end.");
-            spot_turn();
+            //Update where we are.
+            dir = DOWN;
         }
-
+    }
+    /* try left */
+    if (map[row][col-1] != WALL && initial_dir != LEFT)
+    {
+    ROS_INFO("GOING LEFT!");
+        // Turn down.
+        turn_from_to(dir, LEFT);
         follow_line();
+        // Solve recursively.
+        bool solved = solve(row, col-1, RIGHT);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = LEFT;
+        }
+    }
+
+    ROS_INFO("DEAD END FOUND, TURNING BACK.");
+    // we have exhausted all the options. This path is clearly a
+    // dead end. go back to where we come from and return false.
+    turn_from_to(dir, initial_dir);
+    follow_line();
+    return false;
+}
+
+// this function takes in the current direction and turns the scout
+// into it intended direction
+void maze_solve::turn_from_to(int current_dir, int intended_dir) {
+    switch ((4 + intended_dir - current_dir) % 4) 
+    {
+        case 0:
+            spot_turn();
+            break;
+        case 1:
+            turn_left();
+            break;
+        case 2:
+            turn_straight();
+            break;
+        case 3:
+            turn_right();
+            break;
     }
 }
 
+bool maze_solve::look_around(int row, int col, int dir)
+{
+    // look around current place using sonar
+    // store whether or not
+    // there is a wall into the map
+    // stores at row col 2 if point is critical, 1 otherwise
+    
+    int* readings = sonar->get_sonar_readings();
+    spinOnce();
+
+/*
+    // Look to the left.
+    int left_distance = (readings[1] + readings[0] + readings[47])/3;
+    // Look to the front.
+    int front_distance = (readings[35] + readings[36] + readings[37])/3;
+    // Look to the right.
+    int right_distance = (readings[23] + readings[24] + readings[25])/3;
+*/
+    // Look to the left.
+    int left_distance = readings[0];
+    // Look to the front.
+    int front_distance = readings[36];
+    // Look to the right.
+    int right_distance = readings[24];
+
+    ROS_INFO("front: %d  left: %d  right: %d", front_distance, left_distance, right_distance);
+
+    if (right_distance == 0 || front_distance == 0 || left_distance == 0)
+      return false;
+
+    switch (dir)
+    {
+        case UP:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row][col+1] = (left_distance < 500)?WALL:SEEN;
+            map[row+1][col] = (front_distance < 500)?WALL:SEEN;
+            map[row][col-1] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case RIGHT:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row+1][col] = (left_distance < 500)?WALL:SEEN;
+            map[row][col-1] = (front_distance < 500)?WALL:SEEN;
+            map[row-1][col] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case DOWN:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row][col-1] = (left_distance < 500)?WALL:SEEN;
+            map[row-1][col] = (front_distance < 500)?WALL:SEEN;
+            map[row][col+1] = (right_distance < 500)?WALL:SEEN;
+            break;
+        case LEFT:
+            // If the distance is less than 500, mark the area as a wall otherwise
+            // mark it as seen.
+            map[row-1][col] = (left_distance < 500)?WALL:SEEN;
+            map[row][col+1] = (front_distance < 500)?WALL:SEEN;
+            map[row+1][col] = (right_distance < 500)?WALL:SEEN;
+            break;
+    }
+
+    return true;
+}
+
 bool maze_solve::at_destination() 
 {
     vector<uint32_t> readings = linesensor->query();
-    //ROS_INFO("Readings: %d %d %d %d %d %d %d %d.", readings[0], readings[1], readings[2], readings[3], readings[4], readings[5], readings[6], readings[7]);
     if ( readings[0] > 200 &&
          readings[1] < 55 &&
          readings[2] < 55 &&
@@ -131,7 +274,6 @@ bool maze_solve::at_destination()
          readings[6] < 55 &&
          readings[7] > 200 )
     {
-        ROS_INFO("\n\nDESTINATION\n\n");
         return true;
     }
     return false;
