{-# LANGUAGE TemplateHaskell
           , RecordWildCards
           , DataKinds
           , FlexibleContexts
           , GADTs #-}
module GB.CPU where

import Lens.Micro.Platform
import Data.Word
import Data.Bits
import qualified Data.Vector.Sized as V
import Prelude hiding (subtract)

class Convert a b where
  convert :: a -> b

data FlagRegister = FlagRegister { _zero      :: Bool
                                 , _subtract  :: Bool
                                 , _halfCarry :: Bool
                                 , _carry     :: Bool
                                 }
                    deriving Show

makeLenses ''FlagRegister

zeroFlagPosition :: Int
zeroFlagPosition = 7
subtractFlagPosition :: Int
subtractFlagPosition = 6
halfCarryFlagPosition :: Int
halfCarryFlagPosition = 5
carryFlagPosition :: Int
carryFlagPosition = 4

instance Convert FlagRegister Word8 where
  convert r =
    ((`shiftL` zeroFlagPosition) $ if r ^. zero then 1 else 0) +
    ((`shiftL` subtractFlagPosition) $ if r ^. subtract then 1 else 0) +
    ((`shiftL` halfCarryFlagPosition) $ if r ^. halfCarry then 1 else 0) +
    ((`shiftL` carryFlagPosition) $ if r ^. carry then 1 else 0)
  
instance Convert Word8 FlagRegister where
  convert w =
    let _zero = (shiftR w zeroFlagPosition .&. 1 /= 0)
        _subtract = (shiftR w subtractFlagPosition .&. 1 /= 0)
        _halfCarry = (shiftR w halfCarryFlagPosition .&. 1 /= 0)
        _carry = (shiftR w carryFlagPosition .&. 1 /= 0) in
      FlagRegister {..}
      
  
data Registers = Registers { _a     :: Word8
                           , _b     :: Word8
                           , _c     :: Word8
                           , _d     :: Word8
                           , _e     :: Word8
                           , _f     :: Word8
                           , _g     :: Word8
                           , _h     :: Word8
                           , _l     :: Word8
                           , _flags :: FlagRegister
                           }

makeLenses ''Registers

getBC :: Registers -> Word16
getBC r = (( .<<. 8) . fromIntegral $ r ^. b) + (fromIntegral $ r ^. c)

setBC :: Registers -> Word16 -> Registers
setBC r w = r & b %~ (const $ fromIntegral $ w .>>. 8) & c %~ (const $ fromIntegral $ w .&. 255)

getHL :: Registers -> Word16
getHL r = ((.<<. 8) . fromIntegral $ r ^. h) + (fromIntegral $ r ^. l)

setHL :: Registers -> Word16 -> Registers
setHL r w = r & h %~ (const $ fromIntegral $ w .>>. 8) & l %~ (const $ fromIntegral $ w .&. 255)

data CPU = CPU { _registers :: Registers
               , _pc :: Word16
               , _sp :: Word16
               , _bus :: (V.Vector 65536 Word8) }
makeLenses ''CPU

type ArithmeticTarget = Lens' Registers Word8

data Instruction where
  AddR :: ArithmeticTarget -> Bool -> Instruction
  AddHL :: Bool -> Instruction
  AddN :: Word8 -> Bool -> Instruction
  SubR :: ArithmeticTarget -> Bool -> Instruction
  SubHL :: Bool -> Instruction
  SubN :: Word8 -> Bool -> Instruction
  CpR :: ArithmeticTarget -> Bool -> Instruction
  CpHL :: Bool -> Instruction
  CpN :: Word8 -> Bool -> Instruction
  IncR :: ArithmeticTarget -> Instruction
  IncHL :: Instruction
  DecR :: ArithmeticTarget -> Instruction
  DecHL :: Instruction

execute :: CPU -> Instruction -> CPU
execute cpu = \case
  AddR t _c -> let value = cpu ^. registers . t
                   (newValue, newFlags) = add (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
               cpu & registers . a .~ newValue & registers . flags .~ newFlags
  AddHL _c -> let value = fetch cpu . getHL $ cpu ^. registers
                  (newValue, newFlags) = add (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
              cpu & registers . a .~ newValue & registers . flags .~ newFlags
  AddN value _c -> let (newValue, newFlags) = add (cpu ^. registers . a) value $ _c && cpu ^. registers .flags . carry in
                cpu & registers . a .~ newValue & registers . flags .~ newFlags
  SubR t _c -> let value =  cpu ^. registers . t
                   (newValue, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
               cpu & registers . a .~ newValue & registers . flags .~ newFlags
  SubHL _c -> let value = fetch cpu . getHL $ cpu ^. registers
                  (newValue, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
              cpu & registers . a .~ newValue & registers . flags .~ newFlags
  SubN value _c -> let (newValue, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers .flags . carry in
                   cpu & registers . a .~ newValue & registers . flags .~ newFlags
  CpR t _c -> let value = cpu ^. registers . t
                  (_, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
              cpu & registers . flags .~ newFlags
  CpHL _c -> let value = fetch cpu . getHL $ cpu ^. registers
                 (_, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers . flags . carry in
           cpu & registers . flags .~ newFlags
  CpN value _c -> let (_, newFlags) = sub (cpu ^. registers . a) value $ _c && cpu ^. registers .flags . carry in
                cpu & registers . flags .~ newFlags
  IncR t -> let (value, newFlags) = add (cpu ^. registers . t) 1 False in
            cpu & registers . t .~ value & registers . flags .~ newFlags
  IncHL -> let target = getHL $ cpu ^. registers
               value = fetch cpu target
               (newValue, newFlags) = add value 1 False in
           write cpu target newValue & registers . flags .~ newFlags
  DecR t -> let (value, newFlags) = sub (cpu ^. registers . t) 1 False in
            cpu & registers . t .~ value & registers . flags .~ newFlags
  DecHL -> let target = getHL $ cpu ^. registers
               value = fetch cpu target
               (newValue, newFlags) = sub value 1 False in
           write cpu target newValue & registers . flags .~ newFlags
  where
    add :: Word8 -> Word8 -> Bool -> (Word8, FlagRegister)
    add o n _c = let new        = o + n + if _c then 1 else 0
                     _zero      = new == 0
                     _subtract  = False
                     _carry     = o > new
                     _halfCarry = o .&. 16 + n .&. 16 > 16 in
                 (new, FlagRegister {..})

    sub :: Word8 -> Word8 -> Bool -> (Word8, FlagRegister)
    sub o n _c = let new        = o - n - if _c then 1 else 0
                     _zero      = new == 0
                     _subtract  = True
                     _carry     = o > new
                     _halfCarry = o .&. 16 + n .&. 16 > 16 in
                 (new, FlagRegister {..})

    fetch :: CPU -> Word16 -> Word8
    fetch _cpu addr = (`V.index` (fromIntegral addr)) $ _cpu ^. bus

    write :: CPU -> Word16 -> Word8 -> CPU
    write _cpu target value = _cpu & bus %~  (`V.update` V.singleton (fromIntegral target, value))