diff --git a/a_gwiazdka.py b/a_gwiazdka.py
new file mode 100644
index 0000000..6dfdd09
--- /dev/null
+++ b/a_gwiazdka.py
@@ -0,0 +1,124 @@
+from collections import deque
+from queue import PriorityQueue
+
+from stan_nastepnik import *
+
+
+class NastepnikZKosztemSciezki:
+    def __init__(self, kosztSciezki: int, nastepnik: Nastepnik):
+        self.kosztSciezki = kosztSciezki
+        self.nastepnik = nastepnik
+
+    def skopiuj(self):
+        return NastepnikZKosztemSciezki(self.kosztSciezki, self.nastepnik.skopiuj())
+
+    def __lt__(self, other):
+        return self.kosztSciezki <= other.kosztSciezki
+
+
+def goaltest(stan: Stan, cel: Stan):
+    if stan.poleStartoweGorne.wiersz != cel.poleStartoweGorne.wiersz:
+        return False
+    elif stan.poleStartoweGorne.kolumna != cel.poleStartoweGorne.kolumna:
+        return False
+    else:
+        return True
+
+
+def stan_w_liscie_nastepnikow(stan: Stan, lista_nastepnikow):
+    for i in lista_nastepnikow:
+        if i.stan.kierunek != stan.kierunek:
+            continue
+        elif i.stan.poleStartoweGorne.wiersz != stan.poleStartoweGorne.wiersz:
+            continue
+        elif i.stan.poleStartoweGorne.kolumna != stan.poleStartoweGorne.kolumna:
+            continue
+        else:
+            return True
+    return False
+
+
+def stan_w_kolejce_nastepnikow(stan: Stan, kolejka_nastepnikow: PriorityQueue):
+    pom = []
+    znaleziono = False
+    while not kolejka_nastepnikow.empty():
+        element = kolejka_nastepnikow.get()
+        pom.append(element)
+        nastepnik_z_kosztem_sciezki: NastepnikZKosztemSciezki = element[1]
+        n = nastepnik_z_kosztem_sciezki.nastepnik
+        if n.stan.kierunek != stan.kierunek:
+            continue
+        elif n.stan.poleStartoweGorne.wiersz != stan.poleStartoweGorne.wiersz:
+            continue
+        elif n.stan.poleStartoweGorne.kolumna != stan.poleStartoweGorne.kolumna:
+            continue
+        else:
+            znaleziono = True
+            break
+    for e in pom:
+        kolejka_nastepnikow.put(e)
+    return znaleziono
+
+
+def stos_akcji(stan_koncowy: Nastepnik):
+    stos = deque()
+    while stan_koncowy.poprzednik is not None:
+        stos.append(stan_koncowy.akcja)
+        stan_koncowy = stan_koncowy.poprzednik
+    return stos
+
+
+def heurystyka(z: Stan, cel: Stan):
+    kroki_w_pionie = abs(cel.poleStartoweGorne.wiersz - z.poleStartoweGorne.wiersz)
+    kroki_w_poziomie = abs(cel.poleStartoweGorne.kolumna - z.poleStartoweGorne.kolumna)
+    return kroki_w_pionie + kroki_w_poziomie
+
+
+def koszt_wjechania(z: Stan, do: Stan):
+    z_wiersz = z.poleStartoweGorne.wiersz
+    z_kolumna = z.poleStartoweGorne.kolumna
+    do_wiersz = do.poleStartoweGorne.wiersz
+    do_kolumna = do.poleStartoweGorne.kolumna
+    krata = z.poleStartoweGorne.krata.krata
+    if z_wiersz == do_wiersz and z_kolumna == do_kolumna and z.kierunek == do.kierunek:
+        return 0
+    elif z_wiersz == do_wiersz and z_kolumna == do_kolumna and z.kierunek != do.kierunek:
+        return ZawartoscPolaNaKosztObrotu[krata[do_wiersz][do_kolumna]]
+    elif z.kierunek == do.kierunek:
+        krok_w_pionie = (abs(z_wiersz - do_wiersz) == 1 and z_kolumna == do_kolumna)
+        krok_w_poziomie = (abs(z_kolumna - do_kolumna) == 1 and z_kolumna == z_kolumna)
+        if krok_w_pionie or krok_w_poziomie:
+            return ZawartoscPolaNaKosztWjechania[krata[do_wiersz][do_kolumna]]
+        else:
+            raise "Stany nie są połączone."
+
+
+def priorytet(koszt_sciezki: int, z: Stan, do: Stan, cel: Stan):
+    return koszt_sciezki + koszt_wjechania(z, do) + heurystyka(do, cel)
+
+
+def graphsearch(istate: Stan, cel: Stan):
+    fringe = PriorityQueue()
+    explored = []
+    pom = NastepnikZKosztemSciezki(0, Nastepnik(None, istate, None))
+    fringe.put((priorytet(0, istate, istate, cel), pom))
+    # fringe.append(Nastepnik(None, istate, None))
+    while not fringe.empty():
+        # for i in fringe:
+        #     print("F",i.stan.kierunek,i.stan.poleStartoweGorne.wiersz,i.stan.poleStartoweGorne.kolumna,end=" ")
+        # print()
+        element: NastepnikZKosztemSciezki = fringe.get()[1]
+        koszt_sciezki = element.kosztSciezki
+        nastepnik = element.nastepnik
+        if goaltest(nastepnik.stan, cel):
+            return stos_akcji(nastepnik)
+        explored.append(nastepnik)
+        for nowy in succ(nastepnik):
+            if not stan_w_kolejce_nastepnikow(nowy.stan, fringe) and not stan_w_liscie_nastepnikow(nowy.stan, explored):
+                z = nastepnik.stan
+                do = nowy.stan
+                pom = NastepnikZKosztemSciezki(koszt_sciezki + koszt_wjechania(z, do), nowy)
+                pom2 = (priorytet(koszt_sciezki, z, do, cel), pom)
+                fringe.put(pom2)
+                # print("dodano",pom.nastepnik.stan.kierunek,pom.nastepnik.stan.poleStartoweGorne.wiersz,pom.nastepnik.stan.poleStartoweGorne.kolumna)
+    return False
diff --git a/agent.py b/agent.py
index 9e193e0..b77c4e4 100644
--- a/agent.py
+++ b/agent.py
@@ -1,9 +1,11 @@
 import random
 from collections import deque
 
