intersections = {0: [0.7801603911549438, 0.49474860768712914],
                     1: [0.5249831588690298, 0.14953665513987202],
                     2: [0.8085335344099086, 0.7696330846542071],
                     3: [0.2599134798656856, 0.14485659826020547],
                     4: [0.7353838928272886, 0.8089961609345658],
                     5: [0.09088671576431506, 0.7222846879290787],
                     6: [0.313999018186756, 0.01876171413125327],
                     7: [0.6824813442515916, 0.8016111783687677],
                     8: [0.20128789391122526, 0.43196344222361227],
                     9: [0.8551947714242674, 0.9011339078096633],
                     10: [0.7581736589784409, 0.24026772497187532],
                     11: [0.25311953895059136, 0.10321622277398101],
                     12: [0.4813859169876731, 0.5006237737207431],
                     13: [0.9112422509614865, 0.1839028760606296],
                     14: [0.04580558670435442, 0.5886703168399895],
                     15: [0.4582523173083307, 0.1735506267461867],
                     16: [0.12939557977525573, 0.690016328140396],
                     17: [0.607698913404794, 0.362322730884702],
                     18: [0.719569201584275, 0.13985272363426526],
                     19: [0.8860336256842246, 0.891868301175821],
                     20: [0.4238357358399233, 0.026771817842421997],
                     21: [0.8252497121120052, 0.9532681441921305],
                     22: [0.47415009287034726, 0.7353428557575755],
                     23: [0.26253385360950576, 0.9768234503830939],
                     24: [0.9363713903322148, 0.13022993020357043],
                     25: [0.6243437191127235, 0.21665962402659544],
                     26: [0.5572917679006295, 0.2083567880838434],
                     27: [0.7482655725962591, 0.12631654071213483],
                     28: [0.6435799740880603, 0.5488515965193208],
                     29: [0.34509802713919313, 0.8800306496459869],
                     30: [0.021423673670808885, 0.4666482714834408],
                     31: [0.640952694324525, 0.3232711412508066],
                     32: [0.17440205342790494, 0.9528527425842739],
                     33: [0.1332965908314021, 0.3996510641743197],
                     34: [0.583993110207876, 0.42704536740474663],
                     35: [0.3073865727705063, 0.09186645974288632],
                     36: [0.740625863119245, 0.68128520136847],
                     37: [0.3345284735051981, 0.6569436279895382],
                     38: [0.17972981733780147, 0.999395685828547],
                     39: [0.6315322816286787, 0.7311657634689946]}

roads = [[36, 34, 31, 28, 17],
 [35, 31, 27, 26, 25, 20, 18, 17, 15, 6],
 [39, 36, 21, 19, 9, 7, 4],
 [35, 20, 15, 11, 6],
 [39, 36, 21, 19, 9, 7, 2],
 [32, 16, 14],
 [35, 20, 15, 11, 1, 3],
 [39, 36, 22, 21, 19, 9, 2, 4],
 [33, 30, 14],
 [36, 21, 19, 2, 4, 7],
 [31, 27, 26, 25, 24, 18, 17, 13],
 [35, 20, 15, 3, 6],
 [37, 34, 31, 28, 22, 17],
 [27, 24, 18, 10],
 [33, 30, 16, 5, 8],
 [35, 31, 26, 25, 20, 17, 1, 3, 6, 11],
 [37, 30, 5, 14],
 [34, 31, 28, 26, 25, 18, 0, 1, 10, 12, 15],
 [31, 27, 26, 25, 24, 1, 10, 13, 17],
 [21, 2, 4, 7, 9],
 [35, 26, 1, 3, 6, 11, 15],
 [2, 4, 7, 9, 19],
 [39, 37, 29, 7, 12],
 [38, 32, 29],
 [27, 10, 13, 18],
 [34, 31, 27, 26, 1, 10, 15, 17, 18],
 [34, 31, 27, 1, 10, 15, 17, 18, 20, 25],
 [31, 1, 10, 13, 18, 24, 25, 26],
 [39, 36, 34, 31, 0, 12, 17],
 [38, 37, 32, 22, 23],
 [33, 8, 14, 16],
 [34, 0, 1, 10, 12, 15, 17, 18, 25, 26, 27, 28],
 [38, 5, 23, 29],
 [8, 14, 30],
 [0, 12, 17, 25, 26, 28, 31],
 [1, 3, 6, 11, 15, 20],
 [39, 0, 2, 4, 7, 9, 28],
 [12, 16, 22, 29],
 [23, 29, 32],
 [2, 4, 7, 22, 28, 36]]

