Added large amount of test code.
Fixed finding's long loop problem.
Fixed Priority Queue so that copies of the same PathNode at the same location would never be added.