类似于推箱子的小游戏 寻找 最短路径

实现效果如下

类似 推箱子小游戏 的变种 C/C++版本 BFS最短路径

黑色代表墙壁 不能越过

蓝色代表HOME点 灰色代表要找的小箱子

绿色代表路径  

最终目标是将灰色的小箱子移动到蓝色的HOME点 

需要两次搜索 第一次是 出发点到灰色小箱子 

第二次是灰色小箱子到蓝色HOME点

BFS 搜索路径之后 找到一条最短路径 

动画效果用的是JAVA的 一个jar包 

完整的代码 包含动画效果已经上传点击这里下载

C语言编译

gcc box.c graphics.c -o box

C++编译

g++ box.cpp graphics.c -o box

需要安装jdk 17版本  

如下图 将 安装包里的所有文件放到 如下的jdk bin目录  

执行如下代码  最后的jar包 用绝对路径就可以 

./box | ./java -jar /home/QMCY/robot/drawppp.jar

代码很乱  临时记录下 

C语言版本

#include 
#include "graphics.h"



#define GRID_SIZE_X 10
#define GRID_SIZE_Y 10

#define ROBOT 'R'



#define HOME_X	9
#define HOME_Y	2


#define MARKER_X	7
#define MARKER_Y	7


#define	ROBOT_X	6
#define	ROBOT_Y	5
#define MAX_N	105

int g_father[MAX_N][MAX_N];	
int dist[MAX_N][MAX_N];
int last_dir[MAX_N][MAX_N];

int dir[MAX_N*MAX_N];	


char g_has_visited[MAX_N][MAX_N];		//vis[x][y]表示xy点是否遍历过


int Queue[MAX_N*MAX_N];			//用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u  出队是u=Q[front++]

int coordinate[MAX_N*MAX_N];


const int squareSize = 50;

const int windowSize = 600;

// Define the grid elements
const char EMPTY = ' ';
const char BLOCK = '#';
const char MARKER = '*';
const char HOME = 'H';

// Define robot directions
typedef enum   { WEST , EAST, NORTH, SOUTH }Direction;

// Define the robot struct
typedef struct  {
    int x;
    int y;
    Direction direction;
    char carryingMarker;
}Robot;


void drawStep(int homeX, int homeY)
{
    background();
    setColour(pink);
    homeX = homeX*squareSize;
    homeY = homeY*squareSize;
    fillRect(homeX, homeY, squareSize, squareSize);
}

