Add boundary checks, fix test