Complete code for the CPU

CPU.hs

{-# LANGUAGE GeneralizedNewtypeDeriving, BangPatterns, TemplateHaskell, ExistentialQuantification, MultiParamTypeClasses, FunctionalDependencies #-}

module SNES.CPU where

-- An emulator for the 65c816 processor of the SNES.
-- This is a 16-bit processor slightly customized for use in the SNES.
-- Reference material: docs/cpu, http://www.westerndesigncenter.com/wdc/datasheets/Programmanual.pdf
-- NB: double-byte storage is done little-endian.
--

import Prelude hiding (read,break)

import Control.Monad
import Control.Monad.State.Strict
import Control.Monad.Writer.Strict
import Control.Monad.Error
import Control.Applicative

import Data.Bits
import Data.Word
import Data.Int
import Data.List hiding (break)
import Data.Array.IO hiding (index)
import Data.Array.MArray hiding (index)
import qualified Data.Map as M

import qualified Numeric (showHex)

import Control.Lens
import Data.Bits.Lens
import Data.List.Lens

fi :: forall a b . (Integral a, Num b) => a -> b
fi = fromIntegral

signExtend :: Word8 -> Word16
signExtend val = fi (fi (fi val :: Int8) :: Int16) :: Word16


type Mem = IOUArray Word32 Word8

-- Backgrounds get a single byte register each, and need double-writing. The backgroundOffsetPartial
-- above gets written with the previous value according to the following formula:
-- For BGnHOFS: (NewByte<<8) | (PrevByte&~7) | ((CurrentValue>>8)&7)
-- For BGnVOFS: (NewByte<<8) | PrevByte
data Background = Background {
  _tilemapAddress :: !Word8,     -- BGnSC 0x210[7-a].2-7
  _tilemapHigh    :: !Word8,     -- BGnSC 0x210[b-c]
  _verticalMirror :: !Bool,      -- BGnSC 0x210[7-a].1
  _horizontalMirror :: !Bool,    -- BGnSC 0x210[7-a].0
  _horizontalOffset :: !Word16,  -- BGnHOFS 0x210[d,f,1,3]
  _verticalOffset :: !Word16     -- BGnVOFS 0x210[e,0,2,4]
}
makeLenses ''Background

data Hardware = Hardware {
  _screenBlank :: !Bool,                -- INIDISP 0x2100.7
  _screenBrightness :: !Word8,          -- INIDISP 0x2100.0-3
  _objectSize :: !Word8,                -- OBSEL   0x2101.5-7
  _objectBase :: !Word8,                -- OBSEL   0x2101.0-2
  _objectPriority :: !Bool,             -- OAMADDL 0x2102.7
  _objectTableSelect :: !Bool,          -- OAMADDL 0x2102.0
  _objectTableAddress :: !Word16,       -- OAMADDH 0x2102
  _objectTableLowByte :: !(Maybe Word8),-- OAMDATA 0x2104

  _bgSize :: ![Bool],                   -- BGMODE 0x2105.4-7
  _mode1BG3Priority :: !Bool,           -- BGMODE 0x2105.3
  _graphicsMode :: !Word8,              -- BGMODE 0x2105.0-2

  _backgrounds :: ![Background],        -- BG control containers
  _backgroundOffsetPartial :: !Word8,   -- special

  _mosaicSize :: !Word8,                -- MOSAIC 0x2106.4-7
  _mosaicBGs  :: ![Bool],               -- MOSAIC 0x2106.0-3

  _incrementAfterHigh :: !Bool,         -- VMAIN 0x2115.7
  _vramAddressRemapping :: !Word8,      -- VMAIN 0x2115.2-3
  _vramAddressIncrementing :: !Word8,   -- VMAIN 0x2115.0-1
  _vramAddress :: !Word16,              -- VMADDL/H 0x2116/7

  _oam :: IOUArray Word16 Word8         -- Object Attr Memory (544B)
}
makeLenses ''Hardware


data CPUState = CPUState {
  _mem        :: !Mem,
  -- registers
  _a          :: !Word16,   -- accumulator
  _x          :: !Word16,   -- X index
  _y          :: !Word16,   -- Y index
  _sp         :: !Word16,   -- stack pointer
  _status     :: !Word8,    -- P status register
  _pc         :: !Word16,   -- program counter
  _dp         :: !Word16,   -- direct page register
  _pbr        :: !Word8,    -- program bank register
  _dbr        :: !Word8,    -- data bank register
  _emulation  :: !Bool,     -- emulation mode flag
  _hardware   :: !Hardware  -- collected hardware controls
}
makeLenses ''CPUState

newtype CPU a = CPU (StateT CPUState (WriterT [Int] IO) a)
  deriving (Monad, Applicative, Functor, MonadIO, MonadState CPUState, MonadWriter [Int])

-- lenses for portions of the status register
negative, overflow, memorySelect, indexSelect, decimal, irqDisable, zero, carry :: Lens CPUState CPUState Bool Bool
negative = status . bitAt 7
overflow = status . bitAt 6
memorySelect = status . bitAt 5
indexSelect = status . bitAt 4
decimal = status . bitAt 3
irqDisable = status . bitAt 2
zero = status . bitAt 1
carry = status . bitAt 0

break = status . bitAt 4

shortMemory :: CPU Bool
shortMemory = do
  e <- use emulation
  m <- use memorySelect
  return $ e || m

shortIndex :: CPU Bool
shortIndex = do
  e <- use emulation
  ix <- use indexSelect
  return $ e || ix

-- Constructors for each flag, can be used for flag-updating convenience functions.
data Flag = N | V | M | X | D | I | Z | C | B

-- Sets the flags appropriately.
flags :: [Flag] -> Word16 -> CPU ()
flags fs val = do
  m <- shortMemory
  if m then flagsB fs (fi val) else flagsW fs val

flagsX :: [Flag] -> Word16 -> CPU ()
flagsX fs val = do
  ix <- shortIndex
  if ix then flagsB fs (fi val) else flagsW fs val

flagsB :: [Flag] -> Word8 -> CPU ()
flagsB [] _ = return ()
flagsB (Z:rest) val = (zero .= (val == 0)) >> flagsB rest val
flagsB (N:rest) val = (negative .= val ^. bitAt 7) >> flagsB rest val

flagsW :: [Flag] -> Word16 -> CPU ()
flagsW [] _ = return ()
flagsW (Z:rest) val = (zero .= (val == 0)) >> flagsW rest val
flagsW (N:rest) val = (negative .= val ^. bitAt 15) >> flagsW rest val

-- some raw memory access functions, taking a 32-bit address
--
-- Memory mapping: Currently hard-coded for LoROM configurations.
-- This is handled in two steps: first the mmap call normalizes the
-- address to the "canonical", source-of-mirroring address.
-- Then this canonical address is checked against the registers M.Map.
-- This map contains special handler code for the various memory-mapped
-- hardware registers on the SNES for controlling the screen, DMA, etc.
rb_ :: Word32 -> CPU Word8
rb_ addr = do
  m <- use mem
  addr' <- mmap addr
  case M.lookup addr' registersRead of
    Nothing -> liftIO $ readArray m addr
    Just f  -> f

wb_ :: Word32 -> Word8 -> CPU ()
wb_ addr val = do
  m <- use mem
  addr' <- mmap addr
  case M.lookup addr' registersWrite of
    Nothing -> liftIO $ writeArray m addr val
    Just f  -> f val

rw_ :: Word32 -> CPU Word16
rw_ addr = do
  lo <- rb_ addr
  hi <- rb_ (addr+1)
  return $ (fi hi `shiftL` 8) + fi lo


ww_ :: Word32 -> Word16 -> CPU ()
ww_ addr val = do
  wb_ addr (fi (val .&. 255))
  wb_ (addr+1) (fi (val `shiftR` 8))


-- reads a "long", 24-bit value, stored little-endian
rl_ :: Word32 -> CPU Word32
rl_ addr = do
  lo <- rb_ addr
  hi <- rb_ (addr+1)
  bank <- rb_ (addr+2)
  return $ (fi bank `shiftL` 16) + (fi hi `shiftL` 8) + fi lo

-- some more useful memory accessors that use the DBR.
withBase :: Word8 -> Word16 -> Word32
withBase base addr = (fi base `shiftL` 16) + fi addr

withDBR :: Word16 -> CPU Word32
withDBR addr = do
  d <- use dbr
  return $ withBase d addr

withPBR :: Word16 -> CPU Word32
withPBR addr = do
  p <- use pbr
  return $ withBase p addr

rb :: Word16 -> CPU Word8
rb = rb_ <=< withDBR

rw :: Word16 -> CPU Word16
rw = rw_ <=< withDBR

wb :: Word16 -> Word8 -> CPU ()
wb addr val = do
  ra <- withDBR addr
  wb_ ra val

ww :: Word16 -> Word16 -> CPU ()
ww addr val = do
  ra <- withDBR addr
  ww_ ra val


-- General readers
-- Read or write either a byte or a double-byte, depending on Pm.
read :: Word32 -> CPU Word16
read addr = do
  m <- shortMemory
  if m then fmap fi (rb_ addr) else rw_ addr

write :: Word32 -> Word16 -> CPU ()
write addr val = do
  m <- shortMemory
  if m then wb_ addr (fi $ val .&. 255) else ww_ addr val

-- Accessors for the registers, based on the current value of the flags
getReg :: ALens' CPUState Word16 -> Bool -> CPU Word16
getReg l accum = do
  base <- use (cloneLens l) -- get the full 16-bit value
  flag <- use (if accum then memorySelect else indexSelect)
  case flag of
    True  -> return $ base .&. 255
    False -> return base

setReg :: ALens' CPUState Word16 -> Bool -> Word16 -> CPU ()
setReg l_ accum val = do
  let l = cloneLens l_
  flag <- use (if accum then memorySelect else indexSelect)
  case flag of
    False -> l .= val
    True  -> l %= (\base -> (base .&. complement 255) + (val .&. 255))


getA, getX, getY :: CPU Word16
getA = getReg a True
getX = getReg x False
getY = getReg y False

setA, setX, setY :: Word16 -> CPU ()
setA = setReg a True
setX = setReg x False
setY = setReg y False



-- stack manipulation
pushB :: Word8 -> CPU ()
pushB val = do
  addr <- use sp
  wb_ (fi addr) val
  sp %= subtract 1

pushW :: Word16 -> CPU ()
pushW val = do
  addr <- use sp
  wb_ (fi addr) (fi $ val `shiftR` 8)
  wb_ (fi addr - 1) (fi $ val .&. 255)
  sp %= subtract 2


pullB :: CPU Word8
pullB = do
  addr <- sp <%= (+1)
  rb_ (fi addr)

pullW :: CPU Word16
pullW = do
  addr <- sp <%= (+2)
  hi <- rb_ (fi addr)
  lo <- rb_ (fi addr - 1)
  return $ fi hi `shiftL` 8 + fi lo



-- reads a byte from the current PC, bumping it (but constraining it to the current bank)
pcPeek :: CPU Word8
pcPeek = do
  p <- use pc
  addr <- withPBR p
  rb_ addr

pcBumpBy :: Int -> CPU ()
pcBumpBy v = pc %= (+ fi v)

pcBump :: CPU ()
pcBump = pcBumpBy 1


-- Gets the value at PC and bumps it.
pcFetch :: CPU Word8
pcFetch = do
  val <- pcPeek
  pcBump
  return val

-- reads a double-byte from the PC
pcFetch2 :: CPU Word16
pcFetch2 = do
  lo <- pcFetch
  hi <- pcFetch
  return $ (fi hi `shiftL` 8) + fi lo

pcFetch3 :: CPU Word32
pcFetch3 = do
  lo <- pcFetch
  hi <- pcFetch
  bank <- pcFetch
  return $ (fi bank `shiftL` 16) + (fi hi `shiftL` 8) + fi lo



type Op = CPU ()
type OpB = Word8 -> Op
type OpBB = Word8 -> Word8 -> Op
type OpW = Word16 -> Op
type OpA = Word32 -> Op
type OpMove = Word32 -> Word32 -> Word16 -> Op

-- op wrappers: these correspond to different addressing modes

absoluteData :: OpA -> Op
absoluteData op = do
  raw <- pcFetch2
  eff <- withDBR raw
  op eff

absoluteInstr :: OpA -> Op
absoluteInstr op = do
  raw <- pcFetch2
  eff <- withPBR raw
  op eff


absoluteIndexedX :: OpA -> Op
absoluteIndexedX op = do
  base <- pcFetch2 >>= withDBR
  index <- getX
  op $ base + fi index

absoluteIndexedY :: OpA -> Op
absoluteIndexedY op = do
  base <- pcFetch2 >>= withDBR
  index <- getY
  op $ base + fi index


-- TODO: Not entirely certain about this one. The diagram says the indirect value is
-- found in "Program Bank Memory".
absoluteIndexedIndirect :: OpA -> Op
absoluteIndexedIndirect op = do
  base <- pcFetch2 >>= withPBR
  index <- getX
  let ref = base + fi index
  indirect <- rw_ ref
  eff <- withPBR indirect
  op eff


absoluteIndirect :: OpA -> Op
absoluteIndirect op = do
  ptr <- pcFetch2
  indirect <- rw_ (fi ptr) -- data bank 0, not withDBR
  eff <- withPBR indirect
  op eff


absoluteIndirectLong :: OpA -> Op
absoluteIndirectLong op = do
  ptr <- pcFetch2
  indirect <- rw_ (fi ptr)
  bank <- rb_ (fi ptr + 2)
  op $ (fi bank `shiftL` 16) + fi indirect


-- Doesn't need to be split data vs. instruction, since the bank is included as a literal.
absoluteLong :: OpA -> Op
absoluteLong op = pcFetch3 >>= op

absoluteLongIndexedX :: OpA -> Op
absoluteLongIndexedX op = do
  base <- pcFetch3
  index <- getX
  op $ base + fi index


accumulator :: OpW -> Op
accumulator op = getA >>= op . fi


blockMove :: OpMove -> Op
blockMove op = do
  dstBank <- pcFetch
  srcBank <- pcFetch
  srcIndex <- getX
  dstIndex <- getY
  let srcEff = fi srcBank `shiftL` 16 + fi srcIndex
      dstEff = fi dstBank `shiftL` 16 + fi dstIndex
  len <- use a -- get the whole value, not adjusted for the m flag.
  op srcEff dstEff len


directPage :: OpA -> Op
directPage op = do
  base <- use dp
  index <- pcFetch
  op $ fi base + fi index


directPageIndexedX :: OpA -> Op
directPageIndexedX op = do
  base <- use dp
  index1 <- pcFetch
  index2 <- getX
  op $ fi base + fi index1 + fi index2

directPageIndexedY :: OpA -> Op
directPageIndexedY op = do
  base <- use dp
  index1 <- pcFetch
  index2 <- getY
  op $ fi base + fi index1 + fi index2


directPageIndexedIndirectX :: OpA -> Op
directPageIndexedIndirectX op = do
  base <- use dp
  index1 <- pcFetch
  index2 <- getX
  let ptr = fi base + fi index1 + fi index2
  indirect <- rw_ ptr -- reading from bank 0
  eff <- withDBR indirect
  op eff


directPageIndirect :: OpA -> Op
directPageIndirect op = do
  base <- use dp
  index <- pcFetch
  let ptr = fi base + fi index
  indirect <- rw_ ptr -- bank 0
  eff <- withDBR indirect
  op eff

directPageIndirectLong :: OpA -> Op
directPageIndirectLong op = do
  base <- use dp
  index <- pcFetch
  let ptr = fi base + fi index
  eff <- rl_ ptr -- read long from bank 0
  op eff


directPageIndirectIndexedY :: OpA -> Op
directPageIndirectIndexedY op = do
  base <- use dp
  index <- pcFetch
  let ptr = fi base + fi index
  indirect <- rw_ ptr -- bank 0
  base' <- withDBR indirect
  index' <- getY
  let eff = base' + fi index'
  op eff

directPageIndirectLongIndexedY :: OpA -> Op
directPageIndirectLongIndexedY op = do
  base <- use dp
  index <- pcFetch
  let ptr = fi base + fi index
  indirect <- rl_ ptr -- bank 0
  index' <- getY
  let eff = indirect + fi index'
  op eff


immediateB :: OpB -> Op
immediateB op = pcFetch >>= op

immediateW :: OpW -> Op
immediateW op = pcFetch2 >>= op


immediate :: OpW -> Op
immediate op = do
  short <- shortMemory
  if short then immediateB (op.fi) else immediateW op


implied :: Op -> Op
implied = id


pcRelative :: OpA -> Op
pcRelative op = do
  offset <- signExtend <$> pcFetch
  thePC <- use pc -- points, after the pcFetch above, at the next instruction, which is correct.
  let thePC' = thePC + offset
  eff <- withPBR thePC'
  op eff


-- TODO: Double-check. The diagram shows that the bank is maintained and doesn't
-- receive the carry from the adding of offset and PC, if any.
pcRelativeLong :: OpA -> Op
pcRelativeLong op = do
  offset <- pcFetch2
  thePC <- use pc
  let thePC' = thePC + offset
  eff <- withPBR thePC'
  op eff


stackAbsolute :: OpW -> Op
stackAbsolute op = pcFetch2 >>= op

-- TODO: double-check me
stackDirectPageIndirect :: OpW -> Op
stackDirectPageIndirect op = do
  base <- use dp
  index <- pcFetch
  let ptr = fi base + fi index
  eff <- rw_ ptr -- bank 0
  op eff


stackPCRelative :: OpW -> Op
stackPCRelative op = do
  i <- pcFetch2
  j <- use pc
  op $ i + j


-- stackPull addressing: Depends on the size of the target, so these instructions
-- have custom handling. See: PLA, PLB, PLD, PLP, PLX, PLY.

-- stackPush addressing: Depends on the size of the source, so these instructions
-- have custom handling. See: PHA, PHB, PHD, PHK, PHP, PHX.

-- stackRTI addressing: Custom handling in RTI.
-- stackRTL addressing: Custom handling in RTL.
-- stackRTS addressing: Custom handling in RTS.

stackRelative :: OpA -> Op
stackRelative op = do
  index <- pcFetch
  base <- use sp
  op $ fi base + fi index -- bank 0, wherein is constrained the stack


stackRelativeIndirectIndexedY :: OpA -> Op
stackRelativeIndirectIndexedY op = do
  base <- use sp
  index <- pcFetch
  let ptr = fi base + fi index
  indirect <- rw_ ptr -- bank 0
  fullIndirect <- withDBR indirect
  index' <- getY
  op $ fullIndirect + fi index'






-- OPCODES

-- Runs a single instruction, and appropriate followup checks.
interpret :: CPU ()
interpret = do
  opcode <- pcFetch
  case M.lookup opcode opcodes of
    Nothing -> error $ "Illegal opcode: " ++ show opcode
    Just op -> op

-- Entry point, initializes the registers appropriately and starts execution.
reset :: CPU ()
reset = do
  -- reset state, as given in Table 13-3 on page 201.
  emulation .= True
  memorySelect .= True
  indexSelect .= True
  decimal .= False
  irqDisable .= True
  dbr .= 0
  pbr .= 0
  x %= (255 .&.)
  y %= (255 .&.)
  dp .= 0
  sp %= (\s -> (s .&. 255) + 256)
  -- PC begins set to the contents of the RESET vector, which exists only in emulation
  -- mode (which we are always in on a RESET), and is found at 0x00:fffc,d
  startPC <- rw_ 0x00fffc
  pc .= startPC


opcodes :: M.Map Word8 Op
opcodes = M.fromList [
    -- ADC
    (0x69, immediate op_ADC_immed),
    (0x6d, absoluteData op_ADC),
    (0x6f, absoluteLong op_ADC),
    (0x65, directPage op_ADC),
    (0x72, directPageIndirect op_ADC),
    (0x67, directPageIndirectLong op_ADC),
    (0x7d, absoluteIndexedX op_ADC),
    (0x7f, absoluteLongIndexedX op_ADC),
    (0x79, absoluteIndexedY op_ADC),
    (0x75, directPageIndexedX op_ADC),
    (0x61, directPageIndexedIndirectX op_ADC),
    (0x71, directPageIndexedY op_ADC),
    (0x77, directPageIndirectLongIndexedY op_ADC),
    (0x63, stackRelative op_ADC),
    (0x73, stackRelativeIndirectIndexedY op_ADC),

    -- AND
    (0x29, immediate op_AND_immed),
    (0x2d, absoluteData op_AND),
    (0x2f, absoluteLong op_AND),
    (0x25, directPage op_AND),
    (0x32, directPageIndirect op_AND),
    (0x27, directPageIndirectLong op_AND),
    (0x3d, absoluteIndexedX op_AND),
    (0x3f, absoluteLongIndexedX op_AND),
    (0x39, absoluteIndexedY op_AND),
    (0x35, directPageIndexedX op_AND),
    (0x21, directPageIndexedIndirectX op_AND),
    (0x31, directPageIndirectLongIndexedY op_AND),
    (0x37, stackRelative op_AND),
    (0x33, stackRelativeIndirectIndexedY op_AND),

    -- ASL
    (0x0a, accumulator op_ASL_acc),
    (0x0e, absoluteData op_ASL),
    (0x06, directPage op_ASL),
    (0x1e, absoluteIndexedX op_ASL),
    (0x16, directPageIndexedX op_ASL),


    -- BCC
    (0x90, pcRelative op_BCC),
    -- BCS
    (0xb0, pcRelative op_BCS),
    -- BEQ
    (0xf0, pcRelative op_BEQ),
    -- BMI
    (0x30, pcRelative op_BMI),
    -- BNE
    (0xd0, pcRelative op_BNE),
    -- BPL
    (0x10, pcRelative op_BPL),
    -- BRA
    (0x80, pcRelative op_BRA),
    -- BRL
    (0x82, pcRelativeLong op_BRL),
    -- BVC
    (0x50, pcRelative op_BVC),
    -- BVS
    (0x70, pcRelative op_BVS),


    -- BIT
    (0x89, immediate op_BIT_immed),
    (0x2c, absoluteData op_BIT),
    (0x24, directPage op_BIT),
    (0x3c, absoluteIndexedX op_BIT),
    (0x34, directPageIndexedX op_BIT),

    -- BRK
    (0x00, op_BRK),

    -- CLC
    (0x18, op_CLC),
    -- CLD
    (0xd8, op_CLD),
    -- CLI
    (0x58, op_CLI),
    -- CLV
    (0xb8, op_CLV),


    -- CMP
    (0xc9, immediate op_CMP_immed),
    (0xcd, absoluteData op_CMP),
    (0xcf, absoluteLong op_CMP),
    (0xc5, directPage op_CMP),
    (0xd2, directPageIndirect op_CMP),
    (0xc7, directPageIndirectLong op_CMP),
    (0xdd, absoluteIndexedX op_CMP),
    (0xdf, absoluteLongIndexedX op_CMP),
    (0xd9, absoluteIndexedY op_CMP),
    (0xd5, directPageIndexedX op_CMP),
    (0xc1, directPageIndexedIndirectX op_CMP),
    (0xd1, directPageIndirectIndexedY op_CMP),
    (0xd7, directPageIndirectLongIndexedY op_CMP),
    (0xc3, stackRelative op_CMP),
    (0xd3, stackRelativeIndirectIndexedY op_CMP),

    -- COP
    (0x02, op_COP),

    -- CPX
    (0xe0, immediate op_CPX_immed),
    (0xec, absoluteData op_CPX),
    (0xe4, directPage op_CPX),

    -- CPY
    (0xc0, immediate op_CPY_immed),
    (0xcc, absoluteData op_CPY),
    (0xc4, directPage op_CPY),

    -- DEC
    (0x3a, op_DEC_acc),
    (0xce, absoluteData op_DEC),
    (0xc6, directPage op_DEC),
    (0xde, absoluteIndexedX op_DEC),
    (0xd6, directPageIndexedX op_DEC),

    -- DEX
    (0xca, op_DEX),
    -- DEY
    (0x88, op_DEY),

    -- EOR
    (0x49, immediate op_EOR_immed),
    (0x4d, absoluteData op_EOR),
    (0x4f, absoluteLong op_EOR),
    (0x45, directPage op_EOR),
    (0x52, directPageIndirect op_EOR),
    (0x47, directPageIndirectLong op_EOR),
    (0x5d, absoluteIndexedX op_EOR),
    (0x5f, absoluteLongIndexedX op_EOR),
    (0x59, absoluteIndexedY op_EOR),
    (0x55, directPageIndexedX op_EOR),
    (0x41, directPageIndexedIndirectX op_EOR),
    (0x51, directPageIndirectIndexedY op_EOR),
    (0x57, directPageIndirectLongIndexedY op_EOR),
    (0x43, stackRelative op_EOR),
    (0x53, stackRelativeIndirectIndexedY op_EOR),


    -- INC
    (0x1a, op_INC_acc),
    (0xee, absoluteData op_INC),
    (0xe6, directPage op_INC),
    (0xfe, absoluteIndexedX op_INC),
    (0xf6, directPageIndexedX op_INC),
    -- INX
    (0xe8, op_INX),
    -- INY
    (0xc8, op_INY),

    -- JMP
    (0x4c, absoluteInstr op_JMP),
    (0x6c, absoluteIndirect op_JMP),
    (0x7c, absoluteIndexedIndirect op_JMP),
    (0x5c, absoluteLong op_JML),
    (0xdc, absoluteIndirectLong op_JML),

    -- JSL
    (0x22, absoluteLong op_JSL),
    -- JSR
    (0x20, absoluteInstr op_JSR),
    (0xfc, absoluteIndexedIndirect op_JSR),

    -- LDA
    (0xa9, immediate op_LDA_immed),
    (0xad, absoluteData op_LDA),
    (0xaf, absoluteLong op_LDA),
    (0xa5, directPage op_LDA),
    (0xb2, directPageIndirect op_LDA),
    (0xa7, directPageIndirectLong op_LDA),
    (0xbd, absoluteIndexedX op_LDA),
    (0xbf, absoluteLongIndexedX op_LDA),
    (0xb9, absoluteIndexedY op_LDA),
    (0xb5, directPageIndexedX op_LDA),
    (0xa1, directPageIndexedIndirectX op_LDA),
    (0xb1, directPageIndirectIndexedY op_LDA),
    (0xb7, directPageIndirectLongIndexedY op_LDA),
    (0xa3, stackRelative op_LDA),
    (0xb3, stackRelativeIndirectIndexedY op_LDA),
    -- LDX
    (0xa2, immediate op_LDX_immed),
    (0xae, absoluteData op_LDX),
    (0xa6, directPage op_LDX),
    (0xbe, absoluteIndexedY op_LDX),
    (0xb6, directPageIndexedY op_LDX),
    -- LDY
    (0xa0, immediate op_LDY_immed),
    (0xac, absoluteData op_LDY),
    (0xa4, directPage op_LDY),
    (0xbc, absoluteIndexedX op_LDY),
    (0xb4, directPageIndexedX op_LDY),

    -- LSR
    (0x4a, op_LSR_acc),
    (0x4e, absoluteData op_LSR),
    (0x46, directPage op_LSR),
    (0x5e, absoluteIndexedX op_LSR),
    (0x56, directPageIndexedX op_LSR),

    -- MVN
    (0x54, blockMove op_MVN),
    -- MVP
    (0x44, blockMove op_MVP),

    -- NOP
    (0xea, return ()),

    -- ORA
    (0x09, immediate op_ORA_immed),
    (0x0d, absoluteData op_ORA),
    (0x0f, absoluteLong op_ORA),
    (0x05, directPage op_ORA),
    (0x12, directPageIndirect op_ORA),
    (0x07, directPageIndirectLong op_ORA),
    (0x1d, absoluteIndexedX op_ORA),
    (0x1f, absoluteLongIndexedX op_ORA),
    (0x19, absoluteIndexedY op_ORA),
    (0x15, directPageIndexedX op_ORA),
    (0x01, directPageIndexedIndirectX op_ORA),
    (0x11, directPageIndirectIndexedY op_ORA),
    (0x17, directPageIndirectLongIndexedY op_ORA),
    (0x03, stackRelative op_ORA),
    (0x13, stackRelativeIndirectIndexedY op_ORA),

    -- PEA
    (0xf4, stackAbsolute op_PEA),
    -- PEI
    (0xd4, stackDirectPageIndirect op_PEI),
    -- PER
    (0x62, op_PER),

    -- PHA
    (0x48, op_PHA),
    -- PHB
    (0x8b, op_PHB),
    -- PHD
    (0x0b, op_PHD),
    -- PHK
    (0x4b, op_PHK),
    -- PHP
    (0x08, op_PHP),
    -- PHX
    (0xda, op_PHX),
    -- PHY
    (0x5a, op_PHY),

    -- PLA
    (0x68, op_PLA),
    -- PLX
    (0xfa, op_PLX),
    -- PLY
    (0x7a, op_PLY),
    -- PLB
    (0xab, op_PLB),
    -- PLD
    (0x2b, op_PLD),
    -- PLP
    (0x28, op_PLP),

    -- REP
    (0xc2, op_REP),

    -- ROL
    (0x2a, op_ROL_acc),
    (0x2e, absoluteData op_ROL),
    (0x26, directPage op_ROL),
    (0x3e, absoluteIndexedX op_ROL),
    (0x36, directPageIndexedX op_ROL),
    -- ROR
    (0x6a, op_ROR_acc),
    (0x6e, absoluteData op_ROR),
    (0x66, directPage op_ROR),
    (0x7e, absoluteIndexedX op_ROR),
    (0x76, directPageIndexedX op_ROR),


    -- RTI
    (0x40, op_RTI),
    -- RTL
    (0x6b, op_RTL),
    -- RTS
    (0x60, op_RTS),

    -- SBC
    (0xe9, immediate op_SBC_immed),
    (0xed, absoluteData op_SBC),
    (0xef, absoluteLong op_SBC),
    (0xe5, directPage op_SBC),
    (0xf2, directPageIndirect op_SBC),
    (0xe7, directPageIndirectLong op_SBC),
    (0xfd, absoluteIndexedX op_SBC),
    (0xff, absoluteLongIndexedX op_SBC),
    (0xf9, absoluteIndexedY op_SBC),
    (0xf5, directPageIndexedX op_SBC),
    (0xe1, directPageIndexedIndirectX op_SBC),
    (0xf1, directPageIndirectIndexedY op_SBC),
    (0xf7, directPageIndirectLongIndexedY op_SBC),
    (0xe3, stackRelative op_SBC),
    (0xf3, stackRelativeIndirectIndexedY op_SBC),

    -- SEC
    (0x38, op_SEC),
    -- SED
    (0xf8, op_SED),
    -- SEI
    (0x78, op_SEI),

    -- SEP
    (0xe2, op_SEP),

    -- STA
    (0x8d, absoluteData op_STA),
    (0x8f, absoluteLong op_STA),
    (0x85, directPage op_STA),
    (0x92, directPageIndirect op_STA),
    (0x87, directPageIndirectLong op_STA),
    (0x9d, absoluteIndexedX op_STA),
    (0x9f, absoluteLongIndexedX op_STA),
    (0x99, absoluteIndexedY op_STA),
    (0x95, directPageIndexedX op_STA),
    (0x81, directPageIndexedIndirectX op_STA),
    (0x91, directPageIndirectIndexedY op_STA),
    (0x97, directPageIndirectLongIndexedY op_STA),
    (0x83, stackRelative op_STA),
    (0x93, stackRelativeIndirectIndexedY op_STA),

    -- STP
    (0xdb, op_STP),

    -- STX
    (0x8e, absoluteData op_STX),
    (0x86, directPage op_STX),
    (0x96, directPageIndexedY op_STX),
    -- STY
    (0x8c, absoluteData op_STY),
    (0x84, directPage op_STY),
    (0x94, directPageIndexedX op_STY),

    -- STZ
    (0x9c, absoluteData op_STZ),
    (0x64, directPage op_STZ),
    (0x9e, absoluteIndexedX op_STZ),
    (0x74, directPageIndexedX op_STZ),

    -- TAX
    (0xaa, op_TAX),
    -- TAY
    (0xa8, op_TAY),
    -- TCD
    (0x5b, op_TCD),
    -- TCS
    (0x1b, op_TCS),
    -- TDC
    (0x7b, op_TDC),

    -- TRB
    (0x1c, absoluteData op_TRB),
    (0x14, directPage op_TRB),

    -- TSB
    (0x0c, absoluteData op_TSB),
    (0x04, directPage op_TSB),

    -- TSC
    (0x3b, op_TSC),
    -- TSX
    (0xba, op_TSX),
    -- TXA
    (0x8a, op_TXA),
    -- TXS
    (0x9a, op_TXS),
    -- TXY
    (0x9b, op_TXY),
    -- TYA
    (0x98, op_TYA),
    -- TYX
    (0xbb, op_TYX),

    -- WAI
    (0xcb, op_WAI),

    -- WDM
    (0x42, op_WDM),

    -- XBA
    (0xeb, op_XBA),

    -- XCE
    (0xfb, op_XCE)
  ]



-- Given two lists of digits (least significant first) and the carry (1 or 0) returns
-- the combined list and the carry.
iterateDecimalPlus :: [Word16] -> [Word16] -> Word16 -> ([Word16], Word16)
iterateDecimalPlus l r c = (\(res, c') -> (reverse res, c')) $ iterateDecimalPlus' l r c []
  where iterateDecimalPlus' [] [] c res = (res, c)
        iterateDecimalPlus' (l:ls) (r:rs) c res | l+r+c >= 10 = iterateDecimalPlus' ls rs 1 (l+r+c - 10 : res)
                                                | otherwise = iterateDecimalPlus' ls rs 0 (l+r+c : res)


op_ADC :: OpA
op_ADC addr = read addr >>= op_ADC_immed

op_ADC_immed :: OpW
op_ADC_immed val = do
  acc <- getA
  c <- use carry
  d <- use decimal
  if d
    then do
      m <- shortMemory
      let shifts = if m then [0,4] else [0,4,8,12]
          dsVal = map (\b -> (val `shiftR` b) .&. 15) shifts
          dsAcc = map (\b -> (acc `shiftR` b) .&. 15) shifts
          (dsRes, c') = iterateDecimalPlus dsVal dsAcc (if c then 1 else 0)
          res = foldl' (\r (s,v) -> (r `shiftL` s) + v) 0 $ zip shifts dsRes
      setA res
      carry .= (c' == 1)
      negative .= False
      overflow .= False
      zero .= (res == 0)
    else do
      let result_ = fi val + fi acc + (if c then 1 else 0) :: Word32
          result  = fi result_
      -- store the result
      setA result

      -- adjust the flags
      carry .= result_ ^. bitAt 16
      flags [N,Z] result

      m <- shortMemory
      let b = if m then 7 else 15
          sVal = val ^. bitAt b
          sAcc = acc ^. bitAt b
          sRes = result ^. bitAt b
      overflow .= (sVal == sAcc && sRes /= sVal)


op_AND :: OpA
op_AND addr = read addr >>= op_AND_immed

op_AND_immed :: OpW
op_AND_immed val = do
  acc <- getA
  let res = acc .&. val
  setA res
  flags [N,Z] res


op_ASL_acc :: OpW
op_ASL_acc val = do
  r <- doASL val
  setA r
  flags [N,Z] r

op_ASL :: OpA
op_ASL addr = do
  val <- read addr
  r <- doASL val
  write addr r
  flags [N,Z] r

doASL :: Word16 -> CPU Word16
doASL val = do
  m <- shortMemory
  carry .= val ^. bitAt (if m then 8 else 16)
  return (val `shiftL` 1)



-- branch helper
branchOn :: ALens' CPUState Bool -> Bool -> Word32 -> CPU ()
branchOn l b1 delta = do
  b2 <- use (cloneLens l)
  if b1 == b2
    then pc %= (+ (fi $ delta .&. 65535))
    else return ()


op_BCC :: OpA
op_BCC = branchOn carry False

op_BCS :: OpA
op_BCS = branchOn carry True

op_BEQ :: OpA
op_BEQ = branchOn zero True

op_BNE :: OpA
op_BNE = branchOn zero False

op_BMI :: OpA
op_BMI = branchOn negative True

op_BPL :: OpA
op_BPL = branchOn negative False

op_BVC :: OpA
op_BVC = branchOn overflow False

op_BVS :: OpA
op_BVS = branchOn overflow True


op_BRA :: OpA
op_BRA addr = do
  let newPC = fi (addr .&. 65535)
  pc .= newPC


op_BRL :: OpA
op_BRL addr = do
  let newPC = fi $ addr .&. 65535
  pc .= newPC



op_BIT_immed' :: Bool -> OpW
op_BIT_immed' setNV val = do
  m <- shortMemory
  let (hibit, lobit) = if m then (8, 7) else (15, 14)
  when setNV $ do
    negative .= val ^. bitAt hibit
    overflow .= val ^. bitAt lobit

  acc <- getA
  let res = val .&. acc
  zero .= (res == 0)

op_BIT_immed :: OpW
op_BIT_immed = op_BIT_immed' False

op_BIT :: OpA
op_BIT addr = do
  val <- read addr
  op_BIT_immed' True val



op_BRK :: Op
op_BRK = do
  _ <- pcFetch -- skip over the signature byte
  e <- use emulation
  if e
    then do
      thePC <- use pc
      pushW thePC
      break .= True
      p <- use status
      pushB p
      irqDisable .= True
      decimal .= False
      vector <- rw_ 0xfffe
      pc .= vector
    else do
      bank <- use pbr
      pushB bank
      thePC <- use pc
      pushW thePC
      p <- use status
      pushB p
      irqDisable .= True
      pbr .= 0
      vector <- rw_ 0x00ffe6
      pc .= vector
      decimal .= False



op_CLC :: Op
op_CLC = carry .= False

op_CLD :: Op
op_CLD = decimal .= False

op_CLI :: Op
op_CLI = irqDisable .= False

op_CLV :: Op
op_CLV = overflow .= False


op_CMP_immed :: OpW
op_CMP_immed val = do
  acc <- getA
  let res = acc - val
  flags [N,Z] res
  carry .= (acc >= val)


op_CMP :: OpA
op_CMP addr = read addr >>= op_CMP_immed


op_COP :: Op
op_COP = do
  _ <- pcFetch -- fetch the dummy signature byte in order to increment the PC.
  m <- shortMemory
  if m
    then do -- emulation mode
      thePC <- use pc
      pushW thePC
      p <- use status
      pushB p
      irqDisable .= True
      vector <- rw_ 0xfff4
      pc .= vector
      decimal .= False
    else do
      bank <- use pbr
      pushB bank
      thePC <- use pc
      pushW thePC
      p <- use status
      pushB p
      irqDisable .= True
      pbr .= 0
      vector <- rw_ 0x00ffe4
      decimal .= False



op_CPX_immed :: OpW
op_CPX_immed val = do
  base <- getX
  let res = base - val
  flags [N,Z] res
  carry .= (base >= val)

op_CPX :: OpA
op_CPX addr = read addr >>= op_CPX_immed


op_CPY_immed :: OpW
op_CPY_immed val = do
  base <- getY
  let res = base - val
  flags [N,Z] res
  carry .= (base >= val)

op_CPY :: OpA
op_CPY addr = read addr >>= op_CPY_immed



op_DEC_acc :: Op
op_DEC_acc = regAdjuster getA setA (subtract 1)

op_DEC :: OpA
op_DEC addr = do
  val <- read addr
  write addr (val-1)
  flags [N,Z] (val-1)



regAdjuster :: CPU Word16 -> (Word16 -> CPU ()) -> (Word16 -> Word16) -> Op
regAdjuster r w f = do
  val <- r
  let res = f val
  w res
  flags [N,Z] res

op_DEX :: Op
op_DEX = regAdjuster getX setX (subtract 1)

op_DEY :: Op
op_DEY = regAdjuster getY setY (subtract 1)



op_EOR_immed :: OpW
op_EOR_immed val = regAdjuster getA setA (xor val)

op_EOR :: OpA
op_EOR addr = read addr >>= op_EOR_immed



op_INC_acc :: Op
op_INC_acc = regAdjuster getA setA (+1)

op_INC :: OpA
op_INC addr = regAdjuster (read addr) (write addr) (+1)

op_INX :: Op
op_INX = regAdjuster getX setX (+1)

op_INY :: Op
op_INY = regAdjuster getY setY (+1)



-- JMP and JML are the same, except for long addressing.
-- Here they're implemented as two separate instructions, so that we can overwrite the
-- pbr for long jumps.
-- We can't check whether the bank segment is 0, since that might be a legit long jump.
op_JMP :: OpA
op_JMP addr = pc .= fi addr

op_JML :: OpA
op_JML addr = do
  pc .= fi addr
  pbr .= fi (addr `shiftR` 16)



op_JSL :: OpA
op_JSL addr = do
  ret <- subtract 1 <$> use pc
  retBank <- use pbr
  pushB retBank
  pushW ret
  pbr .= fi (addr `shiftR` 16)
  pc .= fi addr

-- NB: Only short (16-bit) address jumps come here. Long jumps go to JSL above.
op_JSR :: OpA
op_JSR addr = do
  ret <- subtract 1 <$> use pc
  pushW ret
  pc .= fi addr


op_LDA_immed :: OpW
op_LDA_immed val = regAdjuster (return val) setA id

op_LDA :: OpA
op_LDA addr = regAdjuster (read addr) setA id


op_LDX_immed :: OpW
op_LDX_immed val = regAdjuster (return val) setX id

op_LDX :: OpA
op_LDX addr = regAdjuster (read addr) setX id

op_LDY_immed :: OpW
op_LDY_immed val = regAdjuster (return val) setY id

op_LDY :: OpA
op_LDY addr = regAdjuster (read addr) setY id


doLSR :: Word16 -> CPU Word16
doLSR val = do
  carry .= val ^. bitAt 0
  return (val `shiftR` 1)

op_LSR_acc :: Op
op_LSR_acc = do
  acc <- getA
  acc' <- doLSR acc
  setA acc
  flags [N,Z] acc

op_LSR :: OpA
op_LSR addr = do
  val <- read addr
  val' <- doLSR val
  write addr val'
  flags [N,Z] val'


-- TODO: Should be block moves be able to copy bytes into the next bank, or should
-- they wrap within the bank? Currently the former.
op_MVN :: OpMove
op_MVN src dst len = do
  let loop s d l = do
        v <- rb_ s
        wb_ d v
        if l == 0
          then return (s+1, d+1, l-1)
          else loop (s+1) (d+1) (l-1)
  (src', dst', len') <- loop src dst len
  setX (fi src')
  setY (fi dst')
  a .= 0xffff
  dbr .= fi (dst `shiftR` 16)

op_MVP :: OpMove
op_MVP src dst len = do
  let loop s d l = do
        v <- rb_ s
        wb_ d v
        if l == 0
          then return (s-1, d-1, l-1)
          else loop (s-1) (d-1) (l-1)
  (src', dst', len') <- loop src dst len
  setX (fi src')
  setY (fi dst')
  a .= 0xffff
  dbr .= fi (dst `shiftR` 16)


op_ORA_immed :: OpW
op_ORA_immed val = regAdjuster getA setA (.|. val)

op_ORA :: OpA
op_ORA addr = read addr >>= op_ORA_immed


op_PEA :: OpW
op_PEA = pushW

op_PEI :: OpW
op_PEI = pushW

op_PER :: Op
op_PER = do
  offset <- pcFetch2
  thePC <- use pc
  let newPC = offset + thePC
  pushW newPC


doPush :: ALens' CPUState Bool -> Word16 -> CPU ()
doPush l val = do
  short <- use (cloneLens l)
  if short
    then pushB (fi val)
    else pushW val

op_PHA :: Op
op_PHA = getA >>= doPush memorySelect

op_PHB :: Op
op_PHB = use dbr >>= pushB


op_PHD :: Op
op_PHD = use dp >>= pushW

op_PHK :: Op
op_PHK = use pbr >>= pushB

op_PHP :: Op
op_PHP = use status >>= pushB


op_PHX :: Op
op_PHX = getX >>= doPush indexSelect

op_PHY :: Op
op_PHY = getY >>= doPush indexSelect


doPull :: ALens' CPUState Bool -> (Word16 -> CPU ()) -> CPU ()
doPull l store = do
  short <- use (cloneLens l)
  if short
    then doPullShort (store.fi)
    else do
      w <- pullW
      store w
      negative .= w ^. bitAt 15
      zero .= (w == 0)

op_PLA :: Op
op_PLA = doPull memorySelect setA

op_PLX :: Op
op_PLX = doPull indexSelect setX

op_PLY :: Op
op_PLY = doPull indexSelect setY


doPullShort :: (Word8 -> CPU ()) -> CPU ()
doPullShort store = do
  b <- pullB
  store b
  negative .= b ^. bitAt 7
  zero .= (b == 0)

op_PLB :: Op
op_PLB = doPullShort (dbr .=)

op_PLD :: Op
op_PLD = do
  w <- pullW
  flags [N,Z] w
  dp .= w

op_PLP :: Op
op_PLP = doPullShort (status .=)



op_REP :: Op
op_REP = do
  mask_ <- pcFetch
  e <- use emulation
  let mask = if e then 0xcf .&. mask_ else mask_ -- mask out bits 4 and 5 in emu mode
  status %= (.&. complement mask) -- and mask the status byte with the inversion


op_ROL_acc :: Op
op_ROL_acc = getA >>= doRollLeft >>= setA

doRollLeft :: Word16 -> CPU Word16
doRollLeft val = do
  m <- shortMemory
  if m
    then do
      let acc = fi val :: Word8
          newC = acc ^. bitAt 7
      oldC <- use carry
      let acc' = acc `shiftL` 1
          acc'' = (acc' .&. 0xfe) + (if oldC then 1 else 0)
      carry .= newC
      negative .= acc'' ^. bitAt 7
      zero .= (acc'' == 0)
      return (fi acc'')
    else do
      let acc = val
          newC = acc ^. bitAt 15
      oldC <- use carry
      let acc' = acc `shiftL` 1
          acc'' = (acc' .&. 0xfffe) + (if oldC then 1 else 0)
      carry .= newC
      flags [N,Z] acc''
      return acc''


op_ROL :: OpA
op_ROL addr = read addr >>= doRollLeft >>= write addr



doRollRight :: Word16 -> CPU Word16
doRollRight val = do
  m <- shortMemory
  if m
    then do
      let acc = fi val :: Word8
          newC = acc ^. bitAt 0
      oldC <- use carry
      let acc' = acc `shiftR` 1
          acc'' = (acc' .&. 0x7f) .|. (if oldC then 128 else 0)
      carry .= newC
      negative .= acc'' ^. bitAt 7
      zero .= (acc'' == 0)
      return (fi acc'')
    else do
      let acc = val
          newC = acc ^. bitAt 0
      oldC <- use carry
      let acc' = acc `shiftR` 1
          acc'' = (acc' .&. 0x7fff) .|. (if oldC then 32768 else 0)
      carry .= newC
      flags [N,Z] acc''
      return acc''

op_ROR_acc :: Op
op_ROR_acc = getA >>= doRollRight >>= setA

op_ROR :: OpA
op_ROR addr = read addr >>= doRollRight >>= write addr



op_RTI :: Op
op_RTI = do
  retAddr <- pullW
  pc .= retAddr

  e <- use emulation
  when (not e) $ do
    bank <- pullB
    pbr .= bank

  p <- pullB
  status .= p


op_RTL :: Op
op_RTL = do
  retAddr <- pullW
  pc .= retAddr+1

  bank <- pullB
  pbr .= bank

op_RTS :: Op
op_RTS = do
  retAddr <- pullW
  pc .= retAddr+1


-- Given two lists of digits (least significant first) and the carry (1 or 0) returns
-- the combined list and the carry.
iterateDecimalSub :: [Word16] -> [Word16] -> Word16 -> ([Word16], Word16)
iterateDecimalSub l r c = iterateDecimalSub' l r c []
  where iterateDecimalSub' [] [] c res = (reverse res, c)
        iterateDecimalSub' (l:ls) (r:rs) c res | l-r-c < 0 = iterateDecimalSub' ls rs 1 (l-r-c + 10 : res)
                                                | otherwise = iterateDecimalSub' ls rs 0 (l-r-c : res)


op_SBC :: OpA
op_SBC addr = read addr >>= op_SBC_immed

op_SBC_immed :: OpW
op_SBC_immed val = do
  acc <- getA
  c <- use carry
  d <- use decimal
  if d
    then do
      m <- shortMemory
      let shifts = if m then [0,4] else [0,4,8,12]
          dsVal = map (\b -> (val `shiftR` b) .&. 15) shifts
          dsAcc = map (\b -> (acc `shiftR` b) .&. 15) shifts
          (dsRes, c') = iterateDecimalSub dsAcc dsVal (if c then 0 else 1)
          res = foldl' (\r (s,v) -> (r `shiftL` s) + v) 0 $ zip shifts dsRes
      setA res
      carry .= (c' == 0)
      negative .= False
      overflow .= False
      zero .= (res == 0)
    else do
      let result_ = fi acc - fi val + (if c then 0 else 1) :: Word32
          result  = fi result_
      -- store the result
      setA result

      -- adjust the flags
      carry .= (val <= acc) -- set if a borrow was not required
      flags [N,Z] result

      m <- shortMemory
      let b = if m then 7 else 15
          sVal = val ^. bitAt b
          sAcc = acc ^. bitAt b
          sRes = result ^. bitAt b
      overflow .= (sVal == sAcc && sRes /= sVal)

op_SEC :: Op
op_SEC = carry .= True

op_SED :: Op
op_SED = decimal .= True

op_SEI :: Op
op_SEI = irqDisable .= True


op_SEP :: Op
op_SEP = do
  b <- pcFetch
  e <- use emulation
  let mask = if e then b .&. 0xcf else b
  status %= (.|. mask)


op_STA :: OpA
op_STA addr = getA >>= write addr


-- TODO: Implement STP properly when resets are implemented too.
op_STP :: Op
op_STP = error "STP"


doStoreIndex :: Word32 -> Word16 -> CPU ()
doStoreIndex addr val = do
  ix <- shortIndex
  if ix
    then wb_ addr (fi val)
    else ww_ addr val


op_STX :: OpA
op_STX addr = getX >>= doStoreIndex addr

op_STY :: OpA
op_STY addr = getY >>= doStoreIndex addr


op_STZ :: OpA
op_STZ addr = write addr 0


op_TAX :: Op
op_TAX = do
  m <- shortMemory
  ix <- shortIndex
  case (m,ix) of
    (True, True) -> do
      b <- fi <$> getA
      flagsB [N,Z] b
      x .= fi b
    (True, False) -> do
      w <- use a
      flagsW [N,Z] w
      x .= w
    (False, True) -> do
      w <- getA
      let b = fi w
      flagsB [N,Z] b
      x .= fi b
    (False, False) -> do
      w <- getA
      flagsW [N,Z] w
      x .= w

op_TAY :: Op
op_TAY = do
  m <- shortMemory
  ix <- shortIndex
  case (m, ix) of
    (True, True) -> do
      b <- fi <$> getA
      flagsB [N,Z] b
      y .= fi b
    (True, False) -> do
      w <- use a
      flagsW [N,Z] w
      y .= w
    (False, True) -> do
      w <- getA
      let b = fi w
      flagsB [N,Z] b
      y .= fi b
    (False, False) -> do
      w <- getA
      flagsW [N,Z] w
      y .= w


op_TCD :: Op
op_TCD = do
  acc <- use a
  dp .= acc
  flagsW [N,Z] acc


op_TCS :: Op
op_TCS = do
  acc <- use a
  e <- use emulation
  let val = if e
              then 256 .|. (acc .&. 255)
              else acc
  sp .= val

op_TDC :: Op
op_TDC = do
  val <- use dp
  a .= val
  flagsW [N,Z] val


op_TRB :: OpA
op_TRB addr = do
  acc <- getA
  val <- read addr
  let res = complement acc .&. val
  write addr res
  zero .= (0 == (acc .&. val)) -- yes, this is a separate computation from the above.


op_TSB :: OpA
op_TSB addr = do
  acc <- getA
  val <- read addr
  let res = acc .|. val
  write addr res
  zero .= (0 == (acc .&. val)) -- yes, this is a separate computation from the above.

op_TSC :: Op
op_TSC = do
  val <- use sp
  a .= val
  flagsW [N,Z] val


op_TSX :: Op
op_TSX = do
  val <- use sp
  ix <- shortIndex
  if ix
    then do
      flagsB [N,Z] (fi val) -- TODO: Is this correct? Probably, but manual isn't clear.
      setX val
    else do
      flagsW [N,Z] val
      setX val


op_TXA :: Op
op_TXA = do
  m <- shortMemory
  ix <- shortIndex
  case (m,ix) of
    (True, True) -> do
      b <- fi <$> use x
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (False, True) -> do
      b <- fi <$> use x
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (True, False) -> do
      b <- fi <$> use x
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (False, False) -> do
      w <- use x
      flagsW [N,Z] w
      a .= w


op_TXS :: Op
op_TXS = do
  val <- use x
  e <- use emulation
  if e
    then sp .= 256 + (255 .&. val)
    else sp .= val

op_TXY :: Op
op_TXY = do
  val <- getX
  setY val
  flagsX [N,Z] val


op_TYA :: Op
op_TYA = do
  m <- shortMemory
  ix <- shortIndex
  case (m,ix) of
    (True, True) -> do
      b <- fi <$> use y
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (False, True) -> do
      b <- fi <$> use y
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (True, False) -> do
      b <- fi <$> use y
      flagsB [N,Z] b
      a %= (\acc -> fi b .|. (0xff00 .&. acc))
    (False, False) -> do
      w <- use y
      flagsW [N,Z] w
      a .= w


op_TYX :: Op
op_TYX = do
  val <- getY
  setX val
  flagsX [N,Z] val


-- TODO: Implement me when we have proper interrupt handling.
op_WAI :: Op
op_WAI = error "WAI"


-- Treat the reserved-for-expansion placeholder WDM as a two-byte NOP.
op_WDM :: Op
op_WDM = pcFetch >> return ()


op_XBA :: Op
op_XBA = do
  acc <- use a
  let lo = (fi $ acc .&. 255) :: Word8
      hi = (fi $ acc `shiftR` 8) :: Word8
      acc' = (fi lo `shiftL` 8) .|. fi hi
  a .= acc'
  flagsB [N,Z] hi -- set based on the /new/ low-order byte, regardless of mode.


op_XCE :: Op
op_XCE = do
  e <- use emulation
  if e
    then do -- switching from emulation to native
      carry .= True
      emulation .= False
      -- m and x are set to 1
      indexSelect .= True
      memorySelect .= True
    else do
      carry .= False
      emulation .= True
      -- TODO: Figure out the m and x bits to always be 1 in emulation mode.
      -- Overwrite the high bytes of the index registers
      x %= (.&. 255)
      y %= (.&. 255)
      sp %= (\s -> (s .&. 255) + 256)





-- Memory maps

mmap :: Word32 -> CPU Word32
mmap addr = case (fi (addr `shiftR` 16), fi addr :: Word16) of
  (bank, base)
    | bank <= 0x3f && base <= 0x1fff -> return $ 0x7e0000 + fi base -- LowRAM mirror 1
    | bank <= 0x3f && 0x2000 <= bank && bank <= 0x4fff -> return $ fi base -- hardware control
    | bank /= 0x7e && bank /= 0x7f && base >= 0x8000 -> return $ fi base -- cart ROM
    | 0x70 <= bank && bank <= 0x77 && base < 0x8000  -> return addr    -- TODO: save RAM
    | 0x80 <= bank && bank <= 0xef -> mmap $ (fi (base-0x80) `shiftL` 16) + fi base -- mirror
    | 0xf0 <= bank && base >= 0x8000 -> return $ fi base -- ROM mirror


registersRead :: M.Map Word32 (CPU Word8)
registersRead = M.fromList []


registersWrite :: M.Map Word32 (Word8 -> CPU ())
--registersWrite = M.empty
registersWrite = M.fromList [
  (0x2100, bitmapByte [Bit 7 screenBlank, Range 0xf screenBrightness]),
  (0x2101, bitmapByte [Range 0xe0 objectSize, {- Range 0x18 UNIMPLEMENTED, -} Range 0x07 objectBase]),
  (0x2102, \b -> hardware.objectTableAddress .= fi b * 2),
  (0x2103, bitmapByte [Bit 7 objectPriority, Bit 0 objectTableSelect]),
  (0x2104, objectTableWrite),
  (0x2105, bitmapByte [FlagList 0xf0 bgSize, Bit 3 mode1BG3Priority, Range 0x07 graphicsMode]),
  (0x2106, bitmapByte [Range 0xf0 mosaicSize, FlagList 0x0f mosaicBGs]),
  (0x2107, backgroundAddressAndSize 0),
  (0x2108, backgroundAddressAndSize 1),
  (0x2109, backgroundAddressAndSize 2),
  (0x210a, backgroundAddressAndSize 3),
  (0x210b, bitmapByte [Range 0xf0 (singular (backgrounds.ix 1.tilemapHigh)), Range 0x0f (singular (backgrounds.ix 0.tilemapHigh))]),
  (0x210c, bitmapByte [Range 0xf0 (singular (backgrounds.ix 3.tilemapHigh)), Range 0x0f (singular (backgrounds.ix 1.tilemapHigh))]),

  (0x210d, backgroundHorizontalOffsetWrite 0),
  (0x210e, backgroundVerticalOffsetWrite 0),
  (0x210f, backgroundHorizontalOffsetWrite 1),
  (0x2110, backgroundVerticalOffsetWrite 1),
  (0x2111, backgroundHorizontalOffsetWrite 2),
  (0x2112, backgroundVerticalOffsetWrite 2),
  (0x2113, backgroundHorizontalOffsetWrite 3),
  (0x2114, backgroundVerticalOffsetWrite 3),

  (0x2115, bitmapByte [Bit 7 incrementAfterHigh, Range 0x0c vramAddressRemapping, Range 0x03 vramAddressIncrementing]),
  (0x2116, bitmapByte [Lo vramAddress]),
  (0x2117, bitmapByte [Hi vramAddress]),


  (0, undefined)
  ]


backgroundAddressAndSize :: Int -> Word8 -> CPU ()
backgroundAddressAndSize n = bitmapByte [Range 0xfc (singular (backgrounds.ix n.tilemapAddress)), Bit 1 (singular (backgrounds.ix n.verticalMirror)), Bit 0 (singular (backgrounds.ix n.horizontalMirror))]

-- Takes the background number, and then performs the necessary permutations. See below above data Background
-- for how the background offsets are computed.
backgroundHorizontalOffsetWrite :: Int -> Word8 -> CPU ()
backgroundHorizontalOffsetWrite n new = do
  partial <- use (hardware.backgroundOffsetPartial)
  old <- use $ singular (hardware.backgrounds.ix n.horizontalOffset)
  let val = (fi new `shiftL` 8) .|. (fi partial .&. 0xf8) .|. ((old `shiftR` 8) .&. 0x07)
  hardware.backgrounds.ix n.horizontalOffset .= val
  hardware.backgroundOffsetPartial .= new

backgroundVerticalOffsetWrite :: Int -> Word8 -> CPU ()
backgroundVerticalOffsetWrite n new = do
  partial <- use (hardware.backgroundOffsetPartial)
  let val = (fi new `shiftL` 8) .|. fi partial
  hardware.backgrounds.ix n.verticalOffset .= val
  hardware.backgroundOffsetPartial .= new


objectTableWrite :: Word8 -> CPU ()
objectTableWrite val = do
  table <- use $ hardware.objectTableSelect
  addr  <- use $ hardware.objectTableAddress
  m     <- use $ hardware.oam
  if table
    then do -- high table
      -- immediately write the specified byte, and increment the address
      liftIO $ writeArray m (512 + fi addr) val
      hardware.objectTableAddress += 1
    else do -- low table
      -- Either write two bytes, or cache this low byte.
      mlo <- use $ hardware.objectTableLowByte
      case mlo of
        Nothing -> do
          hardware.objectTableLowByte .= Just val
          hardware.objectTableAddress += 1
        Just lo -> do
          liftIO $ writeArray m (fi addr - 1) lo
          liftIO $ writeArray m (fi addr) val
          hardware.objectTableLowByte .= Nothing



data Bitmap = Bit Int (ALens' Hardware Bool)
            | Range Word8 (ALens' Hardware Word8)
            | Byte (ALens' Hardware Word8)
            | FlagList Word8 (ALens' Hardware [Bool])
            | Lo (ALens' Hardware Word16)
            | Hi (ALens' Hardware Word16)

bitmapByte :: [Bitmap] -> Word8 -> CPU ()
bitmapByte [] _ = return ()
bitmapByte (Bit n l:rest) val         = (hardware.cloneLens l .= (val ^. bitAt n)) >> bitmapByte rest val
bitmapByte (Range mask l:rest) val    = (hardware.cloneLens l .= (val .&. mask)) >> bitmapByte rest val
bitmapByte (Byte l:rest) val          = (hardware.cloneLens l .= val) >> bitmapByte rest val
bitmapByte (Lo l:rest) val            = (hardware.cloneLens l %= (\v -> (v .&. 0xff00) .|. fi val)) >> bitmapByte rest val
bitmapByte (Hi l:rest) val            = (hardware.cloneLens l %= (\v -> (fi val `shiftL` 8) .|. (v .&. 0xff))) >> bitmapByte rest val
bitmapByte (FlagList mask l:rest) val = do
  -- Count the trailing zeroes in the mask, and then shift by that amount.
  let trailing = fst . head . dropWhile (not.snd) $ map (\n -> (n, testBit val n)) [0..]
      val' = (val .&. mask) `shiftR` trailing
      leading = fst . head . dropWhile (not.snd) $ map (\n -> (n, testBit val' n)) (reverse [0..7])
  hardware.cloneLens l .= map (testBit val') [0..leading]
  bitmapByte rest val