void drawMarker(int x,int y)
{
    background();

    setColour(gray);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


void drawBlocks(int x,int y)
{
    background();
    setColour(black);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


void drawEmpty(int x,int y)
{
    foreground();
    setColour(white);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


// Function to initialize the grid
void initializeGrid(char grid[][GRID_SIZE_Y]) {
    // Initialize the grid with empty spaces
    for (int i = 0; i < GRID_SIZE_X; ++i) {
        for (int j = 0; j < GRID_SIZE_X; ++j) {
            grid[i][j] = EMPTY;
        }
    }

    // Place blocks, markers, and home square
	grid[8][8] = BLOCK;

	grid[9][5] = BLOCK;
	grid[8][5] = BLOCK;

	grid[7][5] = BLOCK;


	grid[6][7] = BLOCK;
	grid[8][7] = BLOCK; 
	grid[7][8] = BLOCK; 
	grid[7][8] = BLOCK; 

	grid[2][2] = BLOCK; 
	grid[3][3] = BLOCK; 
	grid[4][4] = BLOCK; 
	grid[5][5] = BLOCK; 
	grid[6][6] = BLOCK; 

	 
    grid[MARKER_X][MARKER_Y] = MARKER;

    grid[HOME_X][HOME_Y] = HOME;
}

// Function to display the grid
void displayGrid(const char grid[][GRID_SIZE_X]) {


	setWindowSize(windowSize, windowSize);
	background();   // Must draw on the background layer.

	int x;
	int y;

	for (x=0; xcarryingMarker) {
        return; // Robot is not carrying a marker
    }

    grid[robot->x][robot->y] = MARKER;
    robot->carryingMarker = 0;
	//drawRobot(robot.x, robot.y, (int)robot.direction);

}

void drawRobot(int x, int y, int direction)
{
    foreground();
    clear();

	 
    setColour(green);
    x = x*squareSize;
    y = y*squareSize;

	 switch (direction)
    {
        case NORTH: draw_north(x, y); break;
        case EAST: draw_east(x, y); break;
        case SOUTH: draw_south(x, y); break;
        case WEST: draw_west(x, y); break;
    }

	 
}

void drawRobotWithBg(int x, int y, int direction)
{

	foreground();
	clear();
	setColour(gray);

	x = x*squareSize;
	y = y*squareSize;


	fillRect(x, y, squareSize, squareSize); 

	setColour(green);

	switch (direction)
	{
	  case NORTH: draw_north(x, y); break;
	  case EAST: draw_east(x, y); break;
	  case SOUTH: draw_south(x, y); break;
	  case WEST: draw_west(x, y); break;
	}

}


void drawHome(int homeX, int homeY)
{
    background();
    setColour(blue);
    homeX = homeX*squareSize;
    homeY = homeY*squareSize;
    fillRect(homeX, homeY, squareSize, squareSize);
}


void drawShort(int homeX, int homeY)
{
    background();
    setColour(orange);
    homeX = homeX*squareSize;
    homeY = homeY*squareSize;
    fillRect(homeX, homeY, squareSize, squareSize);
}




void forward(Robot *robot, char grid[][GRID_SIZE_X]) {
    // Calculate the next position based on the direction
    int nextX = robot->x;
    int nextY = robot->y;

    if (robot->direction == NORTH) {
        --nextX;
    } else if (robot->direction == SOUTH) {
        ++nextX;
    } else if (robot->direction == EAST) {
        ++nextY;
    } else if (robot->direction == WEST) {
        --nextY;
    }

    // Check if the next position is valid
    if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
        // Move the robot
        grid[robot->x][robot->y] = EMPTY;
        robot->x = nextX;
        robot->y = nextY;
        grid[robot->x][robot->y] = ROBOT;
    }

	drawRobot(robot->x, robot->y, robot->direction);

	 
}



char markersLeft(char grid[][GRID_SIZE_X]) {
    for (int i = 0; i < GRID_SIZE_X; ++i) {
        for (int j = 0; j < GRID_SIZE_X; ++j) {
            if (grid[i][j] == MARKER) {
                return 1;
            }
        }
    }
    return 0;
}


void turn_left(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = WEST;
    } else if (robot->direction == SOUTH) {
        robot->direction = EAST;
    } else if (robot->direction == EAST) {
        robot->direction = NORTH;
    } else if (robot->direction == WEST) {
        robot->direction = SOUTH;
    }

	drawRobot(robot->x, robot->y, robot->direction);

}


void turn_right(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = EAST;
    } else if (robot->direction == SOUTH) {
        robot->direction = WEST;
    } else if (robot->direction == EAST) {
        robot->direction = SOUTH;
    } else if (robot->direction == WEST) {
        robot->direction = NORTH;
    }

	drawRobot(robot->x, robot->y, robot->direction);

	 
}

// Function to pick up a marker
void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
    if (grid[robot->x][robot->y] == MARKER) {
        robot->carryingMarker = 1;
        grid[robot->x][robot->y] = EMPTY;
    }
}


void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
    while (markersLeft(grid)) {
        int initialX = robot->x;
        int initialY = robot->y;
        Direction initialDirection = robot->direction;

        // Use the "right hand rule" to navigate
        if (robot->direction == NORTH) {
            if (grid[robot->x][robot->y + 1] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x - 1][robot->y] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == SOUTH) {
            if (grid[robot->x][robot->y - 1] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x + 1][robot->y] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == EAST) {
            if (grid[robot->x + 1][robot->y] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x][robot->y + 1] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == WEST) {
            if (grid[robot->x - 1][robot->y] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x][robot->y - 1] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        }

        if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
            // Robot is stuck, rotate 180 degrees
            turn_left(robot);
            turn_left(robot);
        }

        forward(robot, grid);
        sleep(500); // Adjust sleep duration for animation speed
        pickUpMarker(robot, grid);
    }
}


