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)

def addnode(local_graph, direction):
    if direction == 'N':
        local_graph.append((current[0] - 1, current[1]))
    elif direction == 'S':
        local_graph.append((current[0] + 1, current[1]))
    elif direction == 'W':
        local_graph.append((current[0], current[1] - 1 ))
    else:
        local_graph.append((current[0], current[1] + 1 ))

    return local_graph

def getrotation(a, b):
    y = b[0] - a[0]
    x = b[1] - a[1]
    if y > 0:
        return 'S'
    elif y < 0:
        return 'N'
    elif x > 0:
        return 'E'
    else:
        return 'W'


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

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

    while not frontier.empty():
        local_graph = []

        for direction in directions:
            if direction == last_node:
                continue

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

            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(0.7)
            writer.close()

            if result == "OK":
                local_graph = addnode(local_graph, direction)

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


        current = frontier.get()
        if not start_flag:
            rotation = getrotation(current_position, current)
            if rotation != current_rotation:
                reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
                writer.write(("rotate " + rotation + "\n").encode())
                await reader.readline()
                current_rotation = rotation
                time.sleep(0.7)
                writer.close()
            last_node = directions[(directions.index(rotation) - 2) % 4]

            reader, writer = await asyncio.open_connection('127.0.0.1', 8888)
            writer.write("move\n".encode())
            await reader.readline()
            current_position = current
            time.sleep(0.7)
            writer.close()

        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)
                print(next)

        start_flag = False
    writer.close()

if __name__ == "__main__":
    start = (0,0)
    goal = (10,6)
    asyncio.run(move_tractor(start, goal))