Рефакторинг плохого дизайна?

Может ли кто-нибудь помочь мне с рефакторингом старого очень сложного кода?

В оригинале использовалась только установка, а все остальное было в одном ISR. Но так как он был все больше и больше, код довольно медленный и иногда довольно случайный, даже эмулятор ПК не сильно помогал (вероятно, некоторые длительные операции, такие как печать, иногда прерываются следующим прерыванием в зависимости от аналоговых значений, кнопок и т. д.).

Есть ли какое-нибудь руководство по поддержке такого проекта или есть только выход начать все сначала?

Поскольку существует несколько важных правил, также потребуется очередь событий, но не уверен, есть ли какой-либо шаблон или шаблон, с которого можно начать в случае перезапуска. A-Star 32U4 Micro имеет флэш-память < 32 КБ, и код пока трудно вместить (есть также большие шрифты).

Пример кода распаковки шрифта, но надеюсь, что с этой частью все в порядке:

void Adafruit_GFX::drawChar(int16_t x, int16_t y, unsigned char c,
  uint16_t color, uint16_t bg, uint8_t size) {
...
    uint8_t blockSize = 0;
    if(packed) blockSize = pgm_read_byte(&bitmap[bo++]); // для первого символа выбрать только 1-й битовый массив
    BitStreamsReader bitStream(&bitmap[bo++], packed?blockSize:0); // указатель, размер блока (если упаковано)
    BitXorBuffer rowXorBuf(w); // Буфер XOR строк (с каждой строкой выполняется XOR)
    bool chk;
    for(yy=0; yy<h; yy++) {
        rowXorBuf.rewind();
        for(xx=0; xx<w; xx++) {
            chk = bitStream; // чтение 1b
            if(packed) chk = rowXorBuf.xorMove(chk); // xor с буфером XOR
            if(chk) {
                if(size == 1) {
                    writePixel(x+xo+xx, y+yo+yy, color);
                } else {
                    writeFillRect(x+(xo16+xx)*size, y+(yo16+yy)*size,
                        size, size, color);
                }
            }
        }
    }
...

class BitStreamsReader {
public:
    BitStreamsReader(uint8_t *arr, unsigned int maxSize) {
        blockSize = maxSize;
        bitMask = 0x80;
        bytePtr = arr;
        zerosCount = 0;
        remainingBits = 0;
        markBitInit();
    }
    operator bool() {
        if(!blockSize) { // распакованный поток
            bool retVal = !!(pgm_read_byte(bytePtr) & bitMask);
            next();
            return retVal;
        } else if(remainingBits) {
            bool retVal = !!(pgm_read_byte(bytePtr) & bitMask);
            next();
            if(!--remainingBits) {
                markBitInit();
            }
            return retVal;
        } else if(zerosCount) {
            if(!--zerosCount) {
                markBitInit();
            }
            return 0;
        }
        return 0; //бросать "??";
    }
private:
    void markBitInit() {
        if(blockSize) {
            if(pgm_read_byte(bytePtr) & bitMask) {
                remainingBits = blockSize;
            } else {
                zerosCount = blockSize;
            }
            next();
        }
    }
    void next () {
        if(bitMask == 0x01) {
            bitMask = 0x80;
            bytePtr++;
        } else {
            bitMask >>= 1;
        }
    };
    uint8_t *bytePtr, bitMask, zerosCount, remainingBits, blockSize;
};

class BitXorBuffer {
public:
    BitXorBuffer(int bufSize) {
        bufSize = bufSize/8+!!(bufSize&7);
        bitBuf = new uint8_t[bufSize];
        memset(bitBuf, 0, bufSize);
        rewind();
    }
    ~BitXorBuffer() {
        delete[] bitBuf;
    }
    void rewind() {
        bitMask = 128;
        bytePtr = bitBuf;
    }
    bool xorMove(bool by) {
        if(bitMask & *bytePtr) by ^= true;
        *bytePtr |= bitMask;
        if(!by) {
                *bytePtr ^= bitMask;
        }
        nextBit();
        return by;
    }
private:
    void nextBit() {
        if(bitMask > 1) {
            bitMask >>= 1; // перемещаем бит в буфер XOR
        } else {
            bitMask = 128;
            bytePtr++;
        }
    }
    uint8_t *bytePtr, *bitBuf, bitMask;
};