Fixed the subgoaling with a new method. Also added subgoaling timeouts for unreachable goals