#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>

#include "keyMatrix.h"
#include "hwInterface.h"

// nice routines from friend
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
#define bitToggle(value, bit) ((value) ^= (1UL << (bit)))
#define bitWrite(value, bit, bitvalue) ((bitvalue) ? bitSet(value, bit) : bitClear(value, bit))

// just easy use in long routine
#define bit0 0
#define bit1 1
#define bit2 2
#define bit3 3
#define bit4 4
//#define bit5 5
#define bit6 6
#define bit7 7

#define RAM0x27 0
#define RAM0x28 1
#define RAM0x29 2
#define RAM0x2A 3
uint8_t unknown[4]; // RAM[0x27-0x2A]

//uint8_t RAM[0x40];

// busSatus register flags
//#define OBF 0       // Output buffer full. CPU not read data yet
//#define F1  3       // data (0)/command(1) Flag. Set when CPU send command
//#define ST4 4       // User settable flag - maybe used? unknown
//#define ST5 5       // User settable flag - SHIFT key?
//#define ST6 6       // User settable flag - CODE key?
//#define ST7 7       // User settable flag - CTRL key?


#define enableKeyScan 0         // Keyboard scan enabled
#define enableDMA 1             // WDT1 enable
#define enableWDT 2             // WDT0 enable
#define enableLongRoutine 3     // used in "long routine"
#define enablePinPAL 4          // pinPAL status controlled by CPU
//#define stateBit5 5             // Not used
//#define stateBit5 6             // Not used
#define keyOverFlowFlag 7       // set in timerIRQ when key read counter overflows
uint8_t globalState = 0;

#define TIMERCONST 0xAD
#define MAINLOOPCONST 2
bool timerOverFlowFlag;

uint8_t dmaCnt;         // RAM[0x39]

#define wdtData 0
#define keyReapeatRate 1
#define keyPressDelay 2
#define dmaData 3
uint8_t cpuConfig[4];   // RAM[0x20-0x23]

#define WDTDEFAULT 1
#define DMADEFAULT 50

#define flagShiftKey 5
#define flagCodeKey 6
#define flagCtrlKey 7

uint8_t keyMatrixBuffer[3]; // RAM[0x24-0x26]
uint8_t keyCount;           // RAM[0x37]

struct _keyParseEntry {
    uint8_t keyCode;
    uint8_t flags;      // related to ST4-ST7 flags
    uint8_t cnt;        // some kind of counter?
};

void irqTimer();
void kbcLoop();
void kbcSetup();
void irqExt();
void scanKeyMatrix();
void scanModKeys();
void sendKey();
void clean_146();
//uint8_t translateScanCode(uint8_t data);
uint8_t translateScanCode(uint8_t data, uint8_t flags);
void sendData(uint8_t data, uint8_t flags);

void resetWDT1() {
    dmaCnt = cpuConfig[dmaData];
}


uint8_t RAM[0x40];

int main(int argc, char **argv) {
    
    keyCount = 1;
    keyMatrixBuffer[0] = 52;

for (uint8_t i = 0; i < 0x40; i++) {
    RAM[i] = 0;
}

RAM[0x21] = 15;
RAM[0x22] = 1;

RAM[0x37] = 1;
RAM[0x24] = 52;

bitSet(globalState, enableLongRoutine);
    //for (uint8_t i = 0; i < 4; i++) {
        //printf("In KeyMatrix[%d]: %02X\n", i, keyMatrixBuffer[i]);
    //}

    for (uint8_t i = 0; i < 8; i++) {
        //printf("pass:%d\nKey matrix before:\n", i);
        //for (uint8_t i = 0; i < 4; i++) {
            //printf("KeyMatrix[%d]: %02X\n", i, keyMatrixBuffer[i]);
        //}
        //printf("IN: keyCount: %d\n", keyCount);

        //sendKey();
        clean_146();
    }

    //for (uint8_t i = 0; i < 4; i++) {
        //printf("Out KeyMatrix[%d]: %02X\n", i, keyMatrixBuffer[i]);
    //}

    return 0;

    kbcSetup();

    while (true) {
        irqExt();
        irqTimer();
        kbcLoop(); // main keyboard loop
    }
}

