Got some tests working.

Iterator still doesn't stop at the floor