After many weeks, I now have a reliable, working navigation system for PvE. it even works on diagonals (red squares were for debugging)