/*======================================================================
Use
R7b1 - global state
R6b1 - WDT0
R2b1 - timer overflow count

RAM[0x27]
RAM[0x28]
RAM[0x29]
*/
void irqTimer() {
    static uint8_t keyReadCnt = MAINLOOPCONST;

    timerStop();
    if ( busReadOBF() == 0 ) {
        pinSetIRQ4(LOW);
        if ( unknown[RAM0x28] ) {
            busWriteData(unknown[RAM0x27]);
            busWriteStatus(unknown[RAM0x28]);
            unknown[RAM0x28] = 0;
            pinSetIRQ4(HIGH);
        } else {
            busWriteStatus(unknown[RAM0x29]);
        }
    }

    timerSet(TIMERCONST);
    timerStart();

    if ( bitRead(globalState, enableWDT) ) {
        static uint8_t WDT = WDTDEFAULT;    // must be WDT = RAM[0x20] = 1;
        WDT--;
        if (WDT == 0) {
            WDT = cpuConfig[wdtData];
            sendData(0xFF, unknown[RAM0x29]);
        }
    }

    keyReadCnt--;
    if (keyReadCnt == 0) {
        keyReadCnt = MAINLOOPCONST;
        bitSet(globalState, keyOverFlowFlag);
    }
    // Return from Interrupt
}

/*======================================================================
*/
void kbcLoop() {

    // Why it fucks with interrup?
    // Not use simple NOPs?
    enableExternalInterrupt();
    disableExternalInterrupt();

    if ( bitRead(globalState, keyOverFlowFlag) == 0 ) return;

    bitClear(globalState, keyOverFlowFlag); // we will handle this, so clear!
    enableExternalInterrupt();

    /*
    // There is something strange:
    // * No reload DMA counter (RAM[0x39])?
    // * No clear DMA enable flag in globasState
    // *Only way to get things done
    // is to hope that CCPROM will
    // manage this case.
    // *Do I need to reset DMA counter?
    // Without resetting and action from
    // CCPROM, DMA counter will roll over and
    // it will take approx 5 times longer
    // to get next timeout triggering
    */
    if ( bitRead(globalState, enableDMA) ) {
        dmaCnt--;
        if ( dmaCnt == 0 ) {
            pinSetPAL(LOW);
            sendData(0xFD, 0);
        }
    }

    if ( bitRead(globalState, enableKeyScan) ) {
        scanModKeys();
        scanKeyMatrix();
        if ( keyCount > 0 && keyCount < 3 ) {
            sendKey();
        }
    }
}

/*======================================================================
*/
void kbcSetup() {
    pinSetWTF(LOW);
    pinSetNMI(LOW);
    pinSetIRQ4(LOW);

    cpuConfig[wdtData] = WDTDEFAULT;
    cpuConfig[keyReapeatRate] = 15;
    cpuConfig[keyPressDelay] = 2;
    cpuConfig[dmaData] = DMADEFAULT;

    resetWDT1();

    timerSet(TIMERCONST);
    pinSetWTF(HIGH);
    timerStart();
    enableExternalInterrupt();
    timerEnableIRQ();
}

/*======================================================================
Changed:
R0b1 - save for accumulator/ RAm address for stack fuckUp
R1b1 - RAM[0x38]
R3b1 - save for accumulator
R6b1 - WDT0
R7b1 - global flags
*/
void irqExt() {
    // RAM[0x38]
    static uint8_t configRegPtr = 0;    // assing 0 just because gcc and lint like it
    uint8_t busData;

    //configRegPtr = RAM[0x38];
    busData = busReadData();

    if ( busReadF1() == 0) {
        cpuConfig[configRegPtr] = busData;
        resetWDT1();
        return;     // Return from Interrupt
    }

    if ( busData == 0) {
        disableExternalInterrupt();
        return;     // Return from Interrupt
    }

    if ( bitRead(busData, bit7) ) {
        globalState = busData;

        uint8_t tmp;
        tmp = (busData & 0x10) ? HIGH : LOW;
        pinSetPAL(tmp);

        resetWDT1();
        return;     // Return from Interrupt
    }

    if ( bitRead(busData, bit6) == 0 ) {
        configRegPtr = busData & 0x03;     // cppcheck-suppress[unreadVariable]
    }

    resetWDT1();
    // Return from Interrupt
}

