#include "6502m.h"
#include "mem.h"

    // P register Flags
    #define fC 0b00000001   // Carry
    #define fZ 0b00000010   // Zero
    #define fI 0b00000100   // Interrupt disable
    #define fD 0b00001000   // Decimal
    #define fB 0b00010000   // Interrupt type?
    #define f1 0b00100000   // -Unused-
    #define fV 0b01000000   // Overflow
    #define fN 0b10000000   // Negative

    // Jump types
    #define cUnconditional 255
    #define cNZ 0
    #define cZ  1
    #define cNC 2
    #define cC  3
    #define cNN 4
    #define cN  5
    #define cNV 6
    #define cV  7

    // Addressing modes
    #define aAccumulator    0
    #define aImmediate      1
    #define aZeroPage       2
    #define aZeroPageX      3
    #define aZeroPageY      4
    #define aRelative       5 // Is really needed?
    #define aIndirect       6
    #define aIndirectX      7
    #define aIndirectY      8
    #define aAbsolute       9
    #define aAbsoluteX      10
    #define aAbsoluteY      11

namespace m6502
{
    uint8_t interrupt_type = 0xfe;

    uint8_t regs[13];

    uint8_t *_rA = &regs[0];
    uint8_t *_rX = &regs[1];
    uint8_t *_rY = &regs[2];
    uint8_t *_rS = &regs[3];
    uint8_t *_rP = &regs[4];
    uint8_t *_rPC_lo = &regs[5];
    uint8_t *_rPC_hi = &regs[6];
    uint16_t *_rPC = (uint16_t*)&regs[5];

    uint8_t *_rB = &regs[7];
    uint8_t *_rT = &regs[8];
    uint8_t *_rI = &regs[9];
    uint8_t *_rAD_lo = &regs[10];
    uint8_t *_rAD_hi = &regs[11];
    uint16_t *_rAD = (uint16_t*)&regs[10];
    uint8_t *_rTEMP_lo = &regs[12];
    uint8_t *_rTEMP_hi = &regs[13];
    uint16_t *_rTEMP = (uint16_t*)&regs[12];

    #define rA (*_rA)
    #define rX (*_rX)
    #define rY (*_rY)
    #define rS (*_rS)
    #define rP (*_rP)
    #define rPC_lo (*_rPC_lo)
    #define rPC_hi (*_rPC_hi)
    #define rPC (*_rPC)

    #define rB (*_rB)
    #define rT (*_rT)
    #define rI (*_rI)
    #define rAD_lo (*_rAD_lo)
    #define rAD_hi (*_rAD_hi)
    #define rAD (*_rAD)
    #define rTEMP_lo (*_rTEMP_lo)
    #define rTEMP_hi (*_rTEMP_hi)
    #define rTEMP (*_rTEMP)

    uint8_t microcode[256];
    uint8_t microcode_pos = 0;
    uint8_t microcode_last = 0;

    enum microInstructions {
        miFetchOpcode,
        miFakeFetchOperand,
        miFetchOperandLo,
        miFetchOperandHi,
        miFetchOperandHiAndIndexX,
        miFetchOperandHiAndIndexY,
        miPushPCHi,
        miPushPCLo,
        miPushP,
        miPCLoInt,
        miPCHiInt,
        miIndexX,
        miFetchAddressLo,
        miFetchAddressHi,
        miFetchAddressHiAndIndex,
        miReadAddress,
        miReadAddressAndSkip,
        miWriteRegister,
        miNumMicroinstructions
    };


    uint8_t instructions[256][7] = {
        /* 0x00 BRK       */ { miFetchOperandLo, miPushPCHi, miPushPCLo, miPushP, miPCLoInt, miPCHiInt, miFetchOpcode },
        /* 0x01 ORA X,ind */ { miFetchOperandLo, miIndexX, miFetchAddressLo, miFetchAddressHi, miReadAddress, miFetchOpcode },
        /* 0x02 ---       */ { miFetchOpcode },
        /* 0x03 ---       */ { miFetchOpcode },
        /* 0x04 ---       */ { miFetchOpcode },
        /* 0x05 ORA zpg   */ { miFetchOperandLo, miReadAddress, miFetchOpcode },
    };

