SI_Traktor/functions.py

import pygame
import sys, time
from pygame.locals import *
import config
from queue import PriorityQueue

def quit():
    print("Zamykanie...")
    pygame.quit()
    sys.exit()

class Node():
    def __init__(self, parent=None, position=None):
        self.parent = parent
        self.position = position

        self.g = 0
        self.h = 0
        self.f = 0

    def __eq__(self, other):
        return self.position == other.position

def pressed(key, traktor_poz):
    if key[K_d]:
        return(move_right())
    if key[K_s]:
        return(move_down())
    if key[K_a]:
        return(move_left())
    if key[K_w]:
        return(move_up())
    if key[K_SPACE]:
        work(traktor_poz)
    if key[K_1]:
        config.activity.activity_val(0)
    if key[K_2]:
        config.activity.activity_val(1)
    if key[K_3]:
        config.activity.activity_val(2)
    if key[K_4]:
        config.activity.activity_val(3)
    if key[K_p]:
        pathfinding()


def move_left():
    if config.TRAKTOR_POZ[0]==config.POLE_POZ[1]:
        pass
    else:
        config.TRAKTOR_POZ[0]-=70
        print("Ruch w lewo")
        config.traktor.traktor_turn(1)
        return 1


def move_up():
    if config.TRAKTOR_POZ[1]==config.POLE_POZ[0]:
        pass
    else:
        config.TRAKTOR_POZ[1]-=70
        print("Ruch w górę")
        config.traktor.traktor_turn(2)
        return 1



def move_right():
    if config.TRAKTOR_POZ[0]==705:
        pass
    else:
        config.TRAKTOR_POZ[0]+=70
        config.traktor.traktor_turn(3)
        print("Ruch w prawo")
        return 1
def move_down():
    if config.TRAKTOR_POZ[1]==705:
        pass
    else:
        config.TRAKTOR_POZ[1]+=70
        config.traktor.traktor_turn(0)
        print("Ruch w dół")
        return 1


def work(traktor_poz):
    if config.activity.activity_get_value()==0:
        water(traktor_poz)
        print("Podlanie pola")

    if config.activity.activity_get_value()==1:
        weeds(traktor_poz)
        print("Odchwaszczenie pola")

    if config.activity.activity_get_value()==2:
        plant(traktor_poz)
        print("Zasadzenie pola")

    if config.activity.activity_get_value()==3:
        harvest(traktor_poz)
        print("Zbiory z pola")

def water(position):
    if config.POLE_STAN[position[0],position[1]] in [0,1,2,3]:
        config.mat_val([position[0],position[1]],4)

def weeds(position):
    if config.POLE_STAN[position[0], position[1]] in [1,3,5,7]:
        config.mat_val([position[0], position[1]], -1)
def plant(position):
    if config.POLE_STAN[position[0], position[1]] in [0,1,4,5]:
        config.mat_val([position[0], position[1]], 2)
def harvest(position):
    if config.POLE_STAN[position[0], position[1]] == 8:
        config.mat_val([position[0], position[1]], -8)


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


def pathfinding():
    activity = config.activity.activity_get_value()
    avaiable_value = []
    if activity == 0:
        avaiable_value = [0,1,2,3]
    elif activity == 1:
        avaiable_value = [1,3,5,7]
    elif activity == 2:
        avaiable_value = [0,1,4,5]
    elif activity == 3:
        avaiable_value = [8]
    opened = []
    closed = []
    start_position = [int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)]
    end_point = search(start_position,avaiable_value)
    if start_position == end_point:
        work([int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)])
    else:
        route = a_star(start_position,end_point)
        for i in route[::-1]:
            poz = config.TRAKTOR_POZ
            if i[0]< poz[0]:
                move_down()
            elif i[0]> poz[0]:
                move_up()
            elif i[1]> poz[1]:
                move_right()
            elif i[1]< poz[1]:
                move_left()
            time.sleep(0.2)
        work([int(((config.TRAKTOR_POZ[1]-5)/70)-1), int(((config.TRAKTOR_POZ[0]-5)/70)-1)])

def a_star(start, end):
    a_queue = PriorityQueue()
    a_queue.put(start,0)
    cost = {tuple(start): 0}
    path_from = {tuple(start): None}
    finall_path = [tuple(end)]
    found = 0
    while not a_queue.empty():
        current = tuple(a_queue.get())

        if current == tuple(end):
            break

        for next in points(current):
            new_cost = cost[tuple(current)] + int(config.POLE_STAN[next[0],next[1]])
            if tuple(next) not in cost or new_cost < cost[tuple(next)]:
                cost[tuple(next)] = new_cost
                priority = new_cost + heuristic(end, next)
                a_queue.put(next,priority)
                path_from[tuple(next)] = current
                if next == end:
                    found = 1
                    break
        if found:
            break

    pth = path_from[tuple(end)]
    while not pth==tuple(start):
        finall_path.append(pth)
        pth = path_from[pth]

    return finall_path

closed = []
def search(point,values):
    global closed
    if config.POLE_STAN[point[0],point[1]] in values:
        closed = []
        return point
    else:
        closed.append(point)
        childs = points(point)
        while childs:
            child = childs.pop()
            if child not in closed:
                return search(child,values)


def points(point):
    points = []
    for i in [[point[0],point[1]-1],[point[0]-1,point[1]],[point[0],point[1]+1],[point[0]+1,point[1]]]:
        if i[0] in [-1,10] or i[1] in [-1,10]:
            pass
        else:
            points.append(i)
    return points