Implement iteration steps

TODO
Converge detection
Code cleanup