Got basic pathifinding working