from math import inf, sqrt
from heapq import heappop, heappush

def find_path (source_point, destination_point, mesh):

    """
    Searches for a path from source_point to destination_point through the mesh

    Args:
        source_point: starting point of the pathfinder
        destination_point: the ultimate goal the pathfinder must reach
        mesh: pathway constraints the path adheres to

    Returns:

        A path (list of points) from source_point to destination_point if exists
        A list of boxes explored by the algorithm
    """

    all_boxes = mesh['boxes']
    boxes = {}
    path =[]

    # find the box the source_point lies in
    def inBox(point):
        px = point[0]
        py = point[1]

        return_box = (0,0,0,0)
        for box in all_boxes:
            # check if box is within bounds of a box
            if px >= box[0] and px <= box[1] and py >= box[2] and py <= box[3]:
                return_box = box

        return return_box

    source_box = inBox(source_point)
    destination_box = inBox(destination_point)

    # case for when either source box or destination box is not found
    if source_box == (0,0,0,0) or destination_box == (0,0,0,0):
        print("No Path")
        return path, boxes

    # case when source point and destination point is initially in the same box
    if source_box == destination_box:
        path = [(source_point,destination_point)]
        boxes[source_box] = None
        return path, boxes.keys()

    # search for shortest path useing A*
    path_points, boxes = a_star(source_box, destination_box, source_point, destination_point, mesh)

    # build the list of points or tuples that connect the source and destination
    for i in range(0,len(path_points)-1):
        path.append((path_points[i],path_points[i+1]))
        i = i + 1

    return path, boxes.keys()

# function that returns the direct path from one point to the other
def euclidean(point_a, point_b):
    return sqrt((point_a[0] - point_b[0])**2 + (point_a[1] - point_b[1])**2)

def a_star(source_box, destination_box, source_point, destination_point, mesh):
    """ Searches for a minimal cost path through a graph using A* algorithm.

    Args:
        source_box: The initial point from which the path extends.
        destination_box: The end box location for the path.
        source_point: current
        destination_point: An adjacency function returning cells adjacent to a given cell as well as their respective edge costs.
        mesh: 

    Returns:
        If a path exits, return a list containing all cells from initial_position to destination.
        Otherwise, return None.

    """
    # The priority queue containing cost, the box, and the root direction (source or destination)
    queue = [(0, source_box, 0)]
    heappush(queue, (0, destination_box, 1))

    # create the set of data structures for each side of the path
    s_detail_points = {}
    s_detail_points[source_box] = source_point
    
    t_detail_points = {}
    t_detail_points[destination_box] = destination_point

    # The dictionary that will be returned with the costs
    s_estimated_distances = {}
    s_estimated_distances[source_box] = euclidean(source_point, destination_point)
    
    t_estimated_distances = {}
    t_estimated_distances[destination_box] = euclidean(source_point, destination_point)

    s_distances = {}
    s_distances[source_box] = 0

    t_distances = {}
    t_distances[destination_box] = 0

    # The dictionary that will store the backpointers
    s_backpointers = {}
    s_backpointers[source_box] = None
    
    t_backpointers = {}
    t_backpointers[destination_box] = None

    while queue:
        current_dist, current_box, from_source = heappop(queue)

        # Check if the two sides have discovered each other
        if (current_box in t_distances.keys()) and (current_box in s_distances.keys()):
            # List containing all points from initial_position to mid point
            s_path = []
            current_back_box = current_box
            while current_back_box is not None:
                s_path.append(s_detail_points[current_back_box])
                current_back_box = s_backpointers[current_back_box]
            s_path = s_path[::-1]

            # list containing all points from destination_position to mid point
            t_path = []
            current_back_box = current_box
            while current_back_box is not None:
                t_path.append(t_detail_points[current_back_box])
                current_back_box = t_backpointers[current_back_box]

            # concatenate the two path lists to create the entire path list
            path = s_path + t_path

            # build the path or list of points 
            for key, value in t_backpointers.items():
                s_backpointers[key] = value

            return (path, s_backpointers)

        # when searching for a path from initial position
        if from_source == 0:
            for adj_box, adj_point, adj_box_cost in navigation_edges(mesh, s_detail_points[current_box], current_box):
                pathcost = s_distances[current_box] + adj_box_cost
                pathcost_estimated = pathcost + euclidean((adj_point),(destination_point))

                # If the cost is new update data structures
                if adj_box not in s_distances or pathcost < s_distances[adj_box]:
                    s_estimated_distances[adj_box] = pathcost_estimated
                    s_backpointers[adj_box] = current_box
                    s_detail_points[adj_box] = adj_point
                    s_distances[adj_box] = pathcost
                    heappush(queue, (pathcost_estimated, adj_box, 0))
        # when searching for a path from destination position
        else:

            for adj_box, adj_point, adj_box_cost in navigation_edges(mesh, t_detail_points[current_box], current_box):
                pathcost = t_distances[current_box] + adj_box_cost
                pathcost_estimated = pathcost + euclidean((adj_point),(source_point))

                # If the cost is new update data structures
                if adj_box not in t_distances or pathcost < t_distances[adj_box]:
                    t_estimated_distances[adj_box] = pathcost_estimated
                    t_backpointers[adj_box] = current_box
                    t_detail_points[adj_box] = adj_point
                    t_distances[adj_box] = pathcost
                    heappush(queue, (pathcost_estimated, adj_box, 1))

    return None

