feature(BFS): Mapping

Mapping Algo Update

frtos/map/map_test.c

@@ -8,7 +8,7 @@ main(void)
     stdio_usb_init();
     maze_t       maze;
 
-    sleep_ms(7000);
+    sleep_ms(3000);
 
     printf("Test started!\n");
 

frtos/map/mapping.h

@@ -34,64 +34,80 @@ generate_random(int min, int max)
 }
 
 // Define a queue structure for BFS
-typedef struct {
+typedef struct
+{
     int x;
     int y;
 } QueueNode;
 
-typedef struct {
+typedef struct
+{
     QueueNode *array;
-    int front, rear, size;
+    int        front, rear, size;
-    unsigned capacity;
+    unsigned   capacity;
 } Queue;
 
 // Function to create a new queue
-Queue* createQueue(unsigned capacity) {
+Queue *
-    Queue* queue = (Queue*)malloc(sizeof(Queue));
+createQueue(unsigned capacity)
+{
+    Queue *queue    = (Queue *)malloc(sizeof(Queue));
     queue->capacity = capacity;
     queue->front = queue->size = 0;
-    queue->rear = capacity - 1;
+    queue->rear                = capacity - 1;
-    queue->array = (QueueNode*)malloc(capacity * sizeof(QueueNode));
+    queue->array = (QueueNode *)malloc(capacity * sizeof(QueueNode));
     return queue;
 }
 
 // Function to check if the queue is empty
-bool isEmpty(Queue* queue) {
+bool
+isEmpty(Queue *queue)
+{
     return (queue->size == 0);
 }
 
 // Function to check if the queue is full
-bool isFull(Queue* queue) {
+bool
+isFull(Queue *queue)
+{
     return (queue->size == queue->capacity);
 }
 
 // Function to enqueue a cell in the queue
-void enqueue(Queue* queue, int x, int y) {
+void
+enqueue(Queue *queue, int x, int y)
+{
     if (isFull(queue))
         return;
-    queue->rear = (queue->rear + 1) % queue->capacity;
+    queue->rear                 = (queue->rear + 1) % queue->capacity;
     queue->array[queue->rear].x = x;
     queue->array[queue->rear].y = y;
-    queue->size = queue->size + 1;
+    queue->size                 = queue->size + 1;
 }
 
 // Function to dequeue a cell from the queue
-QueueNode dequeue(Queue* queue) {
+QueueNode
+dequeue(Queue *queue)
+{
     QueueNode cell = queue->array[queue->front];
-    queue->front = (queue->front + 1) % queue->capacity;
+    queue->front   = (queue->front + 1) % queue->capacity;
-    queue->size = queue->size - 1;
+    queue->size    = queue->size - 1;
     return cell;
 }
 
 // Function to perform BFS and find the shortest path
-void bfs_shortest_path(maze_t *maze, int startX, int startY) {
+void
+bfs_shortest_path(maze_t *maze, int startX, int startY)
+{
     // Create a queue for BFS
-    Queue* queue = createQueue(maze->height * maze->width);
+    Queue *queue = createQueue(maze->height * maze->width);
 
     // Initialize visited array
     bool visited[maze->height][maze->width];
-    for (int i = 0; i < maze->height; i++) {
+    for (int i = 0; i < maze->height; i++)
-        for (int j = 0; j < maze->width; j++) {
+    {
+        for (int j = 0; j < maze->width; j++)
+        {
             visited[i][j] = false;
         }
     }
@@ -105,25 +121,32 @@ void bfs_shortest_path(maze_t *maze, int startX, int startY) {
     int dy[] = { 0, 0, -1, 1 };
 
     // Perform BFS
-    while (!isEmpty(queue)) {
+    while (!isEmpty(queue))
+    {
         // Dequeue a cell and process it
         QueueNode current = dequeue(queue);
-        int x = current.x;
+        int       x       = current.x;
-        int y = current.y;
+        int       y       = current.y;
 
         // Process the cell (you can customize this part based on your needs)
-        // Here, we mark the cell with a special character to indicate it's part of the shortest path
+        // Here, we mark the cell with a special character to indicate it's part
+        // of the shortest path
         maze->mazecells[y][x].type = 'P'; // 'P' for path
 
         // Explore adjacent cells
-        for (int i = 0; i < 4; i++) {
+        for (int i = 0; i < 4; i++)
+        {
             int newX = x + dx[i];
             int newY = y + dy[i];
 
             // Check if the new position is within the maze boundaries
-            if (newX >= 0 && newX < maze->width && newY >= 0 && newY < maze->height) {
+            if (newX >= 0 && newX < maze->width && newY >= 0
+                && newY < maze->height)
+            {
                 // Check if the cell is not a wall and hasn't been visited
-                if (maze->mazecells[newY][newX].type != 'X' && !visited[newY][newX]) {
+                if (maze->mazecells[newY][newX].type != 'X'
+                    && !visited[newY][newX])
+                {
                     // Mark the new cell as visited and enqueue it
                     visited[newY][newX] = true;
                     enqueue(queue, newX, newY);
@@ -188,54 +211,6 @@ create_map(maze_t *maze)
     }
 }
 
-/**
- * Create a hardcoded map with a clear path from start to goal
- * @param maze
- */
-void
-create_hardcoded_map(maze_t *maze)
-{
-    // Set fixed height and width during initialization
-    maze->height = 5;
-    maze->width  = 5;
-
-    // Create the map with a clear path
-    char hardcoded_map[5][5] = {
-        { 'S', ' ', ' ', ' ', 'G' }, { ' ', ' ', 'X', ' ', ' ' },
-        { ' ', ' ', ' ', ' ', ' ' }, { ' ', 'X', ' ', ' ', ' ' },
-        { 'C', ' ', 'X', ' ', ' ' },
-    };
-
-    // Copy the hardcoded map to the maze structure
-    for (int i = 0; i < maze->height; i++)
-    {
-        for (int j = 0; j < maze->width; j++)
-        {
-            maze->mazecells[i][j].type      = hardcoded_map[i][j];
-            maze->mazecells[i][j].reachable = 0;
-            maze->mazecells[i][j].visited   = 0;
-        }
-    }
-}
-
-/**
- * @brief Mapping Initialization
- */
-void
-mapping_init(maze_t *p_maze)
-{
-    printf("Initializing mapping\n");
-
-    // Set fixed height and width during initialization
-    p_maze->height = MAX_HEIGHT / 2;
-    p_maze->width  = MAX_WIDTH / 2;
-
-    // Create the maze
-    printf("Creating maze\n");
-    create_hardcoded_map(p_maze);
-    printf("Maze created\n");
-}
-
 /**
  * @brief Print the map
  * @param maze
@@ -278,6 +253,57 @@ print_map(maze_t *maze)
     }
 }
 
+/**
+ * Create a hardcoded map with a clear path from start to goal
+ * @param maze
+ */
+void
+create_hardcoded_map(maze_t *maze)
+{
+    // Set fixed height and width during initialization
+    maze->height = 5;
+    maze->width  = 5;
+
+    // Create the map based on the image
+    char hardcoded_map[5][5] = { { ' ', ' ', ' ', ' ', ' ' },
+                                 { 'S', 'X', 'X', 'X', ' ' },
+                                 { ' ', 'X', ' ', ' ', ' ' },
+                                 { ' ', 'X', ' ', 'X', ' ' },
+                                 { ' ', ' ', ' ', 'X', 'G' } };
+
+    // Copy the hardcoded map to the maze structure
+    for (int i = 0; i < maze->height; i++)
+    {
+        for (int j = 0; j < maze->width; j++)
+        {
+            maze->mazecells[i][j].type      = hardcoded_map[i][j];
+            maze->mazecells[i][j].reachable = 0;
+            maze->mazecells[i][j].visited   = 0;
+        }
+    }
+
+    printf("Here is the hardcoded map:\n");
+    print_map(maze);
+}
+
+/**
+ * @brief Mapping Initialization
+ */
+void
+mapping_init(maze_t *p_maze)
+{
+    printf("Initializing mapping\n");
+
+    // Set fixed height and width during initialization
+    p_maze->height = 5;
+    p_maze->width  = 5;
+
+    // Create the maze
+    printf("Creating maze\n");
+    create_hardcoded_map(p_maze);
+    printf("Maze created\n");
+}
+
 /**
  * @brief Print the map with the reachable cells
  * @param maze
@@ -322,9 +348,9 @@ floodfill(maze_t *maze, int x, int y, int value)
 }
 
 /**
- * @brief Function to check if the entire maze has been explored
+ * @brief Function to check if the entire map is filled
  * @param maze
- * @return true if all cells are visited, false otherwise
+ * @return true if the entire map is filled, false otherwise
  */
 bool
 maze_explored(const maze_t *maze)
@@ -334,7 +360,10 @@ maze_explored(const maze_t *maze)
         for (int j = 0; j < maze->width; j++)
         {
             if (maze->mazecells[j][i].type != 'X'
-                && maze->mazecells[j][i].type != 'V')
+                && maze->mazecells[j][i].type != 'V'
+                && maze->mazecells[j][i].type != 'C'
+                && maze->mazecells[j][i].type != 'G'
+                && maze->mazecells[j][i].type != 'S')
             {
                 return false;
             }
@@ -343,8 +372,11 @@ maze_explored(const maze_t *maze)
     return true;
 }
 
-// Update the find_shortest_path function with the newly created bfs_shortest_path function
+// Update the find_shortest_path function with the newly created
-void find_shortest_path(maze_t *maze) {
+// bfs_shortest_path function
+void
+find_shortest_path(maze_t *maze)
+{
     // Assuming the starting point is the bottom-left corner (0, 0)
     int startX = 0;
     int startY = 0;
@@ -353,113 +385,109 @@ void find_shortest_path(maze_t *maze) {
     bfs_shortest_path(maze, startX, startY);
 }
 
+/**
+ * @brief Function to backtrack to the start from the goal iteratively
+ * @param maze
+ * @param currentX pointer to the current X position
+ * @param currentY pointer to the current Y position
+ */
 void
 backtrack_to_start(maze_t *maze, int *currentX, int *currentY)
 {
-    // Get the current cell type
+    printf("Backtracking to the start...\n");
-    char currentCellType = maze->mazecells[*currentX][*currentY].type;
 
-    // Base case: Stop if the current cell is the start
+    // Continue backtracking until reaching the start
-    if (currentCellType == 'S')
+    while (*currentX != 0 || *currentY != 0)
     {
-        printf("Backtracking completed. Reached the start!\n");
+        printf("Backtracking...\n");
-        return;
+        print_map(maze);
-    }
 
-    // Update the current cell as part of the backtracking path
+        // Update the current cell as part of the backtracking path
-    maze->mazecells[*currentX][*currentY].type = 'P'; // 'P' for path
+        maze->mazecells[*currentX][*currentY].type = 'P'; // 'P' for path
 
-    // Initialize newX and newY
+        // Move the car towards the starting point
-    int newX = *currentX;
+        if (*currentX > 0)
-    int newY = *currentY;
-
-    // Explore adjacent cells in all directions
-    for (int i = 0; i < 4; i++)
-    {
-        // Adjust the new position based on the movement direction
-        switch ((mapping_direction_t)i)
         {
-            case up:
+            (*currentX)--;
-                newY++;
+        }
-                break;
+        else if (*currentY > 0)
-            case down:
+        {
-                newY--;
+            (*currentY)--;
-                break;
-            case left:
-                newX--;
-                break;
-            case right:
-                newX++;
-                break;
         }
 
-        // Check if the new position is within the maze boundaries
+        // Print the map after updating the current cell during backtracking
-        if (newX >= 0 && newX < maze->width && newY >= 0 && newY < maze->height)
+        printf("Map after updating current cell during backtracking:\n");
-        {
+        print_map(maze);
-            // Check if the new cell is part of the backtracking path
-            if (maze->mazecells[newX][newY].type == 'V'
-                || maze->mazecells[newX][newY].type == 'P')
-            {
-                // Move to the new position
-                *currentX = newX;
-                *currentY = newY;
 
-                // Recursively backtrack from the new position
+        // Print the car's position in the map
-                backtrack_to_start(maze, currentX, currentY);
+        printf("Car's position during backtracking: (%d, %d)\n",
+               *currentX,
+               *currentY);
 
-                // If backtracking is successful, stop exploring other
+        vTaskDelay(
-                // directions
+            pdMS_TO_TICKS(100)); // Delay to simulate time between movements
-                return;
-            }
-        }
-
-        // Reset newX and newY to the original values
-        newX = *currentX;
-        newY = *currentY;
     }
 
-    // If no valid adjacent cells are found, backtrack to the previous position
+    printf("Backtracking completed. Reached the start!\n");
-    switch (currentCellType)
-    {
-        case 'C':
-            maze->mazecells[*currentX][*currentY].type
-                = ' '; // Clear the car's position
-            break;
-        default:
-            maze->mazecells[*currentX][*currentY].type
-                = 'V'; // Mark as visited during backtracking
-            break;
-    }
-
-    // Print the map during backtracking
-    printf("Map during backtracking:\n");
-    print_map(maze);
-
-    // Move back to the previous position (if not at the start)
-    if (currentCellType != 'S')
-    {
-        // Update the current position to the previous position
-        *currentX = newX;
-        *currentY = newY;
-    }
-
-    // Print the map after moving back during backtracking
-    printf("Map after moving back during backtracking:\n");
-    print_map(maze);
 }
 
 /**
- * @brief Task to explore the maze, find the shortest path, and reach the goal
+ * @brief Task to demonstrate the car following the shortest path from start to
+ * goal
  * @param pvParameters
  */
 void
-combined_task(void *pvParameters)
+demo_shortest_path_task(void *pvParameters)
+{
+    maze_t *maze = (maze_t *)pvParameters;
+
+    // Assuming the starting point is the bottom-left corner (0, 0)
+    int currentX = 0;
+    int currentY = 0;
+
+    // Find the shortest path using BFS
+    bfs_shortest_path(maze, currentX, currentY);
+
+    printf("Shortest path found. Demonstrating the car's movement...\n");
+
+    // Iterate through the path and demonstrate the car's movement
+    for (int i = maze->height - 1; i >= 0; i--)
+    {
+        for (int j = 0; j < maze->width; j++)
+        {
+            if (maze->mazecells[i][j].type == 'P')
+            {
+                // Move the car to the cell in the shortest path
+                currentX = j;
+                currentY = i;
+
+                // Print the map with the car's position
+                printf("Map with the car's position (BFS):\n");
+                print_map(maze);
+
+                // Delay to simulate the car's movement
+                vTaskDelay(pdMS_TO_TICKS(500));
+            }
+        }
+    }
+
+    printf("Car reached the goal following the shortest path!\n");
+
+    vTaskDelete(NULL); // Delete the demonstration task
+}
+
+/**
+ * @brief Task to perform mapping of the maze
+ * @param pvParameters
+ */
+void
+mapping_task(void *pvParameters)
 {
     maze_t *maze     = (maze_t *)pvParameters;
     int     currentX = 0; // Initial X position
     int     currentY = 0; // Initial Y position
 
-    // Reset maze before floodfill
+    // Reset maze before mapping
     for (int i = 0; i < maze->height; i++)
     {
         for (int j = 0; j < maze->width; j++)
@@ -473,12 +501,13 @@ combined_task(void *pvParameters)
     {
         // Simulate car movement (you can replace this logic with your actual
         // movement algorithm)
+
         mapping_direction_t moveDirection
             = (mapping_direction_t)(get_rand_32() % 4);
 
         // Update the previously visited position before moving
-        if (maze->mazecells[currentX][currentY].type
+        if (maze->mazecells[currentX][currentY].type != 'S'
-            != 'S') // Check if it's not the start position
+            && maze->mazecells[currentX][currentY].type != 'G')
         {
             maze->mazecells[currentX][currentY].type = 'V'; // 'V' for visited
         }
@@ -516,8 +545,7 @@ combined_task(void *pvParameters)
         }
 
         // Update the car's position in the maze
-        if (maze->mazecells[currentX][currentY].type
+        if (maze->mazecells[currentX][currentY].type != 'S')
-            != 'S') // Check if it's not the start position
         {
             maze->mazecells[currentX][currentY].type = 'C'; // 'C' for car
         }
@@ -530,13 +558,30 @@ combined_task(void *pvParameters)
         floodfill(maze, maze->width - 1, 0, 0);
 
         // Check if the car has explored the entire maze
+        printf("%d\n", maze_explored(maze));
         if (maze_explored(maze))
         {
-            printf("Entire maze explored! Now finding the shortest path.\n");
+            printf("Entire maze explored!\n");
 
+            // Continue with backtracking, BFS, and demonstration of the
+            // shortest path
+            printf("Now Backtracking...\n");
             backtrack_to_start(maze, &currentX, &currentY);
 
+            printf("Map after backtracking:\n");
+            print_map(maze);
+
+            // Find the shortest path after backtracking
+            printf("Finding the shortest path...\n");
             find_shortest_path(maze);
+
+            // Create a task to demonstrate the shortest path
+            xTaskCreate(demo_shortest_path_task,
+                        "demo_shortest_path_task",
+                        configMINIMAL_STACK_SIZE,
+                        (void *)maze,
+                        PRIO,
+                        NULL);
         }
 
         vTaskDelay(
@@ -544,6 +589,100 @@ combined_task(void *pvParameters)
     }
 }
 
+/**
+ * @brief Task to perform backtracking from the goal to the start
+ * @param pvParameters
+ */
+void
+backtracking_task(void *pvParameters)
+{
+    maze_t *maze = (maze_t *)pvParameters;
+
+    int currentX = 0; // Initial X position
+    int currentY = 0; // Initial Y position
+
+    printf("Backtracking to the start...\n");
+    backtrack_to_start(maze, &currentX, &currentY);
+
+    printf("Map after backtracking:\n");
+    print_map(maze);
+
+    vTaskDelete(NULL); // Delete the backtracking task
+}
+
+/**
+ * @brief Task to show the movement from start to goal
+ * @param pvParameters
+ */
+void
+movement_task(void *pvParameters)
+{
+    maze_t *maze = (maze_t *)pvParameters;
+
+    int currentX = 0; // Initial X position
+    int currentY = 0; // Initial Y position
+
+    for (;;)
+    {
+        // Simulate car movement (you can replace this logic with your actual
+        // movement algorithm)
+        mapping_direction_t moveDirection
+            = (mapping_direction_t)(get_rand_32() % 4);
+
+        // Update the previously visited position before moving
+        if (maze->mazecells[currentX][currentY].type != 'S'
+            && maze->mazecells[currentX][currentY].type != 'G')
+        {
+            maze->mazecells[currentX][currentY].type = 'V'; // 'V' for visited
+        }
+
+        switch (moveDirection)
+        {
+            case up:
+                if (currentY < maze->height - 1
+                    && maze->mazecells[currentX][currentY + 1].type != 'X')
+                {
+                    currentY++;
+                }
+                break;
+            case down:
+                if (currentY > 0
+                    && maze->mazecells[currentX][currentY - 1].type != 'X')
+                {
+                    currentY--;
+                }
+                break;
+            case left:
+                if (currentX > 0
+                    && maze->mazecells[currentX - 1][currentY].type != 'X')
+                {
+                    currentX--;
+                }
+                break;
+            case right:
+                if (currentX < maze->width - 1
+                    && maze->mazecells[currentX + 1][currentY].type != 'X')
+                {
+                    currentX++;
+                }
+                break;
+        }
+
+        // Update the car's position in the maze
+        if (maze->mazecells[currentX][currentY].type != 'S')
+        {
+            maze->mazecells[currentX][currentY].type = 'C'; // 'C' for car
+        }
+
+        // Print the map with the car's position
+        printf("Map with the car's position:\n");
+        print_map(maze);
+
+        vTaskDelay(
+            pdMS_TO_TICKS(100)); // Delay to simulate time between movements
+    }
+}
+
 /**
  * @brief Initialise tasks for the Maze
  * @param maze
@@ -551,13 +690,46 @@ combined_task(void *pvParameters)
 void
 mapping_tasks_init(maze_t *maze)
 {
-    TaskHandle_t combined_task_handle = NULL;
+    // Task handles
-    xTaskCreate(combined_task,
+    TaskHandle_t mapping_task_handle       = NULL;
-                "combined_task",
+    TaskHandle_t backtracking_task_handle  = NULL;
+    TaskHandle_t demo_shortest_path_handle = NULL;
+    TaskHandle_t movement_task_handle      = NULL;
+
+    // Create tasks
+    xTaskCreate(mapping_task,
+                "mapping_task",
                 configMINIMAL_STACK_SIZE,
                 (void *)maze,
                 PRIO,
-                &combined_task_handle);
+                &mapping_task_handle);
+
+    xTaskCreate(backtracking_task,
+                "backtracking_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)maze,
+                PRIO,
+                &backtracking_task_handle);
+
+    // Shortest path task demo
+    xTaskCreate(demo_shortest_path_task,
+                "demo_shortest_path_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)maze,
+                PRIO,
+                &demo_shortest_path_handle);
+
+    xTaskCreate(movement_task,
+                "movement_task",
+                configMINIMAL_STACK_SIZE,
+                (void *)maze,
+                PRIO,
+                &movement_task_handle);
+
+    // Suspend all tasks except the mapping task
+    vTaskSuspend(backtracking_task_handle);
+    vTaskSuspend(demo_shortest_path_handle);
+    vTaskSuspend(movement_task_handle);
 }
 
 #endif /* MAPPING_H */