diff --git a/lib/Floodfill/mms src/Floodfill_Backtracking.cpp b/lib/Floodfill/mms src/Floodfill_Backtracking.cpp
new file mode 100644
index 0000000..6d9e1b1
--- /dev/null
+++ b/lib/Floodfill/mms src/Floodfill_Backtracking.cpp	
@@ -0,0 +1,465 @@
+// Micromouse floodfill maze-solving algorithm
+// Compatible with MMS Simulator
+// Written in C++ for robotics competitions
+#include "API.h"
+#include <array>
+#include <queue>
+#include <string>
+#include <iostream>
+#include <utility>
+#include "Floodfill.h"
+#include <vector>
+
+using namespace std;
+Floodfill floodfill;
+
+enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND};
+
+int idx[] = {0,1,3,2};
+int curRow = 0, curCol = 0, curDir = 0;  // 0=north,1=east,2=south,3=west
+bool reachedCenter = false;
+
+
+float getStat(const std::string& statName) {
+    std::cout << "getStat " << statName << std::endl;
+    std::string response;
+    std::cin >> response;
+    try {
+        return std::stof(response); // handles both int and float
+    } catch (...) {
+        std::cerr << "Failed to parse stat: " << statName << std::endl;
+        return -1;
+    }
+}
+
+bool atGoal(int row, int col) {
+    return (row == 7 || row == 8) && (col == 7 || col == 8);
+}
+
+void Floodfill::setWall(int row, int col, int direction) {
+    if(direction == 0) {
+        maze.vertical_walls[row][col].first = 1; // North wall
+        if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above
+    }
+    if(direction == 1) {
+        maze.horizontal_walls[row][col].second = 1; // East wall
+        if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right
+    }
+    if(direction == 2) {
+        maze.vertical_walls[row][col].second = 1; // South wall
+        if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below
+    }
+    if(direction == 3) {
+        maze.horizontal_walls[row][col].first = 1; // West wall
+        if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left
+    }
+    char cdir = "nesw"[direction];
+    API::setWall(col, row, cdir);
+}
+void Floodfill::detectWalls(vector<int> sensorDistances, int row, int col, int direction){
+    if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds
+    if(API::wallFront()) { // North Wall
+        setWall(row, col, direction);
+        // Serial.print("Wall detected at ");
+        // Serial.print(direction);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[2]);
+    }
+    if(API::wallLeft()) {
+        setWall(row, col, (direction + 3) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 3) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[0]);
+    }
+    //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction);
+    if(API::wallRight()) {
+        setWall(row, col, (direction + 1) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 1) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[4]);
+    }
+}
+bool Floodfill::hasWall(int row, int col, int dir) {
+    if(dir == 0) return maze.vertical_walls[row][col].first; // North wall
+    if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall
+    if(dir == 2) return maze.vertical_walls[row][col].second; // South wall
+    if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall
+    return false; // No walls
+}
+
+void Floodfill::floodfill() {
+    queue<pair<int, int>> q;    
+
+    for (auto& row : maze.manhattan_distances)
+        row.fill(255); // Re-initialize all distances to a large number
+
+    maze.manhattan_distances[8][8] = 0; //row,column
+    maze.manhattan_distances[8][7] = 0;
+    maze.manhattan_distances[7][8] = 0;
+    maze.manhattan_distances[7][7] = 0; // Goal
+
+    maze.visited[8][8] = true;
+    maze.visited[8][7] = true;
+    maze.visited[7][8] = true;
+    maze.visited[7][7] = true;
+    maze.visited[0][0] = true;
+
+    
+    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+        
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) {
+            maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) {
+            maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) {
+            maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) {
+            maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+    }
+
+    for (int r = 0; r < 16; ++r) {
+        for (int c = 0; c < 16; ++c) {
+            int dist = maze.manhattan_distances[r][c];
+            if (dist != 255) {
+                API::setText(c, r, to_string(dist));  // NOTE: col=x, row=y
+            }
+        }
+    }
+}
+
+void Floodfill::floodfillToStart() {
+    queue<pair<int, int>> q;
+
+    for (auto& row : maze.reverse_manhattan_distances)
+        row.fill(255); // Reset distances
+
+    // New goal is the start: bottom-left of the maze
+    maze.reverse_manhattan_distances[0][0] = 0;
+    q.push({0, 0});
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[0][0] = true;
+
+    while (!q.empty()) {
+        pair<int, int> cell = q.front();
+        q.pop();
+
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        // WEST
+        if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) {
+            maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) {
+            maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) {
+            maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) {
+            maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+    }
+}
+void Floodfill::final_floodfill() {
+    queue<pair<int, int>> q;    
+
+    for (auto& row : maze.manhattan_distances)
+        row.fill(255); // Re-initialize all distances to a large number
+
+    maze.manhattan_distances[8][8] = 0; //row,column
+    maze.manhattan_distances[8][7] = 0;
+    maze.manhattan_distances[7][8] = 0;
+    maze.manhattan_distances[7][7] = 0; // Goal
+
+    
+    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+        
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        // WEST
+        if (col > 0 && !maze.horizontal_walls[row][col].first && maze.visited[row][col - 1] && !reached[row][col - 1]) {
+            maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && maze.visited[row + 1][col] && !reached[row + 1][col]) {
+            maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && maze.visited[row][col + 1] && !reached[row][col + 1]) {
+            maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && maze.visited[row - 1][col] && !reached[row - 1][col]) {
+            maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+
+    }
+    for (int r = 0; r < 16; ++r) {
+        for (int c = 0; c < 16; ++c) {
+            int dist = maze.manhattan_distances[r][c];
+            if (dist != 255) {
+                API::setText(c, r, to_string(dist));  
+            }
+        }
+    }
+}
+
+int Floodfill::getNextMove(int row, int col) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    for (int dir = 0; dir < 4; ++dir) {
+        int r = row, c = col;
+
+        if (dir == 0 && !hasWall(row, col, 0)) r++;
+        else if (dir == 1 && !hasWall(row, col, 1)) c++;
+        else if (dir == 2 && !hasWall(row, col, 2)) r--;
+        else if (dir == 3 && !hasWall(row, col, 3)) c--;
+        else continue;  // skip if there's a wall
+
+        if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+        int dist = maze.manhattan_distances[r][c];
+
+        if (dist < minDist || (dist == minDist && dir == curDir)) {
+            minDist = dist;
+            bestDirection = dir;
+        }
+    }
+
+    return bestDirection;
+}
+int Floodfill::reverse_getNextMove(int row, int col) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    for (int dir = 0; dir < 4; ++dir) {
+        int r = row, c = col;
+
+        if (dir == 0 && !hasWall(row, col, 0)) r++;
+        else if (dir == 1 && !hasWall(row, col, 1)) c++;
+        else if (dir == 2 && !hasWall(row, col, 2)) r--;
+        else if (dir == 3 && !hasWall(row, col, 3)) c--;
+        else continue;  // skip if there's a wall
+
+        if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+        int dist = maze.reverse_manhattan_distances[r][c];
+
+        if (dist < minDist || (dist == minDist && dir == curDir)) {
+            minDist = dist;
+            bestDirection = dir;
+        }
+    }
+
+    return bestDirection;
+}
+Action rotateTo(int newDir) {
+    int diff = (newDir - curDir + 4) % 4;
+    if (diff == 0) return FORWARD;
+    if (diff == 1) return RIGHT;
+    if (diff == 3) return LEFT;
+    if (diff == 2) return AROUND;  // U-turn
+    return IDLE;
+}
+void moveForwardUpdatePos() {
+    if (curDir == 0) curRow++;
+    if (curDir == 1) curCol++;
+    if (curDir == 2) curRow--;
+    if (curDir == 3) curCol--;
+
+}
+Action solver() {
+    floodfill.detectWalls({}, curRow, curCol, curDir);
+    if (!reachedCenter && (
+        (curRow == 8 && curCol == 8) ||
+        (curRow == 8 && curCol == 7) ||
+        (curRow == 7 && curCol == 8) ||
+        (curRow == 7 && curCol == 7))) {
+        reachedCenter = true;
+        std::cerr << "Reached center! Recomputing best path to start...\n";
+    }
+    int bestDir=0;
+    if(reachedCenter){
+        floodfill.floodfillToStart();
+        floodfill.floodfill();
+        bestDir = floodfill.reverse_getNextMove(curRow, curCol);
+    }
+    else {
+        floodfill.floodfill();
+        bestDir = floodfill.getNextMove(curRow, curCol);
+    }
+    Action action = rotateTo(bestDir);
+    std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl;
+    //API::setColor(curRow, curCol, 'g');
+    std::cerr << "Action decided: " << action << std::endl;
+    if (action == IDLE) {
+        std::cerr << "No valid move found, staying idle." << std::endl;
+    }
+    return action;
+}
+
+void log(const string& text) {
+    cerr << text << endl;
+}
+
+int main(int argc, char* argv[]) {
+    log("Running...");
+    while (true) {
+
+        if (reachedCenter && curRow == 0 && curCol == 0) {
+            std::cerr << "Returned to start!\n";
+            log("Goal reached!");
+            std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl;
+            std::cerr << "Total Distance               : " << getStat("total-distance") << std::endl;
+            std::cerr << "Total Turns                  : " << getStat("total-turns") << std::endl;
+            std::cerr << "Best Run Distance            : " << getStat("best-run-distance") << std::endl;
+            std::cerr << "Best Run Turns               : " << getStat("best-run-turns") << std::endl;
+            std::cerr << "Current Run Distance         : " << getStat("current-run-distance") << std::endl;
+            std::cerr << "Current Run Turns            : " << getStat("current-run-turns") << std::endl;
+            std::cerr << "Total Effective Distance     : " << getStat("total-effective-distance") << std::endl;
+            std::cerr << "Best Run Effective Distance  : " << getStat("best-run-effective-distance") << std::endl;
+            std::cerr << "Current Run Effective Dist   : " << getStat("current-run-effective-distance") << std::endl;
+            std::cerr << "Final Score                  : " << 2000 - getStat("score") << std::endl;
+            std::cerr << "Highest Possible score is 2000 " << std::endl;
+
+            API::turnRight();
+            API::turnRight();
+            curDir = (curDir + 2) % 4;
+
+            // Highlight explored shortest path from start to center using known walls only
+            floodfill.final_floodfill();
+            int row = 0, col = 0;
+            API::setColor(col, row, 'C');  // Highlight starting point
+
+            while (!atGoal(row, col)) {
+                int currentDist = floodfill.maze.manhattan_distances[row][col];
+                int nextRow = row, nextCol = col;
+
+                for (int dir = 0; dir < 4; ++dir) {
+                    int r = row, c = col;
+
+                    if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++;
+                    else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++;
+                    else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--;
+                    else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--;
+                    else continue;
+
+                    if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.manhattan_distances[r][c] < currentDist) {
+                        nextRow = r;
+                        nextCol = c;
+                        break;
+                    }
+                }
+
+                row = nextRow;
+                col = nextCol;
+                API::setColor(col, row, 'C');  // Color path step
+            }
+
+            break;
+        }
+        if (!reachedCenter && atGoal(curRow, curCol)) {
+            log("Reached center. Reversing goal to return home...");
+            reachedCenter = true;
+        }
+
+        Action action = solver();
+        
+        if (action == FORWARD) {
+            API::moveForward();            
+            API::setColor(curCol, curRow, 'c');
+            floodfill.maze.visited[curRow][curCol] = true;
+            API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol]));
+            moveForwardUpdatePos();
+            
+            
+
+        } else if (action == LEFT) {
+            API::turnLeft();
+            curDir = (curDir + 3) % 4;
+        } else if (action == RIGHT) {
+            API::turnRight();
+            curDir = (curDir + 1) % 4;
+        } else if (action == AROUND) {
+            API::turnRight();
+            API::turnRight();
+            curDir = (curDir + 2) % 4;
+        }
+        
+    }
+    return 0;
+}
diff --git a/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp b/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp
index e69de29..d0e2f43 100644
--- a/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp	
+++ b/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp	
@@ -0,0 +1,504 @@
+// Micromouse floodfill maze-solving algorithm
+// Compatible with MMS Simulator
+// Written in C++ for robotics competitions
+#include "API.h"
+#include <array>
+#include <queue>
+#include <string>
+#include <iostream>
+#include <utility>
+#include "Floodfill.h"
+#include <vector>
+
+using namespace std;
+Floodfill floodfill;
+
+enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND};
+
+int idx[] = {0,1,3,2};
+int curRow = 0, curCol = 0, curDir = 0;  // 0=north,1=east,2=south,3=west
+bool reachedCenter = false;
+bool rerun = false; 
+
+float getStat(const std::string& statName) {
+    std::cout << "getStat " << statName << std::endl;
+    std::string response;
+    std::cin >> response;
+    try {
+        return std::stof(response); // handles both int and float
+    } catch (...) {
+        std::cerr << "Failed to parse stat: " << statName << std::endl;
+        return -1;
+    }
+}
+
+bool atGoal(int row, int col) {
+    return (row == 7 || row == 8) && (col == 7 || col == 8);
+}
+
+void Floodfill::setWall(int row, int col, int direction) {
+    if(direction == 0) {
+        maze.vertical_walls[row][col].first = 1; // North wall
+        if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above
+    }
+    if(direction == 1) {
+        maze.horizontal_walls[row][col].second = 1; // East wall
+        if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right
+    }
+    if(direction == 2) {
+        maze.vertical_walls[row][col].second = 1; // South wall
+        if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below
+    }
+    if(direction == 3) {
+        maze.horizontal_walls[row][col].first = 1; // West wall
+        if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left
+    }
+    char cdir = "nesw"[direction];
+    API::setWall(col, row, cdir);
+}
+void Floodfill::detectWalls(vector<int> sensorDistances, int row, int col, int direction){
+    if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds
+    if(API::wallFront()) { // North Wall
+        setWall(row, col, direction);
+        // Serial.print("Wall detected at ");
+        // Serial.print(direction);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[2]);
+    }
+    if(API::wallLeft()) {
+        setWall(row, col, (direction + 3) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 3) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[0]);
+    }
+    //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction);
+    if(API::wallRight()) {
+        setWall(row, col, (direction + 1) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 1) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[4]);
+    }
+}
+bool Floodfill::hasWall(int row, int col, int dir) {
+    if(dir == 0) return maze.vertical_walls[row][col].first; // North wall
+    if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall
+    if(dir == 2) return maze.vertical_walls[row][col].second; // South wall
+    if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall
+    return false; // No walls
+}
+
+void Floodfill::floodfill() {
+    queue<pair<int, int>> q;    
+
+    for (auto& row : maze.manhattan_distances)
+        row.fill(255); // Re-initialize all distances to a large number
+
+    maze.manhattan_distances[8][8] = 0; //row,column
+    maze.manhattan_distances[8][7] = 0;
+    maze.manhattan_distances[7][8] = 0;
+    maze.manhattan_distances[7][7] = 0; // Goal
+
+    maze.visited[8][8] = true;
+    maze.visited[8][7] = true;
+    maze.visited[7][8] = true;
+    maze.visited[7][7] = true;
+    maze.visited[0][0] = true;
+
+    
+    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+        
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) {
+            maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) {
+            maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) {
+            maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) {
+            maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+    }
+
+    for (int r = 0; r < 16; ++r) {
+        for (int c = 0; c < 16; ++c) {
+            int dist = maze.manhattan_distances[r][c];
+            if (dist != 255) {
+                API::setText(c, r, to_string(dist));  // NOTE: col=x, row=y
+            }
+        }
+    }
+}
+
+void Floodfill::floodfillToStart() {
+    queue<pair<int, int>> q;
+
+    for (auto& row : maze.reverse_manhattan_distances)
+        row.fill(255); // Reset distances
+
+    // New goal is the start: bottom-left of the maze
+    maze.reverse_manhattan_distances[0][0] = 0;
+    q.push({0, 0});
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[0][0] = true;
+
+    while (!q.empty()) {
+        pair<int, int> cell = q.front();
+        q.pop();
+
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        // WEST
+        if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) {
+            maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) {
+            maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) {
+            maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) {
+            maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+    }
+}
+void Floodfill::final_floodfill() {
+    queue<pair<int, int>> q;    
+
+    for (auto& row : maze.manhattan_distances)
+        row.fill(255); // Re-initialize all distances to a large number
+
+    maze.manhattan_distances[8][8] = 0; //row,column
+    maze.manhattan_distances[8][7] = 0;
+    maze.manhattan_distances[7][8] = 0;
+    maze.manhattan_distances[7][7] = 0; // Goal
+
+    
+    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+        
+        int row = cell.first;
+        int col = cell.second;
+
+        if (row < 0 || row > 15 || col < 0 || col > 15) continue;
+
+        // WEST
+        if (col > 0 && !maze.horizontal_walls[row][col].first && maze.visited[row][col - 1] && !reached[row][col - 1]) {
+            maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col - 1});
+            reached[row][col - 1] = true;
+        }
+
+        // NORTH
+        if (row < 15 && !maze.vertical_walls[row][col].first && maze.visited[row + 1][col] && !reached[row + 1][col]) {
+            maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row + 1, col});
+            reached[row + 1][col] = true;
+        }
+
+        // EAST
+        if (col < 15 && !maze.horizontal_walls[row][col].second && maze.visited[row][col + 1] && !reached[row][col + 1]) {
+            maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1;
+            q.push({row, col + 1});
+            reached[row][col + 1] = true;
+        }
+
+        // SOUTH
+        if (row > 0 && !maze.vertical_walls[row][col].second && maze.visited[row - 1][col] && !reached[row - 1][col]) {
+            maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1;
+            q.push({row - 1, col});
+            reached[row - 1][col] = true;
+        }
+
+    }
+    for (int r = 0; r < 16; ++r) {
+        for (int c = 0; c < 16; ++c) {
+            int dist = maze.manhattan_distances[r][c];
+            if (dist != 255) {
+                API::setText(c, r, to_string(dist));  
+            }
+        }
+    }
+}
+
+int Floodfill::getNextMove(int row, int col) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    for (int dir = 0; dir < 4; ++dir) {
+        int r = row, c = col;
+
+        if (dir == 0 && !hasWall(row, col, 0)) r++;
+        else if (dir == 1 && !hasWall(row, col, 1)) c++;
+        else if (dir == 2 && !hasWall(row, col, 2)) r--;
+        else if (dir == 3 && !hasWall(row, col, 3)) c--;
+        else continue;  // skip if there's a wall
+
+        if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+        int dist = maze.manhattan_distances[r][c];
+
+        if (dist < minDist || (dist == minDist && dir == curDir)) {
+            minDist = dist;
+            bestDirection = dir;
+        }
+    }
+
+    return bestDirection;
+}
+int Floodfill::reverse_getNextMove(int row, int col) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    for (int dir = 0; dir < 4; ++dir) {
+        int r = row, c = col;
+
+        if (dir == 0 && !hasWall(row, col, 0)) r++;
+        else if (dir == 1 && !hasWall(row, col, 1)) c++;
+        else if (dir == 2 && !hasWall(row, col, 2)) r--;
+        else if (dir == 3 && !hasWall(row, col, 3)) c--;
+        else continue;  // skip if there's a wall
+
+        if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+        int dist = maze.reverse_manhattan_distances[r][c];
+
+        if (dist < minDist || (dist == minDist && dir == curDir)) {
+            minDist = dist;
+            bestDirection = dir;
+        }
+    }
+
+    return bestDirection;
+}
+Action rotateTo(int newDir) {
+    int diff = (newDir - curDir + 4) % 4;
+    if (diff == 0) return FORWARD;
+    if (diff == 1) return RIGHT;
+    if (diff == 3) return LEFT;
+    if (diff == 2) return AROUND;  // U-turn
+    return IDLE;
+}
+void moveForwardUpdatePos() {
+    if (curDir == 0) curRow++;
+    if (curDir == 1) curCol++;
+    if (curDir == 2) curRow--;
+    if (curDir == 3) curCol--;
+
+}
+Action solver() {
+    floodfill.detectWalls({}, curRow, curCol, curDir);
+    if (!reachedCenter && (
+        (curRow == 8 && curCol == 8) ||
+        (curRow == 8 && curCol == 7) ||
+        (curRow == 7 && curCol == 8) ||
+        (curRow == 7 && curCol == 7))) {
+        reachedCenter = true;
+        std::cerr << "Reached center! Recomputing best path to start...\n";
+    }
+    int bestDir=0;
+    if(reachedCenter){
+        floodfill.floodfillToStart();
+        floodfill.floodfill();
+        bestDir = floodfill.reverse_getNextMove(curRow, curCol);
+    }
+    else {
+        floodfill.floodfill();
+        bestDir = floodfill.getNextMove(curRow, curCol);
+    }
+    Action action = rotateTo(bestDir);
+    std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl;
+    //API::setColor(curRow, curCol, 'g');
+    std::cerr << "Action decided: " << action << std::endl;
+    if (action == IDLE) {
+        std::cerr << "No valid move found, staying idle." << std::endl;
+    }
+    return action;
+}
+
+void log(const string& text) {
+    cerr << text << endl;
+}
+
+int main(int argc, char* argv[]) {
+    log("Running...");
+    while (true) {
+
+        if (reachedCenter && curRow == 0 && curCol == 0) {
+            std::cerr << "Returned to start!\n";
+            log("Goal reached!");
+            std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl;
+            std::cerr << "Total Distance               : " << getStat("total-distance") << std::endl;
+            std::cerr << "Total Turns                  : " << getStat("total-turns") << std::endl;
+            std::cerr << "Best Run Distance            : " << getStat("best-run-distance") << std::endl;
+            std::cerr << "Best Run Turns               : " << getStat("best-run-turns") << std::endl;
+            std::cerr << "Current Run Distance         : " << getStat("current-run-distance") << std::endl;
+            std::cerr << "Current Run Turns            : " << getStat("current-run-turns") << std::endl;
+            std::cerr << "Total Effective Distance     : " << getStat("total-effective-distance") << std::endl;
+            std::cerr << "Best Run Effective Distance  : " << getStat("best-run-effective-distance") << std::endl;
+            std::cerr << "Current Run Effective Dist   : " << getStat("current-run-effective-distance") << std::endl;
+            std::cerr << "Final Score                  : " << 2000 - getStat("score") << std::endl;
+            std::cerr << "Highest Possible score is 2000 " << std::endl;
+
+            API::turnRight();
+            API::turnRight();
+            curDir = (curDir + 2) % 4;
+
+            // Highlight explored shortest path from start to center using known walls only
+            //floodfill.final_floodfill();
+            int row = 0, col = 0;
+            API::setColor(col, row, 'C');  // Highlight starting point
+
+            while (!atGoal(row, col)) {
+                int currentDist = floodfill.maze.manhattan_distances[row][col];
+                int nextRow = row, nextCol = col;
+
+                for (int dir = 0; dir < 4; ++dir) {
+                    int r = row, c = col;
+
+                    if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++;
+                    else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++;
+                    else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--;
+                    else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--;
+                    else continue;
+
+                    if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.visited[r][c] && floodfill.maze.manhattan_distances[r][c] < currentDist) {
+                        nextRow = r;
+                        nextCol = c;
+                        break;
+                    }
+                    }
+                    // If after checking all directions, we didn't move (still at same cell), path is blocked
+                    if (row == nextRow && col == nextCol) {
+                        log("Path to goal is blocked or incomplete. Re-run the floodfill...");
+                        rerun = true; //rerun the robot through the calculated shorter path
+                        // Try to find a new path if the current one is blocked
+                        reachedCenter = false;
+                        goto path_blocked;
+                }
+
+                row = nextRow;
+                col = nextCol;
+                API::setColor(col, row, 'C');  // Color path step
+            }        
+            break;
+        }
+        path_blocked:
+        if (!reachedCenter && atGoal(curRow, curCol)) {
+            log("Reached center. Reversing goal to return home...");
+            reachedCenter = true;
+        }
+        if(rerun && atGoal(curRow, curCol)) {
+            // Highlight explored shortest path from start to center using known walls only
+            floodfill.final_floodfill();
+            int row = 0, col = 0;
+            API::setColor(col, row, 'C');  // Highlight starting point
+            while (!atGoal(row, col)) {
+                int currentDist = floodfill.maze.manhattan_distances[row][col];
+                int nextRow = row, nextCol = col;
+
+                for (int dir = 0; dir < 4; ++dir) {
+                    int r = row, c = col;
+
+                    if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++;
+                    else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++;
+                    else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--;
+                    else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--;
+                    else continue;
+
+                    if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.manhattan_distances[r][c] < currentDist) {
+                        nextRow = r;
+                        nextCol = c;
+                        break;
+                    }
+                }
+
+                row = nextRow;
+                col = nextCol;
+                API::setColor(col, row, 'C');  // Color path step
+            }
+            break;
+        }
+
+        Action action = solver();
+        
+        if (action == FORWARD) {
+            API::moveForward();            
+            API::setColor(curCol, curRow, 'c');
+            floodfill.maze.visited[curRow][curCol] = true;
+            API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol]));
+            moveForwardUpdatePos();
+            
+            
+
+        } else if (action == LEFT) {
+            API::turnLeft();
+            curDir = (curDir + 3) % 4;
+        } else if (action == RIGHT) {
+            API::turnRight();
+            curDir = (curDir + 1) % 4;
+        } else if (action == AROUND) {
+            API::turnRight();
+            API::turnRight();
+            curDir = (curDir + 2) % 4;
+        }
+        
+    }
+    return 0;
+}
diff --git a/lib/Floodfill/mms src/Floodfill_SearchRun.cpp b/lib/Floodfill/mms src/Floodfill_SearchRun.cpp
new file mode 100644
index 0000000..95c8632
--- /dev/null
+++ b/lib/Floodfill/mms src/Floodfill_SearchRun.cpp	
@@ -0,0 +1,257 @@
+// Micromouse floodfill maze-solving algorithm
+// Compatible with MMS Simulator
+// Written in C++ for robotics competitions
+#include "API.h"
+#include <array>
+#include <queue>
+#include <string>
+#include <iostream>
+#include <utility>
+#include "Floodfill.h"
+#include <vector>
+
+using namespace std;
+Floodfill floodfill;
+
+enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND};
+
+const int SIZE = 16;
+
+int curRow = 0, curCol = 0, curDir = 0;  // 0=north,1=east,2=south,3=west
+
+float getStat(const std::string& statName) {
+    std::cout << "getStat " << statName << std::endl;
+    std::string response;
+    std::cin >> response;
+    try {
+        return std::stof(response); // handles both int and float
+    } catch (...) {
+        std::cerr << "Failed to parse stat: " << statName << std::endl;
+        return -1;
+    }
+}
+
+bool atGoal(int row, int col) {
+    return (row == 7 || row == 8) && (col == 7 || col == 8);
+}
+
+void Floodfill::setWall(int row, int col, int direction) {
+    if(direction == 0) {
+        maze.vertical_walls[row][col].first = 1; // North wall
+        if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above
+    }
+    if(direction == 1) {
+        maze.horizontal_walls[row][col].second = 1; // East wall
+        if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right
+    }
+    if(direction == 2) {
+        maze.vertical_walls[row][col].second = 1; // South wall
+        if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below
+    }
+    if(direction == 3) {
+        maze.horizontal_walls[row][col].first = 1; // West wall
+        if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left
+    }
+    char cdir = "nesw"[direction];
+    API::setWall(col, row, cdir);
+}
+void Floodfill::detectWalls(vector<int> sensorDistances, int row, int col, int direction){
+    if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds
+    if(API::wallFront()) { // North Wall
+        setWall(row, col, direction);
+        // Serial.print("Wall detected at ");
+        // Serial.print(direction);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[2]);
+    }
+    if(API::wallLeft()) {
+        setWall(row, col, (direction + 3) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 3) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[0]);
+    }
+    //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction);
+    if(API::wallRight()) {
+        setWall(row, col, (direction + 1) % 4);
+        // Serial.print("Wall detected at ");
+        // Serial.print((direction + 1) % 4);
+        // Serial.print(" with distance: ");
+        // Serial.println(sensorDistances[4]);
+    }
+}
+bool Floodfill::hasWall(int row, int col, int dir) {
+    if(dir == 0) return maze.vertical_walls[row][col].first; // North wall
+    if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall
+    if(dir == 2) return maze.vertical_walls[row][col].second; // South wall
+    if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall
+    return false; // No walls
+}
+
+void Floodfill::floodfill() {
+    queue<pair<int, int>> q;    
+
+    for (auto& row : maze.manhattan_distances)
+        row.fill(255); // Re-initialize all distances to a large number
+
+    maze.manhattan_distances[8][8] = 0; //row,column
+    maze.manhattan_distances[8][7] = 0;
+    maze.manhattan_distances[7][8] = 0;
+    maze.manhattan_distances[7][7] = 0; // Goal
+    
+    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    array<array<bool, 16>, 16> reached = {};
+    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+
+        if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds
+
+        if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left
+            maze.manhattan_distances[cell.first][cell.second - 1] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second - 1});
+            reached[cell.first][cell.second - 1] = true; // Marking the cell as reached            
+        }
+        if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above
+            maze.manhattan_distances[cell.first + 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            q.push({cell.first + 1, cell.second});
+            reached[cell.first + 1][cell.second] = true; // Marking the cell as reached
+        }
+        if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right
+            maze.manhattan_distances[cell.first][cell.second + 1] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second + 1});
+            reached[cell.first][cell.second + 1] = true; // Marking the cell as reached
+        }
+        if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below
+            maze.manhattan_distances[cell.first - 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            q.push({cell.first - 1, cell.second});
+            reached[cell.first - 1][cell.second] = true; // Marking the cell as reached
+        }
+    }
+
+    for (int r = 0; r < 16; ++r) {
+        for (int c = 0; c < 16; ++c) {
+            int dist = maze.manhattan_distances[r][c];
+            if (dist != 255) {
+                API::setText(c, r, to_string(dist));  // NOTE: col=x, row=y
+            }
+        }
+    }
+}
+
+int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    // int nextRow = row;
+    // int nextCol = col;
+
+    if(!hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) {
+        minDist = maze.manhattan_distances[row + 1][col];
+        bestDirection = 0;
+    }
+    if(!hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) {
+        minDist = maze.manhattan_distances[row][col + 1];
+        bestDirection = 1;
+    }
+    if(!hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) {
+        minDist = maze.manhattan_distances[row - 1][col];
+        bestDirection = 2;
+    }
+    if(!hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) {
+        minDist = maze.manhattan_distances[row][col - 1];
+        bestDirection = 3;
+    }
+
+    // Serial.print("Let's move to the direction coded as ");
+    // Serial.println(bestDirection);
+    cout << "Let's move to the direction coded as " << bestDirection << endl;
+
+    return bestDirection; // Return the best direction to move based on the minimum distance
+}
+
+
+Action rotateTo(int newDir) {
+    int diff = (newDir - curDir + 4) % 4;
+    if (diff == 0) return FORWARD;
+    if (diff == 1) return RIGHT;
+    if (diff == 3) return LEFT;
+    if (diff == 2) return AROUND;  // U-turn
+    return IDLE;
+}
+
+
+void moveForwardUpdatePos() {
+    if (curDir == 0) curRow++;
+    if (curDir == 1) curCol++;
+    if (curDir == 2) curRow--;
+    if (curDir == 3) curCol--;
+
+}
+
+
+Action solver() {
+    floodfill.detectWalls({}, curRow, curCol, curDir);
+    floodfill.floodfill();
+    int bestDir = floodfill.getNextMove(curRow, curCol);
+
+    Action action = rotateTo(bestDir);
+    std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl;
+    //API::setColor(curRow, curCol, 'g');
+    std::cerr << "Action decided: " << action << std::endl;
+    if (action == IDLE) {
+        std::cerr << "No valid move found, staying idle." << std::endl;
+    }
+    return action;
+}
+
+void log(const string& text) {
+    cerr << text << endl;
+}
+int main(int argc, char* argv[]) {
+    log("Running...");
+    while (true) {
+
+        if (atGoal(curRow, curCol)) {
+            log("Goal reached!");
+            std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl;
+            std::cerr << "Total Distance               : " << getStat("total-distance") << std::endl;
+            std::cerr << "Total Turns                  : " << getStat("total-turns") << std::endl;
+            std::cerr << "Best Run Distance            : " << getStat("best-run-distance") << std::endl;
+            std::cerr << "Best Run Turns               : " << getStat("best-run-turns") << std::endl;
+            std::cerr << "Current Run Distance         : " << getStat("current-run-distance") << std::endl;
+            std::cerr << "Current Run Turns            : " << getStat("current-run-turns") << std::endl;
+            std::cerr << "Total Effective Distance     : " << getStat("total-effective-distance") << std::endl;
+            std::cerr << "Best Run Effective Distance  : " << getStat("best-run-effective-distance") << std::endl;
+            std::cerr << "Current Run Effective Dist   : " << getStat("current-run-effective-distance") << std::endl;
+            std::cerr << "Final Score                  : " << 2000 - getStat("score") << std::endl;
+            std::cerr << "Highest Possible score is 2000 " << std::endl;
+            break;
+        }
+
+        Action action = solver();
+        
+        if (action == FORWARD) {
+            API::moveForward();
+            API::setColor(curCol, curRow, 'c');
+            API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol]));
+            moveForwardUpdatePos();
+        } else if (action == LEFT) {
+            API::turnLeft();
+            curDir = (curDir + 3) % 4;
+        } else if (action == RIGHT) {
+            API::turnRight();
+            curDir = (curDir + 1) % 4;
+        } else if (action == AROUND) {
+            API::turnRight();
+            API::turnRight();
+            curDir = (curDir + 2) % 4;
+        }
+        
+    }
+    return 0;
+}
diff --git a/lib/Floodfill/src/Floodfill.h b/lib/Floodfill/src/Floodfill.h
index 336a106..641d0cd 100644
--- a/lib/Floodfill/src/Floodfill.h
+++ b/lib/Floodfill/src/Floodfill.h
@@ -10,6 +10,8 @@ struct grid {
         array<array<pair<int, int>, 16>, 16> horizontal_walls; // (left,right)
         array<array<pair<int, int>, 16>, 16> vertical_walls; // (up,down)
         array<array<int, 16>, 16> manhattan_distances = {}; // assigned false at the beginning
+        array<array<int, 16>, 16> reverse_manhattan_distances = {}; // assigned false at the beginning
+        array<array<bool, 16>, 16> visited{};
     };
 
 class Floodfill {
@@ -18,16 +20,23 @@ class Floodfill {
     int wall_threshhold = 120;
     int direction;
     int row, col;
-    bool front, left, right;
+    bool front, left, right;    
+    
 public:
     grid maze;
     // void setThreshhold(int threshhold) {wall_threshhold = threshhold;}
+    bool RobotDone = false;
+    bool LastRun = false;
+
     void setWall(int row, int col, int direction);
     void detectWalls(vector<int> sensorDistances, int row, int col, int direction);    
-    void floodfill();
+    void floodfill(std::array<std::array<int, 16>, 16> &manhattan_distances, int goalRow, int goalCol);
     bool hasWall(int row, int col, int dir);
-    int getNextMove(int row, int col); //next row column will also be automatically update from this function
+    int getNextMove(array<array<int, 16>, 16> &dist,int row, int col); //next row column will also be automatically update from this function
+    int reverse_getNextMove(int row, int col); //next row column will also be automatically update from this function
     bool atGoal(int row, int col); //check if robot came to the goal
+
+    void floodfillToStart();
 };
 //both detectWall and updateWall do the same thing but with different parameters
 #endif
diff --git a/lib/Floodfill/src/Floodfill_SearchRun.cpp b/lib/Floodfill/src/Floodfill_SearchRun.cpp
index c3c52d1..8b9a67d 100644
--- a/lib/Floodfill/src/Floodfill_SearchRun.cpp
+++ b/lib/Floodfill/src/Floodfill_SearchRun.cpp
@@ -1,7 +1,3 @@
-// Micromouse floodfill maze-solving algorithm (on-robot variant)
-// Removes MMS Simulator API usage and main(); integrates with robot by:
-// 1) Detecting walls from sensor distances (threshold-based)
-// 2) Providing floodfill() and getNextMove() you can call from your code
 #include <array>
 #include <queue>
 #include <utility>
@@ -19,7 +15,10 @@ extern std::vector<int> getDistances(); // returns VL6180X distances: [left,...,
 // Action and robot pose/state (kept here so main.cpp stays minimal)
 enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND };
 static int curRow = 0, curCol = 0, curDir = 0;  // 0=N,1=E,2=S,3=W
