[WIP] Cleanup code and adds some speedups

Replaces #523 & #522.

@merk What do you think about this? Should I continue those changes?