Fixed it

Now the best path is considering the obstacles in the way :D

Time for
testing and writing!