+ add goap core algorithm

- needs to be tweaked yet