'''

@author: olivier.massot, 2019
'''
import heapq
import time


class Queue():
    def __init__(self):
        self.items = []
    
    def __bool__(self):
        return bool(self.items)

    def __repr__(self):
        return str(self.items)

    def values(self):
        return (v for _, v in self.items)

    def put(self, item, priority):
        heapq.heappush(self.items, (priority, item))

    def fput(self, item, priority):
        while priority in [p for p, _ in self.items]:
            priority += 1
        self.put(item, priority)

    def get(self):
        return heapq.heappop(self.items)[1]
    
class PathNode(tuple):
    def __new__(self, x, y, parent=None):
        n = tuple.__new__(self, (x, y))
        n.parent = parent
        n.cost = 0
        return n
    
    def __repr__(self):
        return f"<{self[0]}, {self[1]}, c:{self.cost}>"

    def ancestors(self):
        ancestors = []
        p = self.parent
        while p:
            ancestors.insert(0, p)
            p = p.parent
        return ancestors
            
            
class Waypoint(tuple):
    def __new__(self, x, y, neighbors=None):
        n = tuple.__new__(self, (x, y))
        n.neighbors = neighbors or []
        return n

    def __repr__(self):
        return f"<({self[0]},{self[1]}), n:{self.neighbors}>"

class Grid():
    
    def __init__(self, grid, start, target):
        rows = grid.strip().split("\n")
        self.w = len(rows[0])
        self.h = len(rows)
        self.cells = {(x, y): c for y, row in enumerate(rows) for x, c in enumerate(list(row))}
        self.control_room = target
        self.kirk = start
        
        self.neighbors = {p: self.get_neighbors(*p) for p in self.cells}
        self.update_corners()
        
    def _repr_cell(self, p, show=[]):
        if p == self.kirk:
            return "T"
        elif p == self.control_room:
            return "C"
        elif p in show:
            return "x"
        else:
            return self.cells[p]
        
    def graph(self, show=[]):
        return "\n".join(["".join([self._repr_cell((x, y), show) for x in range(self.w)]) for y in range(self.h)])
    
    @staticmethod
    def dist(from_, to_):
        xa, ya = from_
        xb, yb = to_
        return abs(xa - xb) + abs(ya - yb) 
    
    def get_neighbors(self, x, y, diags=False):
        neighs = [(x, y - 1), (x - 1, y), (x + 1, y), (x, y + 1)]
        if diags:
            neighs += [(x - 1, y - 1), (x + 1, y - 1), (x - 1, y + 1), (x + 1, y + 1)]
        return [(x, y) for x, y in neighs if 0 <= x < self.w and 0 <= y < self.h]

    def line(self, x1, y1, x2, y2):
        # special case
        if (x1, y1) == (x2, y2):
            return [(x1, y1)]

        # diagonal symmetry
        vertically_oriented = (abs(y2 - y1) > abs(x2 - x1))
        if vertically_oriented:
            y1, x1, y2, x2 = x1, y1, x2, y2

        # horizontal symmetry
        reversed_sym = (x1 > x2)
        if reversed_sym:
            x2, y2, x1, y1 = x1, y1, x2, y2

        # angle
        dx, dy = x2 - x1, y2 - y1
        alpha = (abs(dy) / dx)

        offset = 0.0
        step = 1 if dy > 0 else -1

        result = []
        y = y1
        for x in range(x1, x2 + 1):
            if vertically_oriented:
                result.append((y, x))
            else:
                result.append((x, y))

            offset += alpha
            if offset > 0.5:
                y += step
                offset -= 1.0

        if reversed_sym:
            result.reverse()
        return result

    def _get_quarters(self, x, y):
        return {(x - 1, y - 1): [(x - 1, y), (x - 1, y - 1), (x, y - 1)], 
                (x + 1, y - 1): [(x, y - 1), (x + 1, y - 1), (x + 1, y)], 
                (x + 1, y + 1): [(x + 1, y), (x + 1, y + 1), (x, y + 1)], 
                (x - 1, y + 1): [(x, y + 1), (x - 1, y + 1), (x - 1, y)]}
        
    def update_corners(self):
        corners = set()
        for p, c in self.cells.items():
            if c == "#":
                for corner, quarter in self._get_quarters(*p).items():
                    if all(self.cells.get(n, "#") != "#" for n in quarter):
                        corners.add(corner)
        self.corners = list(corners)
        
    def print_nodes(self, node):
        
        def _get_parent(node):
            parents = []
            if node.parent:
                parents.insert(0, node.parent)
                parents = _get_parent(node.parent) + parents
            return parents
        
        path_to = _get_parent(node)
        print(node, path_to)
        
    def way(self, start, target, known_only=False):
        nodes = Queue()
        origin = PathNode(*start)
        nodes.put(origin, 0)
        
        waypoints = set(self.corners) | {start, target}
        neighbors = {}
        passable = ["."] if known_only else [".", "?"]
        
        for waypoint in waypoints:
            neighbors[waypoint] = []
            for other in waypoints:
                if other is waypoint:
                    continue
                sight = self.line(*other, *waypoint)[1:-1]
                if all(self.cells.get(p, "#") in passable for p in sight) and not any(w in sight for w in waypoints):
                    neighbors[waypoint].append((other, Grid.dist(waypoint, other)))
        
        while nodes:
            current = nodes.get()
            
