Changed some code to avoid index out of bounds (possibly fixes #201, please test it)