//#define ARM_DEBUG

namespace GarboDev
{
    using System;

    public class ArmCore
    {
        private const uint COND_EQ = 0;	    // Z set
        private const uint COND_NE = 1;	    // Z clear
        private const uint COND_CS = 2;	    // C set
        private const uint COND_CC = 3;	    // C clear
        private const uint COND_MI = 4;	    // N set
        private const uint COND_PL = 5;	    // N clear
        private const uint COND_VS = 6;	    // V set
        private const uint COND_VC = 7;	    // V clear
        private const uint COND_HI = 8;	    // C set and Z clear
        private const uint COND_LS = 9;	    // C clear or Z set
        private const uint COND_GE = 10;	// N equals V
        private const uint COND_LT = 11;	// N not equal to V
        private const uint COND_GT = 12;	// Z clear AND (N equals V)
        private const uint COND_LE = 13;	// Z set OR (N not equal to V)
        private const uint COND_AL = 14;	// Always
        private const uint COND_NV = 15;	// Never execute

        private const uint OP_AND = 0x0;
        private const uint OP_EOR = 0x1;
        private const uint OP_SUB = 0x2;
        private const uint OP_RSB = 0x3;
        private const uint OP_ADD = 0x4;
        private const uint OP_ADC = 0x5;
        private const uint OP_SBC = 0x6;
        private const uint OP_RSC = 0x7;
        private const uint OP_TST = 0x8;
        private const uint OP_TEQ = 0x9;
        private const uint OP_CMP = 0xA;
        private const uint OP_CMN = 0xB;
        private const uint OP_ORR = 0xC;
        private const uint OP_MOV = 0xD;
        private const uint OP_BIC = 0xE;
        private const uint OP_MVN = 0xF;

        private delegate void ExecuteInstruction();
        private ExecuteInstruction[] NormalOps = null;

        private Arm7Processor parent;
        private Memory memory;
        private uint[] registers;

        private uint instructionQueue;
        private uint curInstruction;

        // CPU flags
        private uint zero, carry, negative, overflow;
        private uint shifterCarry;

        private bool thumbMode;

        public ArmCore(Arm7Processor parent, Memory memory)
        {
            this.parent = parent;
            this.memory = memory;
            this.registers = this.parent.Registers;

            this.NormalOps = new ExecuteInstruction[8]
                {
                    this.DataProcessing,
                    this.DataProcessingImmed,
                    this.LoadStoreImmediate,
                    this.LoadStoreRegister,
                    this.LoadStoreMultiple,
                    this.Branch,
                    this.CoprocessorLoadStore,
                    this.SoftwareInterrupt
                };
        }

        public void BeginExecution()
        {
            this.FlushQueue();
        }

        public void Step()
        {
            this.UnpackFlags();

            this.curInstruction = this.instructionQueue;
            this.instructionQueue = this.memory.ReadU32(registers[15]);
            registers[15] += 4;

            uint cond = 0;
            switch (this.curInstruction >> 28)
            {
                case COND_AL: cond = 1; break;
                case COND_EQ: cond = zero; break;
                case COND_NE: cond = 1 - zero; break;
                case COND_CS: cond = carry; break;
                case COND_CC: cond = 1 - carry; break;
                case COND_MI: cond = negative; break;
                case COND_PL: cond = 1 - negative; break;
                case COND_VS: cond = overflow; break;
                case COND_VC: cond = 1 - overflow; break;
                case COND_HI: cond = carry & (1 - zero); break;
                case COND_LS: cond = (1 - carry) | zero; break;
                case COND_GE: cond = (1 - negative) ^ overflow; break;
                case COND_LT: cond = negative ^ overflow; break;
                case COND_GT: cond = (1 - zero) & (negative ^ (1 - overflow)); break;
                case COND_LE: cond = (negative ^ overflow) | zero; break;
            }

            if (cond == 1)
            {
                // Execute the instruction
                this.NormalOps[(curInstruction >> 25) & 0x7]();
            }

            this.parent.Cycles -= this.memory.WaitCycles;

            if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
            {
                this.parent.ReloadQueue();
            }

            this.PackFlags();
        }

