Pathfinding working, but need a lot of twaking

Tweak, tweak... Uploading for version control (in case I mess up :P )