def navigation_edges(mesh, source_point, source_box):
    """ Provides a list of adjacent cells and their respective costs from the given cell.

    Args:
        level: A loaded level, containing walls, spaces, and waypoints.
        cell: A target location.

    Returns:
        A list of tuples containing an adjacent cell's coordinates and the cost of the edge joining it and the
        originating cell.

        E.g. from (0,0):
            [((0,1), 1),
             ((1,0), 1),
             ((1,1), 1.4142135623730951),
             ... ]
    """

    adj_result = []
    all_boxes = mesh['boxes']
    adj_boxes = mesh['adj']

    # current point of interest
    px = source_point[0]
    py = source_point[1]

    # iterate through surrounding boxes and calculate shortest distance to the box
    for adj_box in adj_boxes[source_box]:
        # valid x axis for source_box x and adj_box x
        x_range = [max(source_box[0], adj_box[0]), min(source_box[1], adj_box[1])]
        # valid y axis for source_box y and adj_box y
        y_range = [max(source_box[2], adj_box[2]), min(source_box[3], adj_box[3])]

        # when px is within x_range
        if px > x_range[0] and px < x_range[1]:
            distance = min(euclidean((px,py),(px,y_range[0])),euclidean((px,py),(px,y_range[1])))
            if(py < y_range[0]):
                detail_point = (px, y_range[0])
            else:
                detail_point = (px, y_range[1])
            adj_result.append((adj_box, detail_point, distance))
        
        # when py is within y_range    
        elif py > y_range[0] and py < y_range[1]:
            distance = min(euclidean((px,py),(x_range[0],py)),euclidean((px,py),(x_range[1],py)))
            if(px < x_range[0]):
                detail_point = (x_range[0], py)
            else:
                detail_point = (x_range[1], py)
            adj_result.append((adj_box, detail_point, distance))
        
        # else when px and py outside the bounds
        else:
            distance = min(euclidean((px,py),(x_range[0],y_range[0])), euclidean((px,py),(x_range[1],y_range[1])))
            if px < x_range[0]:
                detail_x = x_range[0]
            else:
                detail_x = x_range[1]
            if py < y_range[0]:
                detail_y = y_range[0]
            else:
                detail_y = y_range[1]
            detail_point = (detail_x, detail_y)
            adj_result.append((adj_box, detail_point, distance))

    return adj_result
