Implementing TODO stuff

Greate code, which I would like to use in a project - I agree on your previous improvement/TODO comments, and
I have therefore implemented the improvements in this pull request.