fix implementation, with additional tests

Got it working against the test vectors.