/*======================================================================
RAM[0x24]
RAM[0x25]
RAM[0x26]
RAM[0x37]
*/
void scanKeyMatrix() {
    uint8_t bufPtr = 0;
    keyCount = 0;

    for (uint8_t column = 8; column > 0; column--) {
        uint8_t accumulator;
        pinSetColumn(column);
        accumulator = ~portRead();
        if ( accumulator == 0 ) continue;
        for (uint8_t row = 8; row > 0; row--) {
            // fix it. It goes thru all row (input bits)
            bool tmpCarry = false;
            bool carry;
            carry = false;
            if (bitRead(accumulator, bit0)) tmpCarry = true;
            accumulator = accumulator >> 1;
            if (carry) bitSet(accumulator, 7);
            carry = tmpCarry;

            if ( carry ) {
                uint8_t saveAccum;
                saveAccum = accumulator;
                keyCount++;

                if ( keyCount < 3 ) {
                    // rows to bit 0,1,2; columns to 3,4,5
                    accumulator = (row & 0x07) | ((column << 3) & 0x38);
                    keyMatrix[bufPtr] = accumulator;
                    bufPtr++;
                    accumulator = saveAccum;
                }
            }
        }
    }
}

/*======================================================================
Return:
RAM[0x29]
RAM[0x2A]
*/
void scanModKeys() {
    uint8_t localFlags = 0;
    //uint16_t tmp16;

    if ( pinT0() == 0 ) bitSet(localFlags, flagShiftKey);  // Shift key pressed
    if ( pinT1() == 0 ) bitSet(localFlags, flagCodeKey);  // Code key pressed

    // to set correct column combination for Ctrl key
    pinSetColumn(0b010);

    if ( bitRead(portRead(), bit0) == 0 ) bitSet(localFlags, flagCtrlKey); // Ctrl key pressed

/*
    if ( unknown[RAM0x2A] != localFlags ) {
        unknown[RAM0x2A] = localFlags;
    } else {
        unknown[RAM0x29] = localFlags;
    }
*/
    if ( unknown[RAM0x2A] == localFlags ) {
        unknown[RAM0x29] = localFlags;
    } else {
        unknown[RAM0x2A] = localFlags;
    }

}

/*======================================================================
*/
uint8_t translateScanCode(uint8_t data, uint8_t flags) {
    uint8_t tmp8;
    // key code to bits 7,6,5,4,3,2
    // CODE and SHIFT keys to 1, 0 (Stored in RAM[0x29])

    tmp8 = (data << 2) & 0xFC;  // shift left and mask key code
    //tmp8 = ((unknown[RAM0x2A] >> 5) & 0x03) | tmp8;    // shift right and mask mod keys, merge with key code
    //tmp8 = ((unknown[RAM0x29] >> 5) & 0x03) | tmp8;    // shift right and mask mod keys, merge with key code
    tmp8 = ((flags >> 5) & 0x03) | tmp8;    // shift right and mask mod keys, merge with key code
    tmp8 = keyMatrix[tmp8];         // get key code from lookup table

    //if ( bitRead(unknown[RAM0x29], flagCtrlKey) ) {
    if ( bitRead(flags, flagCtrlKey) ) {
        tmp8 &= 0x9F;           // Ctrl pressed. Apply Ctrl mask to key code
    }
    return tmp8;
}

/*======================================================================
Return:
accumulator = R5 = flags
RAM[0x27]
RAM[0x28]
*/
void sendData(uint8_t data, uint8_t flags) {

    if ( data == 0xFE ) {
        pinSetNMI(HIGH);
        return;
    }

    if ( busReadOBF() == 0) {
        pinSetIRQ4(LOW);        // busOut read by CPU. We can write new data
        busWriteData(data);
        busWriteStatus(flags);
        pinSetIRQ4(HIGH);
        return;
    }

    if ( unknown[RAM0x28] ) {
        unknown[RAM0x27] = 0xFE;
        unknown[RAM0x28] = data;
    } else {
        unknown[RAM0x27] = data;
        unknown[RAM0x28] = flags;
    }

}

