Removed some debugging code
Started stop/stop-gap visualization and navigation