When removing entries while iterating, use a iterator.
Also correct math to figure out when to remove items.
Fixes #386