# TO USE KEYPAD TO MANIPULATE DATA:
#
# A button: run/halt
# B button: when halted, toggles address/data entry
# Right button: when halted, single-steps
#
# Address entry: press the two digits for the address. It is entered immediately (there's no "enter" key)
# Data entry: likewise. After you press the second digit, it will automatically go to the next address.


import time
import board
import keypad
import digitalio
from tm1637_display import TM1637Display
import board
import busio # for led matrix
from adafruit_ht16k33 import matrix

class TwoDigitHexInput:
    def __init__(self):
        self.digits = [0x0, 0x0]
        self.currentDigit = 0
        self.value = 0
       
    def input(self, d):
        self.digits[self.currentDigit] = d
        self.value = (self.digits[0] * 16) + self.digits[1]
        print("INPUT", self.digits, "current digit: " + str(self.currentDigit), "value: " + str(self.value))
        self.currentDigit = 0 if self.currentDigit else 1
        
    def clear(self):
        self.__init__()
        print(self.digits)
        
    def set(self, n):
        self.value = n
        self.digits[0] = n >> 4
        self.digits[1] = n & 0xF
        
        
        

class CPU:
    def __init__(self):
        self.running = False
        self.IP = 254
        self.acc = 0
        self.flags = { 'C': False, 'Z': False, 'N': False, 'Eq': False }
        self.instruction = { 'opcode': False, 'operand': False }
        self.memory = False


    def load_memory(self, bytes):
        self.memory = bytes + bytearray(256 - len(bytes))
        # print(self.memory)

    def start(self):
        self.running = True

    def step(self):
        if self.IP >= 255: # TODO CHECK
            self.IP = 0
        print("IP:", toHex(self.IP))
        self.instruction['opcode'] = self.memory[self.IP]
        self.IP = self.IP+1
        self.instruction['operand'] = self.memory[self.IP]
        self.IP = self.IP+1
        self.nums2mnems[self.instruction['opcode']](self, self.instruction['operand'])

        print("instr:", toHex(self.instruction['opcode']), toHex(self.instruction['operand']))
        print("mnem:", self.nums2mnems[self.instruction['opcode']])
        print("acc:", self.acc, "N:", self.flags['N'])
        print("running:", self.running)
        print()
        # self.print_screen()
        print("byte 26 (keyboard):", self.memory[26])
        print()

    def hlt(self, operand):
        self.running = False

    def nop(self, operand):
        pass

    def lda_lit(self, operand):
        self.acc = operand
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False
        
    def lda_mem(self, operand):
        self.acc = self.memory[operand]
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False

    def sta_lit(self, operand):
        self.memory[operand] = self.acc

    def sta_mem(self, operand):
        self.memory[self.memory[operand]] = self.acc

    def add_lit(self, operand):
        self.acc = self.acc + operand
        if self.acc > 255:
            self.acc = self.acc % 256
            self.flags['C'] = True
        else:
            self.flags['C'] = False
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False

    def add_mem(self, operand):
        self.acc = self.acc + self.memory[operand]
        if self.acc > 255:
            self.acc = self.acc % 256
            self.flags['C'] = True
        else:
            self.flags['C'] = False
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False

    def sub_lit(self, operand):
        self.acc = self.acc - operand
        if self.acc < 0:
            self.acc = self.acc % 256
            self.flags['C'] = True
        else:
            self.flags['C'] = False
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False

    def sub_mem(self, operand):
        self.acc = self.acc - self.memory[operand]
        if self.acc > 255:
            self.acc = self.acc % 256
            self.flags['C'] = True
        else:
            self.flags['C'] = False
        self.flags['Z'] = True if self.acc == 0 else False
        self.flags['Eq'] = True if self.acc == operand else False
        self.flags['N'] = True if self.acc > 127 else False

    def jmp_lit(self, operand):
        self.IP = operand

    def jmp_mem(self, operand):
        self.IP = self.memory[operand]

    def ske(self, operand): # FIXME
#        if self.flags['Eq']:
#            self.IP += 2
        if self.acc == operand:
            self.IP += 2

    def skz(self, operand):
        if self.flags['Z']:
            self.IP += 2

    def skn(self, operand):
        if self.flags['N']:
            self.IP += 2

    def skc(self, operand):
        if self.flags['C']:
            self.IP += 2

    def cst(self, operand):
        self.flags['C'] = True

    def ccl(self, operand):
        self.flags['C'] = False

    nums2mnems = {
        0: hlt,      # x0
        1: nop,      # x1
        2: lda_lit,  # 02
        3: sta_lit,  # 03
        4: add_lit,  # 04
        5: sub_lit,  # 05
        6: jmp_lit,  # 06
        7: ske,      # x7
        8: skz,      # x8
        9: skn,      # x9
        10: skc,     # A
        11: cst,     # B
        12: ccl,     # C
        16: hlt,     # 
        17: nop,     # 
        18: lda_mem, # 12
        19: sta_mem, # 13
        20: add_mem, # 14
        21: sub_mem, # 15
        22: jmp_mem, # 16
        23: ske,
        24: skz,
        25: skn,
        26: skc,
        27: cst,
        28: ccl,
    }


### PI PICO SPECIFIC STUFF ###

# to list board features: print(dir(board))

display_1 = TM1637Display(board.GP0, board.GP1, length=4)
display_2 = TM1637Display(board.GP2, board.GP3, length=4)

i2c = busio.I2C(board.GP17, board.GP16) # scl, sda
matrix = matrix.Matrix8x8(i2c)
matrix.brightness = 1
matrix.blink_rate = 0

