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

#include "opcodes.h"
#include "sdlconsole.h"

#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))

//#define termW 85
//#define termH 44
#define termW 60
#define termH 40

volatile bool systemRunning = true;
volatile bool emuRunning = true;

uint8_t tickcount;

//const uint16_t ROMsize = 1024;
#define ROMsize 1024
uint8_t ROM[ROMsize];
bool retrExecuted;

//const uint16_t RAMsize = 64;
#define RAMsize 64
uint8_t RAM[RAMsize];

uint16_t progCounter;
uint8_t accumulator;
bool globalIrqEnabled;

uint8_t port1 = 0xFF;
uint8_t port2 = 0xFF;

bool test0 = true;
bool test1 = true;

#define bOBF 0
#define bIBF 1
#define bF0  2
#define bF1  3
#define bST4 4
#define bST5 5
#define bST6 6
#define bST7 7

bool extIrqEnabled;
uint8_t busStatus, busIn, busOut;


bool timerIrqEnabled;
bool timerOverflow;
bool timerEnabled;
uint8_t timerPrescaler;
uint8_t timer;

uint8_t PSW;

#define bank  4
#define pF0   5
#define auxC  6
#define carry 7

void delayMicroseconds(uint64_t amount) {
    
    uint64_t lasttime;
    lasttime = timing_getCur();
    while (timing_getCur() - lasttime < amount) {
        ;
    }

}
long loadROM(char *fileName) {
    long ret, fileSize;
    FILE *ptrFile;
    ptrFile = fopen(fileName,"rb");
    if (!ptrFile) {
        printf("Unable to open file!\n");
        exit(-1);
    }

    ret = (long)fseek(ptrFile, 0, SEEK_END);
    if (ret) {
        printf("Problems while seek to end! fseek() err code: %ld\n", ret);
        exit(-1);
    }

    fileSize = ftell(ptrFile);
    if (fileSize != ROMsize) {
        printf("File size incorrect! ftell() err code: %ld\n", fileSize);
        exit(-1);
    }

    ret = (long)fseek(ptrFile, 0, SEEK_SET);
    if (ret) {
        printf("Problems while seek to begin! fseek() err code: %ld\n", ret);
        exit(-1);
    }

    //FIXME: error handling for fread
    ret = (long)fread(ROM, sizeof(uint8_t), fileSize, ptrFile);

    fclose(ptrFile);
    return ret;
}

uint16_t getMaskPattern(int index) {
    char strMask[9];
    uint8_t mask = 0, pattern = 0;
    char tmp[2];

    (void)strncpy(strMask, opcodes[index].pattern, 8);

    for (int i = 0; i < 8; i++) {
        (void)strncpy(tmp, strMask+i, 1);
        if (tmp[0] == '1' || tmp[0] == '0') {
            mask |= 1 << (7 - i);
        }
        if (tmp[0] == '1' ) {
            pattern |= 1 << (7 - i);
        }
    }
    return ((uint16_t)(mask << 8)) | (uint16_t)pattern;
}

int searchOpcode(uint8_t opcode_) {
    int i = 0;

    while (strcmp(opcodes[i].pattern, "eof") != 0) {
        uint8_t mask, pattern;
        uint16_t tmp;

        tmp = getMaskPattern(i);
        mask = (uint8_t)((tmp & 0xFF00) >> 8);
        pattern = (uint8_t)(tmp & 0xFF);
        if ((mask & opcode_) == pattern) {
            //printf("i: %d\tval: %s\t%s\n", i, opcodes[i].pattern, opcodes[i].diasm);
            return i;
        }
        i++;
    }
    return -1;
}