int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X]) 
{
	int nextX = robot->x;
	int nextY = robot->y;

 if (robot->direction == NORTH) {
		  --nextY;
	 } else if (robot->direction == SOUTH) {
		  ++nextY;
	 } else if (robot->direction == EAST) {
		  ++nextX;
	 } else if (robot->direction == WEST) {
		  --nextX;
	 }


    // Check if the next position is valid
    if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
        // Move the robot
        //grid[robot->x][robot->y] = EMPTY;
        //robot->x = nextX;
      	//robot->y = nextY;
        //grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
        return 1;
    }

	 

	 return 0;

}





// Function to turn the robot left (anti-clockwise)
void left(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = WEST;
    } else if (robot->direction ==SOUTH) {
        robot->direction = EAST;
    } else if (robot->direction == EAST) {
        robot->direction =NORTH;
    } else if (robot->direction == WEST) {
        robot->direction = SOUTH;
    }

	 drawRobot(robot->x, robot->y, robot->direction);

	 
}

void right(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = EAST;
    } else if (robot->direction ==SOUTH) {
        robot->direction = WEST;
    } else if (robot->direction ==EAST) {
        robot->direction =SOUTH;
    } else if (robot->direction == WEST) {
        robot->direction =NORTH;
    }

	 drawRobot(robot->x, robot->y, robot->direction);

	 
}




#if 0
bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
    // Use BFS to find the shortest path
    std::queue> q; // Queue for BFS
    std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));

    q.push({robot.x, robot.y});
    visited[robot.x][robot.y] = true;

	int u=robot.x*GRID_SIZE_X+robot.y;

	father[robot.x][robot.y]=u;


	sleep(2000);
    while (!q.empty()) {
        int x = q.front().first;
        int y = q.front().second;
        q.pop();
		  //drawRobot(x, y, (int)robot.direction);

        if (grid[x][y] == MARKER) {
            // Found a marker, pick it up
            pickUpMarker(robot, grid);
            //dropMarker(robot, grid);
		  		//drawMarker(MARKER_X,MARKER_Y);
		  		//drawRobot(x, y, (int)robot.direction);

			
            return true;
        }

        // Explore neighboring cells
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};

        for (int i = 0; i < 4; ++i) {


		 		u=x*GRID_SIZE_X+y;
            int nx = x + dx[i];
            int ny = y + dy[i];

            if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
                q.push({nx, ny});
					 //drawRobot(nx, ny, (int)robot.direction);
						//drawStep(nx, ny);
					
					 //printf("x=%d y=%d \n",nx,ny);
                visited[nx][ny] = true;
					father[nx][ny]=u;
					//dist[nx][ny]=1+dist[x][y];
					last_dir[nx][ny]=i;
            }
				

					// printf("findShortestPath 2222\n");
				 //sleep(200);
        }

		  //drawRobot(robot.x, robot.y, (int)robot.direction);
		  //sleep(10);
    }

    return false; // No markers found
}


#else

char findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X]) {
   // Use BFS to find the shortest path
    //std::queue> q; // Queue for BFS
    //std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));
	int front,rear;

	rear=front=0;

	Robot robot;

	
	robot.x = robot_x;
	robot.y = robot_y;
    //q.push({robot.x, robot.y});


	g_has_visited[robot.x][robot.y] = 1;

	


	int u=robot.x*GRID_SIZE_X+robot.y;

	g_father[robot.x][robot.y]=u;
	Queue[rear++]=u;


	sleep(1000);
    //while (!q.empty()) 
	 	while(rear>front)
	 	{
	 		u=Queue[front++];
			
        //int x = q.front().first;
        //int y = q.front().second;
        //q.pop();
		  //drawRobot(x, y, (int)robot.direction);
		  int x=u/GRID_SIZE_X;
		  int y=u%GRID_SIZE_X;

        if (grid[x][y] == MARKER) {
            // Found a marker, pick it up
            pickUpMarker(&robot, grid);
            //dropMarker(robot, grid);
		  		//drawMarker(MARKER_X,MARKER_Y);
		  		//drawRobot(x, y, (int)robot.direction);

			
            return 1;
        }

        // Explore neighboring cells
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};

        for (int i = 0; i < 4; ++i) {


		 		//u=x*GRID_SIZE_X+y;
            int nx = x + dx[i];
            int ny = y + dy[i];

            if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !g_has_visited[nx][ny] && grid[nx][ny] != BLOCK) {

					 #if 0
					 q.push({nx, ny});
					 //drawRobot(nx, ny, (int)robot.direction);
						//drawStep(nx, ny);
					
					 //printf("x=%d y=%d \n",nx,ny);
                visited[nx][ny] = true;
					father[nx][ny]=u;
					//dist[nx][ny]=1+dist[x][y];
					last_dir[nx][ny]=i;

					#else
						int v=nx*GRID_SIZE_X+ny;
						Queue[rear++]=v;
						g_has_visited[nx][ny]=1;
						g_father[nx][ny]=u;
						dist[nx][ny]=1+dist[x][y];
						last_dir[nx][ny]=i;
						if (grid[nx][ny] == MARKER)
						{
							return 1;
						}
					#endif
					
            }
				

					// printf("findShortestPath 2222\n");
				 //sleep(200);
        }

		  //drawRobot(robot.x, robot.y, (int)robot.direction);
		  //sleep(10);
    }

    return 0; // No markers found
}

#endif

void print_path(int x,int y,char has_marker)
{
	int steps=0;
	int pos_x,pos_y,direction;
	while(1)
	{
		int u=g_father[x][y];
		int fx=u/GRID_SIZE_X;
		int fy=u%GRID_SIZE_X;
		if (fx==x && fy==y)
		{
			break;
		}
		dir[steps]=last_dir[x][y];
		coordinate[steps++] = x*GRID_SIZE_X+y;
			
		x=fx;
		y=fy;
	}
	while(steps--)
	{
		pos_x = coordinate[steps]/GRID_SIZE_X;
		pos_y = coordinate[steps]%GRID_SIZE_X;
		direction = dir[steps];

		if(has_marker)
		{
			drawRobotWithBg(pos_x,pos_y,direction);
		}
		else
		{
			drawRobot(pos_x, pos_y,direction);
		}


		sleep(300);
		
	}
 
}


int main(int argc, char **argv) {

    char grid[GRID_SIZE_X][GRID_SIZE_X];
	 
    initializeGrid(grid);

    Robot robot;
    robot.x = ROBOT_X; // Initial X position
    robot.y = ROBOT_Y; // Initial Y position
    robot.direction = EAST;
    robot.carryingMarker = 0;

    // Display the initial grid

	displayGrid(grid);
	drawHome(HOME_X, HOME_Y);
	drawMarker(MARKER_X,MARKER_Y);

	drawBlocks(8,8);	
	drawBlocks(9,5);
	drawBlocks(8,5);
	drawBlocks(7,5);

	drawBlocks(6,7);
	drawBlocks(8,7);
	drawBlocks(7,8);
	drawBlocks(2,2);

	drawBlocks(3,3);
	drawBlocks(4,4);
	drawBlocks(5,5);
	drawBlocks(6,6);



	 #if 0
	 findAndCollectMarkers(&robot, grid);
	 #elif 0
	 
    while (!robot.carryingMarker) {

		if(canMoveForward(&robot, grid))
		{
			forward(robot, grid);
		}
		else
		{
			left(robot);
		}


		
		sleep(500);
		  //printf("robot.x = %d  y = %d dir = %d\n",robot.x,robot.y,robot.direction);
    }
#else
	
	while (!findShortestPath(robot.x,robot.y, grid)) {
		  forward(&robot, grid);
		  sleep(500); // Adjust sleep duration for animation speed
	 }

	print_path(MARKER_X,MARKER_Y,0);



	robot.x = MARKER_X;
	robot.y = MARKER_Y;

	grid[MARKER_X][MARKER_Y] = EMPTY;
	grid[HOME_X][HOME_Y] = MARKER;


	memset(g_has_visited,0,sizeof(g_has_visited));

	while (!findShortestPath(robot.x,robot.y, grid)) {
		  forward(&robot, grid);
		  sleep(500); // Adjust sleep duration for animation speed
	 }

	print_path(HOME_X,HOME_Y,1);


#endif


    return 0;
}