        public void Execute()
        {
            this.UnpackFlags();
            this.thumbMode = false;

            while (this.parent.Cycles > 0)
            {
                this.curInstruction = this.instructionQueue;
                this.instructionQueue = this.memory.ReadU32Aligned(registers[15]);
                registers[15] += 4;

                if ((this.curInstruction >> 28) == COND_AL)
                {
                    this.NormalOps[(curInstruction >> 25) & 0x7]();
                }
                else
                {
                    uint cond = 0;
                    switch (this.curInstruction >> 28)
                    {
                        case COND_EQ: cond = zero; break;
                        case COND_NE: cond = 1 - zero; break;
                        case COND_CS: cond = carry; break;
                        case COND_CC: cond = 1 - carry; break;
                        case COND_MI: cond = negative; break;
                        case COND_PL: cond = 1 - negative; break;
                        case COND_VS: cond = overflow; break;
                        case COND_VC: cond = 1 - overflow; break;
                        case COND_HI: cond = carry & (1 - zero); break;
                        case COND_LS: cond = (1 - carry) | zero; break;
                        case COND_GE: cond = (1 - negative) ^ overflow; break;
                        case COND_LT: cond = negative ^ overflow; break;
                        case COND_GT: cond = (1 - zero) & (negative ^ (1 - overflow)); break;
                        case COND_LE: cond = (negative ^ overflow) | zero; break;
                    }

                    if (cond == 1)
                    {
                        // Execute the instruction
                        this.NormalOps[(curInstruction >> 25) & 0x7]();
                    }
                }

                this.parent.Cycles -= this.memory.WaitCycles;

                if (this.thumbMode)
                {
                    this.parent.ReloadQueue();
                    break;
                }

#if ARM_DEBUG
                // Check the current PC
                if (this.parent.Breakpoints.ContainsKey(registers[15] - 4U))
                {
                    this.parent.BreakpointHit = true;
                    break;
                }
#endif
            }

            this.PackFlags();
        }

        #region Barrel Shifter
        private const uint SHIFT_LSL = 0;
        private const uint SHIFT_LSR = 1;
        private const uint SHIFT_ASR = 2;
        private const uint SHIFT_ROR = 3;

        private uint BarrelShifter(uint shifterOperand)
        {
            uint type = (shifterOperand >> 5) & 0x3;

            bool registerShift = (shifterOperand & (1 << 4)) == (1 << 4);

            uint rm = registers[shifterOperand & 0xF];

            int amount;
            if (registerShift)
            {
                uint rs = (shifterOperand >> 8) & 0xF;
                if (rs == 15)
                {
                    amount = (int)((registers[rs] + 0x4) & 0xFF);
                }
                else
                {
                    amount = (int)(registers[rs] & 0xFF);
                }

                if ((shifterOperand & 0xF) == 15)
                {
                    rm += 4;
                }
            }
            else
            {
                amount = (int)((shifterOperand >> 7) & 0x1F);
            }

            if (registerShift)
            {
                if (amount == 0)
                {
                    this.shifterCarry = this.carry;
                    return rm;
                }

                switch (type)
                {
                    case SHIFT_LSL:
                        if (amount < 32)
                        {
                            this.shifterCarry = (rm >> (32 - amount)) & 1;
                            return rm << amount;
                        }
                        else if (amount == 32)
                        {
                            this.shifterCarry = rm & 1;
                            return 0;
                        }
                        else
                        {
                            this.shifterCarry = 0;
                            return 0;
                        }

                    case SHIFT_LSR:
                        if (amount < 32)
                        {
                            this.shifterCarry = (rm >> (amount - 1)) & 1;
                            return rm >> amount;
                        }
                        else if (amount == 32)
                        {
                            this.shifterCarry = (rm >> 31) & 1;
                            return 0;
                        }
                        else
                        {
                            this.shifterCarry = 0;
                            return 0;
                        }

                    case SHIFT_ASR:
                        if (amount >= 32)
                        {
                            if ((rm & (1 << 31)) == 0)
                            {
                                this.shifterCarry = 0;
                                return 0;
                            }
                            else
                            {
                                this.shifterCarry = 1;
                                return 0xFFFFFFFF;
                            }
                        }
                        else
                        {
                            this.shifterCarry = (rm >> (amount - 1)) & 1;
                            return (uint)(((int)rm) >> amount);
                        }

                    case SHIFT_ROR:
                        if ((amount & 0x1F) == 0)
                        {
                            this.shifterCarry = (rm >> 31) & 1;
                            return rm;
                        }
                        else
                        {
                            amount &= 0x1F;
                            this.shifterCarry = (rm >> amount) & 1;
                            return (rm >> amount) | (rm << (32 - amount));
                        }
                }
            }
            else
            {
                switch (type)
                {
                    case SHIFT_LSL:
                        if (amount == 0)
                        {
                            this.shifterCarry = this.carry;
                            return rm;
                        }
                        else
                        {
                            this.shifterCarry = (rm >> (32 - amount)) & 1;
                            return rm << amount;
                        }

                    case SHIFT_LSR:
                        if (amount == 0)
                        {
                            this.shifterCarry = (rm >> 31) & 1;
                            return 0;
                        }
                        else
                        {
                            this.shifterCarry = (rm >> (amount - 1)) & 1;
                            return rm >> amount;
                        }

                    case SHIFT_ASR:
                        if (amount == 0)
                        {
                            if ((rm & (1 << 31)) == 0)
                            {
                                this.shifterCarry = 0;
                                return 0;
                            }
                            else
                            {
                                this.shifterCarry = 1;
                                return 0xFFFFFFFF;
                            }
                        }
                        else
                        {
                            this.shifterCarry = (rm >> (amount - 1)) & 1;
                            return (uint)(((int)rm) >> amount);
                        }

                    case SHIFT_ROR:
                        if (amount == 0)
                        {
                            // Actually an RRX
                            this.shifterCarry = rm & 1;
                            return (this.carry << 31) | (rm >> 1);
                        }
                        else
                        {
                            this.shifterCarry = (rm >> (amount - 1)) & 1;
                            return (rm >> amount) | (rm << (32 - amount));
                        }
                }
            }

            // Should never happen...
            throw new Exception("Barrel Shifter has messed up.");
        }
        #endregion

