Improved the pathFinding AI (still  bugged)
PathFinding AI fixed, tests are still a bit wonkey and show some strange
results. MapPoint needs more testing.