import asyncio
import sys
import time
from queue import PriorityQueue
from Graph import Graph

class Tractor:
    sleep_time = 1
    graph = Graph()
    last_location = None
    moved_flag = False
    directions = ['N', 'W', 'S', 'E']

    def __init__(self, current_location, current_rotation):
        self.current_location = current_location
        self.current_rotation = current_rotation
        self.last_node = current_rotation

    def heuristic(self, a, b):
        (x1, y1) = a
        (x2, y2) = b
        return abs(x1 - x2) + abs(y1 - y2)

    async def rotate(self, direction):
        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
        writer.write(("rotate " + direction + "\n").encode())
        await reader.readline()
        time.sleep(self.sleep_time)
        writer.close()

    async def try_move(self):
        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
        writer.write("try\n".encode())
        data = await reader.readline()
        result = data.decode()
        time.sleep(self.sleep_time)
        writer.close()
        return result

    async def move(self):
        reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
        writer.write("move\n".encode())
        await reader.readline()
        time.sleep(self.sleep_time)
        writer.close()

    async def move_to_neighbor(self, current, neighbor, direction):
        x = current[0] - neighbor[0]
        y = current[1] - neighbor[1]
        if x > 0:
            final_direction = 'N'
        elif x < 0:
            final_direction = 'S'
        elif y > 0:
            final_direction = 'W'
        else:
            final_direction = 'E'

        if final_direction != direction:
            await self.rotate(final_direction)

        print("idziemy na " + final_direction)

        await self.move()
        return final_direction

    def dfs_paths(self, start, goal):
        stack = [(start, [start])]
        visited = []
        while stack:
            (vertex, path) = stack.pop()
            if vertex not in visited:
                if vertex == goal:
                    return path
                visited.append(vertex)
                for neighbor in self.graph.nodes[vertex]:
                    stack.append((neighbor, path + [neighbor]))


    async def move_to_current_location(self, last_location):
        if last_location == self.current_location:
            return self.current_rotation

        path = self.dfs_paths(last_location, self.current_location)
        path = path[1:]

        print("~~~~~~~~~~~~~~~~~~")
        print(path)
        pos = last_location
        final_direction = self.current_rotation

        for node in path:
            final_direction = await self.move_to_neighbor(pos, node, final_direction)
            pos = node

        # # final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))
        self.moved_flag = True

        return final_direction

    async def look_around(self):
        for direction in self.directions:
            if direction == self.last_node:
                continue

            await self.rotate(direction)
            result = await self.try_move()
            if result == "OK\n":
                self.graph.add_neighbor(self.current_location, direction)


    async def move_tractor(self, start_position, goal):
        self.current_rotation = 'N'
        last_location = start_position

        frontier = PriorityQueue()
        frontier.put(start_position, 0)
        cost_so_far = {}
        cost_so_far[start_position] = 0

        await self.look_around()

        while not frontier.empty():
            # for key in self.graph.nodes:
                # print ("key: %s , value: %s" % (key, self.graph.nodes[key]))

            self.current_location = frontier.get()

            if self.moved_flag:
                await self.look_around()

            self.current_rotation = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]
            self.last_node = self.directions[(self.directions.index(self.current_rotation) - 1) % 4]

            self.current_rotation = await self.move_to_current_location(last_location)
            # print(last_location)
            # print(self.current_location)
            last_location = self.current_location

            if self.current_location == goal:
                break

            for next in self.graph.neighbors(self.current_location):
                new_cost = cost_so_far[self.current_location] + 1
                if next not in cost_so_far or new_cost < cost_so_far[next]:
                    cost_so_far[next] = new_cost
                    priority = new_cost + self.heuristic(goal, next)
                    frontier.put(next, priority)


if __name__ == "__main__":
    start = (0,0)
    goal = (10,6)
    tractor = Tractor((0,0), 'N')
    asyncio.run(tractor.move_tractor(start, goal))
    # asyncio.run(test())