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(current, direction):
    if direction == 'N':
        return (current[0] - 1, current[1])
    elif direction == 'S':
        return (current[0] + 1, current[1])
    elif direction == 'W':
        return (current[0], current[1] - 1)
    else:
        return (current[0], current[1] + 1)


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 rotate(direction):
    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()

async def try_move():
    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()
    return result

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


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

            await rotate(direction)

            result = await try_move()

            if result == "OK\n":
                local_graph.append(addnode(current_position, 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:
                await rotate(rotation)
            last_node = directions[(directions.index(rotation) - 2) % 4]
            await move()
        current_position = current

        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)

        start_flag = False

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