Fix buffer overrun and refactor a bit