def heuristic_cost(start, goal, graph):
    return 0


def bidirectional_algorithm(graph, start, goal, h=heuristic_cost, is_directed=False):
    def return_path_and_weight_front(c_f, c, s):
        current_node = c_f[c]
        shortest_path = [c, current_node]
        while current_node != s:
            current_node = c_f[current_node]
            shortest_path.append(current_node)
        weight = 0
        for k in range(len(shortest_path) - 1):
            if not is_directed:
                if shortest_path[k] > shortest_path[k+1]:
                    weight += graph[(shortest_path[k], shortest_path[k+1])]
                else:
                    weight += graph[(shortest_path[k + 1], shortest_path[k])]
            else:
                weight += graph[(shortest_path[k + 1], shortest_path[k])]
        return shortest_path, weight

    def return_path_and_weight_back(c_f, c, s):
        current_node = c_f[c]
        shortest_path = [c, current_node]
        while current_node != s:
            current_node = c_f[current_node]
            shortest_path.append(current_node)
        weight = 0
        shortest_path.reverse()
        for k in range(len(shortest_path) - 1):
            if not is_directed:
                if shortest_path[k] > shortest_path[k+1]:
                    weight += graph[(shortest_path[k], shortest_path[k+1])]
                else:
                    weight += graph[(shortest_path[k + 1], shortest_path[k])]
            else:
                weight += graph[(shortest_path[k + 1], shortest_path[k])]
        return shortest_path, weight

    def return_path_and_weight_front_meet_back(c_f_f, c_f_b, c_f, s, g):
        shortest_path_front, weight_front = return_path_and_weight_front(c_f_f, c_f, s)
        shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_f, g)
        shortest_path_back.reverse()
        shortest_path_front.reverse()
        return shortest_path_front + shortest_path_back[1:], weight_front + weight_back

    def return_path_and_weight_back_meet_front(c_f_f, c_f_b, c_b, s, g):
        shortest_path_front, weight_front = return_path_and_weight_front(c_f_f, c_b, s)
        shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_b, g)
        shortest_path_back.reverse()
        shortest_path_front.reverse()
        return shortest_path_front + shortest_path_back[1:], weight_front + weight_back

    point_set_front = dict()
    point_set_back = dict()
    for arc in g.keys():
        point_set_front[arc[0]] = []
        point_set_front[arc[1]] = []
        point_set_back[arc[0]] = []
        point_set_back[arc[1]] = []

    for arc in graph.keys():
        point_set_front[arc[0]].append(arc[1])
        if not is_directed:
            point_set_back[arc[1]].append(arc[0])
            point_set_front[arc[1]].append(arc[0])
            point_set_back[arc[0]].append(arc[1])
        else:
            point_set_back[arc[1]].append(arc[0])

    open_set_front = set()
    open_set_front.add(start)

    open_set_back = set()
    open_set_back.add(goal)

    came_from_front = {}
    came_from_back = {}

    g_score_front = {k: float('inf') for k in point_set_front.keys()}
    g_score_front[start] = 0

    g_score_back = {k: float('inf') for k in point_set_back.keys()}
    g_score_back[goal] = 0

    f_score_front = {k: float('inf') for k in point_set_front.keys()}
    f_score_front[start] = h(start, goal, graph)

    f_score_back = {k: float('inf') for k in point_set_back.keys()}
    f_score_back[goal] = h(goal, start, graph)

    while len(open_set_front) > 0 and len(open_set_back) > 0:
        current_front = list(open_set_front)[0]
        current_back = list(open_set_back)[0]

        for k in open_set_front:
            if f_score_front[k] < f_score_front[current_front]:
                current_front = k

        for k in open_set_back:
            if f_score_back[k] < f_score_back[current_back]:
                current_back = k

        if current_front == goal:
            return return_path_and_weight_front(came_from_front, current_front, start)

        if current_back == start:
            return return_path_and_weight_back(came_from_back, current_back, goal)

        if current_front in came_from_back.keys() and current_back in came_from_front.keys():
            path1, weight1 = return_path_and_weight_front_meet_back(came_from_front, came_from_back, current_front,
                                                                    start, goal)
            path2, weight2 = return_path_and_weight_back_meet_front(came_from_front, came_from_back, current_back,
                                                                    start, goal)
            if weight1 < weight2:
                return path1, weight1
            return path2, weight2

        if current_front in came_from_back.keys():
            return return_path_and_weight_front_meet_back(came_from_front, came_from_back, current_front, start, goal)

        if current_back in came_from_front.keys():
            return return_path_and_weight_back_meet_front(came_from_front, came_from_back, current_back, start, goal)

        open_set_front.remove(current_front)
        open_set_back.remove(current_back)

        for neighbor in point_set_front[current_front]:
            tentative_g_score = g_score_front[current_front]

            if not is_directed:
                if current_front > neighbor:
                    tentative_g_score += graph[(current_front, neighbor)]
                else:
                    tentative_g_score += graph[(neighbor, current_front)]
            else:
                tentative_g_score += graph[(current_front, neighbor)]
            if tentative_g_score < g_score_front[neighbor]:
                came_from_front[neighbor] = current_front
                g_score_front[neighbor] = tentative_g_score
                f_score_front[neighbor] = g_score_front[neighbor] + h(neighbor, goal, graph)
                if neighbor not in open_set_front:
                    open_set_front.add(neighbor)

        for neighbor in point_set_back[current_back]:
            tentative_g_score = g_score_back[current_back]
            if not is_directed:
                if current_back > neighbor:
                    tentative_g_score += graph[(current_back, neighbor)]
                else:
                    tentative_g_score += graph[(neighbor, current_back)]
            else:
                tentative_g_score += graph[(neighbor, current_back)]
            if tentative_g_score < g_score_back[neighbor]:
                came_from_back[neighbor] = current_back
                g_score_back[neighbor] = tentative_g_score
                f_score_back[neighbor] = g_score_back[neighbor] + h(neighbor, goal, graph)
                if neighbor not in open_set_back:
                    open_set_back.add(neighbor)


if __name__ == "__main__":
    g = {
        (2, 1): 3,
        (3, 2): 2,
        (5, 3): 1,
        (9, 5): 5,
        (10, 9): 4,
        (9, 4): 3,
        (4, 1): 4,
        (7, 1): 6,
        (3, 1): 4,
        (6, 2): 3,
        (8, 6): 8,
        (8, 3): 2,
        (9, 1): 8
    }

    g2 = {
        (4, 1): 6,
        (1, 3): 4,
        (1, 2): 2,
        (2, 4): 5,
        (3, 4): 1,
        (5, 3): 1
    }

    # print(bidirectional_algorithm(g, 7, 10, heuristic_cost))
    # print(bidirectional_algorithm(g2, 1, 4, heuristic_cost, is_directed=True))
    # print(bidirectional_algorithm(g2, 1, 5, heuristic_cost, is_directed=True))