-static bool reachedCenter = false;
+bool reachedCenter = false;
+bool FinalRun = false;
+bool readyToFinalRun = false;
+
 static Floodfill floodfill; // solver instance
 
 bool Floodfill::atGoal(int row, int col) {
@@ -67,21 +66,39 @@ bool Floodfill::hasWall(int row, int col, int dir) {
     return false; // No walls
 }
 
-void Floodfill::floodfill() {
+void Floodfill::floodfill(array<array<int, 16>, 16> &dist, int goalRow, int goalCol) {
     queue<pair<int, int>> q;    
+    for (auto &row : dist) {
+        row.fill(200);
+    }
+    // maze.manhattan_distances[8][8] = 0; //row,column
+    // maze.manhattan_distances[8][7] = 0;
+    // maze.manhattan_distances[7][8] = 0;
+    // maze.manhattan_distances[7][7] = 0; // Goal
+    array<array<bool, 16>, 16> reached = {};
+    if(!reachedCenter){
+        dist[7][7] = 0;
+        dist[7][8] = 0;
+        dist[8][7] = 0;
+        dist[8][8] = 0;
+        q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});
+        reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; 
+    } else {
+        dist[0][0] = 0;
+        q.push({0, 0});
+        reached[0][0] = true;
+    }
 
-    for (auto& row : maze.manhattan_distances)
-        row.fill(255); // Re-initialize all distances to a large number
-
-    maze.manhattan_distances[8][8] = 0; //row,column
-    maze.manhattan_distances[8][7] = 0;
-    maze.manhattan_distances[7][8] = 0;
-    maze.manhattan_distances[7][7] = 0; // Goal
+    maze.visited[8][8] = true;
+    maze.visited[8][7] = true;
+    maze.visited[7][8] = true;
+    maze.visited[7][7] = true;
+    maze.visited[0][0] = true;
     
-    q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+    // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
 
-    array<array<bool, 16>, 16> reached = {};
-    reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    // array<array<bool, 16>, 16> reached = {};
+    // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
     
     // BFS to fill the manhattan distances
     while(!q.empty()){
@@ -91,61 +108,102 @@ void Floodfill::floodfill() {
         if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds
 
         if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left
-            maze.manhattan_distances[cell.first][cell.second - 1] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1;
             q.push({cell.first, cell.second - 1});
             reached[cell.first][cell.second - 1] = true; // Marking the cell as reached            
         }
         if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above
-            maze.manhattan_distances[cell.first + 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1;
             q.push({cell.first + 1, cell.second});
             reached[cell.first + 1][cell.second] = true; // Marking the cell as reached
         }
         if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right
-            maze.manhattan_distances[cell.first][cell.second + 1] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1;
             q.push({cell.first, cell.second + 1});
             reached[cell.first][cell.second + 1] = true; // Marking the cell as reached
         }
         if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below
-            maze.manhattan_distances[cell.first - 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1;
+            dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1;
             q.push({cell.first - 1, cell.second});
             reached[cell.first - 1][cell.second] = true; // Marking the cell as reached
         }
     }