C++版本

#include  // For sleep function

#include "graphics.h"


#define GRID_SIZE_X 10
#define GRID_SIZE_Y 10

#define ROBOT 'R'

const int maxn=105;


#define HOME_X	0
#define HOME_Y	0


#define MARKER_X	7
#define MARKER_Y	7


#define	ROBOT_X	9
#define	ROBOT_Y	0


int father[maxn][maxn];	
int dist[maxn][maxn];
int last_dir[maxn][maxn];

int dir[maxn*maxn];	


bool visit_first[maxn][maxn];		//vis[x][y]表示xy点是否遍历过
bool visit_second[maxn][maxn];		//vis[x][y]表示xy点是否遍历过

int Queue[maxn*maxn];			//用Q来模拟队列 给定两个下标 front和rear 那么入队则是Q[rear++]=u  出队是u=Q[front++]

int coordinate[maxn*maxn];


const int squareSize = 50;

const int windowSize = 600;

// Define the grid elements
const char EMPTY = ' ';
const char BLOCK = '#';
const char MARKER = '*';
const char HOME = 'H';

// Define robot directions
enum  Direction { WEST , EAST, NORTH, SOUTH };

// Define the robot struct
struct Robot {
    int x;
    int y;
    Direction direction;
    bool carryingMarker;
};


void drawStep(int homeX, int homeY)
{
    background();
    setColour(pink);
    homeX = homeX*squareSize;
    homeY = homeY*squareSize;
    fillRect(homeX, homeY, squareSize, squareSize);
}