void execOpcode(uint8_t opcode_, int index) {
    int tmp;
    uint8_t reg;
    switch (index) {
        case 0:                 // add a, reg
            tmp = (int)accumulator;
            reg = opcode_ & 0b111;
            tmp += RAM[reg + bitRead(PSW,bank) * 24];
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            //printf("PC:%03X\tidx:%d\top:%02X\tadd a reg%d\tresult:%02X\n", progCounter, index, opcode_, reg, accumulator);
            progCounter++;
            break;
        case 1:                 // add a, RAM[reg]
            tmp = (int)accumulator;
            reg = opcode_ & 0b1;
            tmp += RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            //printf("PC:%03X\tidx:%d\top:%02X\tadd a RAM[%d]=%02X\tresult: a=%02X\n", progCounter, index, opcode_, reg, RAM[ RAM[reg+bank*24] & 0b111111], accumulator);
            break;
        case 2:                 // add a, #num
            tmp = (int)accumulator;
            progCounter++;
            tmp += ROM[progCounter];
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            //printf("PC:%03X\tidx:%d\top:%02X%02X\tadd a %02X\tresult: a=%02X\n", progCounter, index, opcode_, ROM[progCounter], ROM[progCounter], accumulator);
            progCounter++;
            break;
        case 3:                 // add a, reg + carry
            tmp = (int)accumulator;
            reg = opcode_ & 0b111;
            tmp += RAM[reg + bitRead(PSW,bank) * 24];
            if ( bitRead(PSW, carry)) {
                tmp++;
            }
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 4:                 // add a, RAM[reg] + carry
            tmp = (int)accumulator;
            reg = opcode_ & 0b1;
            tmp += RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            if ( bitRead(PSW, carry)) {
                tmp++;
            }
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 5:                 // add a, #num + carry
            tmp = (int)accumulator;
            progCounter++;
            tmp += ROM[progCounter];
            if ( bitRead(PSW, carry)) {
                tmp++;
            }
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            } else {
                bitClear(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 6:                 // and a, reg
            reg = opcode_ & 0b111;
            accumulator &= RAM[reg + bitRead(PSW,bank) * 24];
            progCounter++;
            break;
        case 7:                 // and a, RAM[reg]
            reg = opcode_ & 0b1;
            accumulator &= RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            progCounter++;
            break;
        case 8:                 // and a, #num
            progCounter++;
            accumulator &= ROM[progCounter];
            progCounter++;
            break;
        case 9:                 // and port1, #num
            progCounter++;
            port1 &= ROM[progCounter];
            progCounter++;
            break;
        case 10:                // and port2, #num
            progCounter++;
            port2 &= ROM[progCounter];
            progCounter++;
            break;
//**********************************************************************
        case 12:                // call address
            progCounter++;
            progCounter++;
            reg = PSW & 0b111;
            RAM[8 + reg * 2] = (uint8_t)(progCounter & 0xFF);
            RAM[8 + reg * 2 + 1] = (uint8_t)((progCounter >> 8) & 0b11);
            RAM[8 + reg * 2 + 1] += (PSW & 0xF0);
            reg++;
            PSW = (PSW & 0xF0) + reg;
            tmp = (int)(opcode_ & 0b01100000) << 3;
            progCounter--;
            tmp += ROM[progCounter];
            progCounter = tmp;
            break;
        case 13:                // clr a
            accumulator = 0;
            progCounter++;
            break;
        case 14:                // clr carry
            bitClear(PSW, carry);
            progCounter++;
            break;
        case 15:                // clr F1
            bitClear(busStatus, bF1);
            progCounter++;
            break;
        case 16:                // clr F0
            bitClear(busStatus, bF0);
            bitClear(PSW, pF0);
            progCounter++;
            break;
        case 17:                // cpl a ; not a
            accumulator = ~accumulator;
            progCounter++;
            break;
        case 18:                // cpl carry ; not carry
            bitToggle(PSW, carry);
            progCounter++;
            break;
        case 19:                // cpl F0 ; not F0
            bitToggle(busStatus, bF0);
            bitToggle(PSW, pF0);
            progCounter++;
            break;
        case 20:                // cpl F1 ; not F1
            bitToggle(busStatus, bF1);
            progCounter++;
            break;
//**********************************************************************
        case 22:                // dec a        ; what about carry flag?
            accumulator--;
            progCounter++;
            break;
        case 23:                // dec reg
            reg = opcode_ & 0b111;
            RAM[reg + bitRead(PSW,bank) * 24]--;
            progCounter++;
            break;
        case 24:                // dis i ; disable external (but in) interrupt (IBF flag)
            //printf("Disable external interrupt not implementer yet!\n");
            extIrqEnabled = false;
            progCounter++;
            break;
        case 25:                // dis tcnti ; disable timer interrupt
            //printf("Disable timer interrupt not implementer yet!\n");
            timerIrqEnabled = false;
            progCounter++;
            break;
        case 26:                // djnz reg, address
            reg = opcode_ & 0b111;
            progCounter++;
            RAM[reg + bitRead(PSW,bank) * 24]--;
            if (RAM[reg + bitRead(PSW,bank) * 24] != 0) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
            } else {
                progCounter++;
            }
            break;
        case 27:                // en dma ; enable DMA
            printf("Enable DMA not implementer yet!\n");
            progCounter++;
            break;
        case 28:                // en flags ; enable global interrupts
            printf("Enable global FLAGS inteerupt not implementer yet!\n");
            globalIrqEnabled = true;
            progCounter++;
            break;
        case 29:                // en i ; enable external (but in) interrupt (IBF flag)
            //printf("Enable external interrupt not implementer yet!\n");
            extIrqEnabled = true;
            progCounter++;
            break;
        case 30:                // en tcnti ; enable timer interrup
            //printf("Enable timer interrupt not implementer yet!\n");
            timerIrqEnabled = true;
            progCounter++;
            break;
        case 31:                // in a, DBB ; in a, data bus register
            accumulator = busIn;
            bitClear(busStatus, bIBF);
            progCounter++;
            break;
        case 32:                // in a, port1
            accumulator = port1;
            progCounter++;
            break;
        case 33:                // in a, port2
            accumulator = port2;
            progCounter++;
            break;
        case 34:                // inc a ; carry set or no?
            accumulator++;
            progCounter++;
            break;
        case 35:                // inc reg
            reg = opcode_ & 0b111;
            RAM[reg + bitRead(PSW,bank) * 24]++;
            progCounter++;
            break;
        case 36:                // inc RAM[reg]
            reg = opcode_ & 0b1;
            RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111]++;
            progCounter++;
            break;
        case 37:                // jbBIT address ; jump if bit X in accumilator set
            reg = (opcode_ & 0b11100000) >> 5;
            reg = (1 << reg);
            progCounter++;
            if ( (accumulator & reg) != 0) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 38:                // jc address ; jump if carry flag set
            progCounter++;
            if (bitRead(PSW, carry)) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 39:                // jf0 ; jump if F0 set
            progCounter++;
            if (bitRead(busStatus, bF0)) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 40:                // jf1 ; jump if F1 set
            progCounter++;
            if (bitRead(busStatus, bF1)) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 41:                // jump address
//printf("PC:%03X\tidx:%d\top:%02X\tadd a RAM[%d]=%02X\tresult: a=%02X\n", progCounter, index, opcode_, reg, RAM[ RAM[reg+bank*24] & 0b111111], accumulator);
            //printf("PC:%03X\tidx:%d\top:%02X ",progCounter, index, opcode_);
            progCounter++;
            progCounter = ROM[progCounter];
            //printf("%02X\t", progCounter);
            progCounter += ((uint16_t)((opcode_ & 0b11100000) >> 5)) << 8;
            //printf("jmp %03X\n", progCounter);
            break;
        case 42:                // jmpp ROM[a] ; jump to addres what is in ROM[a]
            progCounter &= 0b1100000000;
            progCounter |= (uint16_t)accumulator;
            break;
        case 43:                // jc address ; jump if carry flag clear
                progCounter++;
            if (!bitRead(PSW, carry)) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 44:                // jnibf    ; jump if IBF flag is clear
            progCounter++;
            if (!bitRead(busStatus, bIBF)) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 45:                //  jnt0    ; jump if test 0 clear (low)
            progCounter++;
            if (test0 == false) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 46:                //  jnt0    ; jump if test 1 clear (low)
            progCounter++;
            if (test1 == false) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 47:                // jnz    ; jump if a not zero
            progCounter++;
            if (accumulator != 0) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 48:                // jobf    ; jump if OBF flag is set
            progCounter++;
            if ( bitRead(busStatus, bOBF) ) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 49:                //  jtf    ; jump if timer overflow set
            progCounter++;
            if (timerOverflow == true) {
                timerOverflow = false;
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            timerOverflow = false;
            progCounter++;
            break;
        case 50:                //  jt0    ; jump if test0 set (high)
            progCounter++;
            if (test0 == true) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 51:                //  jt1    ; jump if test1 set (high)
            progCounter++;
            if (test1 == true) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 52:                // jz    ; jump if a zero
            progCounter++;
            if (accumulator == 0) {
                progCounter = (progCounter & 0b1100000000) + ROM[progCounter];
                break;
            }
            progCounter++;
            break;
        case 53:                // mov a, #num
            progCounter++;
            accumulator = ROM[progCounter];
            progCounter++;
            break;
        case 54:                // mov  a,psw
            accumulator = PSW;
            progCounter++;
            break;
        case 55:                // mov a, reg
            reg = opcode_ & 0b111;
            accumulator = RAM[reg + bitRead(PSW,bank) * 24];
            progCounter++;
            break;
        case 56:                // mov a, RAM[reg]
            reg = opcode_ & 0b1;
            accumulator = RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            progCounter++;
            break;
        case 57:                // mov a, timer
            accumulator = timer;
            progCounter++;
            break;
        case 58:                // mov PSW, a
            PSW = accumulator;
            progCounter++;
            break;
        case 59:                // mov reg, a
            reg = opcode_ & 0b111;
            RAM[reg + bitRead(PSW,bank) * 24] = accumulator;
            progCounter++;
            break;
        case 60:                // mov reg, #num
            reg = opcode_ & 0b111;
            progCounter++;
            RAM[reg + bitRead(PSW,bank) * 24] = ROM[progCounter];
            progCounter++;
            break;
        case 61:                // mov RAM[reg], a
            reg = opcode_ & 0b1;
            RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111] = accumulator;
            progCounter++;
            break;
        case 62:                // mov RAM[reg], #num
            reg = opcode_ & 0b1;
            progCounter++;
            RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111] = ROM[progCounter];
            progCounter++;
            break;
        case 63:                // mov busStatus, a
            tmp = accumulator & 0b11110000;
            busStatus &= 0b1111;
            busStatus |= tmp;
            progCounter++;
            break;
        case 64:                // mov timer, a
            //printf("double check 65: mov timer,a\n");
            timer =  accumulator;
            progCounter++;
            break;