keymatrix = keypad.KeyMatrix(
    row_pins = (board.GP5, board.GP6, board.GP7, board.GP8),
    column_pins = (board.GP9, board.GP10, board.GP11, board.GP12, board.GP13) )

keymap = {
    15:"0", 16:"1", 17:"2", 18:"3", 19:"runhalt",
    10:"4", 11:"5", 12:"6", 13:"7", 14:"step",
    5:"8", 6:"9", 7:"A", 8:"B", 9:"addr",
    0:"C", 1:"D", 2:"E", 3:"F", 4:"data" }

numericKeys = [ "0","1","2","3","4","5","6","7","8","9","A","B","C","D","E","F" ]

def toHex(n):
    return "%0.2X" % n

class Monitor:
    def __init__(self, cpu):
        self.cpu = cpu
        self.monitorMode = 'addressEntry' # or dataEntry
        self.monitorAddressInput = TwoDigitHexInput()
        self.monitorDataInput = TwoDigitHexInput()

        # In data entry mode, when a full byte is keyed in,
        # the next keypress advances to the next address and continues entering data there.
        # This variable tracks whether it's time to do that or not.
        self.advanceDataEntryNextPress = False
        
    def handleKeys(self):
        keypad_event = keymatrix.events.get()
        keyPressed = True if (keypad_event and keypad_event.released ) else False
        key = keymap[keypad_event.key_number] if keyPressed else False
        numericKeyPressed = True if (keyPressed and (key in numericKeys)) else False


        if self.cpu.running:
          if key == "runhalt":
              print("HALT PRESSED")
              self.cpu.running = False
              time.sleep(0.5) # lazy debounce
              # km.events.clear() # don't track keypresses from during the run

          if numericKeyPressed:
            self.cpu.memory[26] = int(key, 16)

        elif not self.cpu.running:
            if key == "runhalt":
                self.cpu.running = True
                print("\nSTARTING")
                time.sleep(0.5) # lazy debounce

            if key == "addr":
                self.monitorMode = 'addressEntry'
                print("\nENTERING", self.monitorMode, "MODE")
                self.monitorAddressInput.currentDigit = 0
                time.sleep(0.5) # lazy debounce
            if key == "data":
                self.monitorMode = 'dataEntry'
                print("\nENTERING", self.monitorMode, "MODE")
                self.monitorDataInput.clear()
                self.advanceDataEntryNextPress = False
                time.sleep(0.5) # lazy debounce

            if key == "step":
                print("\nSINGLE STEP FROM MONITOR ADDR")
                # self.IP = self.monitorAddressInput.value
                self.cpu.step()
                time.sleep(0.5) # lazy debounce

            if numericKeyPressed:
                if self.monitorMode == 'addressEntry':
                    self.monitorAddressInput.input(int(key, 16))
                    self.cpu.IP = self.monitorAddressInput.value
                    print("MA", self.cpu.IP)

                if self.monitorMode == 'dataEntry':
                    if self.advanceDataEntryNextPress:
                        print("ADVANCING")
                        self.cpu.IP = (self.cpu.IP + 1) % 256
                        # self.monitorDataInput.clear() # reset .currentDigit
                        self.monitorDataInput.set(self.cpu.memory[self.cpu.IP])
                        self.advanceDataEntryNextPress = False
                    self.monitorDataInput.input(int(key, 16))
                    self.cpu.memory[self.cpu.IP] = self.monitorDataInput.value
                    print("MD", self.monitorDataInput.value)
                    if self.monitorDataInput.currentDigit == 0: # that was the second keypress, so next keypress is for the next address
                        self.advanceDataEntryNextPress = True
                        
                print("Acc", self.cpu.acc, "IP", self.cpu.IP, "Data", self.cpu.memory[self.cpu.IP], "\n")
            
            
    def displayScreen(self):
        for x in range(8):
            for y in range(8):
                matrix[x, y] = self.cpu.memory[x + (8*y)]



    def run(self):
        #self.cpu.start()
        t = time.time()
        while (time.time() - t) < 120: # TODO: add a time delta or sth maybe so this doesn't just burn cycles
            self.handleKeys()
            display_1.print(toHex(self.cpu.IP) + toHex(self.cpu.memory[self.cpu.IP]))
            # display_1.print(toHex(self.monitorAddressInput.value) + toHex(self.cpu.memory[self.cpu.IP]))
            # display_2.print(toHex(self.cpu.IP) + toHex(self.cpu.acc))
            display_2.print(toHex(self.cpu.acc))
            self.displayScreen()
            if self.cpu.running:
                self.cpu.step()
            # time.sleep(0.5) # TODO ?
        print("timeout")
        print(self.cpu.memory)


cpu = CPU()
monitor = Monitor(cpu)

# preamble = '00 ' * 64
# prog = preamble + '02 01   13 f0   12 f0   04 02   03 f0   12 f0   05 41   08 00   06 40   00 00' # STRIPES
# offset = 64
# prog = preamble + '02 01  13 f0  12 f0  04 02  03 f0  05 08  09 00  04 09  03 f0  07 41  06' + toHex(offset) + '00 00'
#prog = '00'
# program_bytes = bytearray.fromhex(prog.replace(" ", ""))

# Add jmp at addr 254:
#program_with_jump = program_bytes + bytearray(254 - len(program_bytes)) + bytearray.fromhex('0600') # jump to addr 00
# program_with_jump = program_bytes + bytearray(254 - len(program_bytes)) + bytearray.fromhex('0640') # jump to addr 0x40 (dec 64)

with open('test-multiply2.bin', 'rb') as file:
    program_bytes = bytearray(file.read())

cpu.load_memory(program_bytes)

monitor.run()