This finds always the path, but its not always the shortest! It has to be modified further...
The code is 100% by me. Want to read the wikipedia article about it? https://en.wikipedia.org/wiki/A*_search_algorithm
