Getting there with a*, down to the infinate loop stage, hopefully will be on the memory leak stage
Fixed some bugs, it still isnt finding the correct path