        #region Flag helpers
        public void OverflowCarryAdd(uint a, uint b, uint r)
        {
            overflow = ((a & b & ~r) | (~a & ~b & r)) >> 31;
            carry = ((a & b) | (a & ~r) | (b & ~r)) >> 31;
        }

        public void OverflowCarrySub(uint a, uint b, uint r)
        {
            overflow = ((a & ~b & ~r) | (~a & b & r)) >> 31;
            carry = ((a & ~b) | (a & ~r) | (~b & ~r)) >> 31;
        }
        #endregion

        #region Opcodes
        private void DoDataProcessing(uint shifterOperand)
        {
            uint rn = (this.curInstruction >> 16) & 0xF;
            uint rd = (this.curInstruction >> 12) & 0xF;
            uint alu;

            bool registerShift = (this.curInstruction & (1 << 4)) == (1 << 4);
            if (rn == 15 && ((this.curInstruction >> 25) & 0x7) == 0 && registerShift)
            {
                rn = registers[rn] + 4;
            }
            else
            {
                rn = registers[rn];
            }

            uint opcode = (this.curInstruction >> 21) & 0xF;

            if (((this.curInstruction >> 20) & 1) == 1)
            {
                // Set flag bit set
                switch (opcode)
                {
                    case OP_ADC:
                        registers[rd] = rn + shifterOperand + carry;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarryAdd(rn, shifterOperand, registers[rd]);
                        break;

                    case OP_ADD:
                        registers[rd] = rn + shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarryAdd(rn, shifterOperand, registers[rd]);
                        break;

                    case OP_AND:
                        registers[rd] = rn & shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_BIC:
                        registers[rd] = rn & ~shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_CMN:
                        alu = rn + shifterOperand;

                        negative = alu >> 31;
                        zero = alu == 0 ? 1U : 0U;
                        this.OverflowCarryAdd(rn, shifterOperand, alu);
                        break;

                    case OP_CMP:
                        alu = rn - shifterOperand;

                        negative = alu >> 31;
                        zero = alu == 0 ? 1U : 0U;
                        this.OverflowCarrySub(rn, shifterOperand, alu);
                        break;

                    case OP_EOR:
                        registers[rd] = rn ^ shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_MOV:
                        registers[rd] = shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_MVN:
                        registers[rd] = ~shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_ORR:
                        registers[rd] = rn | shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_RSB:
                        registers[rd] = shifterOperand - rn;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarrySub(shifterOperand, rn, registers[rd]);
                        break;

                    case OP_RSC:
                        registers[rd] = shifterOperand - rn - (1U - carry);

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarrySub(shifterOperand, rn, registers[rd]);
                        break;

                    case OP_SBC:
                        registers[rd] = rn - shifterOperand - (1U - carry);

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarrySub(rn, shifterOperand, registers[rd]);
                        break;

                    case OP_SUB:
                        registers[rd] = rn - shifterOperand;

                        negative = registers[rd] >> 31;
                        zero = registers[rd] == 0 ? 1U : 0U;
                        this.OverflowCarrySub(rn, shifterOperand, registers[rd]);
                        break;

                    case OP_TEQ:
                        alu = rn ^ shifterOperand;

                        negative = alu >> 31;
                        zero = alu == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;

                    case OP_TST:
                        alu = rn & shifterOperand;

                        negative = alu >> 31;
                        zero = alu == 0 ? 1U : 0U;
                        carry = this.shifterCarry;
                        break;
                }

                if (rd == 15)
                {
                    // Prevent writing if no SPSR exists (this will be true for USER or SYSTEM mode)
                    if (this.parent.SPSRExists) this.parent.WriteCpsr(this.parent.SPSR);
                    this.UnpackFlags();

                    // Check for branch back to Thumb Mode
                    if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
                    {
                        this.thumbMode = true;
                        return;
                    }

                    // Otherwise, flush the instruction queue
                    this.FlushQueue();
                }
            }
            else
            {
                // Set flag bit not set
                switch (opcode)
                {
                    case OP_ADC: registers[rd] = rn + shifterOperand + carry; break;
                    case OP_ADD: registers[rd] = rn + shifterOperand; break;
                    case OP_AND: registers[rd] = rn & shifterOperand; break;
                    case OP_BIC: registers[rd] = rn & ~shifterOperand; break;
                    case OP_EOR: registers[rd] = rn ^ shifterOperand; break;
                    case OP_MOV: registers[rd] = shifterOperand; break;
                    case OP_MVN: registers[rd] = ~shifterOperand; break;
                    case OP_ORR: registers[rd] = rn | shifterOperand; break;
                    case OP_RSB: registers[rd] = shifterOperand - rn; break;
                    case OP_RSC: registers[rd] = shifterOperand - rn - (1U - carry); break;
                    case OP_SBC: registers[rd] = rn - shifterOperand - (1U - carry); break;
                    case OP_SUB: registers[rd] = rn - shifterOperand; break;

                    case OP_CMN:
                        // MSR SPSR, shifterOperand
                        if ((this.curInstruction & (1 << 16)) == 1 << 16 && this.parent.SPSRExists)
                        {
                            this.parent.SPSR &= 0xFFFFFF00;
                            this.parent.SPSR |= shifterOperand & 0x000000FF;
                        }
                        if ((this.curInstruction & (1 << 17)) == 1 << 17 && this.parent.SPSRExists)
                        {
                            this.parent.SPSR &= 0xFFFF00FF;
                            this.parent.SPSR |= shifterOperand & 0x0000FF00;
                        }
                        if ((this.curInstruction & (1 << 18)) == 1 << 18 && this.parent.SPSRExists)
                        {
                            this.parent.SPSR &= 0xFF00FFFF;
                            this.parent.SPSR |= shifterOperand & 0x00FF0000;
                        }
                        if ((this.curInstruction & (1 << 19)) == 1 << 19 && this.parent.SPSRExists)
                        {
                            this.parent.SPSR &= 0x00FFFFFF;
                            this.parent.SPSR |= shifterOperand & 0xFF000000;
                        }

                        // Queue will be flushed since rd == 15, so adjust the PC
                        registers[15] -= 4;
                        break;

                    case OP_CMP:
                        // MRS rd, SPSR
                        if (this.parent.SPSRExists) registers[rd] = this.parent.SPSR;
                        break;

                    case OP_TEQ:
                        if (((this.curInstruction >> 4) & 0xf) == 1)
                        {
                            // BX
                            uint rm = this.curInstruction & 0xf;

                            this.PackFlags();

                            this.parent.CPSR &= ~Arm7Processor.T_MASK;
                            this.parent.CPSR |= (registers[rm] & 1) << Arm7Processor.T_BIT;

                            registers[15] = registers[rm] & (~1U);

                            this.UnpackFlags();

                            // Check for branch back to Thumb Mode
                            if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
                            {
                                this.thumbMode = true;
                                return;
                            }

                            // Queue will be flushed later because rd == 15
                        }
                        else if (((this.curInstruction >> 4) & 0xf) == 0)
                        {
                            // MSR CPSR, shifterOperand
                            bool userMode = (this.parent.CPSR & 0x1F) == Arm7Processor.USR;

                            this.PackFlags();

                            uint tmpCPSR = this.parent.CPSR;

                            if ((this.curInstruction & (1 << 16)) == 1 << 16 && !userMode)
                            {
                                tmpCPSR &= 0xFFFFFF00;
                                tmpCPSR |= shifterOperand & 0x000000FF;
                            }
                            if ((this.curInstruction & (1 << 17)) == 1 << 17 && !userMode)
                            {
                                tmpCPSR &= 0xFFFF00FF;
                                tmpCPSR |= shifterOperand & 0x0000FF00;
                            }
                            if ((this.curInstruction & (1 << 18)) == 1 << 18 && !userMode)
                            {
                                tmpCPSR &= 0xFF00FFFF;
                                tmpCPSR |= shifterOperand & 0x00FF0000;
                            }
                            if ((this.curInstruction & (1 << 19)) == 1 << 19)
                            {
                                tmpCPSR &= 0x00FFFFFF;
                                tmpCPSR |= shifterOperand & 0xFF000000;
                            }

                            this.parent.WriteCpsr(tmpCPSR);

                            this.UnpackFlags();

                            // Check for branch back to Thumb Mode
                            if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
                            {
                                this.thumbMode = true;
                                return;
                            }

                            // Queue will be flushed since rd == 15, so adjust the PC
                            registers[15] -= 4;
                        }
                        break;

                    case OP_TST:
                        // MRS rd, CPSR
                        this.PackFlags();
                        registers[rd] = this.parent.CPSR;
                        break;
                }

                if (rd == 15)
                {
                    // Flush the queue
                    this.FlushQueue();
                }
            }
        }

