Improved code readability and optimized for speed

[JoHof]

Hi, I have reorganized the code a bit and also added some unit-tests. If you still have suggestions, please create a PR against the current main