commit f878b5f9e5a40b40095b04e43da9bd4f74f4de83
Author: Priya <pdeo@Phlegethon.(none)>
Date:   Sun Dec 2 17:03:18 2012 -0500

    Finished writing up the maze solving behavior.

diff --git a/scout/libscout/src/behaviors/maze_solve.cpp b/scout/libscout/src/behaviors/maze_solve.cpp
index 3586540..c5c9924 100644
--- a/scout/libscout/src/behaviors/maze_solve.cpp
+++ b/scout/libscout/src/behaviors/maze_solve.cpp
@@ -48,59 +48,96 @@ void maze_solve::run(){
     // TODO:first initialize map to all 0's
     solve(25,25, RIGHT);
 }
-bool maze_solve::solve(row, col, dir)
-{   
+
+bool maze_solve::solve(int row, int col, int dir)
+{
+    int intial_dir = dir;
+
     // use backtracking to solve the maze
-    if (at_destination()) {
-        // not really sure what to do next... just chill here
+    if (at_destination())
         return true;
-    }
 
-    else
+    // this function should fill the adjacent cells around me with
+    // wall's or paths
+    look_around(row, col, dir);
+
+    /* try go up */
+    if (map[row-1][col] != WALL && initial_dir != UP)
     {
-        // this function should fill the adjacent cells around me with
-        // wall's or paths
-        look_around(row, col, dir);
-        
-        if (at_dead_end(row, col, dir))
+        // Turn up.
+        turn_from_to(dir, UP);
+        line_follow();
+        // Solve recursively.
+        bool solved = solve(row-1, col, DOWN);
+        if (solved)
         {
-            // set the scout to a back tracking direction
-            spot_turn();
-            return false;
+            return solved;
         }
-        
-        /* try go up */
-        // no need to check range because we assume our map is big enough
-        if (map[row-1][col] != WALL)
+        else
         {
-            turn_from_to(dir, UP);
-            line_follow();
-            // halts at intersection
-            bool solved = solve(row-1, col)
-            if (solved) {
-                //yeah!!! don't really know what to do now...
-                // hopefully send some message to some other scout
-                // waiting for my valuable information
-            }
-            else {
-                // whoops, that path did not work out
-                // redo the move, since the recursive call has 
-                // already put me into the right direction, I will just 
-                // drive back and halt at intersection
-                line_follow();
-                // undo the turning
-                turn_from_to()
-            }            
+            //Update where we are.
+            dir = UP;
         }
-        
-        /* try right */
-        if (map[row][col+1] != WALL)
+    }
+    /* try right */
+    if (map[row][col+1] != WALL && initial_dir != RIGHT)
+    {
+        // Turn right.
+        turn_from_to(dir, RIGHT);
+        line_follow();
+        // Solve recursively.
+        bool solved = solve(row, col+1, LEFT);
+        if (solved)
         {
-            turn_from
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = RIGHT;
         }
-
     }
-
+    /* try down */
+    if (map[row+1][col] != WALL && initial_dir != DOWN)
+    {
+        // Turn down.
+        turn_from_to(dir, DOWN);
+        line_follow();
+        // Solve recursively.
+        bool solved = solve(row+1, col, UP);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = DOWN;
+        }
+    }
+    /* try left */
+    if (map[row][col-1] != WALL && initial_dir != LEFT)
+    {
+        // Turn down.
+        turn_from_to(dir, LEFT);
+        line_follow();
+        // Solve recursively.
+        bool solved = solve(row, col-1, RIGHT);
+        if (solved)
+        {
+            return solved;
+        }
+        else
+        {
+            //Update where we are.
+            dir = LEFT;
+        }
+    }
+    // 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);
+    line_follow();
+    return false;
 }
 
 // this function takes in the current direction and turns the scout
@@ -108,32 +145,80 @@ bool maze_solve::solve(row, col, dir)
 void maze_solve::turn_from_to(int current_dir, int intended_dir) {
     switch ((4 + intended_dir - current_dir) % 4) 
         case 0:
-            turn_straight();
+            spot_turn();
             break;
         case 1:
             turn_right();
             break;
         case 2:
+            turn_straight();
             break;
         case 3:
             turn_left();
             break;
 }
 
-int maze_solve::look_around(row, col)
+void 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
-}
-
-bool maze_solve::at_destination() 
-{
     
+    int* readings = get_sonar_readings();
+
+    // Look to the right.
+    int right_distance = (readings[1] + readings[0] + readings[47])/3;
+    // Look to the front.
+    int front_distance = (readings[11] + readings[12] + readings[13])/3;
+    // Look to the left.
+    int left_distance = (readings[23] + readings[24] + readings[25])/3;
+    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;
+    }
 }
 
-bool wall_in_direction()
+bool maze_solve::at_destination() 
 {
-    
+    vector<uint32_t> readings = linesensor->query();
+    if ( readings[0] > 200 &&
+         readings[1] < 55 &&
+         readings[2] < 55 &&
+         readings[3] > 200 &&
+         readings[4] > 200 &&
+         readings[5] < 55 &&
+         readings[6] < 55 &&
+         readings[7] > 200 )
+    {
+        return true;
+    }
+    return false;
 }
diff --git a/scout/libscout/src/behaviors/maze_solve.h b/scout/libscout/src/behaviors/maze_solve.h
index a7e2561..60f5421 100644
--- a/scout/libscout/src/behaviors/maze_solve.h
+++ b/scout/libscout/src/behaviors/maze_solve.h
@@ -34,6 +34,11 @@ class maze_solve: public line_follow
         maze_solve(std::string scoutname, Sensors* sensors):
             line_follow(scoutname, "maze_solve", sensors) {};
         void run();
+    private:
+        bool solve(int row, int col, int dir);
+        void turn_from_to(int current_dir, int intended_dir);
+        void look_around(int row, int col, int dir);
+        bool at_destination();
 }
 #endif
 
