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

import Lens.Micro.Platform
import Data.Word
import Data.Bits
import Data.Int
import qualified Data.Vector.Sized as V

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

data FlagRegister = FlagRegister { _fZero      :: Bool
                                 , _fNegative  :: Bool
                                 , _fHalfCarry :: Bool
                                 , _fCarry     :: Bool
                                 }
                    deriving Show

makeLenses ''FlagRegister

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

instance Convert FlagRegister Word8 where
  convert r =
    ((.<<. zeroFlagPosition) $ if r ^. fZero then 1 else 0) +
    ((.<<. negativeFlagPosition) $ if r ^. fNegative then 1 else 0) +
    ((.<<. halfCarryFlagPosition) $ if r ^. fHalfCarry then 1 else 0) +
    ((.<<. carryFlagPosition) $ if r ^. fCarry then 1 else 0)
  
instance Convert Word8 FlagRegister where
  convert w =
    let _fZero = (w `testBit` zeroFlagPosition)
        _fNegative = (w `testBit` negativeFlagPosition)
        _fHalfCarry = (w `testBit` halfCarryFlagPosition)
        _fCarry = (w `testBit` carryFlagPosition) in
      FlagRegister {..}
      
  
data Registers = Registers { _ra     :: Word8
                           , _rb     :: Word8
                           , _rc     :: Word8
                           , _rd     :: Word8
                           , _re     :: Word8
                           , _rf     :: Word8
                           , _rg     :: Word8
                           , _rh     :: Word8
                           , _rl     :: Word8
                           , _rflags :: FlagRegister
                           }

makeLenses ''Registers

rbc :: Lens' Registers Word16
rbc = lens (\r -> (( .<<. 8) . fromIntegral $ r ^. rb) + (fromIntegral $ r ^. rc))
          (\r w -> r & rb %~ (const $ fromIntegral $ w .>>. 8) & rc %~ (const $ fromIntegral $ w .&. 255))

rde :: Lens' Registers Word16
rde = lens (\r -> (( .<<. 8) . fromIntegral $ r ^. rd) + (fromIntegral $ r ^. re))
          (\r w -> r & rd %~ (const $ fromIntegral $ w .>>. 8) & re %~ (const $ fromIntegral $ w .&. 255))

rhl :: Lens' Registers Word16
rhl = lens (\r -> ((.<<. 8) . fromIntegral $ r ^. rh) + (fromIntegral $ r ^. rl))
          (\r w -> r & rh %~ (const $ fromIntegral $ w .>>. 8) & rl %~ (const $ fromIntegral $ w .&. 255))

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

a, b, c, d, e, f, g, h, l :: Lens' CPU Word8
a = lens (^. registers . ra) (\cpu w -> cpu & registers . ra .~ w)
b = lens (^. registers . rb) (\cpu w -> cpu & registers . rb .~ w)
c = lens (^. registers . rc) (\cpu w -> cpu & registers . rc .~ w)
d = lens (^. registers . rd) (\cpu w -> cpu & registers . rd .~ w)
e = lens (^. registers . re) (\cpu w -> cpu & registers . re .~ w)
f = lens (^. registers . rf) (\cpu w -> cpu & registers . rf .~ w)
g = lens (^. registers . rg) (\cpu w -> cpu & registers . rg .~ w)
h = lens (^. registers . rh) (\cpu w -> cpu & registers . rh .~ w)
l = lens (^. registers . rl) (\cpu w -> cpu & registers . rl .~ w)

flags :: Lens' CPU FlagRegister
flags = lens (^. registers . rflags) (\cpu _f -> cpu & registers . rflags .~ _f)

bc, de, hl :: Lens' CPU Word16
bc = lens (^. registers . rbc) (\cpu w -> cpu & registers . rbc .~ w)
de = lens (^. registers . rde) (\cpu w -> cpu & registers . rde .~ w)
hl = lens (^. registers . rhl) (\cpu w -> cpu & registers . rhl .~ w)

zero, negative, halfCarry, carry :: Lens' CPU Bool
zero      = lens (^. flags . fZero)      (\cpu _f -> cpu & flags . fZero .~ _f)
negative  = lens (^. flags . fNegative)  (\cpu _f -> cpu & flags . fNegative .~ _f)
halfCarry = lens (^. flags . fHalfCarry) (\cpu _f -> cpu & flags . fHalfCarry .~ _f)
carry     = lens (^. flags . fCarry)     (\cpu _f -> cpu & flags . fCarry .~ _f)