from collections import defaultdict
import math


def shortest_path(M, start, goal):
    # create frontier and closed dicts, open needs to be a Priority Queue
    frontier = set()  # set of intersections that are open
    closed = set()  # set of closed intersections (i.e.; no longer on the frontier)
    g_values = {}  # dict to hold the g values (dist from start to a node)
    f_values = defaultdict(int)  # dict to hold the f values, if a key does not exist it is created with 0 as the value
    parents = {}  # dict to hold the keep track of the path taken
    path_taken = []  # will store the optimal path intersections
    came_from = []   # will store progression of looked at intersections

    # make the start vertex current
    current_vertex = start

    # calculate heuristic distance of start to destination (h)
    h = calc_distance(M.intersections[start], M.intersections[goal])

    # starting g is 0
    g_values[current_vertex] = 0

    # calculate f value for start vertex
    f_values[current_vertex] = g_values[current_vertex] + h

    # while current vertex is not the destination:
    while current_vertex != goal:

        # consider each adjacent vertex
        for adj_vertex in M.roads[current_vertex]:

            # if vertex not in closed list and not in frontier list
            if adj_vertex not in closed and adj_vertex not in frontier:

                # add vertex to frontier list (f_value is unknown at this point)
                frontier.add(adj_vertex)

                # set parent to current vertex
                parents[adj_vertex] = current_vertex

            # calc distance from start (g), distance to current vertex + distance to this node.
            new_cost = g_values[current_vertex] + calc_distance(M.intersections[current_vertex],
                                                             M.intersections[adj_vertex])

            # if this vertex is not in g_values, add it
            # or if its new cost is less than the previous cost, update the cost
            if adj_vertex not in g_values or new_cost < g_values[adj_vertex]:
                g_values[adj_vertex] = new_cost

                # calc heruristic distance to destination (h)
                h = calc_distance(M.intersections[adj_vertex], M.intersections[goal])

                # check to see if this vertex already has an f value, if not it will default to 0
                existing_f = f_values[adj_vertex]

                # calc f value
                f = g_values[adj_vertex] + h

                # if new f value < existing f value or no existing f value
                if existing_f == 0 or f < existing_f:
                    # update f value
                    f_values[adj_vertex] = f

                    # set parent to current vertex
                    parents[adj_vertex] = current_vertex

        # Add current vertex to closed list
        closed.add(current_vertex)

        # Remove vertex with lowest f value from open set and make it current
        lowest_f = min_f(frontier, f_values)
        frontier.remove(lowest_f)
        current_vertex = lowest_f
        came_from.append(current_vertex)

    # out of main loop, generate the path taken
    v = goal
    while v != start:
        path_taken.append(v)
        v = parents[v]
    path_taken.append(start)

    # path will be from goal to start, reverse it
    path_taken.reverse()

    return path_taken


def calc_distance(node1, node2):
    '''calculates the euclidian distance between two nodes
        :param start: starting node
        :param goal: goal node (ending node)
    '''
    x1, y1 = node1
    x2, y2 = node2
    return math.sqrt(math.pow(x2 - x1, 2) + math.pow(y2 - y1, 2))


def min_f(frontier, f_values):
    '''sorts the frontier set and returns the node with the minimum f value.
       :param frontier: dictionary containing frontier nodes
       :return: key of the item with the lowest f-value
    '''
    temp_dict = {}
    for v in frontier:
        temp_dict[v] = f_values[v]
    return sorted(temp_dict, key=temp_dict.__getitem__)[0]



if __name__ == '__main__':
    x = shortest_path(8, 24)
    print(x)
