Updated Navigation Capabilities, now using a simple Path Finding Algorithm