import fibheap
import math


def heuristic_cost(node1, node2):
    return 0


def heuristic_cost_manhattan(node1, node2):
    part1 = (node1[0]-node2[0])**2
    part1 = part1
    part2 = (node1[1]-node2[1])**2
    return math.sqrt(part1 + part2)


def bidirectional_algorithm(s, t, N, A, A_b, w, h):
    if s == t:
        return [s], 0

    H_f = fibheap.makefheap()
    H_b = fibheap.makefheap()

    d_f = {}
    d_b = {}
    heap_node_connection_f = {}
    heap_node_connection_b = {}
    pred_f = {}
    pred_b = {}
    f_f = {}
    f_b = {}
    perm_f = {}
    perm_b = {}

    for i in N:
        d_f[i] = float('inf')
        d_b[i] = float('inf')
        perm_f[i] = False
        perm_b[i] = False

    d_f[s] = 0
    d_b[t] = 0

    f_f[s] = d_f[s] + h(s, t)
    f_b[t] = d_b[t] + h(t, s)

    pred_f[s] = None
    pred_b[t] = None
    heap_node_connection_f[s] = fibheap.fheappush(H_f, 0, s)
    heap_node_connection_b[s] = fibheap.fheappush(H_b, 0, t)

    ending_node = s
    success = False
    while H_f.num_nodes > 0 and H_b.num_nodes > 0:
        i_f = fibheap.fheappop(H_f)[1]
        i_b = fibheap.fheappop(H_b)[1]
        if f_f[i_f] <= f_b[i_b]:
            perm_f[i_f] = True
            heap_node_connection_b[i_b] = fibheap.fheappush(H_b, f_b[i_b], i_b)
            if perm_b[i_f] or i_f == t:
                ending_node = i_f
                success = True
                break
            for j in A[i_f]:
                path_sum = d_f[i_f] + w[(i_f, j)]
                if d_f[j] > path_sum:
                    pred_f[j] = i_f
                    f_f[j] = path_sum + h(j, t)
                    if d_f[j] == float('inf'):
                        d_f[j] = path_sum
                        heap_node_connection_f[j] = fibheap.fheappush(H_f, f_f[j], j)
                    else:
                        d_f[j] = path_sum
                        H_f.decrease_key(heap_node_connection_f[j], f_f[j])
        else:
            perm_f[i_f] = True
            heap_node_connection_f[i_f] = fibheap.fheappush(H_f, f_f[i_f], i_f)
            if perm_f[i_b] or i_b == s:
                ending_node = i_b
                success = True
                break
            for j in A_b[i_b]:
                path_sum = d_b[i_b] + w[(j, i_b)]
                if d_b[j] > path_sum:
                    pred_b[j] = i_b
                    f_b[j] = path_sum + h(j, s)
                    if d_b[j] == float('inf'):
                        d_b[j] = path_sum
                        heap_node_connection_b[j] = fibheap.fheappush(H_b, f_b[j], j)
                    else:
                        d_b[j] = path_sum
                        H_b.decrease_key(heap_node_connection_b[j], f_b[j])
    if success:
        if ending_node == t:
            path = [t]
            current_node = ending_node
            while pred_f[current_node] is not None:
                path.append(pred_f[current_node])
                current_node = pred_f[current_node]
            path.reverse()
            return path, d_f[t]
        elif ending_node == s:
            path = [s]
            current_node = s
            while pred_b[current_node] is not None:
                path.append(pred_b[current_node])
                current_node = pred_b[current_node]
            return path, d_b[s]
        else:
            path1 = [ending_node]
            current_node = ending_node
            while pred_f[current_node] is not None:
                path1.append(pred_f[current_node])
                current_node = pred_f[current_node]
            path1.reverse()
            path2 = []
            current_node = ending_node
            while pred_b[current_node] is not None:
                path2.append(pred_b[current_node])
                current_node = pred_b[current_node]
            return path1 + path2, d_f[ending_node] + d_b[ending_node]
    return None, None