//**********************************************************************
        case 67:                // movp a,@a
            accumulator = ROM[(0b1100000000 & progCounter) + (int)accumulator];
            progCounter++;
            break;
        case 68:                // movp3 a,@a
            accumulator = ROM[0b1100000000 + (int)accumulator];
            progCounter++;
            break;
        case 69:                // nop
            progCounter++;
            break;
        case 70:                // orl a, reg
            reg = opcode_ & 0b111;
            accumulator |= RAM[reg + bitRead(PSW,bank) * 24];
            progCounter++;
            break;
        case 71:                // orl a, RAM[reg]
            reg = opcode_ & 0b1;
            progCounter++;
            accumulator |= RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            progCounter++;
            break;
        case 72:                // orl a, #num
            progCounter++;
            accumulator |= ROM[progCounter];
            progCounter++;
            break;
        case 73:                // orl port1, #num
            progCounter++;
            port1 |= ROM[progCounter];
            progCounter++;
            break;
        case 74:                // orl port2, #num
            progCounter++;
            port2 |= ROM[progCounter];
            progCounter++;
            break;
//**********************************************************************
        case 76:                // out DBB,a ; output a to BUS
            busOut = accumulator;
            bitSet(busStatus, bOBF);
            printf("PC: %03X\tBus write (send to CPU): %02X\tBus Status: %02X\n", progCounter, busOut, busStatus);
            progCounter++;
            break;
        case 77:                // out port1, a
            port1 = accumulator;
            progCounter++;
            break;
        case 78:                // out port2, a
            port2 = accumulator;
            progCounter++;
            break;
        case 79:                // ret
            reg = PSW & 0b111;
            reg--;
            tmp = (int)(RAM[8 + reg * 2]);
            tmp += ((int)(RAM[8 + reg * 2+1]) & 0b11) << 8;
            progCounter = tmp;
            break;
        case 80:                // retr
            reg = PSW & 0b111;
            reg--;
            PSW = (RAM[8 + reg * 2+1] & 0xF) + reg;
            if (bitRead(PSW, pF0)) {
                bitSet(busStatus, bF0);
            } else {
                bitClear(busStatus, bF0);
            }
            tmp = (int)(RAM[8 + reg * 2]);
            tmp += ((int)(RAM[8 + reg * 2+1]) & 0b11) << 8;
            progCounter = tmp;
            retrExecuted = true;
            printf("RETR executed\n");
            //emuRunning = false;
            //printf("Auto stop after RETR. Check the stack!\n");

            break;
        case 81:                // rl a ; a = a << 1, bit 7 to bit0
            tmp = (int)accumulator;
            tmp = tmp << 1;
            accumulator = (uint8_t)(tmp & 0xFF);
            if (tmp > 0xff) {
                bitSet(accumulator, 0);
            }
            progCounter++;
            break;
        case 82:                //rlc a ; a = a << 1, carry to bit0, bit7 to carry
            tmp = (int)accumulator;
            tmp = tmp << 1;
            if ( bitRead(PSW, carry) ) {
                bitSet(tmp, 0);
            }
            if (tmp > 0xff) {
                bitSet(PSW, carry);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 83:                //rr a ; a = a >> 1, bit0 to bit7
            tmp = (int)accumulator;
            if ( !bitRead(tmp, 0)) {
                reg = 1;
            }
            tmp = tmp >> 1;
            if (reg == 1) {
                bitSet(tmp, 7);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 84:                //rrc a ; a = a >> 1, bit0 to carry , carry to bit7
            tmp = (int)accumulator;
            if (bitRead(PSW, carry)) {
                bitSet(tmp, 8);
            }
            if ( bitRead(tmp, 0)){
                reg = 1;
            }
            tmp = tmp >> 1;
            if (reg == 1 ) {
                bitSet(tmp, 8);
            }
            accumulator = (uint8_t)(tmp & 0xFF);
            progCounter++;
            break;
        case 85:                // register bank = 0
            bitClear(PSW, bank);
            progCounter++;
            break;
        case 86:                // register bank = 1
            bitSet(PSW, bank);
            progCounter++;
            break;
        case 87:                // stop timer
            timerEnabled = false;
            progCounter++;
            break;              // strt counter
        case 88:
            printf("Start counter not implemented yet!\n");
            progCounter++;
            break;
        case 89:                // strt timer
            timerPrescaler = 0;
            timerEnabled = true;
            progCounter++;
            break;
        case 90:                // swap a ; swap 4 low bites with high 4 bites in accumulator
            reg = accumulator;
            accumulator = accumulator >> 4;
            reg = reg << 4;
            accumulator |= reg;
            progCounter++;
            break;
        case 91:                // xch a, reg ; exchange a, reg
            reg = opcode_ & 0b111;
            tmp = RAM[reg + bitRead(PSW,bank) * 24];
            RAM[reg + bitRead(PSW,bank) * 24] = accumulator;
            accumulator = tmp;
            progCounter++;
            break;
        case 92:                // xch a, RAM[reg], exchange a, RAM[reg]
            reg = opcode_ & 0b1;
            tmp = RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111] = accumulator;
            accumulator = tmp;
            progCounter++;
            break;
//**********************************************************************
        case 94:                // xlr a, reg ; xor a, reg
            reg = opcode_ & 0b111;
            accumulator ^= RAM[reg + bitRead(PSW,bank) * 24];
            progCounter++;
            break;
        case 95:                // xlr a, RAM[reg] ; xor a, RAM[reg]
            reg = opcode_ & 0b1;
            accumulator ^= RAM[ RAM[reg + bitRead(PSW,bank) * 24] & 0b111111];
            progCounter++;
            break;
        case 96:                // xlr a, #num ; xor a number
            progCounter++;
            accumulator ^= ROM[progCounter];
            progCounter++;
            break;
//**********************************************************************
        case 98:                // Do external interrupt
            //bitClear(busStatus, bIFB);
            reg = PSW & 0b111;
            RAM[8 + reg * 2] = (uint8_t)(progCounter & 0xFF);
            RAM[8 + reg * 2 + 1] = (uint8_t)((progCounter >> 8) & 0b11);
            RAM[8 + reg * 2 + 1] += (PSW & 0xF0);
            //drawRAM();
            reg++;
            PSW = (PSW & 0xF0) + reg;
            progCounter = extIRQaddress;
            break;
        case 99:                // Do timer interrupt
            reg = PSW & 0b111;
            RAM[8 + reg * 2] = (uint8_t)(progCounter & 0xFF);
            RAM[8 + reg * 2 + 1] = (uint8_t)((progCounter >> 8) & 0b11);
            RAM[8 + reg * 2 + 1] += (PSW & 0xF0);
            reg++;
            PSW = (PSW & 0xF0) + reg;
            progCounter = timerIRQaddress;
            break;

//**********************************************************************
        case 11:                // anld %P,a ; 8243 related
//**********************************************************************
        case 21:                // da   a       // not found in dump
//**********************************************************************
        case 65:                // movd a,%P ; 8243 related
        case 66:                // movd %P,a ; 8243 related
//**********************************************************************
        case 75:                // orld %P,a ; 8243 related
//**********************************************************************
        case 93:                // xrl  a,@%R   // not found in dump
//**********************************************************************
        case 97:
//**********************************************************************
        default:
            //printf("PC: %03X\tOpCode: %02X\tIndex: %d\t\"%s\" \"%s\" not implementer yet!\n", progCounter,opcode_, index, opcodes[index].pattern, opcodes[index].diasm);
            printf(" not implementer yet!\n");
            exit(-1);
            break;
    }
}

