Please, use some existing library for pathfinding and save yourself some time :-)
I will definitely buy whatever comes next.
