Implemented further steps of the navigation algorithm.
Resource handling steps still remain.