/*
ROM dump was made by Conventional Memories 2024-02-01 13:51
Disassembled by JDat on 2024-02-01 19:12
Disassmbeled with custom version of 8039dasm.c tweaked for 8741A
Diassembler downloaded from https://github.com/daveho/asm48
8039dasm.c roots come from MAME project (see asm48 github repo for more information)

Known commands in CCPROM function cpCatchAll
catchWriteDelayRepeat:
    // set theData in RAM[0x21] and RAM[0x22]
    // used by long routine
    // keyboard repeat rate and repeat delay?
    kbcSend(0x81, commandPort);     // 0b1000 0001
    kbcSend(theData, dataPort);
    kbcSend(0x82, commandPort);     // 0b1000 0010
    kbcSend(theData, dataPort);
    break;
catchRepeat:
    // repeat last key or last 3 keys?
    // interract with long routine
    kbcSend(0x08, commandPort);     // 0b0000 1000
    break;
catchKeyboard:
    // enable keyboard scan
    kbcSend(0x01, commandPort);     // 0b0000 0001
    break;
catchWatchdog:
    // disable? watchdog?
    kbcSend(0x02, commandPort);     // 0b0000 0010
    break;
catchDma:
    // bit7 must be clear.
    // will set pinPAL
    // other bits change global state
    kbcSend(0x10, commandPort);     // 0b0001 0000
    break;
catchResetWatchDog:
    kbcSend(0xC0, commandPort);     // 0b1100 0000
    break;
catchSetWatchDog:
    // interract with watchog counter
    // set theData in RAM[0x23]
    kbcSend(0x83, commandPort);     // 0b1000 0011
    kbcSend(theData, dataPort);


CPU to KBC protocol:
Address A0 clear - data
Address A0 set - command byte
Commands:
bit 7 clear: change master states (See RAM[0x1F] aga R7 bank 1)
* bit 0 - disable/enable keyboard scanning
* bit 1 - WDT1 related
* bit 2 - WDT0 related
* bit 3 - related (long routine). repeat last key? Uset in CpCatchRepeat CCPROM
* bit 4 - state for pinPAL (DMA)
* bit 5 - not used
* bit 6 - not used if bit 7 - clear

bit 7 set, bit 6 clear: data will follow on data port
* bits 0-2 (0,1,2,3) - write into one of config parameter registers (located in RAM 0x20-0x23)
* 0 - related to WDT0, Not used in CCPROM for 1101.
* 1 - key repeat rate/delay between key press
* 2 - key repeat rate/delay between key press
* 3 - related to WDT1. Used in CCPROM code to set WDT1 timeout

Other data: simply restart WDT1 to prevent WDT1 from activating.

KBC to CPU protocol:
* Will rise interrupt when KBC want send data to CPU.
* unknown parameters for bits 4-7 in bus status register
* 0xFF - WDT0 timeout occured
* 0xFE - mentioned in CCPROM. Not found in KBC code
* 0xFD - WDT1 timeout occured
* other - key scan code. status bits 5-7 probably provide modifier keys state
* can send remainig data from key buffer if they exist. Check/send on timer IRQ.
Mechanism not know yet.

PORT 2 Pin 0 - 3 bits to decoderIC for keyboard
PORT 2 Pin 1 - 3 bits to decoderIC for keyboard
PORT 2 Pin 2 - 3 bits to decoderIC for keyboard
PORT 2 Pin 3 - Interrupt pin, goes to 80130 IRQ 4
PORT 2 Pin 4 - Interrupt pin, goes to PAL
PORT 2 Pin 5 - Not connected
PORT 2 Pin 6 - Interrupt pin, goes to CPU NMI
PORT 2 Pin 7 - Not connected on PCB, maybe for KBC debugging?
*/

