import heapq
import math


def length_haversine(p1, p2):
    lat1 = p1.lat
    lng1 = p1.lng
    lat2 = p2.lat
    lng2 = p2.lng
    lat1, lng1, lat2, lng2 = map(math.radians, [lat1, lng1, lat2, lng2])
    dlat = lat2 - lat1
    dlng = lng2 - lng1
    a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlng / 2) ** 2
    c = 2 * math.asin(math.sqrt(a))

    return 6372797.560856 * c  # return the distance in meters


def get_closest_node_id(nodes, source_node):
    """ Search through all nodes and return the id of the node
    that is closest to 'source_node'. """
    min_node = None
    min_value = None

    for node_id, node in nodes.items():
        length = length_haversine(source_node, node)
        if min_node is None or length < min_value:
            min_node = node_id
            min_value = length

    return min_node


def find_shortest_path(nodes, source_id, target_id):
    """ Return the shortest path using Dijkstra's algortihm. """
    queue = []
    visited = set()

    print("neighbours", nodes[source_id].neighbours)
    for neighbour in nodes[source_id].neighbours:
        heapq.heappush(queue, (length_haversine(nodes[source_id], neighbour), (source_id, neighbour.id)))

    while queue:
        cand_dist, cand_path = heapq.heappop(queue)
        walk_end = cand_path[-1]
        if walk_end == target_id:
            return cand_path
        if walk_end in visited:
            continue
        visited.add(walk_end)
        for neighbour in nodes[walk_end].neighbours:
            print(neighbour)
            if neighbour not in visited:
                heapq.heappush(queue, (cand_dist + length_haversine(nodes[walk_end], neighbour), cand_path + (neighbour.id, )))
    # no path found
    return None