First version of pathfinding done
Manual merge