/*
RAM is removed from code and subsituted with variables and arrays

RAM 0x00 - R0 bank0 - Used as index for accessing RAM or temp storage
RAM 0x01 - R1 bank0 - Used as index for accessing RAM or temp storage
RAM 0x02 - R2 bank0 - Used as temp storage or as counters in loops
RAM 0x03 - R3 bank0 - Used as temp storage or as counters in loops
RAM 0x04 - R4 bank0 - Used as temp storage or as counters in loops
RAM 0x05 - R5 bank0 - Used as temp storage or as counters in loops
RAM 0x06 - R6 bank0 - Used as temp storage or as counters in loops
RAM 0x07 - R7 bank0 - Used as temp storage or as counters in loops

RAM 0x08 - stack cell 0
RAM 0x09

RAM 0x0A - stack cell 1
RAM 0x0B

RAM 0x0C - stack cell 2
RAM 0x0D

RAM 0x0E - stack cell 3
RAM 0x0F

RAM 0x10 - stack cell 4
RAM 0x11

RAM 0x12 - stack cell 5
RAM 0x13

RAM 0x14 - stack cell 6
RAM 0x15

RAM 0x16 - stack cell 7
RAM 0x17

RAM 0x18 - R0 bank1 - Used as index for accessing RAM or temp storage
RAM 0x19 - R1 bank1 - Used as index for accessing RAM or temp storage
RAM 0x1A - R2 bank1 - timer overflow counter (when zero, do more) set to 2 when zero
RAM 0x1B - R3 bank1 - Used as temp storage or as counters in loops
RAM 0x1C - R4 bank1 - Used by "long routine" to store something
RAM 0x1D - R5 bank1 - Sometimes used as flag register before sending data to CPU
RAM 0x1E - R6 bank1 - related to watchdog
RAM 0x1F - R7 bank1 - global, can be set from CPU
    bit7 - set in timerIRQ when timer overflow  2
    bit6 -
    bit5 -
    bit4 - pinPAL status contrlled by CPU
    bit3 - used in "long routine"
    bit2 - watchdog related stuff
    bit1 - decrement RAM[0x39]; watchdog related stuff
    bit0 - keyboard scan enable/disable

These 4 locations can be set by CPU
RAM 0x20 - Watchog counter?
RAM 0x21 - Keyboard repeat rate? used in "long sub"
RAM 0x22 - Keyboard repeat rate? used in "long sub"
RAM 0x23 - Watchog counter? Set in INIT, used in ExtIRQ

RAM 0x24 - keyMatrix buffer?
RAM 0x25 - keyMatrix buffer?
RAM 0x26 - keyMatrix buffer?

RAM 0x27 - Related to ST flags in BUS status register
RAM 0x28 - Related to ST flags in BUS status register

RAM 0x29 - Modifier key status or old ST flags
    PORT1-pin0, TEST1, TEST0 states in bits 7,6,5
    also these bits are sent to CPU as ST 7,6,5 flags
    Write only in _11A_sub; Read in many places
RAM 0x2A - Used only in _11A_sub. Related to keyboard modifier keys.

RAM 0x2B - Some kind of buffer up to 0x36?
RAM 0x2C -
RAM 0x2D -

RAM 0x2E -
RAM 0x2F -
RAM 0x30 -

RAM 0x31 -
RAM 0x32 -
RAM 0x33 -

RAM 0x34 -
RAM 0x35 -
RAM 0x36 -

RAM 0x37 - Key count after matrix scan?
RAM 0x38 - Pointer to CPU accesable registers (0x20-0x23); now cpuDataPtr
RAM 0x39 - Watchdog related. Used in main loop and ExtIRQ vector

RAM 0x3A - not used
RAM 0x3B - not used
RAM 0x3C - not used
RAM 0x3D - not used
RAM 0x3E - not used
RAM 0x3F - not used
*/

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

#include "hwInterface.h"
#include "keyMatrix.h"
#include "keymap.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
// need to reduce
// used in extIRQ for parsing
#define bit6 6
#define bit7 7


void kbcRun();
void kbcSetup();
void kbcLoop();

void vectTimerIRQ();
void vectExtIRQ();
uint8_t sendData(uint8_t data, uint8_t flags);
void readModKeys();
void readKeyMatrix();
uint8_t translateScanCode(uint8_t data, uint8_t modKeys);
void sendKey();

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



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

uint8_t currentModFlags, savedModFlags;
uint8_t keyMatrixBuffer[4];
uint8_t keyCount;

#define unknown0 0
#define unknown1 1
uint8_t unknown[2];

#define WDT0data 0
#define keyReapeatRate 1
#define keyPressDelay 2
#define WDT1data 3
uint8_t cpuConfig[4];

volatile uint8_t wdt1Count;

#define enableKeyScan 0         // Keyboard scan enabled
#define enableWDT1 1            // WDT1 enable
#define enableWDT0 2            // WDT0 enable
#define enableLongRoutine 3     // used in "long routine"
#define enablePinPAL 4          // pinPAL status controlled by CPU
#define timerOverFlowFlag 7     // set in timerIRQ when timer overflow
uint8_t globalState;

uint8_t cpuDataPtr;