type ByteTarget = Lens' CPU Word8
type WordTarget = Lens' CPU Word16
type FlagTarget = Lens' CPU Bool

data Instruction where
  ADDR :: { t :: ByteTarget, withCarry :: Bool } -> Instruction
  ADDHL :: { withCarry :: Bool } -> Instruction
  ADDN :: { val :: Word8, withCarry :: Bool } -> Instruction
  SUBR :: { t :: ByteTarget, withCarry :: Bool } -> Instruction
  SUBHL :: { withCarry :: Bool } -> Instruction
  SUBN :: { val :: Word8, withCarry :: Bool } -> Instruction
  CPR :: { t :: ByteTarget } -> Instruction
  CPHL :: Instruction
  CPN :: { val :: Word8 } -> Instruction
  INCR :: { t :: ByteTarget } -> Instruction
  INCHL :: Instruction
  DECR :: { t :: ByteTarget } -> Instruction
  DECHL :: Instruction
  BOR :: { t :: ByteTarget, op :: (Word8 -> Word8 -> Word8), fr :: FlagRegister } -> Instruction
  BOHL :: { op :: (Word8 -> Word8 -> Word8), fr :: FlagRegister } -> Instruction
  BON :: { val :: Word8, op :: (Word8 -> Word8 -> Word8), fr :: FlagRegister } -> Instruction
  CCF :: Instruction
  SCF :: Instruction
  DAA :: Instruction -- TODO: What does this do?
  CPL :: Instruction

  INCRR :: Instruction
  DECRR :: Instruction
  ADDHLRR :: { from' :: WordTarget } -> Instruction
  ADDSP :: { rel :: Int8 } -> Instruction

  RLCA :: Instruction
  RRCA :: Instruction
  RLA :: Instruction
  RRA :: Instruction
  RLC :: { t :: ByteTarget } -> Instruction
  RLCHL :: Instruction
  RRC :: { t :: ByteTarget } -> Instruction
  RRCHL :: Instruction
  RL :: { t :: ByteTarget } -> Instruction
  RLHL :: Instruction
  RR :: { t :: ByteTarget } -> Instruction
  RRHL :: Instruction

  SLA :: { t :: ByteTarget } -> Instruction
  SLAHL :: Instruction
  SR :: { t :: ByteTarget, arithmetic :: Bool } -> Instruction
  SRHL :: { arithmetic :: Bool } -> Instruction

  SWAP :: { t :: ByteTarget } -> Instruction
  SWAPHL :: Instruction
  BIT :: { t :: ByteTarget, bit :: Int } -> Instruction
  BITHL :: { bit :: Int } -> Instruction
  RES :: { t :: ByteTarget, bit :: Int } -> Instruction
  RESHL :: { bit :: Int } -> Instruction
  SET :: { t :: ByteTarget, bit :: Int } -> Instruction
  SETHL :: { bit :: Int } -> Instruction

  LDRR :: { to :: ByteTarget, from :: ByteTarget } -> Instruction
  LDRN :: { to :: ByteTarget, val :: Word8 } -> Instruction
  LDRHL :: { to :: ByteTarget } -> Instruction
  LDXR :: { to' :: WordTarget, from :: ByteTarget } -> Instruction
  LDXN :: { to' :: WordTarget, val :: Word8 } -> Instruction
  LDANN :: { from'' :: Word16 } -> Instruction
  LDNNA :: { to'' :: Word16 } -> Instruction
  LDHAC :: Instruction
  LDHCA :: Instruction
  LDHAN :: { val :: Word8 } -> Instruction
  LDHNA :: { val :: Word8 } -> Instruction
  LDAHL :: { op' :: (Word16 -> Word16) } -> Instruction
  LDHLA :: { op' :: (Word16 -> Word16) } -> Instruction

  LDRRNN :: { to' :: WordTarget, val' :: Word16} -> Instruction
  LDNNSP :: { val' :: Word16 } -> Instruction
  LDSPHL :: Instruction
  PUSHRR :: { from' :: WordTarget } -> Instruction
  POPRR :: { to' :: WordTarget } -> Instruction
  LDHLSPE :: { rel :: Int8 } -> Instruction

  JPNN :: { val' :: Word16 } -> Instruction
  JPHL :: Instruction
  JPCCNN :: { val' :: Word16, flag :: FlagTarget, op'' :: Bool -> Bool } -> Instruction
  JRE :: { rel :: Int8 } -> Instruction
  JRCCE :: { rel :: Int8, flag :: FlagTarget, op'' :: Bool -> Bool } -> Instruction

execute :: CPU -> Instruction -> CPU
execute cpu = \case
  ADDR t _c -> let value = cpu ^. t
                   (newValue, newFlags) = add (cpu ^. a) value $ _c && cpu ^. carry in
               cpu & a .~ newValue
                   & flags .~ newFlags
  ADDHL _c -> let value = fetch cpu $ cpu ^. hl
                  (newValue, newFlags) = add (cpu ^. a) value $ _c && cpu ^. carry in
              cpu & a .~ newValue
                  & flags .~ newFlags
  ADDN value _c -> let (newValue, newFlags) = add (cpu ^. a) value $ _c && cpu ^. carry in
                cpu & a .~ newValue
                    & flags .~ newFlags
  SUBR t _c -> let value =  cpu ^. t
                   (newValue, newFlags) = sub (cpu ^. a) value $ _c && cpu ^. carry in
               cpu & a .~ newValue
                   & flags .~ newFlags
  SUBHL _c -> let value = fetch cpu $ cpu ^. hl
                  (newValue, newFlags) = sub (cpu ^. a) value $ _c && cpu ^. carry in
              cpu & a .~ newValue
                  & flags .~ newFlags
  SUBN value _c -> let (newValue, newFlags) = sub (cpu ^. a) value $ _c && cpu ^. carry in
                   cpu & a .~ newValue
                       & flags .~ newFlags
  CPR t -> let value = cpu ^. t
               (_, newFlags) = sub (cpu ^. a) value $ False in
              cpu & flags .~ newFlags
  CPHL -> let value = fetch cpu $ cpu ^. hl
              (_, newFlags) = sub (cpu ^. a) value $ False in
           cpu & flags .~ newFlags
  CPN value -> let (_, newFlags) = sub (cpu ^. a) value $ False in
                cpu & flags .~ newFlags
  INCR t -> let (value, newFlags) = add (cpu ^. t) 1 False in
            cpu & t .~ value
                & flags .~ newFlags
  INCHL -> let target = cpu ^. hl
               value = fetch cpu target
               (newValue, newFlags) = add value 1 False in
           write cpu target newValue & flags .~ newFlags
  DECR t -> let (value, newFlags) = sub (cpu ^. t) 1 False in
            cpu & t .~ value
                & flags .~ newFlags
  DECHL -> let target = cpu ^. hl
               value = fetch cpu target
               (newValue, newFlags) = sub value 1 False in
           write cpu target newValue & flags .~ newFlags
  BOR t op _f -> let newValue = (cpu ^. a) `op` (cpu ^. t)
                     newFlags = _f {_fZero = newValue == 0} in
                cpu & a .~ newValue
                    & flags .~ newFlags
  BOHL op _f -> let target = cpu ^. hl
                    value = fetch cpu target
                    newValue = (cpu ^. a) `op` value
                    newFlags = _f {_fZero = value == 0} in
                cpu & a .~ newValue
                    & flags .~ newFlags
            
  BON value op _f -> let newValue = (cpu ^. a) `op` value
                         newFlags = _f {_fZero = newValue == 0} in
                    cpu & a .~ newValue
                        & flags .~ newFlags
  CCF -> cpu & negative .~ False
             & halfCarry .~  False
             & carry %~ not
  SCF -> cpu & negative .~ False
             & halfCarry .~  False
             & carry .~ True
  DAA -> undefined -- TODO: undefined in manual
  CPL -> cpu & a %~ complement
             & negative .~ True
             & halfCarry .~ True
  INCRR -> cpu & bc %~ (+1)
  DECRR -> cpu & bc %~ (+1)
  ADDHLRR t -> let _flags = cpu ^. flags
                   _hl = cpu ^. hl
                   val = cpu ^. t
                   (newValue, newFlags) = add16 _hl val _flags in
             cpu & hl .~ newValue
                 & flags .~ newFlags
  ADDSP _e -> let value = cpu ^. sp
                  newValue :: Word16 = fromIntegral $ (fromIntegral value :: Integer) + (fromIntegral _e) in
              cpu & sp .~ newValue
                  & flags .~
                      FlagRegister { _fZero = False,
                                     _fNegative = False,
                                     _fHalfCarry = value .&. 16 + fromIntegral _e .&. 16 > 16, -- TODO: check if this still works out
                                     _fCarry = value > newValue
                                   }
  RLCA -> let _fCarry = (cpu ^. a) `testBit` 8 in
    cpu & a %~ (`rotateL` 1)
        & flags .~ FlagRegister {_fZero = False, _fNegative = False, _fHalfCarry = False, ..}
  RRCA -> let _fCarry = (cpu ^. a) `testBit` 1 in
    cpu & a %~ (`rotateR` 1)
        & flags .~ FlagRegister {_fZero = False, _fNegative = False, _fHalfCarry = False, ..}
  RLA -> let _fCarry = (cpu ^. a) `testBit` 8
             _c = cpu ^. carry in
    cpu & a %~ (if _c then (+1) else id) . (.<<. 1)
        & flags .~ FlagRegister {_fZero = False, _fNegative = False, _fHalfCarry = False, ..}
  RRA -> let _fCarry = (cpu ^. a) `testBit` 1
             _c = cpu ^. carry in
    cpu & a %~ (if _c then (+128) else id) . (.>>. 1)
        & flags .~ FlagRegister {_fZero = False, _fNegative = False, _fHalfCarry = False, ..}
  RLC t -> let _fCarry = (cpu ^. t) `testBit` 8
               newValue = cpu ^. t `rotateL` 1 in
    cpu & t .~ newValue
        & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RLCHL -> let target = cpu ^. hl
               value = fetch cpu target
               _fCarry = (value) `testBit` 8
               newValue = value `rotateL` 1 in
    write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RRC t -> let _fCarry = (cpu ^. t) `testBit` 1
               newValue = cpu ^. t `rotateR` 1 in
    cpu & t .~ newValue
        & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RRCHL -> let target = cpu ^. hl
               value = fetch cpu target
               _fCarry = value `testBit` 1
               newValue = value `rotateR` 1 in
    write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RL t -> let _fCarry = (cpu ^. t) `testBit` 8
              _c = cpu ^. carry
              newValue = (if _c then (+1) else id) $ cpu ^. t .<<. 1 in
    cpu & t .~ newValue
        & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RLHL -> let target = cpu ^. hl
              value = fetch cpu target
              _c = cpu ^. carry
              _fCarry = value `testBit` 8
              newValue = (if _c then (+1) else id) $ value .<<. 1 in
    write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RR t -> let _fCarry = (cpu ^. t) `testBit` 1
              _c = cpu ^. carry
              newValue = (if _c then (+128) else id) $ cpu ^. t .>>. 1 in
    cpu & t .~ newValue
        & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  RRHL -> let target = cpu ^. hl
              value = fetch cpu target
              _c = cpu ^. carry
              _fCarry = value `testBit` 1
              newValue = (if _c then (+128) else id) $ value .>>. 1 in
    write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  SLA t -> let _fCarry = (cpu ^. t) `testBit` 8
               newValue = cpu ^. t .<<. 1
    in
           cpu & t .~ newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  SLAHL -> let target = cpu ^. hl
               value = fetch cpu target
               newValue = value .<<. 1
               _fCarry = value `testBit` 8
    in
           write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  SR t arith -> let _fCarry = (cpu ^. t) `testBit` 1
                    value = cpu ^. t
                    newValue = let temp = value .>>. 1 in if value `testBit` 8 && arith then temp `setBit` 8 else temp 
    in
           cpu & t .~ newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  SRHL arith -> let target = cpu ^. hl
                    value = fetch cpu target
                    newValue = let temp = value .>>. 1 in if value `testBit` 8 && arith then temp `setBit` 8 else temp
                    _fCarry = value `testBit` 1 in
           write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, ..}
  SWAP t -> let newValue = cpu ^. t `rotateR` 4 in
           cpu & t .~ newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, _fCarry = False}
  SWAPHL -> let target = cpu ^. hl
                value = fetch cpu target
                newValue = value `rotateR` 4 in
           write cpu target newValue & flags .~ FlagRegister {_fZero = newValue == 0, _fNegative = False, _fHalfCarry = False, _fCarry = False}
  BIT t i -> let value = cpu ^. t in
    cpu & flags %~ (\_f -> _f { _fZero = not $ value `testBit` i, _fNegative = False, _fHalfCarry = True })
  BITHL i -> let target = cpu ^. hl
                 value = fetch cpu target in
    cpu & flags %~ (\_f -> _f { _fZero = not $ value `testBit` i, _fNegative = False, _fHalfCarry = True })
  RES t i -> cpu & t %~ (`clearBit` i)
  RESHL i -> let target = cpu ^. hl
                 value = fetch cpu target
                 newValue = value `clearBit` i in
             write cpu target newValue
  SET t i -> cpu & t %~ (`setBit` i)
  SETHL i -> let target = cpu ^. hl
                 value = fetch cpu target
                 newValue = value `setBit` i in
             write cpu target newValue
  LDRR t _f -> cpu & t .~ cpu ^. _f
  LDRN t v -> cpu & t .~ v
  LDRHL t -> let target = cpu ^. hl
                 value = fetch cpu target in
               cpu & t .~ value
  LDXR t _f -> let target = cpu ^. t in
    write cpu target $ cpu ^. _f
  LDXN t v -> let target = cpu ^. t in
    write cpu target v
  LDANN _f -> cpu & a .~ fetch cpu _f
  LDNNA _t -> write cpu _t $ cpu ^. a
  LDHAC -> let target = 65280 + (fromIntegral $ cpu ^. c)
               value = fetch cpu target in
             cpu & a .~ value
  LDHCA -> let target = 65280 + (fromIntegral $ cpu ^. c) in
             write cpu target $ cpu ^. a
  LDHAN v -> let target = 65280 + (fromIntegral v)
                 value = fetch cpu target in
               cpu & a .~ value
  LDHNA v -> let target = 65280 + (fromIntegral v) in
             write cpu target $ cpu ^. a
  LDAHL op -> let target = cpu ^. hl
                  value = fetch cpu target in
                cpu & a .~ value
                    & hl %~ op
  LDHLA op -> let target = cpu ^. hl in
                write cpu target (cpu ^. a)
                    & hl %~ op
  LDRRNN t v -> cpu & t .~ v
  LDNNSP v -> write cpu v . fetch cpu $ cpu ^. sp
  LDSPHL -> cpu & sp .~ cpu ^. hl
  PUSHRR t -> let cpu' = cpu & sp %~ subtract 1
                  msb = fromIntegral $ (cpu ^. t) .>>. 4
                  lsb = fromIntegral $ (cpu ^. t) .&. 255
                  cpu'' = write cpu' (cpu' ^. sp) msb & sp %~ subtract 1 in
                 write cpu'' (cpu'' ^. sp) lsb
  POPRR t -> let lsb = fetch cpu (cpu ^. sp)
                 cpu' = cpu & sp %~ (+1)
                 msb = fetch cpu' (cpu' ^. sp)
                 value = (fromIntegral msb .<<. 8) + (fromIntegral lsb)
             in
               cpu & t .~ value
                   & sp %~ (+1)
  LDHLSPE v -> let value = cpu ^. sp
                   newValue :: Word16 = fromIntegral $ (fromIntegral value :: Integer) + (fromIntegral v) in
                 cpu & hl .~ value
                     & flags .~
                         FlagRegister { _fZero = False,
                                        _fNegative = False,
                                        _fHalfCarry = value .&. 16 + fromIntegral v .&. 16 > 16, -- TODO: check if this still works out
                                        _fCarry = value > newValue
                                      }
  JPNN v -> cpu & pc .~ v
  JPHL -> cpu & pc .~ cpu ^. hl
  JPCCNN v _f op -> if op $ cpu ^. _f then cpu & pc .~ v else cpu
  JRE v -> let target = fromIntegral $ (fromIntegral (cpu ^. pc) :: Integer) + fromIntegral v in cpu & pc .~ target
  JRCCE v _f op -> if op $ cpu ^. _f then
                     let target = fromIntegral $ (fromIntegral (cpu ^. pc) :: Integer) + fromIntegral v in
                     cpu & pc .~ target
                   else
                     cpu
  where
    add :: Word8 -> Word8 -> Bool -> (Word8, FlagRegister)
    add o n _c = let new        = o + n + if _c then 1 else 0
                     _fZero      = new == 0
                     _fNegative  = False
                     _fCarry     = o > new
                     _fHalfCarry = o .&. 16 + n .&. 16 > 16 in
                 (new, FlagRegister {..})

    add16 :: Word16 -> Word16 -> FlagRegister -> (Word16, FlagRegister)
    add16 o n _f = let new        = o + n
                       _fNegative  = False
                       _fCarry     = o > new
                       _fHalfCarry = o .&. 16 + n .&. 16 > 16 in
                 (new, _f {_fNegative, _fCarry, _fHalfCarry})

    sub :: Word8 -> Word8 -> Bool -> (Word8, FlagRegister)
    sub o n _c = let new        = o - n - if _c then 1 else 0
                     _fZero      = new == 0
                     _fNegative  = True
                     _fCarry     = o > new
                     _fHalfCarry = 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))