Fixed some bugs in getShortestPath. Cleaned up code. Added testing to the real function in Position class.