Removed repeated heuristics from debug
Added navgrid drawing
Added fix for unit being outside level