Removed lots of unused code, added some simple calculation methods, fixed many pathfinding problems, updates some messages and added a new test.