Fixed the heuristic

Fixed the heuristic, it actually does random things now. Also: Added
some test stuff.