if __name__ == "__main__":
    N = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}
    A = {1: [2, 8, 6], 2: [1, 3, 5], 8: [1, 4], 3: [2], 5: [2, 7, 9, 10], 4: [6, 7, 8], 6: [4, 1], 7: [4, 5], 9: [5], 10: [5]}
    A_b = {2: [1, 3, 5], 1: [2, 8, 6], 8: [1, 4], 3: [2], 5: [2, 7, 9, 10], 6: [4, 1], 4: [6, 7, 8], 7: [4, 5], 9: [5], 10: [5]}
    w = {(1, 2): 84, (2, 1): 84, (1, 8): 52, (8, 1): 52, (2, 3): 4, (3, 2): 4, (2, 5): 39, (5, 2): 39, (4, 6): 74, (6, 4): 74, (4, 7): 81, (7, 4): 81, (5, 7): 45, (7, 5): 45, (5, 9): 66, (9, 5): 66, (5, 10): 19, (10, 5): 19, (4, 8): 87, (8, 4): 87, (1, 6): 4, (6, 1): 4}
    print(bidirectional_algorithm(1, 10, N, A, A_b, w, heuristic_cost))

    N = {(3, 4), (5, 7), (7, 7), (6, 5), (4, 5), (3, 3), (4, 8), (3, 6), (8, 5), (6, 4), (6, 7), (3, 5), (5, 5), (5, 8), (8, 7), (2, 6), (6, 6), (7, 5), (7, 8)}
    A = {(2, 6): [(3, 6)], (3, 3): [(3, 4)], (3, 4): [(3, 3), (3, 5)], (3, 5): [(4, 5), (3, 4), (3, 6)], (3, 6): [(2, 6), (3, 5)], (4, 5): [(3, 5), (5, 5)], (4, 8): [(5, 8)], (5, 5): [(4, 5), (6, 5)], (5, 7): [(6, 7), (5, 8)], (5, 8): [(4, 8), (5, 7)], (6, 4): [(6, 5)], (6, 5): [(5, 5), (7, 5), (6, 4), (6, 6)], (6, 6): [(6, 5), (6, 7)], (6, 7): [(5, 7), (7, 7), (6, 6)], (7, 5): [(6, 5), (8, 5)], (7, 7): [(6, 7), (8, 7), (7, 8)], (7, 8): [(7, 7)], (8, 5): [(7, 5)], (8, 7): [(7, 7)]}
    A_b = {(3, 6): [(2, 6), (3, 5)], (3, 4): [(3, 3), (3, 5)], (3, 3): [(3, 4)], (3, 5): [(3, 4), (3, 6), (4, 5)], (4, 5): [(3, 5), (5, 5)], (2, 6): [(3, 6)], (5, 5): [(4, 5), (6, 5)], (5, 8): [(4, 8), (5, 7)], (6, 5): [(5, 5), (6, 4), (6, 6), (7, 5)], (6, 7): [(5, 7), (6, 6), (7, 7)], (4, 8): [(5, 8)], (5, 7): [(5, 8), (6, 7)], (7, 5): [(6, 5), (8, 5)], (6, 4): [(6, 5)], (6, 6): [(6, 5), (6, 7)], (7, 7): [(6, 7), (7, 8), (8, 7)], (8, 5): [(7, 5)], (8, 7): [(7, 7)], (7, 8): [(7, 7)]}
    w = {((2, 6), (3, 6)): 1, ((3, 3), (3, 4)): 1, ((3, 4), (3, 3)): 1, ((3, 4), (3, 5)): 1, ((3, 5), (4, 5)): 1, ((3, 5), (3, 4)): 1, ((3, 5), (3, 6)): 1, ((3, 6), (2, 6)): 1, ((3, 6), (3, 5)): 1, ((4, 5), (3, 5)): 1, ((4, 5), (5, 5)): 1, ((4, 8), (5, 8)): 1, ((5, 5), (4, 5)): 1, ((5, 5), (6, 5)): 1, ((5, 7), (6, 7)): 1, ((5, 7), (5, 8)): 1, ((5, 8), (4, 8)): 1, ((5, 8), (5, 7)): 1, ((6, 4), (6, 5)): 1, ((6, 5), (5, 5)): 1, ((6, 5), (7, 5)): 1, ((6, 5), (6, 4)): 1, ((6, 5), (6, 6)): 1, ((6, 6), (6, 5)): 1, ((6, 6), (6, 7)): 1, ((6, 7), (5, 7)): 1, ((6, 7), (7, 7)): 1, ((6, 7), (6, 6)): 1, ((7, 5), (6, 5)): 1, ((7, 5), (8, 5)): 1, ((7, 7), (6, 7)): 1, ((7, 7), (8, 7)): 1, ((7, 7), (7, 8)): 1, ((7, 8), (7, 7)): 1, ((8, 5), (7, 5)): 1, ((8, 7), (7, 7)): 1}
    print(bidirectional_algorithm((3, 3), (4, 8), N, A, A_b, w, heuristic_cost_manhattan))