Added code contracts, made sample test pass