AI_PRO/FindPath.py

from constants import *
from queue import Queue


class Node:
    def __init__(self, parent: (int, int), index: (int, int)):
        self.__index = index
        self.__parent = parent

    def get_index(self):
        return self.__index

    def get_parent(self):
        return self.__parent


def whichStateLookingFor(tractor, TillageUnit):
    searching_field = "toPlow"
    if tractor.header and tractor.hitch == "Crop Trailer":
        searching_field = "toCut"
    elif isinstance(tractor.hitch, TillageUnit) and tractor.hitch.load == "Nothing":
        searching_field = "toPlow"
    elif isinstance(tractor.hitch, TillageUnit) and tractor.hitch.load == "Water":
        searching_field = "toWater"
    elif isinstance(tractor.hitch, TillageUnit) and tractor.hitch.load == "Seeds":
        searching_field = "toSeed"
    elif isinstance(tractor.hitch, TillageUnit) and tractor.hitch.load == "Fertilizer":
        searching_field = "toFertilize"
    return searching_field

def nearestLookingField(board, tractor, TillageUnit):
    end_horizontal_index = 0
    end_vertical_index = 0
    searching_field = whichStateLookingFor(tractor, TillageUnit)
    for i in range(0, int(HORIZONTAL_TILES_NUMBER)):
        for j in range(0, int(VERTICAL_TILES_NUMBER)):
            field = board[i][j]
            if searching_field == field.state:
                end_horizontal_index = field.horizontal_index
                end_vertical_index = field.vertical_index
                break
    return end_horizontal_index, end_vertical_index

def graphsearch(tractor, board, TillageUnit, fringe: Queue, explored):
    start_horizontal_index = tractor.horizontal_index
    start_vertical_index = tractor.vertical_index
    start_state = (start_horizontal_index, start_vertical_index)
    end_state = nearestLookingField(board, tractor, TillageUnit)
    print(start_state)
    print(end_state)

    fringe.put(start_state)

    while True:
        if fringe.empty():
            return False

        elem = fringe.get()

        if goaltest(elem, end_state):
            break
            #TODO
            #return droga ktora musi pokonac traktor
        else:
            explored.append(elem)
            elem = succ(start_state, end_state, tractor)
            fringe.put(elem)


def goaltest(elem, end_state):
    if elem == end_state:
        return True
    else:
        return False

def succ(start_state, end_state, tractor):
    if start_state[1] < end_state[1]:
        if tractor.direction == "RIGHT":
            tractor.drive()
        else:
            tractor.direction = "RIGHT"
        print("przesunalem sie w prawo")
    elif start_state[1] > end_state[1]:
        if tractor.direction == "LEFT":
            tractor.drive()
        else:
            tractor.direction = "LEFT"
        print("przesunalem sie w lewo")
    else:
        if start_state[0] < end_state[0]:
            if tractor.direction == "DOWN":
                tractor.drive()
            else:
                tractor.direction = "DOWN"
            print("przesunalem sie w dol")
        elif start_state[0] > end_state[0]:
            if tractor.direction == "UP":
                tractor.drive()
            else:
                tractor.direction = "UP"
            print("przesunalem sie w gore")
        else:
            return tractor.horizontal_index, tractor.horizontal_index