import time
import chardet
import sys
import smbus

TOF_addr = 0x08 #7-bit slave address=ID+0x08 ID=0

TOF_REGISTER_TOTAL_SIZE = 48 #Total length of all registers

TOF_ADDR_MODE = 0x0c #pattern variable address
TOF_SIZE_MODE = 1 #The number of bytes occupied by the pattern variable

TOF_ADDR_ID = 0x0d #ID variable address
TOF_SIZE_ID = 1 #The number of bytes occupied by the ID variable

TOF_ADDR_UART_BAUDRATE = 0x10 #UART baud rate variable address
TOF_SIZE_UART_BAUDRATE = 4 #The number of bytes occupied by the UART baud rate variable

TOF_ADDR_SYSTEM_TIME = 0x20 #System time variable address
TOF_SIZE_SYSTEM_TIME = 4 #The number of bytes occupied by the system time variable

TOF_ADDR_DIS = 0x24 #distance variable address
TOF_SIZE_DIS = 4 #The number of bytes occupied by the distance variable

TOF_ADDR_DIS_STATUS = 0x28 #Distance status indication variable address
TOF_SIZE_DIS_STATUS = 2 #The number of bytes occupied by the distance status indicator variable

TOF_ADDR_SIGNAL_STRENGTH = 0x2a #Signal strength variable address
TOF_SIZE_SIGNAL_STRENGTH = 2 #The number of bytes occupied by the signal strength variable

TOF_ADDR_RANGE_PRECISION = 0x2c #Ranging accuracy variable address
TOF_SIZE_RANGE_PRECISION = 1 #The number of bytes occupied by the ranging precision variable

IIC_CHANGE_TO_UART_DATA = 0x40 #Change the communication mode to the byte data that needs to be sent by UART

interface_mode = 0
id = 0
uart_baudrate = 0
system_time = 0
dis = 0
dis_status = 0
signal_strength = 0
range_precision = 0

try:
    bus = smbus.SMBus(1)
    def read(reg):
        data = bus.read_i2c_block_data(TOF_addr, reg, 1)
        return data[0]

    def read_word(reg):
        data = bus.read_i2c_block_data(TOF_addr, reg, 2)
        return ((data[1] * 256 ) + data[0])

    def write(reg,data):
        temp = [0]
        temp[0] = data & 0xFF
        bus.write_i2c_block_data(TOF_addr,reg,temp)

    def write_word(reg,data):
        temp = [0,0]
        temp[0] = data & 0xFF
        temp[1] =(data & 0xFF00) >> 8
        bus.write_i2c_block_data(TOF_addr,reg,temp)

    def TOF_IIC_Read_N_Byte(reg,num):
        data = bus.read_i2c_block_data(TOF_addr, reg, num)
        return data
    
    def IIC_Get_All_Register_Data():
        # The read function cannot read the 48 bits directly, so it needs to do so in two separate reads
        pdata = TOF_IIC_Read_N_Byte(0x00,TOF_REGISTER_TOTAL_SIZE // 2) + TOF_IIC_Read_N_Byte(0x18,TOF_REGISTER_TOTAL_SIZE // 2)
        return pdata

    def IIC_Unpack_Data():
        pdata = IIC_Get_All_Register_Data()

        interface_mode=pdata[TOF_ADDR_MODE]&0x07
        id=pdata[TOF_ADDR_ID]
        uart_baudrate=((pdata[TOF_ADDR_UART_BAUDRATE])|(pdata[TOF_ADDR_UART_BAUDRATE+1]<<8)|
				                                       (pdata[TOF_ADDR_UART_BAUDRATE+2]<<16)|(pdata[TOF_ADDR_UART_BAUDRATE+3]<<24))
        system_time=((pdata[TOF_ADDR_SYSTEM_TIME])|(pdata[TOF_ADDR_SYSTEM_TIME+1]<<8)|
				                                     (pdata[TOF_ADDR_SYSTEM_TIME+2]<<16)|(pdata[TOF_ADDR_SYSTEM_TIME+3]<<24))
        dis=(float)((pdata[TOF_ADDR_DIS])|(pdata[TOF_ADDR_DIS+1]<<8)|
				                          (pdata[TOF_ADDR_DIS+2]<<16)|(pdata[TOF_ADDR_DIS+3]<<24))/1000

        dis_status=((pdata[TOF_ADDR_DIS_STATUS])|(pdata[TOF_ADDR_DIS_STATUS+1]<<8))
        signal_strength=((pdata[TOF_ADDR_SIGNAL_STRENGTH])|(pdata[TOF_ADDR_SIGNAL_STRENGTH+1]<<8))

        range_precision=pdata[TOF_ADDR_RANGE_PRECISION]

        #print data via terminal
        print("id: %d\r\n"%id)

        print("system_time:%d \r\n"%system_time)

        print("dis: %.2fm\r\n"%dis)

        print("dis_status: %d \r\n"%dis_status)

        print("signal_strength: %d \r\n"%signal_strength)

        print("range_precision: %d \r\n\n"%range_precision)

        time.sleep(0.1);#Query every 100ms

    while True:
        IIC_Unpack_Data()


except KeyboardInterrupt: 
    print("ctrl + c:")
    exit()