uint8_t searchSDLkey(uint16_t data) {
    for (uint8_t i = 0; i < sizeof(keyTranslateMatrix) / sizeof(keyTranslateMatrix[0]); i++) {
        uint8_t a;
        a = i << 2;
        if ( data == keyTranslateMatrix[i].unshifted ) return a;
        if ( data == keyTranslateMatrix[i].shift ) return a + 1;
        if ( data == keyTranslateMatrix[i].code) return a + 2;
        if ( data == keyTranslateMatrix[i].codeShift) return a + 3;
    }
    return 0xFF;
}

void testKeyMatrix() {
    uint8_t a, b, index, modKey;
    for (uint8_t i = 0; i < 64; i++) {
        for (uint8_t j = 0; j < 4; j++) {
            a = translateScanCode(i, j << 5);
            if (a == 0xFF) continue;
            b = searchSDLkey(a);
            index = b >> 2;
            modKey = b & 0x3;

            printf("i: %02X\tj: %02X\txlat: %02X\t", i, j, a);
            printf("idx: %d\tkey: %s\t", index, keyTranslateMatrix[index].name);
            switch(modKey) {
                case 0:
                    if ( a != keyTranslateMatrix[index].unshifted) printf("ERROR!\t");
                    if ( (a & 0x9F) != keyTranslateMatrix[index].ctrl) printf("CTRL ERROR!\t");
                    printf(" ");
                    break;
                case 1:
                    if ( a != keyTranslateMatrix[index].shift) printf("SHIFT ERROR!\t");
                    if ( (a & 0x9F) != keyTranslateMatrix[index].shiftCtrl) printf("CTRL+SHIFT ERROR!\t");
                    printf("SHIFT");
                    break;
                case 2:
                    if ( a != keyTranslateMatrix[index].code) printf("CODE ERROR!\t");
                    if ( (a & 0x9F) != keyTranslateMatrix[index].codeCtrl) printf("CTRL+CODE ERROR!\t");
                    printf("CODE");
                    break;
                case 3:
                    if ( a != keyTranslateMatrix[index].codeShift) printf("CODE+SHIFT ERROR!\t");
                    if ( (a & 0x9F) != keyTranslateMatrix[index].codeShiftCtr) printf("CTRL+CODE+SHIFT ERROR!\t");
                    printf("CODE+SHIFT");
                    break;

            }
            printf("\n");
        }
    }
}

int main(int argc, char *argv[]) {
    // nothing here for now

}

//======================================================================
void vectTimerIRQ() {                   // timer interrupt
    static uint8_t overflowCount;

    stopTimer();

    // Can we send data?
    if (bitRead(busStatus, OBF) == false) {

        portTmp = portRead(PORT2);
        bitClear(portTmp, pinIRQ4);
        portWrite(portTmp, PORT2);

        if ( unknown[unknown1]) {
            busWrite(unknown[unknown0]);
            busStatus = unknown[unknown1];
            unknown[unknown1] = 0;

            portTmp = portRead(PORT2);
            bitSet(portTmp, pinPAL);
            portWrite(portTmp, PORT2);

        } else {
            busStatus = currentModFlags;
        }
    }

    timer = 0xAD;

    startTimer();

    // wdt0 related
    if (bitRead(globalState, enableWDT0) != 0) {
        static uint8_t wdt0Count;
        if (wdt0Count == 0) {
            wdt0Count = cpuConfig[WDT0data];
            sendData(0xFF, currentModFlags);
        }
        wdt0Count--;
    }

    // count/realod timer overflows and set bit7 when reached
    overflowCount--;
    if (overflowCount == 0) {
        overflowCount = 0x02;
        bitSet(globalState, timerOverFlowFlag);
    }
    return;                             // return from timer interrupt
}

//======================================================================
// simulates arduino style flow
void kbcRun() {
    kbcSetup();
    while (true) {
        kbcLoop();
    }
}

//======================================================================
// simulates arduino style flow setup()
void kbcSetup() {
    portTmp = portRead(PORT2);          // set many pins on PORT2
    portTmp &= 0x37;
    portWrite(portTmp, PORT2);

    globalState = 0;                    // master program states

    cpuConfig[WDT0data] = 1;
    cpuConfig[keyReapeatRate] = 0x0F;
    cpuConfig[keyPressDelay] = 2;
    cpuConfig[WDT1data]= 50;

    wdt1Count = cpuConfig[WDT1data];

    unknown[0] = 0;     // clear veird bits, including 0x27
    unknown[1] = 0;     // clear veird bits, including 0x28

    keyCount = 0;

    timer = 0xAD;
    portTmp = portRead(PORT2);
    bitSet(portTmp, pinWTF);            // debug pin active: ready to go?
    portWrite(portTmp, PORT2);

    startTimer();
    enableExternalInterrupt();
    enableTimerInterrupt();
}

