Some basic path finding stuff

The path finding stuff doesn't work completely, but here it is.