void drawRAM() {

    for (uint8_t i = 0; i < 64; i++) {
        uint16_t x, y;

        x = (i / 32) ? 2 + (i / 32) * 11 : 1 + (i / 32) * 11;
        y = 1 + (i % 32);

        for ( int8_t j = 7; j >= 0; j--) {
            sdlconsole_putChar( bitRead(RAM[i], j) ? '1' : '0', x + 3 + 7 - j, y);
        }
    }
}

void drawFlags() {
    //char str[20];
    uint16_t x, y;
    x = 47;
    y = 24;

    sdlconsole_drawSquare(x, y, 4 , 3, "T0", true);
    sdlconsole_putChar( test0 ? '1' : '0', x + 1, y + 1);

    sdlconsole_drawSquare(x + 3, y, 4 , 3, "T1", true);
    sdlconsole_putChar( test1 ? '1' : '0', x + 1 + 3, y + 1);

    sdlconsole_drawSquare(x + 7, y, 3 , 3, "A", true);
    sdlconsole_putChar( bitRead(busStatus, bF1) ? '1' : '0', x + 1 + 7, y + 1);

}

void drawBusOut() {
    
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 21;

    sdlconsole_drawSquare(x, y, 13 , 3, "Bus Out", true);
//    FF 10101010
    sprintf(str, "%02X", busOut);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(busOut, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawBusIn() {
    
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 18;

    sdlconsole_drawSquare(x, y, 13 , 3, "Bus In", true);
//    FF 10101010
    sprintf(str, "%02X", busIn);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(busIn, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawBusStatus() {
    
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 15;

    sdlconsole_drawSquare(x, y, 13 , 3, "Bus Status", true);
//    FF 10101010
    sprintf(str, "%02X", busStatus);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(busStatus, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawPort1() {
    
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 9;

    sdlconsole_drawSquare(x, y, 13 , 3, "Port1", true);
//    FF 10101010
    sprintf(str, "%02X", port1);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(port1, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawPort2() {
    
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 12;

    sdlconsole_drawSquare(x, y, 13 , 3, "Port2", true);
//    FF 10101010
    sprintf(str, "%02X", port2);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(port2, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawPSW() {
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 6;

    sdlconsole_drawSquare(x, y, 13 , 3, "PSW", true);
//    FF 10101010
    sprintf(str, "%02X", PSW);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(PSW, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawAccumulatior() {
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 3;

    sdlconsole_drawSquare(x, y, 13 , 3, "Accumulator", true);
//    FF 10101010
    sprintf(str, "%02X", accumulator);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(accumulator, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }
}

void drawTimer() {
    char str[20];
    uint16_t x, y;
    x = 47;
    y = 0;

    sdlconsole_drawSquare(x, y, 13 , 3, "Timer", true);
//    FF 10101010
    sprintf(str, "%02X", timer);

    sdlconsole_putChar( str[0], x + 1, y + 1);
    sdlconsole_putChar( str[1], x + 2, y + 1);

    for ( int8_t j = 7; j >= 0; j--) {
        sdlconsole_putChar( bitRead(timer, j) ? '1' : '0', x + 4 + 7 - j, y + 1);
    }

}

void tick () {

    if( emuRunning == false) return;
    
    timerPrescaler++;
    if (timerPrescaler > 31) {
        timerPrescaler = 0;
        if (timerEnabled) {
                uint8_t prevTimer;
                prevTimer = timer;
                timer++;
                if (timer < prevTimer ) {
                    timerOverflow = true;
                }
                drawTimer();
            }
    }

    // Do something with cycle count, IRQs and exec()
    // IBF IRQ
    if ( extIrqEnabled == true && bitRead(busStatus, bIBF) != 0  && retrExecuted == true) {
        printf("Executing external interrupt\n");
        retrExecuted = false;
        //emuRunning = false;
        //printf("Auto stop before external IRQ. Check the stack!\n");
        //delayMicroseconds(5000000);
        execOpcode(ROM[progCounter], extIRQindex);
        //printf("Auto stop after external IRQ. Check the stack!\n");

    }

    // OBF IRQ
    // globalEnable IRQs

    // timerOverflow IRQ
    if (timerIrqEnabled == true && timerOverflow == true && retrExecuted == true) {
        printf("Executing timer interrupt\n");
        retrExecuted = false;
        //emuRunning = false;
        //printf("Auto stop before timer IRQ. Check the stack!\n");
        //delayMicroseconds(5000000);
        execOpcode(ROM[progCounter], timerIRQindex);
        //printf("Auto stop after timer IRQ. Check the stack!\n");
    }

    if (tickcount > 16 ) {
        int index;
        tickcount = 0;
        index = searchOpcode(ROM[progCounter]);
        printf("PC: %03X\tOpCode: %02X\tIndex: %d\t\"%s\"\n", progCounter, ROM[progCounter], index, opcodes[index].pattern);
        
        
        char str[40];
        sprintf(str, "PC:%03X O:%02X I:%02d", progCounter, ROM[progCounter], index);
        uint16_t x, y;
        x = 26;
        y = 1;
        sdlconsole_scroll(x, y, 20,32, 1, SCROLL_UP, str);
        
        execOpcode(ROM[progCounter], index);
        
        drawRAM();
        drawAccumulatior();
        drawPSW();
        drawPort1();
        drawPort2();
        drawBusStatus();
        drawBusIn();
        drawBusOut();
        drawFlags();

    }
    tickcount++;
}

int main(int argc, char *argv[]) {

    char str[20];
    
    (void)loadROM(argv[1]);

    if ( sdlconsole_init(termW, termH, "i8741A emulator", "TypeColonial8x8.Font.bmp") != 0) {
        printf("Init error!\n");
        return -1;
    }

    retrExecuted = true;

    // fill RAM with value
    for (uint8_t i = 0; i < RAMsize; i++) {
        RAM[i] = 0x00;
    }


    sdlconsole_drawSquare(25,0, 22, 34, "PC trace", true);

    sdlconsole_drawSquare(0,0,25,34,"RAM", true);

    // draw RAM addresses and other constant things
    for (uint8_t i = 0; i < 64; i++) {
        uint16_t x, y;

        x = (i / 32) ? 2 + (i / 32) * 11 : 1 + (i / 32) * 11;
        y = 1 + (i % 32);
        if (i < 8  ) {
            sprintf(str, "R%01d", i);
        } else if ( i >= 24 && i < 32 ) {
            sprintf(str, "R%01d", i - 24);
        } else {
            sprintf(str, "%02X", i);
        }

        sdlconsole_putChar( str[0], x, y);
        sdlconsole_putChar( str[1], x + 1, y);
        sdlconsole_putChar( ':', x + 2, y);
        // RAM buffer part
        for ( int8_t j = 7; j >= 0; j--) {
            sdlconsole_putChar( bitRead(RAM[i], j) ? '1' : '0', x + 3 + 7 - j, y);
        }
    }

    drawTimer();
    drawRAM();
    drawAccumulatior();
    drawPSW();
    drawPort1();
    drawPort2();
    drawBusStatus();
    drawBusIn();
    drawBusOut();
    drawFlags();
    
    while (systemRunning) {
        tick();

        static uint64_t lasttime = 0;
        #define TIMING_CYCLE 100000
        
        
        if ( !(timing_getCur() - lasttime < TIMING_CYCLE) ) {
            lasttime = timing_getCur();
            int x, y;
            uint32_t ret;
            ret = SDL_GetMouseState(&x, &y);
            x = x / 16;
            y = y / 16;
            //printf("x: %d\ty: %d\n", x, y);
            if ( x == 48 && y == 25 && ret != 0) {
                //bitToggle(busStatus, bF1);
                test0 = !test0;
                printf("T0 toggle\n");
            }

            if ( x == 51 && y == 25 && ret != 0) {
                //bitToggle(busStatus, bF1);
                test1 = !test1;
                printf("T1 toggle\n");
            }

            if ( x == 55 && y == 25 && ret != 0) {
                bitToggle(busStatus, bF1);
                printf("adrr toggle\n");
            }
            if ( y == 19 && x >= 51 && x <= 58 && ret != 0 ) {
                uint16_t  mouseFlag;
                mouseFlag= x - 51;
                bitToggle(busIn, 7 - mouseFlag);
                printf("data toggle: %d\n", 7 - mouseFlag);
                //printf("x: %d\ty: %d\n", x, y);
                //printf("ret: %04X\n", ret);
            }
            if ( y == 10 && x >= 51 && x <= 58 && ret != 0 ) {
                uint16_t  mouseFlag;
                mouseFlag= x - 51;
                bitToggle(port1, 7 - mouseFlag);
                printf("port1 toggle: %d\n", 7 - mouseFlag);
            }

        }
        
        
        
        switch (sdlconsole_loop()) {
            case SDLCONSOLE_EVENT_RETURN:
                printf("IRQ request\n");
                bitWrite(busStatus, bIBF, true);
                bitWrite(busStatus, bOBF, false);
                //bitWrite(PSW, pIBF, true);
                break;
            case SDLCONSOLE_EVENT_SPACE:
                emuRunning = !emuRunning;
                if (emuRunning) {
                    printf("Continue\n");
                } else {
                    printf("Pause\n");
                }
                break;
            case SDLCONSOLE_EVENT_QUIT:
                systemRunning = false;
                emuRunning = false;
                break;
            default:
                break;

        }

    }
    sdlconsole_end();

}