//======================================================================
// simulates arduino style flow loop()
void kbcLoop() {
        // why need to jerk interrupt enable? maybe NOP is better?
        enableExternalInterrupt();
        disableExternalInterrupt();

        // no timer overflow - no party. just loop again
        if (bitRead(globalState, timerOverFlowFlag) == 0) return;

        bitClear(globalState, timerOverFlowFlag);    // clear bit7 - we parsing this!
        enableExternalInterrupt();

        // wdt1 related
        // decrement RAM[0x39]
        if (bitRead(globalState, enableWDT1) == 1) {
            wdt1Count--;

            // if RAM[0x39] = 0, clear pinWTF and send something to CPU
            if (wdt1Count == 0) {
                // need to reload or will overflow and become 0xFF
                wdt1Count = cpuConfig[WDT1data];    // not in original code

                portTmp = portRead(PORT2);
                bitClear(portTmp, pinWTF);
                portWrite(portTmp, PORT2);

                sendData(0xFD, 0);
            }
        }

        if (bitRead(globalState, enableKeyScan) != 0) {
            readModKeys();
            readKeyMatrix();
            // 2 or 3, not shure
            if (keyCount < 4) sendKey();   // need keyCount
        }
}

//======================================================================
void vectExtIRQ() {                     // external interrupt
    uint8_t a;

    // check for data/command mode
    if (bitRead(busStatus, F1) == 0) {  // data mode
        //ramWrite(busRead(), cpuDataPtr);    // RAM[0x38] store recived data in (0x20-0x23)
        cpuConfig[cpuDataPtr] = busRead();
        return;                         // return from IRQ
    }

    // we are in command mode
    a = busRead();
    if (a == 0 ) {
        disableExternalInterrupt();
        return;                         // return from IRQ
    }

    // if bit7 clear in data from CPU, change master program state.
    if (bitRead(a, bit7) == 0) {
        globalState = a;
        a = a & 0x10;                   // extract pinPAL state from CPU data

        portTmp = portRead(PORT2);
        bitWrite(portTmp, pinPAL, a);   // set pinPAL in state from CPU data
        portWrite(portTmp, PORT2);

        return;                         // return from IRQ
    }

    if (bitRead(a, bit6) == 0) {
        //cpuDataPtr = (busRead() & 0x03) | 0x20; // RAM[0x38] data: 0x20-0x23
        cpuDataPtr = 0;
        return;                         // return from IRQ
    }

    // reset wdt1
    wdt1Count = cpuConfig[WDT1data];
}

void readModKeys() {
    uint8_t modFlags = 0;

    // Modifier key part
    // TEST1 pin low - key pressed. set bit6 flag
    if (TEST1 == false) bitSet(modFlags, flagCodeKey);

    // TEST0 pin low - key pressed. set bit5 flag
    if (TEST0 == false) bitSet(modFlags, flagShiftKey);

    // set 3-to-8 decoder to correct column for Ctrl key
    portTmp = portRead(PORT2);
    portTmp = (portTmp & 0xF8) | 0x02;  // clear pin3, set pin1, clear pin0, save other pins
    portWrite(portTmp, PORT2);

    // if pin0 low, key is pressed
    if ( bitRead(portRead(PORT1), pin0) == 0) bitSet(modFlags, flagCtrlKey);

    if (savedModFlags == modFlags) {
        currentModFlags = modFlags;
    } else {
        savedModFlags = modFlags;
    }
}

//======================================================================
// call from main loop
void readKeyMatrix() {                  // read keyboard matrix
    // 8x8 matrix scan for key
    //for (uint8_t row = 8; row > 0; row--) {
    for (uint8_t row = 0; row < 8; row++) {
        uint8_t a;
        // set pins 2, 1, 0
        portTmp = portRead(PORT2);
        //portTmp = (portTmp & 0xF8) | (row & 0x7);   // mix column to existing PORT2 pin state
        portTmp = (portTmp & 0xF8) | row ;   // mix column to existing PORT2 pin state
        portWrite(portTmp, PORT2);

        a = ~portRead(PORT1);           // read PORT1 and invert because pullups and active low
        if (a == 0) break;          // if a = 0, no key pressed in column, skip row loop

        //for (uint8_t column = 8; column > 0; column--) {
        for (uint8_t column = 0; column < 8; column++) {
            uint8_t x;
            //x = bitRead(a, 8 - column);
            x = bitRead(a, column);
            if (x == true) {
                keyCount++;

                if (keyCount < 4) {     // 2 or 3 not shure
                    a = (row << 3) | column;
                    // rows in bits 5, 4, 3
                    // columns in bits 2, 1, 0
                    keyMatrixBuffer[keyCount] = a; // dirty hack, may have problems
                }
            }
        }
    }
}