    void SetFlags(uint8_t flags)
    {
        if (flags&fC) rP = ( rP & ~fC ) | ( rTEMP_hi & fC );
        if (flags&fN) rP = ( rP & ~fN ) | ( rB & fN );
        if (flags&fZ) rP = ( rB ? rP & ~fZ : rP | fZ );
        if (flags&fV) rP = ( (rTEMP_lo ^ rA) & (rTEMP_lo ^ rB) & 0x80 ) ? rP | fV : rP & ~fV;
    }

    void DoOpcodeWork()
    {
        switch (rI&0x03) {
            case 1: /* ALU */
                switch ((rI>>5)&0x07) {
                    case 0: /* ORA */ rB = rA | rB; SetFlags(fN|fZ); rA = rB; break;
                    case 1: /* AND */ rB = rA & rB; SetFlags(fN|fZ); rA = rB; break;
                    case 2: /* EOR */ rB = rA ^ rB; SetFlags(fN|fZ); rA = rB; break;
                    case 3: /* ADC */ rTEMP = rA + rB + fC; SetFlags(fN|fV|fZ|fC); rA = rTEMP_lo; break;
                    case 5: /* LDA */ SetFlags(fN|fZ); rA = rB; break;
                    case 6: /* CMP */ rTEMP = rA + ~rB; SetFlags(fN|fZ|fC); break;
                    case 7: /* SBC */ rTEMP = rA + ~rB + fC; SetFlags(fN|fV|fZ|fC); rA = rTEMP_lo; break;
                };
                break;
        };
    }

    void InsertFetchOpcode()
    {
        microcode[microcode_pos] = miFetchOpcode;
        microcode_last = microcode_pos+1;
    }

    void FetchOpcode()
    {
        DoOpcodeWork();
        rI = mem::read(rPC++);
        int i=0; do { microcode[microcode_last++] = instructions[rI][i++]; } while (instructions[rI][i-1] != miFetchOpcode);
    }

    void FakeFetchOperand() { rAD_lo = mem::read(rPC); }
    void FetchOperandLo() { rTEMP = rAD_lo = rB = mem::read(rPC++); }
    void FetchOperandHi() { rAD_hi = mem::read(rPC++); }
    void FetchOperandHiAndIndexX() { rAD_hi = mem::read(rPC++); rTEMP = rAD_lo + rX; rAD_lo = rTEMP_lo; }
    void FetchOperandHiAndIndexY() { rAD_hi = mem::read(rPC++); rTEMP = rAD_lo + rY; rAD_lo = rTEMP_lo; }

    void PushPCHi() { mem::write(0x0100+(rS++), rPC_hi); }
    void PushPCLo() { mem::write(0x0100+(rS++), rPC_lo); }
    void PushP() { mem::write(0x0100+(rS++), rP|f1|fB); rP |= fI; }
    void PCLoInt() { rPC_lo = mem::read(0xff00+interrupt_type); }
    void PCHiInt() { rPC_hi = mem::read(0xff00+interrupt_type+1); }

    void IndexX() { rB = mem::read(rAD); rAD_lo += rX; }
    void FetchAddressLo() { rT = mem::read(rAD++); }
    void FetchAddressHi() { rAD_hi = mem::read(rAD); rAD_lo = rT; }
    void FetchAddressHiAndIndex() { rAD_hi = mem::read(rAD); rTEMP = rAD_lo + rY; rAD_lo = rTEMP_lo; }
    void ReadAddress() { rB = mem::read(rAD); rAD_hi += rTEMP_hi; }

    void ReadAddressAndSkip() { rB = mem::read(rAD); rAD_hi += rTEMP_hi; if (rTEMP_hi) InsertFetchOpcode(); }
    void WriteRegister() { mem::write(rAD, (rI&3)==1 ? rA : (rI&3)==2 ? rX : rY ); }

    void (*microinstructions[miNumMicroinstructions])(void) {
        FetchOpcode, FakeFetchOperand, FetchOperandLo, FetchOperandHi, FetchOperandHiAndIndexX, FetchOperandHiAndIndexY,
        PushPCHi, PushPCLo, PushP, PCLoInt, PCHiInt,
        IndexX, FetchAddressLo, FetchAddressHi, FetchAddressHiAndIndex, ReadAddress,
        ReadAddressAndSkip, WriteRegister };
    
    void reset()
    {

    }

    void interrupt(uint8_t type)
    {

    }

    void tick()
    {
        uint8_t m_inst = microcode[microcode_pos++];
        microinstructions[m_inst]();
    }

}