Working(?) Implementation of A* search for optimal path planning. Needs more thorough testing.