-
-    for (int r = 0; r < 16; ++r) {
-        for (int c = 0; c < 16; ++c) {
-            int dist = maze.manhattan_distances[r][c];
-        }
-    }
 }
 
-int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) {
+int Floodfill::getNextMove(array<array<int, 16>, 16> &dist,int row, int col /*int direction //for optimize movement(for later improvement)*/) {
     int minDist = 255;
     int bestDirection = -1;
 
     // int nextRow = row;
     // int nextCol = col;
 
-    if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) {
-        minDist = maze.manhattan_distances[row + 1][col];
+    if(row < 15 && !hasWall(row, col, 0) && dist[row + 1][col] < minDist) {
+        minDist = dist[row + 1][col];
         bestDirection = 0;
     }
-    if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) {
-        minDist = maze.manhattan_distances[row][col + 1];
+    if(col < 15 && !hasWall(row, col, 1) && dist[row][col + 1] < minDist) {
+        minDist = dist[row][col + 1];
         bestDirection = 1;
     }
-    if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) {
-        minDist = maze.manhattan_distances[row - 1][col];
+    if(row > 0 && !hasWall(row, col, 2) && dist[row - 1][col] < minDist) {
+        minDist = dist[row - 1][col];
         bestDirection = 2;
     }
-    if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) {
-        minDist = maze.manhattan_distances[row][col - 1];
+    if(col > 0 && !hasWall(row, col, 3) && dist[row][col - 1] < minDist) {
+        minDist = dist[row][col - 1];
         bestDirection = 3;
     }
 
     return bestDirection; // Return the best direction to move based on the minimum distance
+    // int minDist = 255;
+    // int bestDirection = -1;
+
+    // for (int dir = 0; dir < 4; ++dir) {
+    //     int r = row, c = col;
+
+    //     if (dir == 0 && !hasWall(row, col, 0)) r++;
+    //     else if (dir == 1 && !hasWall(row, col, 1)) c++;
+    //     else if (dir == 2 && !hasWall(row, col, 2)) r--;
+    //     else if (dir == 3 && !hasWall(row, col, 3)) c--;
+    //     else continue;  // skip if there's a wall
+
+    //     if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+    //     int dist = maze.manhattan_distances[r][c];
+
+    //     if (dist < minDist || (dist == minDist && dir == curDir)) {
+    //         minDist = dist;
+    //         bestDirection = dir;
+    //     }
+    // }
+
+    // return bestDirection;
 }
+// int Floodfill::reverse_getNextMove(int row, int col) {
+//     int minDist = 255;
+//     int bestDirection = -1;
 
+//     for (int dir = 0; dir < 4; ++dir) {
+//         int r = row, c = col;
+
+//         if (dir == 0 && !hasWall(row, col, 0)) r++;
+//         else if (dir == 1 && !hasWall(row, col, 1)) c++;
+//         else if (dir == 2 && !hasWall(row, col, 2)) r--;
+//         else if (dir == 3 && !hasWall(row, col, 3)) c--;
+//         else continue;  // skip if there's a wall
+
+//         if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+//         int dist = maze.reverse_manhattan_distances[r][c];
+
+//         if (dist < minDist || (dist == minDist && dir == curDir)) {
+//             minDist = dist;
+//             bestDirection = dir;
+//         }
+//     }
+
+//     return bestDirection;
+// }
 
 // --- Solver helpers and main loop (preserve your structure) ---
 static Action rotateTo(int newDir) {
@@ -172,19 +230,46 @@ static Action solver() {
 
     // If you want double-search (return to start after center), toggle reachedCenter here
     if (!reachedCenter && floodfill.atGoal(curRow, curCol)) {
-        reachedCenter = true; // center reached (optional behavior)
+        reachedCenter = true; // center reached (optional behavior) 
+        return IDLE;
+    }
+    else if(reachedCenter && curRow == 0 && curCol == 0) {
+        floodfill.RobotDone = true;
+        floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7);
+        reachedCenter = false; // ensure we are in return mode
+        readyToFinalRun = true;
+        return IDLE;    
     }
 
     // Compute floodfill and choose next direction
-    floodfill.floodfill();
-    int bestDir = floodfill.getNextMove(curRow, curCol);
-    return rotateTo(bestDir);
+    if (!reachedCenter && !readyToFinalRun) {
+        // search phase → goal is center
+        floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7);  
+        int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol);
+        return rotateTo(bestDir);  
+    } else if(!FinalRun){
+        // return phase → goal is start
+        floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); 
+        int bestDir = floodfill.getNextMove(floodfill.maze.reverse_manhattan_distances, curRow, curCol);
+        return rotateTo(bestDir);
+    }
+    else{
+        //Floodfill is being calculated once when the robot came back to the start(in that if nest)
+        //Final Run phase → goal is center
+        int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol);
+
+        if (floodfill.atGoal(curRow, curCol)) {
+            floodfill.LastRun = true; // finished final run
+            return IDLE;
+        }
+    }
+    
 }
 
 // Perform a single floodfill decision + action; call repeatedly from loop()
 void runFloodfillStep() {
     // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit
-
+    
     Action action = solver();
     
     if (action == FORWARD) {
@@ -206,3 +291,16 @@ void runFloodfillStep() {
         return;
     }
 }
+bool isRobotDone() {
+        return floodfill.RobotDone;
+    }
+bool BeginFinalRun() {
+        if(!FinalRun) {
+            FinalRun = true;
+            curRow = curCol = 0;  // reset robot position
+            curDir = 0;
+            reachedCenter = false; // ensure we are in return mode
+            return true;
+        }
+        return false;
+    }
\ No newline at end of file
diff --git a/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h b/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h
index d01bfa1..8980822 100644
--- a/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h
+++ b/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h
@@ -22,7 +22,7 @@ class MotorPIDbyNJ {
     bool isDone;
     long target;
     int tolerance = 5; // 5mm Tolerance for PID control, can be adjusted
-    long metricConverter = 27; // Conversion factor for encoder value, can be adjusted (27.6 encoder = 1 mm)
+    long metricConverter = 24.4; // Conversion factor for encoder value, can be adjusted (24.4 encoder = 1 mm)
 
 public:
     MotorPIDbyNJ(int pin1, int pin2, int encoderPin1, int encoderPin2);
diff --git a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp
index e468ca4..ed528a6 100644
--- a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp
+++ b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp
@@ -29,6 +29,10 @@ int RobotNavigatorV2::calculateWallPID() {
         Serial.println("Error: Not enough sensor data");
         return 0; // or handle error
     }
+    int leftSensor = sensorDistances[0];
+    int rightSensor = sensorDistances[4];
+    if(leftSensor > 80) leftSensor = 45;
+    if(rightSensor > 80) rightSensor = 45;
     // float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100));
 
     // long currentTime = micros();
@@ -48,8 +52,8 @@ int RobotNavigatorV2::calculateWallPID() {
     // previousSensorError = wallError;
     // previousSensorTime = currentTime;
     // return WallPID;
-    int difference = constrain(sensorDistances[0]-sensorDistances[4], -80, 80);
-    return difference;
+
+    return leftSensor - rightSensor;
 }
 
 void RobotNavigatorV2::moveForward() {
@@ -76,8 +80,8 @@ void RobotNavigatorV2::moveForward() {
             moving = false; 
         }
         else{
-            speedL = constrain((leftEncoderPID - currentWallPID/5),-255,255);
-            speedR = constrain((rightEncoderPID + currentWallPID/5),-255,255);
+            speedL = constrain((leftEncoderPID - currentWallPID/2),-255,255);
+            speedR = constrain((rightEncoderPID + currentWallPID/2),-255,255);
             leftMotor->runMotor(speedL);
             rightMotor->runMotor(speedR);
         }
@@ -93,20 +97,20 @@ void RobotNavigatorV2::moveForward() {
 void RobotNavigatorV2::turnLeft() {
     
     unsigned long moveStartTime = millis();
-    if(!moving) {
-        Serial.println("Turning Left");
-        centerInCell();
-        resetEncoders();
-        
-        float yaw = imu->getYaw();
-        while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw
-            delay(10);
-            yaw = imu->getYaw();
-        }
-        imu->setTargetYaw(yaw - 82); // Adjust target yaw for left turn edited for 82 degrees
-        moving = true;
-        cellDone = false;
+    //if(!moving) {
+    Serial.println("Turning Left");
+    centerInCell();
+    resetEncoders();
+    
+    float yaw = imu->getYaw();
+    while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw
+        delay(10);
+        yaw = imu->getYaw();
     }
+    imu->setTargetYaw(yaw - 86);// Adjust target yaw for left turn edited for 82 degrees
+    moving = true;
+    cellDone = false;
+    //}
     while(moving){
         //centerInCell();
         int currentAnglePID = imu->calculateAnglePID();
@@ -130,20 +134,20 @@ void RobotNavigatorV2::turnLeft() {
 }
 void RobotNavigatorV2::turnRight() {
     unsigned long moveStartTime = millis();
-    if(!moving) {
-        Serial.println("Turning Right");
-        
-        centerInCell();
-        resetEncoders();
-        float yaw = imu->getYaw();
-        while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw
-            delay(10);
-            yaw = imu->getYaw();
-        }
-        imu->setTargetYaw(yaw + 82); // Adjust target yaw for right turn
-        moving = true;
-        cellDone = false;
+
+    Serial.println("Turning Right");
+    
+    centerInCell();
+    resetEncoders();
+    float yaw = imu->getYaw();
+    while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw
+        delay(10);
+        yaw = imu->getYaw();
     }
+    imu->setTargetYaw(yaw + 86); // Adjust target yaw for right turn
+    moving = true;
+    cellDone = false;
+    
     while(moving){        
         //centerInCell();
         
@@ -167,49 +171,49 @@ void RobotNavigatorV2::turnRight() {
     }
 }
 
-int RobotNavigatorV2::centeringPID() {
-    sensorDistances = sensorGroup->readAll();
-    if (sensorDistances.size() < 5) {
-        Serial.println("Error: Not enough sensor data");
-        return 0; // or handle error
-    }
-    //float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100));
-    float wallError = sensorDistances[4];
-
-    long currentTime = millis();
-    float deltaTime = ((float)(currentTime - previousSensorTime)) / 1000.0;      
+// int RobotNavigatorV2::centeringPID() {
+//     sensorDistances = sensorGroup->readAll();
+//     if (sensorDistances.size() < 5) {
+//         Serial.println("Error: Not enough sensor data");
+//         return 0; // or handle error
+//     }
+//     //float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100));
+//     float wallError = sensorDistances[4];
+
+//     long currentTime = millis();
+//     float deltaTime = ((float)(currentTime - previousSensorTime)) / 1000.0;      
     
-    // Calculate derivative
-    float derivative = (wallError - previousSensorError) / deltaTime;
+//     // Calculate derivative
+//     float derivative = (wallError - previousSensorError) / deltaTime;
     
-    // Calculate integral
-    integralWallError += wallError * deltaTime;
-    integralWallError = constrain(integralWallError, -300, 300);  // Clamp integral
+//     // Calculate integral
+//     integralWallError += wallError * deltaTime;
+//     integralWallError = constrain(integralWallError, -300, 300);  // Clamp integral
 
-    // Calculate PID output
-    float wallPID = wallKp * wallError + wallKi * integralWallError + wallKd * derivative;
+//     // Calculate PID output
+//     float wallPID = wallKp * wallError + wallKi * integralWallError + wallKd * derivative;
 
-    previousSensorError = wallError;
-    previousSensorTime = currentTime;
-    return wallPID;
-}
+//     previousSensorError = wallError;
+//     previousSensorTime = currentTime;
+//     return wallPID;
+// }
 void RobotNavigatorV2::centerInCell() {
     // Only center if there is a wall on at least one side
     sensorDistances = sensorGroup->readAll();
     //bool leftWall = sensorDistances[0] < 80;   // adjust threshold as needed
     //bool rightWall = sensorDistances[4] < 80;  // adjust threshold as needed
-    bool frontWall = sensorDistances[2] < 100;  // adjust threshold as needed
+    bool frontWall = sensorDistances[2] < 120;  // adjust threshold as needed
     Serial.println("Front Wall Distance: " + String(sensorDistances[2]));
     if (!frontWall) return; // No wall to reference
     Serial.println("Front Wall is in: " + String(sensorDistances[2])+" mm");
-    if(sensorDistances[2] > 30 && sensorDistances[2] < 50) return;
+    if(sensorDistances[2] > 25 && sensorDistances[2] < 40) return;
 
     Serial.println("Centering in cell...");
     resetEncoders();
 
     unsigned long startTime = millis();
-    while (millis() - startTime < 1600 && (sensorDistances[2] < 30 || sensorDistances[2] > 50)) { // Center for up to 1.6s, adjust as needed
-        int speed = (sensorDistances[2] - 40)*2; // target distance is 40mm, adjust as needed
+    while (millis() - startTime < 1600 && (sensorDistances[2] < 25 || sensorDistances[2] > 40)) { // Center for up to 1.6s, adjust as needed
+        int speed = (sensorDistances[2] - 33)*2; // target distance is 33mm, adjust as needed
         speed = constrain(speed, -150, 150); // limit speed
         leftMotor->runMotor(speed);
         rightMotor->runMotor(speed);
@@ -220,29 +224,29 @@ void RobotNavigatorV2::centerInCell() {
     Serial.println("Centered.");
 }
 
-void RobotNavigatorV2::turnAround() {
-    if(!moving) {
-        Serial.println("Turning Around");
-        resetEncoders();
-        imu->setTargetYaw(imu->getYaw() - 180); // Adjust target yaw for left turn
-        moving = true;
-        cellDone = false;
-    }
-    while(moving){        
+// void RobotNavigatorV2::turnAround() {
+//     if(!moving) {
+//         Serial.println("Turning Around");
+//         resetEncoders();
+//         imu->setTargetYaw(imu->getYaw() - 180); // Adjust target yaw for left turn
+//         moving = true;
+//         cellDone = false;
+//     }
+//     while(moving){        
         
-        int currentAnglePID = imu->calculateAnglePID();
-        Serial.println("Current Angle PID: " + String(currentAnglePID));
-
-        if(imu->checkDone() && imu->checkDone()) {
-            Serial.println("Turn Done");
-            cellDone = true;
-            moving = false; 
-        }
-        else{
-            speedL = constrain((0 - currentAnglePID),-255,255);
-            speedR = constrain((0 + currentAnglePID),-255,255);
-            leftMotor->runMotor(speedL);
-            rightMotor->runMotor(speedR);
-        }
-    }
-}
\ No newline at end of file
+//         int currentAnglePID = imu->calculateAnglePID();
+//         Serial.println("Current Angle PID: " + String(currentAnglePID));
+
+//         if(imu->checkDone() && imu->checkDone()) {
+//             Serial.println("Turn Done");
+//             cellDone = true;
+//             moving = false; 
+//         }
+//         else{
+//             speedL = constrain((0 - currentAnglePID),-255,255);
+//             speedR = constrain((0 + currentAnglePID),-255,255);
+//             leftMotor->runMotor(speedL);
+//             rightMotor->runMotor(speedR);
+//         }
+//     }
+// }
\ No newline at end of file
diff --git a/note.cpp b/note.cpp
new file mode 100644
index 0000000..3ecfa01
--- /dev/null
+++ b/note.cpp
@@ -0,0 +1,282 @@
+#include <array>
+#include <queue>
+#include <utility>
+#include <vector>
+#include "Floodfill.h"
+
+using namespace std;
+
+// Extern primitives to be implemented in main.cpp
+extern void moveForward();
+extern void turnLeft();
+extern void turnRight();
+extern std::vector<int> getDistances(); // returns VL6180X distances: [left,...,front,...,right]
+
+// Action and robot pose/state (kept here so main.cpp stays minimal)
+enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND };
+static int curRow = 0, curCol = 0, curDir = 0;  // 0=N,1=E,2=S,3=W
+static bool reachedCenter = false;
+
+static Floodfill floodfill; // solver instance
+
+bool Floodfill::atGoal(int row, int col) {
+    return (row == 7 || row == 8) && (col == 7 || col == 8);
+}
+
+void Floodfill::setWall(int row, int col, int direction) {
+    if(direction == 0) {
+        maze.vertical_walls[row][col].first = 1; // North wall
+        if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above
+    }
+    if(direction == 1) {
+        maze.horizontal_walls[row][col].second = 1; // East wall
+        if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right
+    }
+    if(direction == 2) {
+        maze.vertical_walls[row][col].second = 1; // South wall
+        if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below
+    }
+    if(direction == 3) {
+        maze.horizontal_walls[row][col].first = 1; // West wall
+        if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left
+    }
+    // On-robot: no external API call here
+}
+void Floodfill::detectWalls(vector<int> sensorDistances, int row, int col, int direction){
+    if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds
+    if (sensorDistances[2] >= 0 && sensorDistances[2] < wall_threshhold) { //front
+        setWall(row, col, direction);
+    }
+    if (sensorDistances[0] >= 0 && sensorDistances[0] < wall_threshhold) { //left
+        setWall(row, col, (direction + 3) % 4);
+    } 
+    if (sensorDistances[4] >= 0 && sensorDistances[4] < wall_threshhold) { //right
+        setWall(row, col, (direction + 1) % 4);
+    }
+}
+
+
+bool Floodfill::hasWall(int row, int col, int dir) {
+    if(dir == 0) return maze.vertical_walls[row][col].first; // North wall
+    if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall
+    if(dir == 2) return maze.vertical_walls[row][col].second; // South wall
+    if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall
+    return false; // No walls
+}
+
+void Floodfill::floodfill(array<array<int, 16>, 16> &dist, int goalRow, int goalCol) {
+    queue<pair<int, int>> q;    
+    for (auto &row : dist) {
+        row.fill(200);
+    }
+    // maze.manhattan_distances[8][8] = 0; //row,column
+    // maze.manhattan_distances[8][7] = 0;
+    // maze.manhattan_distances[7][8] = 0;
+    // maze.manhattan_distances[7][7] = 0; // Goal
+    array<array<bool, 16>, 16> reached = {};
+    if(!reachedCenter){
+        dist[7][7] = 0;
+        dist[7][8] = 0;
+        dist[8][7] = 0;
+        dist[8][8] = 0;
+        q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});
+        reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; 
+    } else {
+        dist[0][0] = 0;
+        q.push({0, 0});
+        reached[0][0] = true;
+    }
+
+    maze.visited[8][8] = true;
+    maze.visited[8][7] = true;
+    maze.visited[7][8] = true;
+    maze.visited[7][7] = true;
+    maze.visited[0][0] = true;
+    
+    // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    // array<array<bool, 16>, 16> reached = {};
+    // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+
+        if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds
+
+        if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left
+            dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second - 1});
+            reached[cell.first][cell.second - 1] = true; // Marking the cell as reached            
+        }
+        if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above
+            dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first + 1, cell.second});
+            reached[cell.first + 1][cell.second] = true; // Marking the cell as reached
+        }
+        if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right
+            dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second + 1});
+            reached[cell.first][cell.second + 1] = true; // Marking the cell as reached
+        }
+        if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below
+            dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first - 1, cell.second});
+            reached[cell.first - 1][cell.second] = true; // Marking the cell as reached
+        }
+    }
+}
+
+int Floodfill::getNextMove(array<array<int, 16>, 16> &dist,int row, int col /*int direction //for optimize movement(for later improvement)*/) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    // int nextRow = row;
+    // int nextCol = col;
+
+    if(row < 15 && !hasWall(row, col, 0) && dist[row + 1][col] < minDist) {
+        minDist = dist[row + 1][col];
+        bestDirection = 0;
+    }
+    if(col < 15 && !hasWall(row, col, 1) && dist[row][col + 1] < minDist) {
+        minDist = dist[row][col + 1];
+        bestDirection = 1;
+    }
+    if(row > 0 && !hasWall(row, col, 2) && dist[row - 1][col] < minDist) {
+        minDist = dist[row - 1][col];
+        bestDirection = 2;
+    }
+    if(col > 0 && !hasWall(row, col, 3) && dist[row][col - 1] < minDist) {
+        minDist = dist[row][col - 1];
+        bestDirection = 3;
+    }
+
+    return bestDirection; // Return the best direction to move based on the minimum distance
+    // int minDist = 255;
+    // int bestDirection = -1;
+
+    // for (int dir = 0; dir < 4; ++dir) {
+    //     int r = row, c = col;
+
+    //     if (dir == 0 && !hasWall(row, col, 0)) r++;
+    //     else if (dir == 1 && !hasWall(row, col, 1)) c++;
+    //     else if (dir == 2 && !hasWall(row, col, 2)) r--;
+    //     else if (dir == 3 && !hasWall(row, col, 3)) c--;
+    //     else continue;  // skip if there's a wall
+
+    //     if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+    //     int dist = maze.manhattan_distances[r][c];
+
+    //     if (dist < minDist || (dist == minDist && dir == curDir)) {
+    //         minDist = dist;
+    //         bestDirection = dir;
+    //     }
+    // }
+
+    // return bestDirection;
+}
+// int Floodfill::reverse_getNextMove(int row, int col) {
+//     int minDist = 255;
+//     int bestDirection = -1;
+
+//     for (int dir = 0; dir < 4; ++dir) {
+//         int r = row, c = col;
+
+//         if (dir == 0 && !hasWall(row, col, 0)) r++;
+//         else if (dir == 1 && !hasWall(row, col, 1)) c++;
+//         else if (dir == 2 && !hasWall(row, col, 2)) r--;
+//         else if (dir == 3 && !hasWall(row, col, 3)) c--;
+//         else continue;  // skip if there's a wall
+
+//         if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+//         int dist = maze.reverse_manhattan_distances[r][c];
+
+//         if (dist < minDist || (dist == minDist && dir == curDir)) {
+//             minDist = dist;
+//             bestDirection = dir;
+//         }
+//     }
+
+//     return bestDirection;
+// }
+
+// --- Solver helpers and main loop (preserve your structure) ---
+static Action rotateTo(int newDir) {
+    int diff = (newDir - curDir + 4) % 4;
+    if (diff == 0) return FORWARD;
+    if (diff == 1) return RIGHT;
+    if (diff == 3) return LEFT;
+    if (diff == 2) return AROUND;
+    return IDLE;
+}
+
+void moveForwardUpdatePos() {
+    if (curDir == 0) curRow++;
+    if (curDir == 1) curCol++;
+    if (curDir == 2) curRow--;
+    if (curDir == 3) curCol--;
+
+}
+
+static Action solver() {
+    // Read sensors from main and update walls
+    std::vector<int> distances = getDistances();
+    floodfill.detectWalls(distances, curRow, curCol, curDir);
+
+    // If you want double-search (return to start after center), toggle reachedCenter here
+    if (!reachedCenter && floodfill.atGoal(curRow, curCol)) {
+        reachedCenter = true; // center reached (optional behavior)
+        return IDLE;
+    }
+    else if(reachedCenter && curRow == 0 && curCol == 0) {
+        floodfill.RobotDone = true;
+        return IDLE;    
+    }
+
+    // Compute floodfill and choose next direction
+    if (!reachedCenter) {
+        // search phase → goal is center
+        floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7);  
+        int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol);
+        return rotateTo(bestDir);  
+    } else{
+        // return phase → goal is start
+        floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0);
+        int bestDir = floodfill.getNextMove(floodfill.maze.reverse_manhattan_distances, curRow, curCol);
+        return rotateTo(bestDir);
+    }
+    
+    
+}
+
+// Perform a single floodfill decision + action; call repeatedly from loop()
+void runFloodfillStep() {
+    // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit
+
+    Action action = solver();
+    
+    if (action == FORWARD) {
+        moveForward();
+        moveForwardUpdatePos(); // Update pose used by solver
+    } else if (action == LEFT) {
+        turnLeft();
+        curDir = (curDir + 3) % 4;
+    } else if (action == RIGHT) {
+        turnRight();
+        curDir = (curDir + 1) % 4;
+    } else if (action == AROUND) {
+        // keep structure identical to your sample
+        turnRight();
+        turnRight();
+        curDir = (curDir + 2) % 4;
+    } else {
+        // IDLE or no valid move; nothing to do this tick
+        return;
+    }
+}
+bool isRobotDone() {
+        return floodfill.RobotDone;
+    }
\ No newline at end of file
diff --git a/note2.cpp b/note2.cpp
new file mode 100644
index 0000000..0515b1a
--- /dev/null
+++ b/note2.cpp
@@ -0,0 +1,262 @@
+// Micromouse floodfill maze-solving algorithm (on-robot variant)
+// Removes MMS Simulator API usage and main(); integrates with robot by:
+// 1) Detecting walls from sensor distances (threshold-based)
+// 2) Providing floodfill() and getNextMove() you can call from your code
+#include <array>
+#include <queue>
+#include <utility>
+#include <vector>
+#include "Floodfill.h"
+
+using namespace std;
+
+// Extern primitives to be implemented in main.cpp
+extern void moveForward();
+extern void turnLeft();
+extern void turnRight();
+extern std::vector<int> getDistances(); // returns VL6180X distances: [left,...,front,...,right]
+
+// Action and robot pose/state (kept here so main.cpp stays minimal)
+enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND };
+static int curRow = 0, curCol = 0, curDir = 0;  // 0=N,1=E,2=S,3=W
+static bool reachedCenter = false;
+
+static Floodfill floodfill; // solver instance
+
+bool Floodfill::atGoal(int row, int col) {
+    return (row == 7 || row == 8) && (col == 7 || col == 8);
+}
+
+void Floodfill::setWall(int row, int col, int direction) {
+    if(direction == 0) {
+        maze.vertical_walls[row][col].first = 1; // North wall
+        if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above
+    }
+    if(direction == 1) {
+        maze.horizontal_walls[row][col].second = 1; // East wall
+        if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right
+    }
+    if(direction == 2) {
+        maze.vertical_walls[row][col].second = 1; // South wall
+        if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below
+    }
+    if(direction == 3) {
+        maze.horizontal_walls[row][col].first = 1; // West wall
+        if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left
+    }
+    // On-robot: no external API call here
+}
+void Floodfill::detectWalls(vector<int> sensorDistances, int row, int col, int direction){
+    if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds
+    if (sensorDistances[2] >= 0 && sensorDistances[2] < wall_threshhold) { //front
+        setWall(row, col, direction);
+    }
+    if (sensorDistances[0] >= 0 && sensorDistances[0] < wall_threshhold) { //left
+        setWall(row, col, (direction + 3) % 4);
+    } 
+    if (sensorDistances[4] >= 0 && sensorDistances[4] < wall_threshhold) { //right
+        setWall(row, col, (direction + 1) % 4);
+    }
+}
+
+
+bool Floodfill::hasWall(int row, int col, int dir) {
+    if(dir == 0) return maze.vertical_walls[row][col].first; // North wall
+    if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall
+    if(dir == 2) return maze.vertical_walls[row][col].second; // South wall
+    if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall
+    return false; // No walls
+}
+
+void Floodfill::floodfill(array<array<int, 16>, 16> &dist, int goalRow, int goalCol) {
+    queue<pair<int, int>> q;    
+    for (auto &row : dist) {
+        row.fill(200);
+    }
+    // maze.manhattan_distances[8][8] = 0; //row,column
+    // maze.manhattan_distances[8][7] = 0;
+    // maze.manhattan_distances[7][8] = 0;
+    // maze.manhattan_distances[7][7] = 0; // Goal
+    array<array<bool, 16>, 16> reached = {};
+    if(!reachedCenter){
+        dist[7][7] = 0;
+        dist[7][8] = 0;
+        dist[8][7] = 0;
+        dist[8][8] = 0;
+        q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});
+        reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; 
+    } else {
+        dist[0][0] = 0;
+        q.push({0, 0});
+        reached[0][0] = true;
+    }
+
+    maze.visited[8][8] = true;
+    maze.visited[8][7] = true;
+    maze.visited[7][8] = true;
+    maze.visited[7][7] = true;
+    maze.visited[0][0] = true;
+    
+    // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7});       
+
+    // array<array<bool, 16>, 16> reached = {};
+    // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached
+    
+    // BFS to fill the manhattan distances
+    while(!q.empty()){
+        pair<int,int> cell = q.front();
+        q.pop();
+
+        if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds
+
+        if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left
+            dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second - 1});
+            reached[cell.first][cell.second - 1] = true; // Marking the cell as reached            
+        }
+        if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above
+            dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first + 1, cell.second});
+            reached[cell.first + 1][cell.second] = true; // Marking the cell as reached
+        }
+        if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right
+            dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first, cell.second + 1});
+            reached[cell.first][cell.second + 1] = true; // Marking the cell as reached
+        }
+        if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below
+            dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1;
+            q.push({cell.first - 1, cell.second});
+            reached[cell.first - 1][cell.second] = true; // Marking the cell as reached
+        }
+    }
+}
+
+int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    // int nextRow = row;
+    // int nextCol = col;
+
+    if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) {
+        minDist = maze.manhattan_distances[row + 1][col];
+        bestDirection = 0;
+    }
+    if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) {
+        minDist = maze.manhattan_distances[row][col + 1];
+        bestDirection = 1;
+    }
+    if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) {
+        minDist = maze.manhattan_distances[row - 1][col];
+        bestDirection = 2;
+    }
+    if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) {
+        minDist = maze.manhattan_distances[row][col - 1];
+        bestDirection = 3;
+    }
+
+    return bestDirection; // Return the best direction to move based on the minimum distance
+}
+int Floodfill::reverse_getNextMove(int row, int col) {
+    int minDist = 255;
+    int bestDirection = -1;
+
+    for (int dir = 0; dir < 4; ++dir) {
+        int r = row, c = col;
+
+        if (dir == 0 && !hasWall(row, col, 0)) r++;
+        else if (dir == 1 && !hasWall(row, col, 1)) c++;
+        else if (dir == 2 && !hasWall(row, col, 2)) r--;
+        else if (dir == 3 && !hasWall(row, col, 3)) c--;
+        else continue;  // skip if there's a wall
+
+        if (r < 0 || r >= 16 || c < 0 || c >= 16) continue;
+
+        int dist = maze.reverse_manhattan_distances[r][c];
+
+        if (dist < minDist || (dist == minDist && dir == curDir)) {
+            minDist = dist;
+            bestDirection = dir;
+        }
+    }
+
+    return bestDirection;
+}
+
+// --- Solver helpers and main loop (preserve your structure) ---
+static Action rotateTo(int newDir) {
+    int diff = (newDir - curDir + 4) % 4;
+    if (diff == 0) return FORWARD;
+    if (diff == 1) return RIGHT;
+    if (diff == 3) return LEFT;
+    if (diff == 2) return AROUND;
+    return IDLE;
+}
+
+void moveForwardUpdatePos() {
+    if (curDir == 0) curRow++;
+    if (curDir == 1) curCol++;
+    if (curDir == 2) curRow--;
+    if (curDir == 3) curCol--;
+
+}
+
+static Action solver() {
+    // Read sensors from main and update walls
+    std::vector<int> distances = getDistances();
+    floodfill.detectWalls(distances, curRow, curCol, curDir);
+
+    // If you want double-search (return to start after center), toggle reachedCenter here
+    if (!reachedCenter && floodfill.atGoal(curRow, curCol)) {
+        reachedCenter = true; // center reached (optional behavior)
+        while(1);
+    }
+    else if( reachedCenter && floodfill.atGoal(curRow, curCol)) {
+        // Reached start again; stop moving
+        floodfill.RobotDone = true;
+    }
+
+    // Compute floodfill and choose next direction
+    if (!reachedCenter) {
+        // search phase → goal is center
+        floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7);
+        int bestDir = floodfill.getNextMove(curRow, curCol);
+        return rotateTo(bestDir);
+    } else {
+        // return phase → goal is start
+        floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0);
+        int bestDir = floodfill.reverse_getNextMove(curRow, curCol);
+        return rotateTo(bestDir);
+    }
+    
+}
+
+// Perform a single floodfill decision + action; call repeatedly from loop()
+void runFloodfillStep() {
+    // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit
+
+    Action action = solver();
+    
+    if (action == FORWARD) {
+        moveForward();
+        moveForwardUpdatePos(); // Update pose used by solver
+    } else if (action == LEFT) {
+        turnLeft();
+        curDir = (curDir + 3) % 4;
+    } else if (action == RIGHT) {
+        turnRight();
+        curDir = (curDir + 1) % 4;
+    } else if (action == AROUND) {
+        // keep structure identical to your sample
+        turnRight();
+        turnRight();
+        curDir = (curDir + 2) % 4;
+    } else {
+        // IDLE or no valid move; nothing to do this tick
+        return;
+    }
+}
+bool isRobotDone() {
+        return floodfill.RobotDone;
+    }
\ No newline at end of file
diff --git a/src/src_main/main.cpp b/src/src_main/main.cpp
index ecae1fa..04bcc56 100644
--- a/src/src_main/main.cpp
+++ b/src/src_main/main.cpp
@@ -6,12 +6,17 @@
 #include <GyroPID.h>
 #include "Floodfill.h"
 
