Removed debugging code from nav

Maybe has some sort of #define so that the code doesn't go into
production. We need all the memory we can get.
Renamed some subroutines

Renamed some subroutines to better match what they do. Also since we
will be adding the exploration algo soon it is better to give them
explicit names.
Added nav_size() subroutine

Makes the code a little more readable.
Added the has_visited int to the nav_array struct

This is needed to implement the exploration algo
Deleted unessary recursive header subroutine
Added pos_equal()

Since we will be checking if the postions are equalivent in the
exploration algo it would be nice to have an equals subroutine.