From c3260bf726837eaf25ea3263092c44389c39ff4b Mon Sep 17 00:00:00 2001
From: marcinljablonski <marcinjab@pm.me>
Date: Thu, 9 May 2019 00:47:55 +0200
Subject: [PATCH] refactored to object-oriented form

---
 Graph.py   |  27 ++++++-
 env.py     |   1 +
 tractor.py | 214 +++++++++++++++++++++++++++++++----------------------
 3 files changed, 151 insertions(+), 91 deletions(-)

diff --git a/Graph.py b/Graph.py
index f29fcd0..99e5bf9 100644
--- a/Graph.py
+++ b/Graph.py
@@ -1,7 +1,28 @@
-class SimpleGraph:
+class Graph:
     def __init__(self):
-        self.edges = {}
+        self.nodes = {}
     
     def neighbors(self, id):
-        return self.edges[id]
+        return self.nodes[id]
+
+    def direction2point(self, point, direction):
+        if direction == 'N':
+            return (point[0] - 1, point[1])
+        elif direction == 'S':
+            return (point[0] + 1, point[1])
+        elif direction == 'W':
+            return (point[0], point[1] - 1)
+        else:
+            return (point[0], point[1] + 1)
+
+    def add_neighbor(self, node, direction):
+        neighbor = self.direction2point(node, direction)
+        if node in self.nodes:
+            self.nodes[node].append(neighbor)
+        else:
+            self.nodes[node] = [neighbor]
+        if neighbor in self.nodes:
+            self.nodes[neighbor].append(node)
+        else:
+            self.nodes[neighbor] = [node]
 
diff --git a/env.py b/env.py
index 4bb3c91..a487a77 100644
--- a/env.py
+++ b/env.py
@@ -37,6 +37,7 @@ def print_field(field, tractor):
         os.system("cls")
     else:
         os.system("clear")
+    print(tractor.get_position())
     sys.stdout.write(OKBLUE)
     sys.stdout.write("$")
     sys.stdout.write(" ")
diff --git a/tractor.py b/tractor.py
index 7100c6c..2b84d51 100644
--- a/tractor.py
+++ b/tractor.py
@@ -2,110 +2,148 @@ import asyncio
 import sys
 import time
 from queue import PriorityQueue
+from Graph import Graph
 
-def heuristic(a, b):
-    (x1, y1) = a
-    (x2, y2) = b
-    return abs(x1 - x2) + abs(y1 - y2)
+class Tractor:
+    sleep_time = 1
+    graph = Graph()
+    last_location = None
 
-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 __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
 
 
-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(self, start_position, goal):
+        directions = ['N', 'W', 'S', 'E']
+        self.current_rotation = 'N'
+        last_node = 'N'
+        last_location = start_position
 
-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()
+        frontier = PriorityQueue()
+        frontier.put(start_position, 0)
+        cost_so_far = {}
+        cost_so_far[start_position] = 0
+        start_flag = True
 
 
-async def move_tractor(start_position, goal):
-    directions = ['N', 'W', 'S', 'E']
-    current_rotation = 'N'
-    last_node = None
-    current_position = start_position
+        while not frontier.empty():
+            # for key in self.graph.nodes:
+                # print ("key: %s , value: %s" % (key, self.graph.nodes[key]))
 
-    frontier = PriorityQueue()
-    frontier.put(start, 0)
-    cost_so_far = {}
-    cost_so_far[start] = 0
-    start_flag = True
+            self.current_location = frontier.get()
 
-    while not frontier.empty():
-        local_graph = []
+            if start_flag:
+                for direction in directions:
+                    if direction == last_node:
+                        continue
 
-        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)
 
-            await rotate(direction)
+            self.current_rotation = directions[(directions.index(self.current_rotation) - 1) % 4]
+            last_node = directions[(directions.index(self.current_rotation) - 1) % 4]
 
-            result = await try_move()
+            self.current_rotation = await self.move_to_current_location(last_location)
+            # print("Jestem na %s , %s" % (self.current_location[0], self.current_location[1]))
+            last_location = self.current_location
 
-            if result == "OK\n":
-                local_graph.append(addnode(current_position, direction))
+            if self.current_location == goal:
+                break
 
-        current_rotation = directions[(directions.index(current_rotation) - 1) % 4]
+            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)
 
-
-        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
+            start_flag = False
 
 if __name__ == "__main__":
     start = (0,0)
     goal = (10,6)
-    asyncio.run(move_tractor(start, goal))
+    tractor = Tractor((0,0), 'N')
+    asyncio.run(tractor.move_tractor(start, goal))
+    # asyncio.run(test())