//======================================================================
// call from sendKey sub 2 times
uint8_t translateScanCode(uint8_t data, uint8_t modKeys) {

    uint8_t a, keyMatrixResult;
    a = (modKeys >> 5) & 0x03;          // modifier key Code and Shift states to bits 1,0
    a |= (data << 2) & 0xFC;            // keyMatrix code in bits 7-2
    keyMatrixResult = keyMatrix[a];
    if ( bitRead(modKeys, flagCtrlKey) ) keyMatrixResult &= 0x9F;  // Ctrl key pressed

    return keyMatrixResult;
}

//======================================================================
// call from:
// timer irq
// main loop
// sendKey call from 2 places
uint8_t sendData(uint8_t data, uint8_t flags) {

    if ( data == 0xFE ) {           // NMI when Shift+Code+ESC ?
        portTmp = portRead(PORT2);
        bitSet(portTmp, pinNMI);
        portWrite(portTmp, PORT2);
        return flags;
    }

    if (OBF == false) {
        portTmp = portRead(PORT2);
        bitClear(portTmp, pinIRQ4);
        portWrite(portTmp, PORT2);

        busWrite(data);
        busStatus = flags;

        portTmp = portRead(PORT2);
        bitSet(portTmp, pinIRQ4);
        portWrite(portTmp, PORT2);

        return flags;
    }

    if (unknown[unknown1]) {
        unknown[unknown0] = 0xFE;
        unknown[unknown1] = data;
    } else {
        unknown[unknown0] = data;
        unknown[unknown1] = flags;
    }
    return flags;
}

