movement basics

Signed-off-by: Neerka <kuba.markil0220@gmail.com>

classes/Garbagetruck.py

@@ -26,6 +26,7 @@ class Garbagetruck:
         self.driver = None
         self.orientation = 3  # Niech numery będą tak: N - 0, W - 1, S - 2, E - 3  -- po prostu odwrotnie do zegara
         self.runningtime = 0
+        self.movesequence = []
 
     def getOrientation(self):
         return self.orientation
@@ -132,6 +133,13 @@ class Garbagetruck:
         dist = abs(lok1[0] - lok2[0]) + abs(lok1[1] - lok2[1])
         return dist
 
+    def selfdistance(self, target) -> int:
+        lok = target.getPosition()
+        own = self.getPosition()
+
+        dist = abs(lok[0] - own[0]) + abs(lok[1] - own[1])
+        return dist
+
     def scanTile(self):
         self.state = None
         temp = self.houses[:]
@@ -176,6 +184,28 @@ class Garbagetruck:
             stepX = 1
         self.modPosiotion(stepX, stepY)
 
+    def graphsearch(self, target):
+        fringe = []
+        explored = []
+        result = []
+        initial = self.getPosition()
+        initial.append(self.getOrientation())
+        fringe.append(tuple(initial))
+
+        while True:
+            if not fringe:
+                return False
+
+            elem = fringe.pop(0)
+            if self.selfdistance(target) == 0:
+                return result
+
+            explored.append(elem)
+
+    #     KONIECZNIE SKOŃCZYĆ!!!
+
+
+
     def classifyTrash(self):
         pass
 #     Tutaj jest plan żeby dopiero napisać funkcję jak już będzie klasyfikator