Fixed index out of bounds edge case. Some code cleaning.