        private void DataProcessing()
        {
            // Special instruction
            switch ((this.curInstruction >> 4) & 0xF)
            {
                case 0x9:
                    // Multiply or swap instructions
                    this.MultiplyOrSwap();
                    return;
                case 0xB:
                    // Load/Store Unsigned halfword
                    this.LoadStoreHalfword();
                    return;
                case 0xD:
                    // Load/Store Signed byte
                    this.LoadStoreHalfword();
                    return;
                case 0xF:
                    // Load/Store Signed halfword
                    this.LoadStoreHalfword();
                    return;
            }

            this.DoDataProcessing(this.BarrelShifter(this.curInstruction));
        }

        private void DataProcessingImmed()
        {
            uint immed = this.curInstruction & 0xFF;
            int rotateAmount = (int)(((this.curInstruction >> 8) & 0xF) * 2);

            immed = (immed >> rotateAmount) | (immed << (32 - rotateAmount));

            if (rotateAmount == 0)
            {
                this.shifterCarry = this.carry;
            }
            else
            {
                this.shifterCarry = (immed >> 31) & 1;
            }

            this.DoDataProcessing(immed);
        }

        private void LoadStore(uint offset)
        {
            uint rn = (this.curInstruction >> 16) & 0xF;
            uint rd = (this.curInstruction >> 12) & 0xF;

            uint address = registers[rn];

            bool preIndexed = (this.curInstruction & (1 << 24)) == 1 << 24;
            bool byteTransfer = (this.curInstruction & (1 << 22)) == 1 << 22;
            bool writeback = (this.curInstruction & (1 << 21)) == 1 << 21;

            // Add or subtract offset
            if ((this.curInstruction & (1 << 23)) != 1 << 23) offset = (uint)-offset;

            if (preIndexed)
            {
                address += offset;

                if (writeback)
                {
                    registers[rn] = address;
                }
            }

            if ((this.curInstruction & (1 << 20)) == 1 << 20)
            {
                // Load
                if (byteTransfer)
                {
                    registers[rd] = this.memory.ReadU8(address);
                }
                else
                {
                    registers[rd] = this.memory.ReadU32(address);
                }

                // ARM9 fix here

                if (rd == 15)
                {
                    registers[rd] &= ~3U;
                    this.FlushQueue();
                }

                if (!preIndexed)
                {
                    if (rn != rd)
                        registers[rn] = address + offset;
                }
            }
            else
            {
                // Store
                uint amount = registers[rd];
                if (rd == 15) amount += 4;

                if (byteTransfer)
                {
                    this.memory.WriteU8(address, (byte)(amount & 0xFF));
                }
                else
                {
                    this.memory.WriteU32(address, amount);
                }

                if (!preIndexed)
                {
                    registers[rn] = address + offset;
                }
            }
        }

