Searching State and Pathfinding refinements, Code clean up

Cleaned the code
Now path finding is more responsive  as well
Merge pull request #3 from madrusec/master

Searching State and Pathfinding refinements, Code clean up