+#define FINAL_RUN_BUTTON 2  // GPIO pin for the final run button
+
 // Forward-declare the floodfill step function defined in the Floodfill module
 void runFloodfillStep();
+bool isRobotDone(); // defined in Floodfill module
+bool BeginFinalRun();
+// Floodfill solver instance
 
 // Hardware instances
 int xshutPins[] = {32, 17, 16, 15, 4};
-int sensorCorrections[] = { 6, 16, 0, 43, 26};  // mm to subtract from each sensor
+int sensorCorrections[] = { 0, 16, 0, 43, 26};  // mm to subtract from each sensor
 VL6180XManagerV2 sensorGroup(xshutPins, 5, sensorCorrections);
 
 MotorPIDbyNJ leftMotor(25, 26, 18, 5);
@@ -27,7 +32,6 @@ void updateRightEncoder() { rightMotor.updateEncoder(); }
 void moveForward() { Motors.moveForward(); }
 void turnLeft() { Motors.turnLeft(); }
 void turnRight() { Motors.turnRight(); }
-void turnAround() { Motors.turnAround(); }
 void moveForwardUpdatePos(); // defined in Floodfill module now does pose update
 std::vector<int> getDistances() { return sensorGroup.readAll(); }
 
@@ -46,10 +50,21 @@ void setup() {
   // PID values for encoder values
   leftMotor.setPID(3, 0.05, 0.4, 8);
   rightMotor.setPID(3, 0.05, 0.4, 8);
+
+  pinMode(FINAL_RUN_BUTTON, INPUT_PULLDOWN);
+  pinMode(33, OUTPUT); // onboard buzzer
 }
 
 void loop() {
-  // Run one solver step each tick so loop stays responsive
+  
+  if (digitalRead(FINAL_RUN_BUTTON) == HIGH) {  // assuming active HIGH
+      delay(1000);
+      BeginFinalRun();
+      digitalWrite(33, HIGH);  // turn on buzzer
+      delay(500);
+              // reset heading to North
+  }
   runFloodfillStep();
   delay(20);
 }
