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

sleep_time = 1

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

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

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

    if final_direction != direction:
        await rotate(final_direction)

    await move()
    return final_direction

async def move_to_current_location(graph, last_location, current_location, direction):
    if last_location == current_location:
        return direction

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

    while not frontier.empty():
        current = frontier.get()
        if current == goal:
            break
        for next in 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 + heuristic(goal, next)
                frontier.put(next, priority)
                came_from.append(current)

    # print(came_from)
    for node in came_from[1:]:
        final_direction = await move_to_neighbor(current, node, final_direction)
        current = node

    return final_direction


async def move_tractor(start_position, goal):
    directions = ['N', 'W', 'S', 'E']
    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

    graph = Graph()

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

        current = frontier.get()

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

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

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

        current_rotation = await move_to_current_location(graph, last_location, current, current_rotation)
        print("Jestem na %s , %s" % (current[0], current[1]))
        last_location = current

        if current == goal:
            break

        for next in 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 + 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))
    # asyncio.run(test())