Waypoint pathfinding implemented (no at its best..)