void drawMarker(int x,int y)
{
    background();

    setColour(gray);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


void drawBlocks(int x,int y)
{
    background();
    setColour(black);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


void drawEmpty(int x,int y)
{
    foreground();
    setColour(white);

    x = x*squareSize;
    y = y*squareSize;

	 
    fillRect(x, y, squareSize, squareSize); 
}


// Function to initialize the grid
void initializeGrid(char grid[][GRID_SIZE_Y]) {
    // Initialize the grid with empty spaces
    for (int i = 0; i < GRID_SIZE_X; ++i) {
        for (int j = 0; j < GRID_SIZE_X; ++j) {
            grid[i][j] = EMPTY;
        }
    }

    // Place blocks, markers, and home square
	grid[8][8] = BLOCK;

	grid[9][5] = BLOCK;
	grid[8][5] = BLOCK;

	grid[7][5] = BLOCK;


	grid[6][7] = BLOCK;
	grid[8][7] = BLOCK; 
	grid[7][8] = BLOCK; 
	grid[7][8] = BLOCK; 

	grid[2][2] = BLOCK; 
	grid[3][3] = BLOCK; 
	grid[4][4] = BLOCK; 
	grid[5][5] = BLOCK; 
	grid[6][6] = BLOCK; 

	 
    grid[MARKER_X][MARKER_Y] = MARKER;

    grid[HOME_X][HOME_Y] = HOME;
}

// Function to display the grid
void displayGrid(const char grid[][GRID_SIZE_X]) {


	setWindowSize(windowSize, windowSize);
	background();   // Must draw on the background layer.

	int x;
	int y;

	for (x=0; xx;
    int nextY = robot->y;

    if (robot->direction == NORTH) {
        --nextX;
    } else if (robot->direction == SOUTH) {
        ++nextX;
    } else if (robot->direction == EAST) {
        ++nextY;
    } else if (robot->direction == WEST) {
        --nextY;
    }

    // Check if the next position is valid
    if (nextX >= 0 && nextX < GRID_SIZE_X && nextY >= 0 && nextY < GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
        // Move the robot
        grid[robot->x][robot->y] = EMPTY;
        robot->x = nextX;
        robot->y = nextY;
        grid[robot->x][robot->y] = ROBOT;
    }

	drawRobot(robot->x, robot->y, robot->direction);

	 
}



bool markersLeft(char grid[][GRID_SIZE_X]) {
    for (int i = 0; i < GRID_SIZE_X; ++i) {
        for (int j = 0; j < GRID_SIZE_X; ++j) {
            if (grid[i][j] == MARKER) {
                return true;
            }
        }
    }
    return false;
}


void turn_left(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = WEST;
    } else if (robot->direction == SOUTH) {
        robot->direction = EAST;
    } else if (robot->direction == EAST) {
        robot->direction = NORTH;
    } else if (robot->direction == WEST) {
        robot->direction = SOUTH;
    }

	drawRobot(robot->x, robot->y, robot->direction);

}


void turn_right(Robot *robot) {
    if (robot->direction == NORTH) {
        robot->direction = EAST;
    } else if (robot->direction == SOUTH) {
        robot->direction = WEST;
    } else if (robot->direction == EAST) {
        robot->direction = SOUTH;
    } else if (robot->direction == WEST) {
        robot->direction = NORTH;
    }

	drawRobot(robot->x, robot->y, robot->direction);

	 
}

// Function to pick up a marker
void pickUpMarker(Robot *robot, char grid[][GRID_SIZE_Y]) {
    if (grid[robot->x][robot->y] == MARKER) {
        robot->carryingMarker = true;
        grid[robot->x][robot->y] = EMPTY;
    }
}


void findAndCollectMarkers(Robot *robot, char grid[][GRID_SIZE_X]) {
    while (markersLeft(grid)) {
        int initialX = robot->x;
        int initialY = robot->y;
        Direction initialDirection = robot->direction;

        // Use the "right hand rule" to navigate
        if (robot->direction == NORTH) {
            if (grid[robot->x][robot->y + 1] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x - 1][robot->y] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == SOUTH) {
            if (grid[robot->x][robot->y - 1] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x + 1][robot->y] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == EAST) {
            if (grid[robot->x + 1][robot->y] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x][robot->y + 1] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        } else if (robot->direction == WEST) {
            if (grid[robot->x - 1][robot->y] != BLOCK) {
                turn_right(robot);
            } else if (grid[robot->x][robot->y - 1] != BLOCK) {
                forward(robot, grid);
            } else {
                turn_left(robot);
            }
        }

        if (initialX == robot->x && initialY == robot->y && initialDirection == robot->direction) {
            // Robot is stuck, rotate 180 degrees
            turn_left(robot);
            turn_left(robot);
        }

        forward(robot, grid);
        sleep(500); // Adjust sleep duration for animation speed
        pickUpMarker(robot, grid);
    }
}


int canMoveForward(Robot *robot, char grid[][GRID_SIZE_X]) 
{
	int nextX = robot->x;
	int nextY = robot->y;

 if (robot->direction == Direction::NORTH) {
		  --nextY;
	 } else if (robot->direction == Direction::SOUTH) {
		  ++nextY;
	 } else if (robot->direction == Direction::EAST) {
		  ++nextX;
	 } else if (robot->direction == Direction::WEST) {
		  --nextX;
	 }


    // Check if the next position is valid
    if (nextX >=1 && nextX <= GRID_SIZE_X && nextY >= 1 && nextY <= GRID_SIZE_X && grid[nextX][nextY] != BLOCK) {
        // Move the robot
        //grid[robot->x][robot->y] = EMPTY;
        //robot->x = nextX;
      	//robot->y = nextY;
        //grid[robot->x][robot->y] = 'R'; // Robot represented by 'R'
        return 1;
    }

	 

	 return 0;

}





// Function to turn the robot left (anti-clockwise)
void left(Robot *robot) {
    if (robot->direction == Direction::NORTH) {
        robot->direction = Direction::WEST;
    } else if (robot->direction == Direction::SOUTH) {
        robot->direction = Direction::EAST;
    } else if (robot->direction == Direction::EAST) {
        robot->direction = Direction::NORTH;
    } else if (robot->direction == Direction::WEST) {
        robot->direction = Direction::SOUTH;
    }

	 drawRobot(robot->x, robot->y, robot->direction);

	 
}

void right(Robot *robot) {
    if (robot->direction == Direction::NORTH) {
        robot->direction = Direction::EAST;
    } else if (robot->direction == Direction::SOUTH) {
        robot->direction = Direction::WEST;
    } else if (robot->direction == Direction::EAST) {
        robot->direction = Direction::SOUTH;
    } else if (robot->direction == Direction::WEST) {
        robot->direction = Direction::NORTH;
    }

	 drawRobot(robot->x, robot->y, robot->direction);

	 
}

// Function to pick up a marker
void pickUpMarker(Robot &robot, char grid[][GRID_SIZE_X]) {
    if (grid[robot.x][robot.y] == MARKER) {
        robot.carryingMarker = true;
        grid[robot.x][robot.y] = EMPTY;

		//drawRobot(robot.x, robot.y, (int)robot.direction);
    }
}


#if 0
bool findShortestPath(Robot &robot, char grid[][GRID_SIZE_X]) {
    // Use BFS to find the shortest path
    std::queue> q; // Queue for BFS
    std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));

    q.push({robot.x, robot.y});
    visited[robot.x][robot.y] = true;

	int u=robot.x*GRID_SIZE_X+robot.y;

	father[robot.x][robot.y]=u;


	sleep(2000);
    while (!q.empty()) {
        int x = q.front().first;
        int y = q.front().second;
        q.pop();
		  //drawRobot(x, y, (int)robot.direction);

        if (grid[x][y] == MARKER) {
            // Found a marker, pick it up
            pickUpMarker(robot, grid);
            //dropMarker(robot, grid);
		  		//drawMarker(MARKER_X,MARKER_Y);
		  		//drawRobot(x, y, (int)robot.direction);

			
            return true;
        }

        // Explore neighboring cells
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};

        for (int i = 0; i < 4; ++i) {


		 		u=x*GRID_SIZE_X+y;
            int nx = x + dx[i];
            int ny = y + dy[i];

            if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visited[nx][ny] && grid[nx][ny] != BLOCK) {
                q.push({nx, ny});
					 //drawRobot(nx, ny, (int)robot.direction);
						//drawStep(nx, ny);
					
					 //printf("x=%d y=%d \n",nx,ny);
                visited[nx][ny] = true;
					father[nx][ny]=u;
					//dist[nx][ny]=1+dist[x][y];
					last_dir[nx][ny]=i;
            }
				

					// printf("findShortestPath 2222\n");
				 //sleep(200);
        }

		  //drawRobot(robot.x, robot.y, (int)robot.direction);
		  //sleep(10);
    }

    return false; // No markers found
}


