[iglezz]

MKN,
Посмотрев код по диагонали, могу сказать, что потребуется вдумчиво прогнать всё через дебаггер в голове на предмет выявления мест, где идёт работа на уровне байт, а не символов.
Параллельно можно и оптимизировать немного на производительность.