/*======================================================================
RAM[0x21]
RAM[0x22]

RAM[0x24]
RAM[0x25]
RAM[0x26]

RAM[0x29]

RAM[0x2B]
RAM[0x2C]
RAM[0x2D]

RAM[0x2E]
RAM[0x2F]
RAM[0x30]

RAM[0x31]
RAM[0x32]
RAM[0x33]

RAM[0x34]
RAM[0x35]
RAM[0x36]

RAM[0x37]
*/
void sendKey() {

    static struct _keyParseEntry parseBuffer[4];
    uint8_t tmp8, R3b0, R5b0;

// 0x2B, 0x2E, 0x31, 0x34
// 0x2C, 0x2F, 0x32, 0x35
// 0x2D, 0x30, 0x33, 0x36

    printf("On start\n");
    for (uint8_t i = 0; i < 4; i++) {
        printf("ParseBuffer[%d].keyCode: %02X\t", i, parseBuffer[i].keyCode);
        printf("flags: %02X\t", parseBuffer[i].flags);
        printf("cnt: %02X\n", parseBuffer[i].cnt);
        
    }

    for (uint8_t i = 0; i < 4; i++) {
        bitClear(parseBuffer[i].flags, bit4);    
    }

    printf("After bit4 clear (1st FOR)\n");
    for (uint8_t i = 0; i < 4; i++) {
        printf("ParseBuffer[%d].keyCode: %02X\t", i, parseBuffer[i].keyCode);
        printf("flags: %02X\t", parseBuffer[i].flags);
        printf("cnt: %02X\n", parseBuffer[i].cnt);
        
    }

    if ( keyCount != 0 ) {
        for (uint8_t k = 0; k < keyCount; k++) {
            for (uint8_t i = 0; i < 4; i++) {
                if ( parseBuffer[i].flags != 0 ) {
                    if ( parseBuffer[i].keyCode == keyMatrixBuffer[k] ) {
                        R3b0 = parseBuffer[i].flags;

                        if ( bitRead(R3b0, bit3) != 0) {
                            parseBuffer[i].cnt--;
                            if ( parseBuffer[i].cnt == 0 ) {
                                tmp8 = translateScanCode(keyMatrixBuffer[k], unknown[RAM0x29]);     // this is call
                                R3b0 &= 0xE3;
                                sendData(tmp8, R3b0);        // this is call
                                printf("SendData1\tdata: %02X\tFlags: %02X\n", tmp8, R3b0);
                                R3b0 &= 0xF0;
                                bitSet(R3b0, bit3);
                                parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                            }
                            //goto label_279;
                            bitSet(R3b0, bit4);
                            parseBuffer[i].flags = R3b0;
                            keyMatrixBuffer[k] = 0xFF;
                            break;
                        }

                        if ( bitRead(R3b0, bit2) != 0) {
                            parseBuffer[i].cnt--;
                            if ( parseBuffer[i].cnt == 0 ) {
                                if ( bitRead(globalState, enableLongRoutine) != 0 ) {
                                    R3b0 &= 0xF0;
                                    bitSet(R3b0, bit1);
                                    bitSet(R3b0, bit4);
                                    //goto label_279;
                                    parseBuffer[i].flags = R3b0;
                                    parseBuffer[i].cnt = cpuConfig[keyPressDelay];
                                    keyMatrixBuffer[k] = 0xFF;
                                    break;

                                }
                            }
                            //goto label_279;
                            parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                            bitSet(R3b0, bit4);
                            parseBuffer[i].flags = R3b0;
                            keyMatrixBuffer[k] = 0xFF;
                            break;
                        }

                        if ( bitRead(R3b0, bit1) != 0) {
                            parseBuffer[i].cnt--;
                            if ( parseBuffer[i].cnt == 0 ) {
                                // bits 0-4 in R3b0 must be equal to unknown[unknown2]
                                // bits 5-7 doesn't matter;
                                // Strange checking, because in unknown[unknown2] altered only bits 5-7
                                if ( (R3b0 & 0x1F) == unknown[RAM0x29] ) {
                                    tmp8 = translateScanCode(keyMatrixBuffer[k], unknown[RAM0x29]);         // this is call
                                    R5b0 = R3b0 & 0xE0;
                                    bitSet(R5b0, bit4);
                                    sendData(tmp8, R5b0);                   // this is call
                                    printf("SendData2\tdata: %02X\tFlags: %02X\n", tmp8, R5b0);
                                    parseBuffer[i].cnt = cpuConfig[keyPressDelay];
                                    bitSet(R3b0, bit4);
                                    parseBuffer[i].flags = R3b0;
                                    keyMatrixBuffer[k] = 0xFF;
                                    break;

                                }
                                R3b0 = (parseBuffer[i].cnt & 0xE0 );     // strange? cnt instead of flags? 0x2D, 0x30, 0x33, 0x36
                                //R3b0 = (parseBuffer[i].flags & 0xE0 );     // strange? cnt instead of flags? 0x2D, 0x30, 0x33, 0x36
                                bitSet(R3b0, bit2);
                            }
                            //goto label_279;
                            parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                            bitSet(R3b0, bit4);
                            parseBuffer[i].flags = R3b0;
                            keyMatrixBuffer[k] = 0xFF;
                            break;
                        }

                        //label_279:
                        parseBuffer[i].cnt = 1;
                        R3b0 &= 0xE0;
                        bitSet(R3b0, bit3);
                        bitSet(R3b0, bit4);
                        parseBuffer[i].flags = R3b0;
                        keyMatrixBuffer[k] = 0xFF;
                        break;
                    }
                }
            }   // break point here
        }
    }

    printf("After 1st IF\n");
    for (uint8_t i = 0; i < 4; i++) {
        printf("ParseBuffer[%d].keyCode: %02X\t", i, parseBuffer[i].keyCode);
        printf("flags: %02X\t", parseBuffer[i].flags);
        printf("cnt: %02X\n", parseBuffer[i].cnt);
        
    }

    for (uint8_t i = 0; i < 4; i++) {
        if ( (parseBuffer[i].flags != 0) && (bitRead(parseBuffer[i].flags, bit4) == 0) ) {
            if ( bitRead(parseBuffer[i].flags, bit0) != 0 ) {
                parseBuffer[i].cnt --;
                if ( parseBuffer[i].cnt == 0 ) {
                    parseBuffer[i].flags = 0;   // really clear all flags ???
                    pinSetNMI(LOW);
                }
                continue;
            }
            parseBuffer[i].flags = (parseBuffer[i].flags & 0xF0);
            bitSet(parseBuffer[i].flags, bit0);
            parseBuffer[i].cnt = 2;     // why 2 ???
        }
    }

    printf("After 2nd FOR\n");
    for (uint8_t i = 0; i < 4; i++) {
        printf("ParseBuffer[%d].keyCode: %02X\t", i, parseBuffer[i].keyCode);
        printf("flags: %02X\t", parseBuffer[i].flags);
        printf("cnt: %02X\n", parseBuffer[i].cnt);
        
    }

    if ( keyCount != 0 ) {
        for (uint8_t k = 0; k < keyCount; k++) {
            if ( (keyMatrixBuffer[k] + 1) != 0) {
                if ( keyMatrixBuffer[k] == 0x10 ) {      // Check for Ctrl key pressed
                    continue;
                }
                uint8_t i = 0;
                    label_19B:
                if ( parseBuffer[i].flags == 0 ) {
                    parseBuffer[i].keyCode = keyMatrixBuffer[k];
                    parseBuffer[i].flags = (unknown[RAM0x29] & 0xE0);
                    bitSet(parseBuffer[i].flags, bit3);
                    parseBuffer[i].cnt = 1;
                    for (uint8_t j = 0; j < 4; j++) {
                        if ( ( parseBuffer[j].flags != 0 ) && ( bitRead(parseBuffer[j].flags, 3) == 0 ) && ( bitRead(parseBuffer[j].flags, 0) == 0) ) {     // 0x2C, 0x2F, 0x32, 0x35
                            parseBuffer[j].flags = (parseBuffer[j].flags & 0xF0);
                            bitSet(parseBuffer[j].flags, bit2);
                            parseBuffer[j].cnt = cpuConfig[keyReapeatRate];
                        }
                    }
                    continue;
                }
                i++;                                                // next element
                    goto label_19B;
            }
        }       // continue point
    }

    printf("On return (after 2nd IF)\n");
    for (uint8_t i = 0; i < 4; i++) {
        printf("ParseBuffer[%d].keyCode: %02X\t", i, parseBuffer[i].keyCode);
        printf("flags: %02X\t", parseBuffer[i].flags);
        printf("cnt: %02X\n", parseBuffer[i].cnt);
        
    }

}