-from bfs import Stan, Akcja, graphsearch
+# from bfs import graphsearch
+from a_gwiazdka import graphsearch
 from krata import *
 from obserwacja import *
+from stan_nastepnik import Stan, Akcja
 
 
 class Agent(Obserwowany):
diff --git a/bfs.py b/bfs.py
index 7bf5926..c048d2c 100644
--- a/bfs.py
+++ b/bfs.py
@@ -1,102 +1,6 @@
 from collections import deque
 
-from krata import *
-
-
-class Akcja(Enum):
-    OBROT_W_LEWO = 0
-    OBROT_W_PRAWO = 1
-    KROK_W_PRZOD = 2
-
-
-class Stan:
-    def __init__(self, kierunek: Kierunek, poleStartoweGorne: PoleKraty):
-        self.kierunek = kierunek
-        self.poleStartoweGorne = poleStartoweGorne
-
-    def skopiuj(self):
-        return Stan(self.kierunek, self.poleStartoweGorne.skopiuj())
-
-
-class Nastepnik:
-    def __init__(self, akcja: Akcja or None, stan: Stan, poprzednik):
-        self.akcja = akcja
-        self.stan = stan
-        self._poprzednik = poprzednik
-
-    def getPoprzednik(self):
-        return self._poprzednik
-
-    def setPoprzednik(self, x):
-        raise Exception
-
-    poprzednik = property(getPoprzednik, setPoprzednik)
-
-    def skopiuj(self):
-        return Nastepnik(self.akcja, self.stan.skopiuj(), self.poprzednik)
-
-
-def nastepnik_obrotu_w_lewo(nastepnik: Nastepnik):
-    akcja = Akcja.OBROT_W_LEWO
-    stan = Stan(nastepnik.stan.kierunek.kierunekNaLewo(), nastepnik.stan.poleStartoweGorne)
-    return Nastepnik(akcja, stan, nastepnik)
-
-
-def nastepnik_obrotu_w_prawo(nastepnik: Nastepnik):
-    akcja = Akcja.OBROT_W_PRAWO
-    stan = Stan(nastepnik.stan.kierunek.kierunekNaPrawo(), nastepnik.stan.poleStartoweGorne)
-    return Nastepnik(akcja, stan, nastepnik)
-
-
-def nastepnik_kroku_w_przod(nastepnik: Nastepnik):
-    akcja = Akcja.KROK_W_PRZOD
-    stan = Stan(nastepnik.stan.kierunek, nastepnik.stan.poleStartoweGorne)
-    if stan.kierunek == Kierunek.POLNOC:
-        stan.poleStartoweGorne.wiersz -= 1
-    elif stan.kierunek == Kierunek.POLUDNIE:
-        stan.poleStartoweGorne.wiersz += 1
-    elif stan.kierunek == Kierunek.ZACHOD:
-        stan.poleStartoweGorne.kolumna -= 1
-    elif stan.kierunek == Kierunek.WSCHOD:
-        stan.poleStartoweGorne.kolumna += 1
-    return Nastepnik(akcja, stan, nastepnik)
-
-
-def pole_w_granicach_kraty(pole: PoleKraty):
-    if pole.wiersz not in range(0, pole.krata.liczbaPolPionowo):
-        return False
-    elif pole.kolumna not in range(0, pole.krata.liczbaPolPoziomo):
-        return False
-    else:
-        return True
-
-
-def pole_puste(pole: PoleKraty):
-    if pole.krata.krata[pole.wiersz][pole.kolumna] in (ZawartoscPola.PUSTE, ZawartoscPola.CEL):
-        return True
-    else:
-        return False
-
-
-def mozna_zrobic_krok_w_przod(nastepnik: Nastepnik):
-    nastepnik = nastepnik_kroku_w_przod(nastepnik)
-    if pole_w_granicach_kraty(nastepnik.stan.poleStartoweGorne) and pole_puste(nastepnik.stan.poleStartoweGorne):
-        return True
-    else:
-        return False
-
-
-def succ(nastepnik: Nastepnik):
-    wynik = []
-    pom = nastepnik.skopiuj()
-    wynik.append(nastepnik_obrotu_w_lewo(pom))
-    pom = nastepnik.skopiuj()
-    wynik.append(nastepnik_obrotu_w_prawo(pom))
-    pom = nastepnik.skopiuj()
-    if mozna_zrobic_krok_w_przod(pom):
-        pom = nastepnik.skopiuj()
-        wynik.append(nastepnik_kroku_w_przod(pom))
-    return wynik
+from stan_nastepnik import *
 
 
 def goaltest(stan: Stan, cel: Stan):
