[master] High level navigation algoritm debug