//=======================================================================
// call from main loop
// shitty long background function
// analyzing keys and send to CPU
// can clear NMI
// use some kind of buffer struct
// somehow interract with key repeat rate
// so far buffer is global
void sendKey() {
    static struct _keyParseEntry parseBuffer[4];

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

    if (keyCount) {              // check if there is any key in buffer
        for (uint8_t j = 0; j <= keyCount; j++) {
            for (uint8_t i = 0; i < 4; i++) {
                if (parseBuffer[i].flags != 0 ) {

                    if (keyMatrixBuffer[j] == parseBuffer[i].keyCode ) {    // break
                        uint8_t a, _flags, cnt;
                        _flags = parseBuffer[i].flags;
                        cnt = parseBuffer[i].cnt;
                        a = _flags;

                        if (bitRead(_flags, bit3) == 1) {   // break
                            cnt--;
                            if (cnt == 0) {               // break
                                uint8_t sendFlags, _keyCode;
                                
                                _keyCode = translateScanCode(keyMatrixBuffer[j], currentModFlags);
                                
                                sendFlags = _flags & 0xE0;
                                
                                sendFlags = sendData(_keyCode, sendFlags);
                                
                                _flags = sendFlags & 0xF0;
                                bitSet(_flags, bit2);
                                bitSet(_flags, bit4);
                                parseBuffer[i].flags = _flags;
                                parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                                keyMatrixBuffer[j] = 0xFF;
                                break;
                            }
                            bitSet(_flags, bit4);
                            parseBuffer[i].flags = _flags;
                            parseBuffer[i].cnt = a;
                            keyMatrixBuffer[j] = 0xFF;
                            break;
                        }

                        if (bitRead(_flags, bit2) == 1) {   // break
                            cnt--;
                            if (cnt == 0 ) {              // break
                                if (bitRead(globalState, bit3) == 0) {  // break
                                    bitSet(_flags, bit4);
                                    parseBuffer[i].flags = _flags;
                                    parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                                    keyMatrixBuffer[j] = 0xFF;
                                    break;
                                }
                                _flags = _flags & 0xF0;
                                bitSet(_flags, bit1);
                                bitSet(_flags, bit4);
                                parseBuffer[i].flags = _flags;
                                parseBuffer[i].cnt = cpuConfig[keyPressDelay];
                                keyMatrixBuffer[j] = 0xFF;
                                break;
                            }
                            bitSet(_flags, bit4);
                            parseBuffer[i].flags = _flags;
                            parseBuffer[i].cnt = a;
                            keyMatrixBuffer[j] = 0xFF;
                            break;
                        }

                        if (bitRead(_flags, bit1) == 1) {   // break
                            cnt--;
                            if (cnt == 0) {               // break
                                if (_flags == currentModFlags) {           // break
                                    uint8_t sendFlags, _keyCode;
                                    
                                    _keyCode = translateScanCode(keyMatrixBuffer[j], currentModFlags);
                                    
                                    sendFlags = (_flags & 0xE0) | 0x10;
                                    
                                    sendData(_keyCode, sendFlags);
                                    
                                    bitSet(_flags, bit4);
                                    parseBuffer[i].flags = _flags;
                                    parseBuffer[i].cnt = cpuConfig[keyPressDelay];
                                    keyMatrixBuffer[j] = 0xFF;
                                    break;
                                }
                                parseBuffer[i].flags = (currentModFlags & 0xE0) | 0x14; // save bits 5,6,7; set bits 2, 4; clear bits 0,1,3
                                parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                                keyMatrixBuffer[j] = 0xFF;
                                break;                  // break
                            }
                            bitSet(_flags, bit4);
                            parseBuffer[i].flags = _flags;
                            parseBuffer[i].cnt = a;
                            keyMatrixBuffer[j] = 0xFF;;
                            break;
                        }

                        _flags = ( a & 0xE0 ) | 0x18;   // save bits 5,6,7; set bits 3,4; clear bits 0,1,2
                        parseBuffer[i].flags = _flags;
                        parseBuffer[i].cnt = 0x01;
                        keyMatrixBuffer[j] = 0xFF;
                        break;
                    }
                }
            }
            //break point for IFs
        }
    }

    // manipulate flags
    // decrement or set cnt
    // clear NMI
    // who set bit0 flag???
    // maybe my error when translating or bug in original firmware?
    for (uint8_t i = 0; i < 4; i++) {
        if (parseBuffer[i].flags != 0 ) {   // skip if all flags clear
            if (bitRead(parseBuffer[i].flags, bit4) == 0 ) {    // skip if bit4 set
                if (bitRead(parseBuffer[i].flags, bit0) == 0) { // skip if bit0 set
                    if (parseBuffer[i].cnt == 0xFF ) {
                        parseBuffer[i].flags = 0;

                        portTmp = portRead(PORT2);
                        bitClear(portTmp, pinNMI);      // clear NMI pin
                        portWrite(portTmp, PORT2);
                        continue;           // next buffer element
                    }
                    parseBuffer[i].cnt--;
                    continue;           // next buffer element continue;
                }
                parseBuffer[i].flags = parseBuffer[i].flags & 0xF0;
                bitSet(parseBuffer[i].flags, bit0);
                parseBuffer[i].cnt = 0x02;
                continue;           // next buffer element continue;
            }
        }
    }

    if (keyCount) {              // check if there is any key in keyMatrixBuffer
        for (uint8_t j = 0; j <= keyCount; j++) {   //process every key in keyMatrixBuffer
            if (keyMatrixBuffer[j] != 0xFF ) {     // 0xFF - invalid key (matrix). Codes can be 0 - 64
                if (keyMatrixBuffer[j] != 0x10) {   // no idea why. Maybe skip if Ctrl key pressed?

                    // more work on this "for" searching loop
                    // searching for element with all flags clear
                    // e contains number to found element
                    uint8_t e = 3;      // or e = 0
                    //for (e = 0; e < 4; e++) {
                    for (uint8_t i = 0; i < 4; i++) {   // 0x2C
                        if (parseBuffer[i].flags == 0) {
                            e = i;
                            break;
                        }
                    }

                    parseBuffer[e].keyCode = keyMatrixBuffer[j];
                    parseBuffer[e].flags = currentModFlags & 0xE0;
                    bitSet(parseBuffer[e].flags, bit3);
                    parseBuffer[e].cnt = 0x01;
                    for (uint8_t i = 0; i < 4; i++) {
                        uint8_t a;
                        a = parseBuffer[i].flags;
                        if ( (a != 0) && (bitRead(a, bit3) == 0) && (bitRead(a, bit4) == 0) ) {
                            a = a & 0xF0;
                            bitSet(a, bit2);
                            parseBuffer[i].flags = a;
                            parseBuffer[i].cnt = cpuConfig[keyReapeatRate];
                        }
                    }
                }
            }
        }
    }

    return;
}

