Refactored WallFollow() code.
Performance optimizations. Inlining and refactoring