        private void LoadStoreImmediate()
        {
            this.LoadStore(this.curInstruction & 0xFFF);
        }

        private void LoadStoreRegister()
        {
            // The barrel shifter expects a 0 in bit 4 for immediate shifts, this is implicit in
            // the meaning of the instruction, so it is fine
            this.LoadStore(this.BarrelShifter(this.curInstruction));
        }

        private void LoadStoreMultiple()
        {
            uint rn = (this.curInstruction >> 16) & 0xF;

            this.PackFlags();
            uint curCpsr = this.parent.CPSR;

            bool preIncrement = (this.curInstruction & (1 << 24)) != 0;
            bool up = (this.curInstruction & (1 << 23)) != 0;
            bool writeback = (this.curInstruction & (1 << 21)) != 0;

            uint address;
            uint bitsSet = 0;
            for (int i = 0; i < 16; i++) if (((this.curInstruction >> i) & 1) != 0) bitsSet++;

            if (preIncrement)
            {
                if (up)
                {
                    // Increment before
                    address = this.registers[rn] + 4;
                    if (writeback) this.registers[rn] += bitsSet * 4;
                }
                else
                {
                    // Decrement before
                    address = this.registers[rn] - (bitsSet * 4);
                    if (writeback) this.registers[rn] -= bitsSet * 4;
                }
            }
            else
            {
                if (up)
                {
                    // Increment after
                    address = this.registers[rn];
                    if (writeback) this.registers[rn] += bitsSet * 4;
                }
                else
                {
                    // Decrement after
                    address = this.registers[rn] - (bitsSet * 4) + 4;
                    if (writeback) this.registers[rn] -= bitsSet * 4;
                }
            }

            if ((this.curInstruction & (1 << 20)) != 0)
            {
                if ((this.curInstruction & (1 << 22)) != 0 && ((this.curInstruction >> 15) & 1) == 0)
                {
                    // Switch to user mode temporarily
                    this.parent.WriteCpsr((curCpsr & ~0x1FU) | Arm7Processor.USR);
                }

                // Load multiple
                for (int i = 0; i < 15; i++)
                {
                    if (((this.curInstruction >> i) & 1) != 1) continue;
                    this.registers[i] = this.memory.ReadU32Aligned(address & (~0x3U));
                    address += 4;
                }

                if (((this.curInstruction >> 15) & 1) == 1)
                {
                    // Arm9 fix here

                    this.registers[15] = this.memory.ReadU32Aligned(address & (~0x3U));

                    if ((this.curInstruction & (1 << 22)) != 0)
                    {
                        // Load the CPSR from the SPSR
                        if (this.parent.SPSRExists)
                        {
                            this.parent.WriteCpsr(this.parent.SPSR);
                            this.UnpackFlags();

                            // Check for branch back to Thumb Mode
                            if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
                            {
                                this.thumbMode = true;
                                this.registers[15] &= ~0x1U;
                                return;
                            }
                        }
                    }

                    this.registers[15] &= ~0x3U;
                    this.FlushQueue();
                }
                else
                {
                    if ((this.curInstruction & (1 << 22)) != 0)
                    {
                        // Switch back to the correct mode
                        this.parent.WriteCpsr(curCpsr);
                        this.UnpackFlags();

                        if ((this.parent.CPSR & Arm7Processor.T_MASK) == Arm7Processor.T_MASK)
                        {
                            this.thumbMode = true;
                            return;
                        }
                    }
                }
            }
            else
            {
                if ((this.curInstruction & (1 << 22)) != 0)
                {
                    // Switch to user mode temporarily
                    this.parent.WriteCpsr((curCpsr & ~0x1FU) | Arm7Processor.USR);
                }

                if (((this.curInstruction >> (int)rn) & 1) != 0 && writeback &&
                    (this.curInstruction & ~(0xFFFFFFFF << (int)rn)) == 0)
                {
                    // If the lowest register is also the writeback, we use the original value
                    // Does anybody do this????
                    throw new Exception("Unhandled STM state");
                }
                else
                {
                    // Store multiple
                    for (int i = 0; i < 15; i++)
                    {
                        if (((this.curInstruction >> i) & 1) == 0) continue;
                        this.memory.WriteU32(address, this.registers[i]);
                        address += 4;
                    }

                    if (((this.curInstruction >> 15) & 1) != 0)
                    {
                        this.memory.WriteU32(address, this.registers[15] + 4U);
                    }
                }

                if ((this.curInstruction & (1 << 22)) != 0)
                {
                    // Switch back to the correct mode
                    this.parent.WriteCpsr(curCpsr);
                    this.UnpackFlags();
                }
            }
        }

