Uniform Cost Search UCS

Idea: Always expand the state with the smallest cost so far g(s)

UCS(start, goal):
    frontier = PriorityQueue()
    push (start, cost=0) into frontier

    best_cost[start] = 0
    
    while frontier is not empty:
        state, cost = pop state with smallest cost

        if state is goal:
            return path to state
        
        if cost > best_cost[state]:
            continue

        for each successor next of state:
            new_cost = cost + edge_cost(state, next)
            if next not seen OR new_cost < best_cost[next]:
                best_cost[next] = new_cost
                parent[next] = state
                push(next, new_cost) into frontier

    return failure