#else

bool findShortestPath(int robot_x,int robot_y, char grid[][GRID_SIZE_X],bool visit[][maxn]) {
   // Use BFS to find the shortest path
    //std::queue> q; // Queue for BFS
    //std::vector> visited(GRID_SIZE_X, std::vector(GRID_SIZE_X, false));
	int front,rear;

	rear=front=0;

	Robot robot;

	
	robot.x = robot_x;
	robot.y = robot_y;
    //q.push({robot.x, robot.y});
	bool **visited = NULL;

	visit[robot.x][robot.y] = true;

	


	int u=robot.x*GRID_SIZE_X+robot.y;

	father[robot.x][robot.y]=u;
	Queue[rear++]=u;


	sleep(1000);
    //while (!q.empty()) 
	 	while(rear>front)
	 	{
	 		u=Queue[front++];
			
        //int x = q.front().first;
        //int y = q.front().second;
        //q.pop();
		  //drawRobot(x, y, (int)robot.direction);
		  int x=u/GRID_SIZE_X;
		  int y=u%GRID_SIZE_X;

        if (grid[x][y] == MARKER) {
            // Found a marker, pick it up
            pickUpMarker(robot, grid);
            //dropMarker(robot, grid);
		  		//drawMarker(MARKER_X,MARKER_Y);
		  		//drawRobot(x, y, (int)robot.direction);

			
            return true;
        }

        // Explore neighboring cells
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};

        for (int i = 0; i < 4; ++i) {


		 		//u=x*GRID_SIZE_X+y;
            int nx = x + dx[i];
            int ny = y + dy[i];

            if (nx >= 0 && nx < GRID_SIZE_X && ny >= 0 && ny < GRID_SIZE_X && !visit[nx][ny] && grid[nx][ny] != BLOCK) {

					 #if 0
					 q.push({nx, ny});
					 //drawRobot(nx, ny, (int)robot.direction);
						//drawStep(nx, ny);
					
					 //printf("x=%d y=%d \n",nx,ny);
                visited[nx][ny] = true;
					father[nx][ny]=u;
					//dist[nx][ny]=1+dist[x][y];
					last_dir[nx][ny]=i;

					#else
						int v=nx*GRID_SIZE_X+ny;
						Queue[rear++]=v;
						visit[nx][ny]=true;
						father[nx][ny]=u;
						dist[nx][ny]=1+dist[x][y];
						last_dir[nx][ny]=i;
						if (grid[nx][ny] == MARKER)
						{
							return true;
						}
					#endif
					
            }
				

					// printf("findShortestPath 2222\n");
				 //sleep(200);
        }

		  //drawRobot(robot.x, robot.y, (int)robot.direction);
		  //sleep(10);
    }

    return false; // No markers found
}