void clean_146() {

    uint8_t R0b0, R1b0, R2b0, R3b0, R4b0, R5b0, R6b0, R7b0;
    uint8_t accumulator, R4b1, tmp8;
    uint16_t tmp16;
    //uint8_t RAM[0x40];
    uint8_t PORT[3];
    bool carry;

    printf("clean started\n");
        
    R0b0 = 0x2C;
    R4b0 = 0x04;
    while (R4b0 > 0) {
        bitClear(RAM[R0b0], bit4);
        R0b0 +=3;
        R4b0--;
    }

    //R1b0 = 0x37;
    //accumulator = RAM[0x37];
    if ( RAM[0x37] != 0 ) {
        R1b0 = 0x24;
        for (R7b0 = RAM[0x37]; R7b0 >0; R7b0--) {
            accumulator = RAM[R1b0];

            R2b0 = accumulator;
            
            R4b0 = 0x04;
            R0b0 = 0x2C;
            
            while (R4b0 > 0) {
                accumulator = RAM[R0b0];
                if ( accumulator != 0 ) {
                    R0b0--;

                    accumulator = RAM[R0b0];
                    accumulator = ~accumulator;
                    accumulator++;
                    tmp16 = (uint16_t)accumulator + (uint16_t)R2b0;	// carry affected!
                    accumulator = (uint8_t)(tmp16 & 0xFF);
                    if (tmp16 > 0xFF) carry = true;
                    carry = !carry;

                    if ( accumulator == 0 ) goto label_1EF;

                    R0b0++;
                }

                R0b0 += 3;
                R4b0--;
            }
            
            label_172:
            
            R1b0++;
            //R7b0--;
        }
    }

    R0b0 = 0x2C;
    R4b0 = 0x04;
    while (R4b0 > 0) {
        accumulator = RAM[R0b0];
        if ( accumulator != 0 ) {
            accumulator = ~accumulator;
            if ( bitRead(accumulator, 4) ) {
                accumulator = ~accumulator;
                if ( bitRead(accumulator, 0) ) goto label_1E1;
                accumulator &= 0xF0;
                accumulator |=0x01;
                RAM[R0b0] = accumulator;
                R0b0++;
                accumulator = 0x02;
                RAM[R0b0] = accumulator;

                goto label_180;

                label_1E1:
                R0b0++;
                accumulator = RAM[R0b0];
                accumulator--;
                if ( accumulator != 0 ) {
                    RAM[R0b0] = accumulator;
                    goto label_180;
                }

                R0b0--;
                RAM[R0b0] = accumulator;
                PORT[2] &= 0xBF;
                R0b0++;
                goto label_180;
            }
        }
        R0b0++;
        
        label_180:
        
        R0b0 += 2;
        R4b0--;
    }

    R1b0 = 0x24;
    if ( RAM[0x37] != 0 ) {
        accumulator = RAM[0x37];
        //while (R7b0 > 0 ) {
        for (R7b0 = accumulator; R7b0 >0; R7b0--) {
            accumulator = RAM[R1b0];
            accumulator++;
            if ( accumulator ) {
                if ( bitRead(RAM[R1b0], bit4) == 0) {
                    R1b0++;
                    //R7b0--;
                    continue;
                }
                
                R0b0 = 0x2C;

                label_19B:

                accumulator = RAM[R0b0];
                if ( accumulator == 0 ) {
                    R0b0--;
                    RAM[R0b0] = RAM[R1b0];
                    R0b0++;
                    R2b0 = R1b0;
                    RAM[R0b0] = RAM[0x29] & 0xE0;
                    bitSet(RAM[R0b0], bit3);
                    R0b0++;
                    RAM[R0b0] = 0x01;

                    R6b0 = 0x04;
                    R0b0 = 0x2C;
                    while (R6b0 > 0 ) {
                        accumulator = RAM[R0b0];
                        if ( (RAM[R0b0] != 0) && (bitRead(RAM[R0b0], 3) == 0) && (bitRead(RAM[R0b0], 0) == 0) ) {
                            RAM[R0b0] &= 0xF0;
                            bitSet(RAM[R0b0], bit3);
                            R0b0++;
                            RAM[R0b0] = RAM[0x21];
                            R0b0--;
                        }
                        R0b0 += 3;
                        R6b0--;
                    }
                    R1b0 = R2b0;
                    R1b0++;
                    //R7b0--;
                    continue;
                }

                R0b0 += 3;

                goto label_19B;
            }
            R1b0++;
            //R7b0--;
        }
    }

printf("Clean: keybuf:\t");
for (uint8_t i = 0x24; i < 0x27; i++) {
    printf("%02X:\t", RAM[i]);
}
printf("\n");
// 0x2B, 0x2E, 0x31, 0x34
// 0x2C, 0x2F, 0x32, 0x35
// 0x2D, 0x30, 0x33, 0x36
printf("Clean: parse0:\t");
for (uint8_t i = 0x2B; i < 0x2E; i++) {
    printf("%02X:\t", RAM[i]);
}
printf("\n");
printf("Clean: parse1:\t");
for (uint8_t i = 0x2E; i < 0x31; i++) {
    printf("%02X:\t", RAM[i]);
}
printf("\n");
printf("Clean: parse2:\t");
for (uint8_t i = 0x31; i < 0x34; i++) {
    printf("%02X:\t", RAM[i]);
}
printf("\n");
printf("Clean: parse3:\t");
for (uint8_t i = 0x34; i < 0x37; i++) {
    printf("%02X:\t", RAM[i]);
}
printf("\n");
printf("Clean: keyCount: %d\n", RAM[0x37]);
return;	// Return from subroutine. Check for correct call label!


label_1EF:
R0b0++;
accumulator = RAM[R0b0];
R3b0 = accumulator;
R0b0++;
accumulator = RAM[R0b0];
R6b0 = accumulator;
accumulator = R0b0;
// Bank1 in use
R4b1 = accumulator;
// Bank0 in use
accumulator = R3b0;
if ( bitRead(accumulator, 3) ) goto label_20F;
if ( bitRead(accumulator, 2) ) goto label_22B;
if ( bitRead(accumulator, 1) ) goto label_249;
accumulator &= 0xE0;
accumulator |=0x08;
R3b0 = accumulator;
accumulator = 0x01;

goto label_279;

label_20F:

accumulator = R6b0;
accumulator--;

if ( accumulator == 0 ) goto label_215;

goto label_279;

label_215:

accumulator = R2b0;
//label_28A();	// this is call, check for correct return address
//accumulator = translateScanCode(accumulator);
accumulator = translateScanCode(accumulator, RAM[0x29]);
R2b0 = accumulator;
accumulator = R3b0;
accumulator &= 0xE0;
R5b0 = accumulator;
accumulator = R2b0;
//label_2A7();	// this is call, check for correct return address
//R5b0 = sendData(accumulator, R5b0);
printf("clean:sendData1: data: %02X\tflags: %02X\n", accumulator, R5b0);
sendData(accumulator, R5b0);
accumulator = R5b0;
accumulator &= 0xF0;
accumulator |=0x04;
R3b0 = accumulator;
R0b0 = 0x21;
accumulator = RAM[R0b0];

goto label_279;

label_22B:

accumulator = R6b0;
accumulator--;

if ( accumulator == 0 ) goto label_231;

goto label_279;

label_231:

// Bank1 in use
accumulator = globalState;
// Bank0 in use
accumulator = ~accumulator;

if ( bitRead(accumulator, 3) ) goto label_239;

goto label_23E;

label_239:

R0b0 = 0x21;
accumulator = RAM[R0b0];

goto label_279;

label_23E:

accumulator = R3b0;
accumulator &= 0xF0;
accumulator |=0x02;
R3b0 = accumulator;
R0b0 = 0x22;
accumulator = RAM[R0b0];

goto label_279;

label_249:

accumulator = R6b0;
accumulator--;

if ( accumulator == 0 ) goto label_24F;

goto label_279;

label_24F:

accumulator = R3b0;
accumulator &= 0xE0;
R0b0 = 0x29;
accumulator = ~accumulator;
accumulator++;
tmp16 = (uint16_t)accumulator + (uint16_t)RAM[R0b0];	// carry affected!
accumulator = (uint8_t)(tmp16 & 0xFF);
if (tmp16 > 0xFF) carry = true;
carry = !carry;
if ( accumulator == 0 ) goto label_269;
accumulator = RAM[R0b0];
accumulator &= 0xE0;
accumulator |=0x04;
R3b0 = accumulator;
R0b0 = 0x21;
accumulator = RAM[R0b0];

goto label_279;

label_269:

accumulator = R2b0;
//label_28A();	// this is call, check for correct return address
accumulator = translateScanCode(accumulator, RAM[0x29]);
R2b0 = accumulator;
accumulator = R3b0;
accumulator &= 0xE0;
accumulator |=0x10;
R5b0 = accumulator;
accumulator = R2b0;
printf("clean:sendData2: data: %02X\tflags: %02X\n", accumulator, R5b0);
sendData(accumulator, R5b0);
accumulator = RAM[0x22];
goto label_279;

label_279:
tmp8 = R4b1;
R4b1 = accumulator;
accumulator = tmp8;
R0b0 = accumulator;
RAM[R0b0] = R4b1;
R0b0--;
bitSet(RAM[R0b0], bit4);
RAM[R1b0] = 0xFF;
goto label_172;
}