#             self.print_nodes(current)
            
            if current == target:
                path = []
                previous = current
                while previous:
                    if previous != start:
                        path.insert(0, previous)
                    previous = previous.parent
                return path

            for n, dist in neighbors[current]:
                if n in current.ancestors():
                    continue

                cost = current.cost + dist
#                 cost = current.cost + 1
                priority = cost + Grid.dist(n, target)
                
                node = PathNode(*n, current)
                node.cost = cost
                nodes.put(node, priority)
                
        return None

    def path(self, start, target, known_only=False):
        nodes = Queue()
        origin = PathNode(*start)
        nodes.put(origin, 0)
        neighbors = []

        its, break_on, broken = 0, 10000000, False

        self.costs = {}
        for p, c in self.cells.items():
            cost = 0
            if c == "#":
                cost = -1
            elif known_only and c == "?":
                cost = -1
            else:
                cost = 2 if c == "?" else 1
            self.costs[p] = cost
        
        while nodes:
            current = nodes.get()
            
            if current == target or broken:
                path = []
                previous = current
                while previous:
                    if previous != start:
                        path.insert(0, previous)
                    previous = previous.parent
                return path

            neighbors = self.neighbors[current]

            for x, y in neighbors:
                if (x, y) == current.parent:
                    continue

                its += 1
                if its > break_on:
                    broken = True
                    break

                if self.costs[(x, y)] < 0:
                    continue
                
                cost = current.cost + self.costs[(x, y)]
                priority = cost + Grid.dist((x, y), target)
                
                node = PathNode(x, y, current)
                node.cost = cost
                nodes.put(node, priority)
                
        return None

    def optim_path(self, origin, target, known_only=False, first_steps=False):
        way = self.way(origin, target, known_only)
        print(way)
        if way:
            if first_steps:
                path = self.path(origin, way[0], known_only)
            else:
                way.insert(0, origin)
                path = sum([self.path(start, target, known_only) for start, target in zip(way, way[1:])], [])
        else:
            path = self.path(origin, target, known_only)
        return path    
    
    def explore(self):
        
        buffer = [self.kirk]
        tested = set()
        
        while buffer:
            new_buffer = []
            for p in buffer:
                for n in self.neighbors[p]:
                    if n in tested:
                        continue
                    c = self.cells[n]
                    if c == "?":
                        return n
                    if c != "#":
                        new_buffer.append(n)
                    tested.add(n)
            buffer = new_buffer
            
        return None
    
raw = """
##############################
#............................#
#.#######################.#..#
#.......................#.#..#
#.....#.................#.#..#
#.#######################.#..#
#.....##......##......#....###
#...####..##..##..##..#..#...#
#.........##......##.....#...#
?##########################.##
?......#......#..............#
???....#.....................#
???.#..####################..#
???..........................#
???###########################
"""

start = (4, 11)
target = (6, 3) 

grid = Grid(raw, start, target)

print(grid.graph(grid.corners))

print("\n\n")

t0 = time.time()
path = grid.optim_path(start, target, True)
print(time.time() - t0)
 
print(len(path))
print(grid.graph(path))
print(path)

# t0 = time.time()
# path = grid.path(start, target, True)
# print(time.time() - t0)
#  
# print(len(path))
# print(grid.graph(path))
# print(path)