#endif

void print_path(int x,int y,bool has_marker)
{
	int steps=0;
	int pos_x,pos_y,direction;
	while(true)
	{
		int u=father[x][y];
		int fx=u/GRID_SIZE_X;
		int fy=u%GRID_SIZE_X;
		if (fx==x && fy==y)
		{
			break;
		}
		dir[steps]=last_dir[x][y];
		coordinate[steps++] = x*GRID_SIZE_X+y;
			
		x=fx;
		y=fy;
	}
	while(steps--)
	{
		pos_x = coordinate[steps]/GRID_SIZE_X;
		pos_y = coordinate[steps]%GRID_SIZE_X;
		direction = dir[steps];

		if(has_marker)
		{
			drawRobotWithBg(pos_x,pos_y,direction);
		}
		else
		{
			drawRobot(pos_x, pos_y,direction);
		}


		sleep(300);
		
	}
 
}


int main(int argc, char **argv) {

    char grid[GRID_SIZE_X][GRID_SIZE_X];
	 
    initializeGrid(grid);

    Robot robot;
    robot.x = ROBOT_X; // Initial X position
    robot.y = ROBOT_Y; // Initial Y position
    robot.direction = Direction::EAST;
    robot.carryingMarker = false;

    // Display the initial grid

	displayGrid(grid);
	drawHome(HOME_X, HOME_Y);
	drawMarker(MARKER_X,MARKER_Y);

	drawBlocks(8,8);	
	drawBlocks(9,5);
	drawBlocks(8,5);
	drawBlocks(7,5);

	drawBlocks(6,7);
	drawBlocks(8,7);
	drawBlocks(7,8);
	drawBlocks(2,2);

	drawBlocks(3,3);
	drawBlocks(4,4);
	drawBlocks(5,5);
	drawBlocks(6,6);



	 #if 0
	 findAndCollectMarkers(&robot, grid);
	 #elif 0
	 
    while (!robot.carryingMarker) {

		if(canMoveForward(&robot, grid))
		{
			forward(robot, grid);
		}
		else
		{
			left(robot);
		}


		
		sleep(500);
		  //printf("robot.x = %d  y = %d dir = %d\n",robot.x,robot.y,robot.direction);
    }
#else
	
	while (!findShortestPath(robot.x,robot.y, grid,visit_first)) {
		  forward(&robot, grid);
		  sleep(500); // Adjust sleep duration for animation speed
	 }

	print_path(MARKER_X,MARKER_Y,false);



	robot.x = MARKER_X;
	robot.y = MARKER_Y;

	grid[MARKER_X][MARKER_Y] = EMPTY;
	grid[HOME_X][HOME_Y] = MARKER;



	while (!findShortestPath(robot.x,robot.y, grid,visit_second)) {
		  forward(&robot, grid);
		  sleep(500); // Adjust sleep duration for animation speed
	 }

	print_path(HOME_X,HOME_Y,true);


#endif


    return 0;
}