Fixed code to match reference implementation