Path finding

Pretty much change all the code at this point, with all debug and comment all over the code. And:

• Path-finding finished
• Add two more option to control the behavior and helper to use new system