Searching State and Pathfinding refinements, Code clean up

Cleaned the code
Now path finding is more responsive  as well