diff --git a/enumy_i_slowniki.py b/enumy_i_slowniki.py
index 2c6ca4c..267e871 100644
--- a/enumy_i_slowniki.py
+++ b/enumy_i_slowniki.py
@@ -34,10 +34,30 @@ class ZawartoscPola(Enum):
     PUSTE = 0
     SCIANA = 1
     CEL = 2
+    DYWAN = 3
+    KALUZA = 4
 
 
 ZawartoscPolaNaKolorPola = {
     ZawartoscPola.PUSTE: BIALY,
     ZawartoscPola.SCIANA: CIEMNY_BRAZOWY1,
-    ZawartoscPola.CEL: ZIELONY1
+    ZawartoscPola.CEL: ZIELONY1,
+    ZawartoscPola.DYWAN: ZOLTY1,
+    ZawartoscPola.KALUZA: NIEBIESKI1
+}
+
+ZawartoscPolaNaKosztObrotu = {
+    ZawartoscPola.PUSTE: 1,
+    ZawartoscPola.SCIANA: None,
+    ZawartoscPola.CEL: 1,
+    ZawartoscPola.DYWAN: 5,
+    ZawartoscPola.KALUZA: 3
+}
+
+ZawartoscPolaNaKosztWjechania = {
+    ZawartoscPola.PUSTE: 2,
+    ZawartoscPola.SCIANA: None,
+    ZawartoscPola.CEL: 2,
+    ZawartoscPola.DYWAN: 5,
+    ZawartoscPola.KALUZA: 9
 }
diff --git a/main.py b/main.py
index 1a65e39..601942c 100644
--- a/main.py
+++ b/main.py
@@ -55,24 +55,39 @@ def nadaj_cel_agentowi(agent: Agent):
 
 def main():
     # dla kraty 30 x 15