+
diff --git a/src/src_test/main.cpp b/src/src_test/main.cpp
index b9db8fd..4ceb7f0 100644
--- a/src/src_test/main.cpp
+++ b/src/src_test/main.cpp
@@ -6,7 +6,7 @@
 #include <GyroPID.h>
 
 int xshutPins[] = {32, 17, 16, 15, 4};
-int sensorCorrections[] = { 6, 16, 0, 43, 26};  // mm to subtract from each sensor
+int sensorCorrections[] = { 0, 16, 0, 43, 26};  // mm to subtract from each sensor
 VL6180XManagerV2 sensorGroup(xshutPins, 5, sensorCorrections);
 
 //Floodfill solveMaze;
@@ -53,21 +53,33 @@ void loop() {
   //       runFloodfillRobot();
   //       testMoveDone = true;
   //   }
-  if (!testMoveDone) {
+  // sensorGroup.readAll();
+  // Serial.println("Trying Serial..");
+  // Serial.println("Sensor 1: " + String(sensorGroup.readAll()[0]));
+  // Serial.println("Sensor 2: " + String(sensorGroup.readAll()[1]));
+  // Serial.println("Sensor 3: " + String(sensorGroup.readAll()[2]));
+  // Serial.println("Sensor 4: " + String(sensorGroup.readAll()[3]));
+  // Serial.println("Sensor 5: " + String(sensorGroup.readAll()[4]));
+  delay(400);
 
-    //Motors.turnLeft();
+  gyro.getYaw();
+  Serial.println("Yaw: " + String(gyro.getYaw()));  
 
-    //Motors.moveForward();
-    //Motors.moveForward();
-    Motors.turnRight();
-    Motors.moveForward();
-    Motors.turnRight();
+  // if (!testMoveDone) {
+
+  //   //Motors.turnLeft();
+
+  //   //Motors.moveForward();
+  //   //Motors.moveForward();
+  //   Motors.turnRight();
     Motors.moveForward();
-    Motors.turnRight();
+  //   Motors.turnRight();
+  //   Motors.moveForward();
+  //   Motors.turnRight();
     
-    testMoveDone = true;
-    Serial.println("Exited");
-  }
+  //   testMoveDone = true;
+  //   Serial.println("Exited");
+  // }
   // ...existing code or idle...
 }