Comment and restructure original code

[Frank Brongers]

Comment and restructure the original code from the authors such that it is intuitively understandable, and that which is not, is well commented.