Pomocy! Implementacja algorytmu A*!

Mam problem otóż napisałem już dużo prób implementacji, jednak żadna nie działała poprawnie, główny problem mam z tym że np jeżeli jest taka sytuacja:
[ ] - pojedyncze pole
[S] - start
[M] - meta
[B] - blokada

[ ][ ][ ][ ][B][ ][ ][ ]
[S][ ][ ][ ][B][ ][ ][M]
[ ][ ][ ][ ][B][ ][ ][ ]
[ ][ ][ ][ ][ ][ ][ ][ ]

to mój skrypt pójdzie tak:

[ ][ ][ ][ ][B][ ][ ][ ]
[S][>][>][>][B][ ][>][M]
[ ][ ][ ][/][B][>][ ][ ]
[ ][ ][ ][ ][>][ ][ ][ ]

co nie jest najkrótszą drogą :(
powinien odświeżyć ścieżkę i znaleźć krótszą, ale nie wiem jak to zrobić. Proszę o pomoc, jeżeli ktoś mógłby to prosiłbym o jakiś prosty konsolowy przykład prawidłowego zaimplementowania algorytmu.
Dziękuję!

http://pastebin.com/1a7s14Y0
To najnowsza i zarazem najgorsza. Nie działa wcale..

https://zapodaj.net/d730d2973c2d3.png.html
To stara implementacja w Unity, jak widać też nie działa dobrze.

masz zagnieżdżone 13 warunków(if/while/ for) nie dziwie się ze nie działa, podziel ten kod na mniejsze metody i pewnie sam odkryjesz co jest nie tak :).

Do komentarzy wyżej, to że przerobiłeś

 while(w1){ if(w1){ if(w3) { if(w4) itd. }}}

na while(w1){if(!w2)continue; if(!w3) continue; itd.. }

 jest o niebo lepsze, zwłaszcza że te continu'y są jedne pod drugim na początku while'a ale to trochę tak zamiatanie śmieci pod dywan. Jest czyściej ale nie o to chodziło ;)

Twój algorytm działa dobrze, A* nie znajduje najkrótszej drogi to robi algorytm dijkstry, tylko drogę o najniższym przewidywanym koszcie. Twoja funkcja h jest jaka jest i taki masz wynik. Swoją drogą to że w funkcji H liczącej pozostała odległość iteracyjnie znajdujesz "wyjście" z labiryntu totalnie mi ja się z celem.

Ps. nie  pisze się warunków i tego co one robią w jednej linii. zwłaszcza jak Ci się to na monitorze nie mieści...