from cam_control.base import MemoryDevice
import time

REGISTERS = {
    'GIE' : 0x01C,
    'ISR' : 0x020,
    'IER' : 0x028,
    'SOFTR' : 0x040,
    'CR' : 0x100,
    'SR' : 0x104,
    'TX_FIFO' : 0x108,
    'RX_FIFO' : 0x10C,
    'TX_FIFO_OCY' : 0x114,
    'RX_FIFO_OCY' : 0x118,
    'RX_FIFO_PIRQ' : 0x120,
}

class AXI_IIC(MemoryDevice):
    def __init__(self, base_addr : int = 0x41600000):
        self._initFlag = False
        super().__init__(base_addr)
        self._regs = REGISTERS
        self.Read()
        self._initFlag = True

    
    def Read(self):
        if not self._initFlag:
            self.vals = {}
        for key in self._regs:
            self.vals[key] = self._read(self._regs[key])
    
    def write(self, reg: str, bitsV: list, bitsP: list):
        word = 0
        mask = 0
        for i, pos in enumerate(bitsP):
            mask = mask | (1 << pos)
            word = word | (bitsV[i] << pos)

        current_val = self._read(self._regs[reg])
        setting_val = current_val & (~mask) | word

        self._write(self._regs[reg], setting_val) 
        

    def Write(self):
        for reg in self._regs:
            self._write(self._regs[reg], self.vals[reg])

def ReadAddress(axi_IIC, dev_addr, reg_addr):
    axi_IIC._write(axi_IIC._regs['SOFTR'], 0xa)
    axi_IIC._write(axi_IIC._regs['RX_FIFO_PIRQ'], 0x1)
    #first we need to specify which reg we want
    axi_IIC.write('CR', [1], [0])
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr<<1 | 1<<8)
    axi_IIC.write('CR', [1,1], [2,3])
    axi_IIC.Read()
    print(axi_IIC.vals)
    
    axi_IIC.write('CR', [0], [2])
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], reg_addr | 1<<9)
    axi_IIC.Read()
    print(axi_IIC.vals)

    #now we want to make a single read
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr<<1 + 1)
    axi_IIC.Read()
    print(axi_IIC.vals)
    axi_IIC.write('CR', [1,0], [2,3])
    axi_IIC.write('CR', [1], [4])
    axi_IIC.Read()
    print(axi_IIC.vals)

def EasyAccess(axi_IIC, dev_addr, reg_addr):
    #initialisation
    axi_IIC._write(axi_IIC._regs['SOFTR'], 0xa)
    axi_IIC._write(axi_IIC._regs['RX_FIFO_PIRQ'], 0x0f)

    axi_IIC.write('CR', [1], [1])
    axi_IIC.write('CR', [1], [0])
    axi_IIC.write('CR', [0], [1])

    

    #axi_IIC.Read()
    #print(axi_IIC.vals)
    print(axi_IIC._read(axi_IIC._regs['SR']))

    #first we want to write a register address
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr | 1<<8)
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], reg_addr)

    #axi_IIC.Read()
    #print(axi_IIC.vals)
    print(axi_IIC._read(axi_IIC._regs['SR']))

    #axi_IIC.Read()
    #print(axi_IIC.vals)
    #then lets go for reading from the device
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr | 1<<8 | 1)
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], 1 | 1<<9)
    time.sleep(2)
    axi_IIC.Read()
    print(axi_IIC.vals)

def justRead(axi_IIC):
    axi_IIC.Read()
    print(axi_IIC.vals)

def WithoutEasy(axi_IIC, dev_addr, reg_addr):
    #initialisation
    axi_IIC._write(axi_IIC._regs['SOFTR'], 0xa)
    axi_IIC._write(axi_IIC._regs['RX_FIFO_PIRQ'], 0x0f)

    axi_IIC.write('CR', [1], [1])
    axi_IIC.write('CR', [1], [0])
    axi_IIC.write('CR', [0], [1])

    #give register address
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr)
    axi_IIC.write('CR', [1,1], [2,3])
    axi_IIC.write('CR', [0], [2])
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], reg_addr)

    #now lets read the value
    axi_IIC._write(axi_IIC._regs['TX_FIFO'], dev_addr | 1)
    
    time.sleep(2)
    axi_IIC.write('CR', [1], [4])
    axi_IIC.write('CR', [0], [2])
    axi_IIC.Read()
    print(axi_IIC.vals)
    
    

if __name__ == '__main__':
    axi_IIC = AXI_IIC()
    justRead(axi_IIC)
    EasyAccess(axi_IIC, 0x72, 0x43)