import asyncio
import sys
import time
from queue import PriorityQueue

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


async def tractor():
    directions = ['N', 'W', 'S', 'E']
    current_rotation = 'N'
    last_rotation = None
    start = (0,0)
    current_position = start
    goal = (10,6)

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

    while not frontier.empty():
        local_graph = []
        # is_already_moved = False
        current = frontier.get()
        print(start_flag)
        last_position = current
        if not start_flag:
            if current[0] == last_position[0] and current[1] > last_position[1]:
                current_rotation = 'E'
            elif current[0] == last_position[0] and current[1] < last_position[1]:
                current_rotation = 'W'
            elif current[0] > last_position[0]:
                current_rotation = 'S'
            elif current[0] < last_position[0]:
                current_rotation = 'N'
        last_rotation = current_rotation

        for i, direction in enumerate(directions):
            if direction == last_rotation:
                continue
            reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            # current_rotation = directions[((i - 1) % 4)]
            writer.write(("rotate " + direction + "\n").encode())
            data = await reader.readline()
            writer.close()
            time.sleep(0.7)
            reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            writer.write("move\n".encode())
            data = await reader.readline()
            result = data.decode().split()
            writer.close()
            time.sleep(0.7)
            if result[0] == "OK":
                print("Jade na "+result[1])
                # print("ok"+result[1])
                if direction == 'N':
                    local_graph.append((current_position[0] - 1, current_position[1]))
                elif direction == 'S':
                    local_graph.append((current_position[0] + 1, current_position[1]))
                elif direction == 'W':
                    local_graph.append((current_position[0], current_position[1] - 1 ))
                else:
                    local_graph.append((current_position[0], current_position[1] + 1 ))
                print(local_graph)

                # print("nawrót")
                reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
                writer.write(("rotate " + directions[((i + 2) % 4)] + "\n").encode())
                data = await reader.readline()
                # print(current_rotation)
                writer.close()
                time.sleep(0.7)
                reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
                writer.write("move\n".encode())
                print("hop -nawrót")
                data = await reader.readline()
                writer.close()
                time.sleep(0.7)
                # reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
                # current_rotation = directions[((i - 1) % 4)]
                # writer.write(("rotate " + current_rotation + "\n").encode())
                # data = await reader.readline()
                # writer.close()
                # time.sleep(0.7)
            # else:
            #     reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            #     current_rotation = directions[((i + 1) % 4)]
            #     writer.write(("rotate " + current_rotation + "\n").encode())
            #     data = await reader.readline()
            #     writer.close()
            #     time.sleep(0.7)
                # print(":(")


        if not start_flag:
            print("jaaazda")
            reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            writer.write(("rotate " + current_rotation + "\n").encode())
            data = await reader.readline()
            writer.close()
            time.sleep(0.7)
            reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            writer.write("move\n".encode())
            data = await reader.readline()
            writer.close()
            time.sleep(0.7)

        if current == goal:
            break

        for next in local_graph:
            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 + heuristic(goal, next)
                frontier.put(next, priority)
                came_from[next] = current

        start_flag = False
asyncio.run(tractor())