-    # dodaj_szafke("A", 2, 12, "P", 2, 2)
-    # dodaj_szafke("B", 2, 12, "L", 2, 3)
-    # dodaj_szafke("C", 2, 5, "P", 1, 6)
-    # dodaj_szafke("D", 2, 6, "L", 1, 7)
-    # dodaj_szafke("C", 2, 4, "P", 9, 6)
-    # dodaj_szafke("D", 2, 3, "L", 10, 7)
-    # dodaj_szafke("E", 2, 8, "P", 3, 11)
-    # dodaj_szafke("F", 2, 10, "L", 2, 13)
-    # dodaj_szafke("H", 2, 12, "L", 1, 18)
-    # dodaj_szafke("I", 2, 7, "P", 5, 23)
-    # dodaj_szafke("J", 2, 12, "L", 1, 25)
-    # dodaj_szafke("G", 2, 10, "P", 5, 29)
+    dodaj_szafke("A", 2, 12, "P", 2, 2)
+    dodaj_szafke("B", 2, 12, "L", 2, 3)
+    dodaj_szafke("C", 2, 5, "P", 1, 6)
+    dodaj_szafke("D", 2, 6, "L", 1, 7)
+    dodaj_szafke("C", 2, 4, "P", 9, 6)
+    dodaj_szafke("D", 2, 3, "L", 10, 7)
+    dodaj_szafke("E", 2, 8, "P", 3, 11)
+    dodaj_szafke("F", 2, 10, "L", 2, 13)
+    dodaj_szafke("H", 2, 12, "L", 1, 18)
+    dodaj_szafke("I", 2, 7, "P", 5, 23)
+    dodaj_szafke("J", 2, 12, "L", 1, 25)
+    dodaj_szafke("G", 2, 9, "P", 5, 28)
 
-    # dla kraty 10 x 10
-    dodaj_szafke("A", 1, 8, "P", 1, 1)
-    dodaj_szafke("B", 1, 8, "L", 1, 4)
-    dodaj_szafke("C", 1, 8, "P", 1, 6)
-    dodaj_szafke("C", 1, 8, "P", 1, 8)
+    # # dla kraty 10 x 10
+    # dodaj_szafke("A", 1, 8, "P", 1, 1)
+    # dodaj_szafke("B", 1, 8, "L", 1, 4)
+    # dodaj_szafke("C", 1, 8, "P", 1, 6)
+    # dodaj_szafke("C", 1, 8, "P", 1, 8)
+
+    # # dla kraty 5 x 5
+    # dodaj_szafke("A", 1, 3, "P", 1, 1)
+    # dodaj_szafke("B", 1, 3, "L", 1, 3)
+
+    for i in (
+            (1, 10), (1, 3), (3, 23), (2, 23), (5, 15), (4, 15), (9, 12), (11, 20), (11, 27), (11, 26), (14, 19),
+            (14, 18),
+            (14, 20), (8, 29), (9, 29)):
+        krata_magazynu.krata[i[0]][i[1]] = ZawartoscPola.DYWAN
+    for i in (
+            (0, 10), (13, 20), (13, 6), (13, 14), (14, 13), (9, 26), (9, 16), (9, 15), (9, 27), (9, 16), (9, 26),
+            (5, 8),
+            (5, 9), (7, 9), (7, 10)):
+        krata_magazynu.krata[i[0]][i[1]] = ZawartoscPola.KALUZA
 
     dodaj_agenta()
 
diff --git a/stale.py b/stale.py
index f1715cb..845f87d 100644
--- a/stale.py
+++ b/stale.py
@@ -1,10 +1,10 @@
-FPS = 10
+FPS = 20
 #
 # SZEROKOSC_OKNA = 1500
 # WYSOKOSC_OKNA = 750
 #
-LICZBA_POL_W_POZIOMIE = 10
-LICZBA_POL_W_PIONIE = 10
+LICZBA_POL_W_POZIOMIE = 30
+LICZBA_POL_W_PIONIE = 15
 BOK_POLA = 45
 ODSTEP_MIEDZY_POLAMI = 1
 SZEROKOSC_OKNA = LICZBA_POL_W_POZIOMIE * (BOK_POLA + ODSTEP_MIEDZY_POLAMI) + ODSTEP_MIEDZY_POLAMI
@@ -21,6 +21,8 @@ SZARY1 = (150, 150, 150)
 ZIELONY1 = (26, 122, 26)
 CZARNY = (0, 0, 0)
 CIEMNY_BRAZOWY1 = (60, 19, 33)