        private void Branch()
        {
            if ((this.curInstruction & (1 << 24)) != 0)
            {
                this.registers[14] = (this.registers[15] - 4U) & ~3U;
            }

            uint branchOffset = this.curInstruction & 0x00FFFFFF;
            if (branchOffset >> 23 == 1) branchOffset |= 0xFF000000;

            this.registers[15] += branchOffset << 2;

            this.FlushQueue();
        }

        private void CoprocessorLoadStore()
        {
            throw new Exception("Unhandled opcode - coproc load/store");
        }

        private void SoftwareInterrupt()
        {
            // Adjust PC for prefetch
            this.registers[15] -= 4U;
            this.parent.EnterException(Arm7Processor.SVC, 0x8, false, false);
        }

        private void MultiplyOrSwap()
        {
            if ((this.curInstruction & (1 << 24)) == 1 << 24)
            {
                // Swap instruction
                uint rn = (this.curInstruction >> 16) & 0xF;
                uint rd = (this.curInstruction >> 12) & 0xF;
                uint rm = this.curInstruction & 0xF;

                if ((this.curInstruction & (1 << 22)) != 0)
                {
                    // SWPB
                    byte tmp = this.memory.ReadU8(registers[rn]);
                    this.memory.WriteU8(registers[rn], (byte)(registers[rm] & 0xFF));
                    registers[rd] = tmp;
                }
                else
                {
                    // SWP
                    uint tmp = this.memory.ReadU32(registers[rn]);
                    this.memory.WriteU32(registers[rn], registers[rm]);
                    registers[rd] = tmp;
                }
            }
            else
            {
                // Multiply instruction
                switch ((this.curInstruction >> 21) & 0x7)
                {
                    case 0:
                    case 1:
                        {
                            // Multiply/Multiply + Accumulate
                            uint rd = (this.curInstruction >> 16) & 0xF;
                            uint rn = registers[(this.curInstruction >> 12) & 0xF];
                            uint rs = (this.curInstruction >> 8) & 0xF;
                            uint rm = this.curInstruction & 0xF;

                            int cycles = 4;
                            // Multiply cycle calculations
                            if ((registers[rs] & 0xFFFFFF00) == 0 || (registers[rs] & 0xFFFFFF00) == 0xFFFFFF00)
                            {
                                cycles = 1;
                            }
                            else if ((registers[rs] & 0xFFFF0000) == 0 || (registers[rs] & 0xFFFF0000) == 0xFFFF0000)
                            {
                                cycles = 2;
                            }
                            else if ((registers[rs] & 0xFF000000) == 0 || (registers[rs] & 0xFF000000) == 0xFF000000)
                            {
                                cycles = 3;
                            }

                            registers[rd] = registers[rs] * registers[rm];
                            this.parent.Cycles -= cycles;

                            if ((this.curInstruction & (1 << 21)) == 1 << 21)
                            {
                                registers[rd] += rn;
                                this.parent.Cycles -= 1;
                            }

                            if ((this.curInstruction & (1 << 20)) == 1 << 20)
                            {
                                negative = registers[rd] >> 31;
                                zero = registers[rd] == 0 ? 1U : 0U;
                            }
                            break;
                        }

                    case 2:
                    case 3:
                        throw new Exception("Invalid multiply");

                    case 4:
                    case 5:
                    case 6:
                    case 7:
                        {
                            // Multiply/Signed Multiply Long
                            uint rdhi = (this.curInstruction >> 16) & 0xF;
                            uint rdlo = (this.curInstruction >> 12) & 0xF;
                            uint rs = (this.curInstruction >> 8) & 0xF;
                            uint rm = this.curInstruction & 0xF;

                            int cycles = 5;
                            // Multiply cycle calculations
                            if ((registers[rs] & 0xFFFFFF00) == 0 || (registers[rs] & 0xFFFFFF00) == 0xFFFFFF00)
                            {
                                cycles = 2;
                            }
                            else if ((registers[rs] & 0xFFFF0000) == 0 || (registers[rs] & 0xFFFF0000) == 0xFFFF0000)
                            {
                                cycles = 3;
                            }
                            else if ((registers[rs] & 0xFF000000) == 0 || (registers[rs] & 0xFF000000) == 0xFF000000)
                            {
                                cycles = 4;
                            }

                            this.parent.Cycles -= cycles;

                            switch ((this.curInstruction >> 21) & 0x3)
                            {
                                case 0:
                                    {
                                        // UMULL
                                        ulong result = ((ulong)registers[rm]) * registers[rs];
                                        registers[rdhi] = (uint)(result >> 32);
                                        registers[rdlo] = (uint)(result & 0xFFFFFFFF);
                                        break;
                                    }
                                case 1:
                                    {
                                        // UMLAL
                                        ulong accum = (((ulong)registers[rdhi]) << 32) | registers[rdlo];
                                        ulong result = ((ulong)registers[rm]) * registers[rs];
                                        result += accum;
                                        registers[rdhi] = (uint)(result >> 32);
                                        registers[rdlo] = (uint)(result & 0xFFFFFFFF);
                                        break;
                                    }
                                case 2:
                                    {
                                        // SMULL
                                        long result = ((long)((int)registers[rm])) * ((long)((int)registers[rs]));
                                        registers[rdhi] = (uint)(result >> 32);
                                        registers[rdlo] = (uint)(result & 0xFFFFFFFF);
                                        break;
                                    }
                                case 3:
                                    {
                                        // SMLAL
                                        long accum = (((long)((int)registers[rdhi])) << 32) | registers[rdlo];
                                        long result = ((long)((int)registers[rm])) * ((long)((int)registers[rs]));
                                        result += accum;
                                        registers[rdhi] = (uint)(result >> 32);
                                        registers[rdlo] = (uint)(result & 0xFFFFFFFF);
                                        break;
                                    }
                            }

                            if ((this.curInstruction & (1 << 20)) == 1 << 20)
                            {
                                negative = registers[rdhi] >> 31;
                                zero = (registers[rdhi] == 0 && registers[rdlo] == 0) ? 1U : 0U;
                            }
                            break;
                        }
                } 
            }
        }

