from labjack import ljm

def Convert_U8toF32(a, b):
  value_u16 = a + b * 256
  if value_u16 >= 32768:
    value = value_u16 - 65536
  else:
    value = value_u16
  return(value)


# Open first found LabJack
jack = ljm.openS("ANY", "ANY", "ANY")

#set sda/sdl DIO; FIO0:7 = 0:7, EIO0:7 = 8:15, CIO0:7 = 16:23
ljm.eWriteName(jack, "I2C_SDA_DIONUM", 8) #8=EIO0
ljm.eWriteName(jack, "I2C_SCL_DIONUM", 9) #9=EIO1

# Speed throttle is inversely proportional to clock frequency. 0 = max.
ljm.eWriteName(jack, "I2C_SPEED_THROTTLE", 65300)  # Speed throttle = 65516 (~100 kHz)

# Tell the I2C bus to reset
ljm.eWriteName(jack, "I2C_OPTIONS", 0)

#set slave adress of the BNO055
ljm.eWriteName(jack, "I2C_SLAVE_ADDRESS", 0x28)

#set IMU to NDOF mode (write only operation)
tx_bytes = [0x3D, 0X0C]
ljm.eWriteName(jack, "I2C_NUM_BYTES_TX", len(tx_bytes))
ljm.eWriteName(jack, "I2C_NUM_BYTES_RX", 0)
ljm.eWriteNameByteArray(jack, "I2C_DATA_TX", len(tx_bytes), tx_bytes)
ljm.eWriteName(jack, "I2C_GO", 1)

#point to the start of data in memory
tx_bytes = [0x1A]
ljm.eWriteName(jack, "I2C_NUM_BYTES_TX", len(tx_bytes))
ljm.eWriteName(jack, "I2C_NUM_BYTES_RX", 26) #we'll be reading 26 bytes
ljm.eWriteNameByteArray(jack, "I2C_DATA_TX", len(tx_bytes), tx_bytes)

for i in range(100):
  ljm.eWriteName(jack, "I2C_GO", 1)
  rx = ljm.eReadNameByteArray(jack, "I2C_DATA_RX", 26)
  values = []
  for i in range(len(rx)/2):
    values.append(Convert_U8toF32(rx[(i+1)*2-2], rx[(i+1)*2-1]))
  print(values)

ljm.close(jack)