AStar & utility functions complete

Todo: Find & fix any bugs