        private void LoadStoreHalfword()
        {
            uint rn = (this.curInstruction >> 16) & 0xF;
            uint rd = (this.curInstruction >> 12) & 0xF;

            uint address = registers[rn];

            bool preIndexed = (this.curInstruction & (1 << 24)) != 0;
            bool byteTransfer = (this.curInstruction & (1 << 5)) == 0;
            bool signedTransfer = (this.curInstruction & (1 << 6)) != 0;
            bool writeback = (this.curInstruction & (1 << 21)) != 0;

            uint offset;
            if ((this.curInstruction & (1 << 22)) != 0)
            {
                // Immediate offset
                offset = ((this.curInstruction & 0xF00) >> 4) | (this.curInstruction & 0xF);
            }
            else
            {
                // Register offset
                offset = this.registers[this.curInstruction & 0xF];
            }

            // Add or subtract offset
            if ((this.curInstruction & (1 << 23)) == 0) offset = (uint)-offset;

            if (preIndexed)
            {
                address += offset;

                if (writeback)
                {
                    registers[rn] = address;
                }
            }

            if ((this.curInstruction & (1 << 20)) != 0)
            {
                // Load
                if (byteTransfer)
                {
                    if (signedTransfer)
                    {
                        registers[rd] = this.memory.ReadU8(address);
                        if ((registers[rd] & 0x80) != 0)
                        {
                            registers[rd] |= 0xFFFFFF00;
                        }
                    }
                    else
                    {
                        registers[rd] = this.memory.ReadU8(address);
                    }
                }
                else
                {
                    if (signedTransfer)
                    {
                        registers[rd] = this.memory.ReadU16(address);
                        if ((registers[rd] & 0x8000) != 0)
                        {
                            registers[rd] |= 0xFFFF0000;
                        }
                    }
                    else
                    {
                        registers[rd] = this.memory.ReadU16(address);
                    }
                }

                if (rd == 15)
                {
                    registers[rd] &= ~3U;
                    this.FlushQueue();
                }

                if (!preIndexed)
                {
                    if (rn != rd)
                        registers[rn] = address + offset;
                }
            }
            else
            {
                // Store
                if (byteTransfer)
                {
                    this.memory.WriteU8(address, (byte)(registers[rd] & 0xFF));
                }
                else
                {
                    this.memory.WriteU16(address, (ushort)(registers[rd] & 0xFFFF));
                }

                if (!preIndexed)
                {
                    registers[rn] = address + offset;
                }
            }
        }
        #endregion Opcodes

        private void PackFlags()
        {
            this.parent.CPSR &= 0x0FFFFFFF;
            this.parent.CPSR |= this.negative << Arm7Processor.N_BIT;
            this.parent.CPSR |= this.zero << Arm7Processor.Z_BIT;
            this.parent.CPSR |= this.carry << Arm7Processor.C_BIT;
            this.parent.CPSR |= this.overflow << Arm7Processor.V_BIT;
        }

        private void UnpackFlags()
        {
            this.negative = (this.parent.CPSR >> Arm7Processor.N_BIT) & 1;
            this.zero = (this.parent.CPSR >> Arm7Processor.Z_BIT) & 1;
            this.carry = (this.parent.CPSR >> Arm7Processor.C_BIT) & 1;
            this.overflow = (this.parent.CPSR >> Arm7Processor.V_BIT) & 1;
        }

        private void FlushQueue()
        {
            this.instructionQueue = this.memory.ReadU32(registers[15]);
            registers[15] += 4;
        }
    }
}