Alg can record number of steps (precusor to flood fill/anti-step

Firmware/lib/algorithm/algorithm.cpp

@@ -21,6 +21,21 @@ uint16_t Algorithm::getRaw(int x, int y)
     return maze[x][y];
 }
 
+uint8_t Algorithm::getValue(int x, int y)
+{
+    // And with 1111 1111 and get only the last 8 bits
+    return maze[x][y] & 0xff;
+}
+
+void Algorithm::setValue(int x, int y, uint8_t value)
+{
+    uint16_t _raw = maze[x][y];
+    _raw &= ~0xff; // Clear lower 16
+    _raw |= value; // Set lower 16
+
+    maze[x][y] = _raw;
+}
+
 void Algorithm::turnLeft()
 {
     // This implementation is messy but, then again, so are enums in cpp
@@ -78,6 +93,9 @@ void Algorithm::turnRight()
 void Algorithm::forward()
 {
     // TODO: add map edge detection
+
+    uint8_t _prev = getValue(mapPoseX, mapPoseY);
+
     switch (mapDir)
     {
     case NORTH:
@@ -96,6 +114,8 @@ void Algorithm::forward()
         mapPoseX -= 1;
         break;
     }
+
+    setValue(mapPoseX, mapPoseY, _prev + 1);
 }
 
 void Algorithm::setWall(Cardinal dir)

Firmware/lib/algorithm/algorithm.h

@@ -48,12 +48,30 @@ class Algorithm
     /**
      * @brief Get the raw maze data for a specific cell
      *
-     * @param x
-     * @param y
+     * @param x X coordinate in maze (0-15)
+     * @param y Y coordinate in maze (0-15)
      * @return uint16_t (maze data unit)
      */
     uint16_t getRaw(int x, int y);
 
+    /**
+     * @brief Get the value of a specific tile
+     *
+     * @param x X coordinate in maze (0-15)
+     * @param y Y coordinate in maze (0-15)
+     * @return uint8_t tile value (unsigned int 8 bits)
+     */
+    uint8_t getValue(int x, int y);
+
+    /**
+     * @brief Set the value of a specific tile
+     *
+     * @param x X coordinate in maze (0-15)
+     * @param y Y coordinate in maze (0-15)
+     * @param value An 8 bit number (0-255)
+     */
+    void setValue(int x, int y, uint8_t value);
+
     /** Motion Commands
      *
      * Motion commands are not like robot commands, these

Firmware/sim/main.cpp

@@ -67,8 +67,7 @@ int main(int argc, char *argv[])
 
                 // Process spanning numbers
                 char _val[3];
-                int _num = (_raw >> 8) & 0xff;
-                sprintf(_val, "%d", _num);
+                sprintf(_val, "%d", alg.getValue(x, y));
                 API::setText(x, y, _val);
             }
         }