Clean up.

This is how I see it should be. Could you review changes and test it against corner case you had?