Implemented full algorithm

Works with puzzle 5 generating 110 nodes