Implemented some simple Iterators

Implemented some simple iterators and fixed some bugs. Cleaned up the
code. Put some debug outputs in.