Finished nav algorithm, wating for test