try to have two goals for a robot on a grid

I'm trying to create a grid in Pygame with a robot as a colored cell, a goal for the robot, and some obstacles, but I'm missing two goals; I solved that; I only have one goal and I want to add another goal that once the algorithm starts and reach the first goal it starts searching form that goal to reach the second ; could anyone assist me?

RED = (255, 0, 0)
GREEN = (0, 255, 0)
BLUE = (0, 255, 0)
YELLOW = (255, 255, 0)
WHITE = (255, 255, 255)
BLACK = (0, 0, 0)
PURPLE = (128, 0, 128)
ORANGE = (255, 165, 0)
GREY = (128, 128, 128)
TURQUOISE = (64, 224, 208)


from heapq import *
import pygame
from collections import deque



window_size = 600
window = pygame.display.set_mode((window_size, window_size))
grid_size = 40  # 50x50

pygame.display.set_caption("Satwik's PathFinder ;)")

window.fill(WHITE)
gap = window_size // grid_size
blocksize = gap


class block:
    def __init__(self,row,col):
        self.row = row
        self.col = col
        self.xcor = row*gap
        self.ycor = col*gap
        self.color = BLACK #initial color
        self.neighbors = []

    def makeblock(self):
        pygame.draw.rect(window,self.color,(self.xcor,self.ycor,gap,gap))

    def assign_neighbours(self,grid):
        if(self.row>0 and grid[self.row-1][self.col].color != WHITE):
            self.neighbors.append(grid[self.row-1][self.col])
        if(self.row<grid_size-1 and grid[self.row+1][self.col].color != WHITE):
            self.neighbors.append(grid[self.row+1][self.col])
        if(self.col>0 and grid[self.row][self.col-1].color!=WHITE):
            self.neighbors.append(grid[self.row][self.col-1])
        if(self.col<grid_size-1 and grid[self.row][self.col+1].color!= WHITE):
            self.neighbors.append(grid[self.row][self.col+1])


def makegridlines():
    for i in range(grid_size):
        # pygame.draw.rect(window,BLACK,(i*gap,0),(i*gap,window_size))
        pygame.draw.line(window, GREY, (0, i * gap), (window_size, i * gap))
        for j in range(grid_size):
            # pygame.draw.rect(window,BLACK,(0,j*gap),(window_size,开发者_C百科j*gap))
            pygame.draw.line(window, GREY, (j * gap, 0), (j * gap, window_size))


def draw(grid):
    window.fill(BLACK)

    for i in range(len(grid)):
        for j in range(len(grid)):
            grid[i][j].makeblock()

    makegridlines()
    pygame.display.update()





def bfs(draw,grid,st,end):
    vis = {}
    for i in range(grid_size):
        for j in range(grid_size):
            vis[grid[i][j]] = False
    queue = deque([])
    trackpath = {}
    vis[st] = True
    queue.append(st)
    while(len(queue)!=0):

        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()

        curr = queue.popleft()
        if(curr == end):
            while(curr in trackpath):
                curr = trackpath[curr]
                curr.color = RED
                draw()
            return

        for i in curr.neighbors:
            if(not vis[i]):
                vis[i] = True
                i.color = GREEN
                trackpath[i] = curr
                queue.append(i)

        draw()
        if(curr != st):
            curr.color = ORANGE
    return


def main():
    grid = []
    for i in range(grid_size):
        temp = []
        for j in range(grid_size):
            temp.append(block(i,j))
        grid.append(temp)
    st = False
    end = False
    flag = True
    while(flag):
        draw(grid)
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                flag = False

            if(event.type == pygame.KEYDOWN):

                for i in range(grid_size):
                        for j in range(grid_size):
                            grid[i][j].assign_neighbours(grid)

                if(event.key == pygame.K_c):
                    grid = []
                    for i in range(grid_size):
                        temp = []
                        for j in range(grid_size):
                            temp.append(block(i,j))
                        grid.append(temp)
                    st = False
                    end = False
                else:

                    
                    if(event.key == pygame.K_s): #bfs
                        bfs(lambda:draw(grid),grid,st,end)
                        continue
                    
                    



            if(pygame.mouse.get_pressed()[0]): #clicked left
                x,y= pygame.mouse.get_pos()
                i = x//gap
                j = y//gap

                if(not st and grid[i][j] != end):
                    st = grid[i][j]
                    grid[i][j].color = YELLOW

                elif(not end and grid[i][j] != st):
                    end = grid[i][j]
                    grid[i][j].color = PURPLE

                elif(grid[i][j] != st and grid[i][j]!=end):
                    grid[i][j].color = WHITE

            else:
                if(pygame.mouse.get_pressed()[2]):
                    x,y = pygame.mouse.get_pos()
                    i = x//gap
                    j = y//gap

                    if(grid[i][j] == st):
                        st = False
                        grid[i][j].color = BLACK
                    elif(grid[i][j] == end):
                        end = False
                        grid[i][j].color = BLACK
                    else:
                        grid[i][j].color = BLACK


    pygame.quit()
main()