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

class Tractor:
    sleep_time = 1
    graph = Graph()
    last_location = None

    def __init__(self, current_location, current_rotation):
        self.current_location = current_location
        self.current_rotation = 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

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

        # frontier = PriorityQueue()
        # frontier.put(last_location, 0)
        # came_from = {}
        # cost_so_far = {}
        # came_from = []
        # cost_so_far[last_location] = 0
        # final_direction = self.current_rotation

        # while not frontier.empty():
        #     current = frontier.get()
        #     if current == goal:
        #         break
        #     for next in self.graph.neighbors(current):
        #         new_cost = cost_so_far[current] + 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)
        #             came_from.append(current)

        # for node in came_from[1:]:
        #     final_direction = await self.move_to_neighbor(current, node, final_direction)
        #     current = node
        final_direction = await(self.move_to_neighbor(last_location, self.current_location, self.current_rotation))

        return final_direction


    async def move_tractor(self, start_position, goal):
        directions = ['N', 'W', 'S', 'E']
        self.current_rotation = 'N'
        last_node = 'N'
        last_location = start_position

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


        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 start_flag:
                for direction in directions:
                    if direction == last_node:
                        continue

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

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

            self.current_rotation = await self.move_to_current_location(last_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)

            start_flag = False

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