+ZOLTY1 = (231, 213, 69)
+NIEBIESKI1 = (65, 125, 225)
 ###
 
 ###
diff --git a/stan_nastepnik.py b/stan_nastepnik.py
new file mode 100644
index 0000000..24e9616
--- /dev/null
+++ b/stan_nastepnik.py
@@ -0,0 +1,98 @@
+from krata import *
+
+
+class Akcja(Enum):
+    OBROT_W_LEWO = 0
+    OBROT_W_PRAWO = 1
+    KROK_W_PRZOD = 2
+
+
+class Stan:
+    def __init__(self, kierunek: Kierunek, poleStartoweGorne: PoleKraty):
+        self.kierunek = kierunek
+        self.poleStartoweGorne = poleStartoweGorne
+
+    def skopiuj(self):
+        return Stan(self.kierunek, self.poleStartoweGorne.skopiuj())
+
+
+class Nastepnik:
+    def __init__(self, akcja: Akcja or None, stan: Stan, poprzednik):
+        self.akcja = akcja
+        self.stan = stan
+        self._poprzednik = poprzednik
+
+    def getPoprzednik(self):
+        return self._poprzednik
+
+    def setPoprzednik(self, x):
+        raise Exception
+
+    poprzednik = property(getPoprzednik, setPoprzednik)
+
+    def skopiuj(self):
+        return Nastepnik(self.akcja, self.stan.skopiuj(), self.poprzednik)
+
+
+def nastepnik_obrotu_w_lewo(nastepnik: Nastepnik):
+    akcja = Akcja.OBROT_W_LEWO
+    stan = Stan(nastepnik.stan.kierunek.kierunekNaLewo(), nastepnik.stan.poleStartoweGorne)
+    return Nastepnik(akcja, stan, nastepnik)
+
+
+def nastepnik_obrotu_w_prawo(nastepnik: Nastepnik):
+    akcja = Akcja.OBROT_W_PRAWO
+    stan = Stan(nastepnik.stan.kierunek.kierunekNaPrawo(), nastepnik.stan.poleStartoweGorne)
+    return Nastepnik(akcja, stan, nastepnik)
+
+
+def nastepnik_kroku_w_przod(nastepnik: Nastepnik):
+    akcja = Akcja.KROK_W_PRZOD
+    stan = Stan(nastepnik.stan.kierunek, nastepnik.stan.poleStartoweGorne)
+    if stan.kierunek == Kierunek.POLNOC:
+        stan.poleStartoweGorne.wiersz -= 1
+    elif stan.kierunek == Kierunek.POLUDNIE:
+        stan.poleStartoweGorne.wiersz += 1
+    elif stan.kierunek == Kierunek.ZACHOD:
+        stan.poleStartoweGorne.kolumna -= 1
+    elif stan.kierunek == Kierunek.WSCHOD:
+        stan.poleStartoweGorne.kolumna += 1
+    return Nastepnik(akcja, stan, nastepnik)
+
+
+def pole_w_granicach_kraty(pole: PoleKraty):
+    if pole.wiersz not in range(0, pole.krata.liczbaPolPionowo):
+        return False
+    elif pole.kolumna not in range(0, pole.krata.liczbaPolPoziomo):
+        return False
+    else:
+        return True
+
+
+def mozna_wjechac_na_pole(pole: PoleKraty):
+    if pole.krata.krata[pole.wiersz][pole.kolumna] != ZawartoscPola.SCIANA:
+        return True
+    else:
+        return False
+
+
+def mozna_zrobic_krok_w_przod(nastepnik: Nastepnik):
+    nastepnik = nastepnik_kroku_w_przod(nastepnik)
+    if pole_w_granicach_kraty(nastepnik.stan.poleStartoweGorne) and mozna_wjechac_na_pole(
+            nastepnik.stan.poleStartoweGorne):
+        return True
+    else:
+        return False
+
+
+def succ(nastepnik: Nastepnik):
+    wynik = []
+    pom = nastepnik.skopiuj()
+    wynik.append(nastepnik_obrotu_w_lewo(pom))
+    pom = nastepnik.skopiuj()
+    wynik.append(nastepnik_obrotu_w_prawo(pom))
+    pom = nastepnik.skopiuj()
+    if mozna_zrobic_krok_w_przod(pom):
+        pom = nastepnik.skopiuj()
+        wynik.append(nastepnik_kroku_w_przod(pom))
+    return wynik