Fix and Performance

Fix for the offByOne-error and small performance optimisations