'''
Code snippet for reading data from the kuttypy

'''
import serial, struct, time, platform, os, sys, functools
from utilities import REGISTERS
from collections import OrderedDict
import numpy as np

if 'inux' in platform.system():  # Linux based system
	import fcntl

Byte = struct.Struct("B")  # size 1
ShortInt = struct.Struct("H")  # size 2
Integer = struct.Struct("I")  # size 4


def _bv(x):
	return 1 << x


def connect(**kwargs):
	return KUTTYPY(**kwargs)


def listPorts():
	'''
	Make a list of available serial ports. For auto scanning and connecting
	'''
	import glob
	system_name = platform.system()
	if system_name == "Windows":
		# Scan for available ports.
		available = []
		for i in range(256):
			try:
				s = serial.Serial('COM%d' % i)
				available.append('COM%d' % i)
				s.close()
			except serial.SerialException:
				pass
		return available
	elif system_name == "Darwin":
		# Mac
		return glob.glob('/dev/tty.usb*') + glob.glob('/dev/cu*')
	else:
		# Assume Linux or something else
		return glob.glob('/dev/ttyACM*') + glob.glob('/dev/ttyUSB*')


def isPortFree(portname):
	try:
		fd = serial.Serial(portname, KUTTYPY.BAUD, stopbits=1, timeout=1.0)
		if fd.isOpen():
			if 'inux' in platform.system():  # Linux based system
				try:
					fcntl.flock(fd.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB)
					fd.close()
					return True  # Port is available
				except IOError:
					fd.close()
					return False  # Port is not available

			else:
				fd.close()
				return True  # Port is available
		else:
			fd.close()
			return False  # Port is not available

	except serial.SerialException as ex:
		return False  # Port is not available


def getFreePorts(openPort=None):
	'''
	Find out which ports are currently free 
	'''
	portlist = {}
	for a in listPorts():
		if a != openPort:
			portlist[a] = isPortFree(a)
		else:
			portlist[a] = False
	return portlist


class KUTTYPY:
	VERSIONNUM_168P = Byte.pack(98)
	VERSIONNUM = Byte.pack(99)
	VERSIONNUM_328P = Byte.pack(100)
	VERSIONNUM_UNO = Byte.pack(101)
	GET_VERSION = Byte.pack(1)
	READB = Byte.pack(2)
	WRITEB = Byte.pack(3)
	I2C_READ = Byte.pack(4)
	I2C_WRITE = Byte.pack(5)
	I2C_SCAN = Byte.pack(6)

	BAUD = 38400
	version = 0
	portname = None
	REGS = REGISTERS.VERSIONS[99]['REGISTERS']  # A map of alphanumeric port names to the 8-bit register locations
	REGSTATES = {}  # Store the last written state of the registers
	SPECIALS = REGISTERS.VERSIONS[99]['SPECIALS']
	nano = False  # check if atmega328p is found instead of 32
	blockingSocket = None

	def __init__(self, **kwargs):
		self.sensors = {
			0x39: {
				'name': 'TSL2561',
				'init': self.TSL2561_init,
				'read': self.TSL2561_all,
				'fields': ['total', 'IR'],
				'min': [0, 0],
				'max': [2 ** 15, 2 ** 15],
				'config': [{
					'name': 'gain',
					'options': ['1x', '16x'],
					'function': self.TSL2561_gain
				},
					{
						'name': 'Integration Time',
						'options': ['3 mS', '101 mS', '402 mS'],
						'function': self.TSL2561_timing
					}
				]},
			0x1E: {
				'name': 'HMC5883L',
				'init': self.HMC5883L_init,
				'read': self.HMC5883L_all,
				'fields': ['Mx', 'My', 'Mz'],
				'min': [-5000, -5000, -5000],
				'max': [5000, 5000, 5000],
				'config': [{
					'name': 'gain',
					'options': ['1x', '16x'],
					'function': self.TSL2561_gain
				},
					{
						'name': 'Integration Time',
						'options': ['3 mS', '101 mS', '402 mS'],
						'function': self.TSL2561_timing
					}
				]},
			0x48: {
				'name': 'ADS1115',
				'init': self.ADS1115_init,
				'read': self.ADS1115_read,
				'fields': ['Voltage'],
				'min': [-5],
				'max': [5],
				'config': [{
					'name': 'channel',
					'options': ['UNI_0', 'UNI_1', 'UNI_2', 'UNI_3', 'DIFF_01', 'DIFF_23'],
					'function': self.ADS1115_channel
				},
					{
						'name': 'Data Rate',
						'options': ['8', '16', '32', '64', '128', '250', '475', '860'],
						'function': self.ADS1115_rate
					},
					{
						'name': 'Gain',
						'options': ['GAIN_TWOTHIRDS', 'GAIN_ONE', 'GAIN_TWO', 'GAIN_FOUR', 'GAIN_EIGHT',
						            'GAIN_SIXTEEN'],
						'function': self.ADS1115_gain
					}
				]},
			0x68: {
				'name': 'MPU6050',
				'init': self.MPU6050_init,
				'read': self.MPU6050_all,
				'fields': ['Ax', 'Ay', 'Az', 'Temp', 'Gx', 'Gy', 'Gz'],
				'min': [-1 * 2 ** 15, -1 * 2 ** 15, -1 * 2 ** 15, 0, -1 * 2 ** 15, -1 * 2 ** 15, -1 * 2 ** 15],
				'max': [2 ** 15, 2 ** 15, 2 ** 15, 2 ** 16, 2 ** 15, 2 ** 15, 2 ** 15],
				'config': [{
					'name': 'Gyroscope Range',
					'options': ['250', '500', '1000', '2000'],
					'function': self.MPU6050_gyro_range
				},
					{
						'name': 'Accelerometer Range',
						'options': ['2x', '4x', '8x', '16x'],
						'function': self.MPU6050_accel_range
					},
					{
						'name': 'Kalman',
						'options': ['OFF', '0.001', '0.01', '0.1', '1', '10'],
						'function': self.MPU6050_kalman_set
					}
				]},
			41: {
				'name': 'TCS34725: RGB Sensor',
				'init': self.TCS34725_init,
				'RGB': True,
				'read': self.TCS34725_all,
				'fields': ['RED', 'GREEN', 'BLUE'],
				'min': [0, 0, 0, 0],
				'max': [2 ** 16, 2 ** 16, 2 ** 16],
				'config': [{
					'name': 'Gain',
					'options': ['1', '4', '16', '60'],
					'function': self.TCS34725_gain
				}
				]},
			118: {
				'name': 'BMP280',
				'init': self.BMP280_init,
				'read': self.BMP280_all,
				'fields': ['Pressure', 'Temp', 'Alt'],
				'min': [0, 0, 0],
				'max': [1600, 100, 100],
			},
			12: {  # 0xc
				'name': 'AK8963 Mag',
				'init': self.AK8963_init,
				'read': self.AK8963_all,
				'fields': ['X', 'Y', 'Z'],
				'min': [-32767, -32767, -32767],
				'max': [32767, 32767, 32767],
			},
			119: {
				'name': 'MS5611',
				'init': self.MS5611_init,
				'read': self.MS5611_all,
				'fields': ['Pressure', 'Temp', 'Alt'],
				'min': [0, 0, 0],
				'max': [1600, 100, 10],
			},
			119: {
				'name': 'BMP180',
				'init': self.BMP180_init,
				'read': self.BMP180_all,
				'fields': ['Pressure', 'Temp'],
				'min': [0, 0],
				'max': [1600, 100],
			},
			0x41: {  # A0 pin connected to Vs . Otherwise address 0x40 conflicts with PCA board.
				'name': 'INA3221',
				'init': self.INA3221_init,
				'read': self.INA3221_all,
				'fields': ['CH1', 'CH2', 'CH3'],
				'min': [0, 0, 0],
				'max': [1000, 1000, 1000],
			},
			0x5A: {
				'name': 'MLX90614',
				'init': self.MLX90614_init,
				'read': self.MLX90614_all,
				'fields': ['TEMP'],
				'min': [0],
				'max': [350]}
		}
		self.controllers = {
			self.MCP5725_ADDRESS: {
				'name': 'MCP4725',
				'init': self.MCP4725_init,
				'write': [['CH0', 0, 4095, 0, self.MCP4725_set]],
			},
		}

		self.special = {
			0x40: {
				'name': 'PCA9685 PWM',
				'init': self.PCA9685_init,
				'write': [['Channel 1', 0, 180, 90, functools.partial(self.PCA9685_set, 1)],
				          # name, start, stop, default, function
				          ['Channel 2', 0, 180, 90, functools.partial(self.PCA9685_set, 2)],
				          ['Channel 3', 0, 180, 90, functools.partial(self.PCA9685_set, 3)],
				          ['Channel 4', 0, 180, 90, functools.partial(self.PCA9685_set, 4)],
				          ],
			}
		}

		self.connected = False
		if 'port' in kwargs:
			self.portname = kwargs.get('port', None)
			try:
				self.fd, self.version, self.connected = self.connectToPort(self.portname)
				if self.connected:
					# self.fd.setRTS(0)
					if self.nano:
						self.REGS = REGISTERS.VERSIONS[self.version][
							'REGISTERS']  # A map of alphanumeric port names to the 8-bit register locations
						self.REGSTATES = {}  # Store the last written state of the registers
						self.SPECIALS = REGISTERS.VERSIONS[self.version]['SPECIALS']
						for a in ['B', 'C', 'D']:  # Initialize all inputs
							self.setReg('DDR' + a, 0)  # All inputs
							self.setReg('PORT' + a, 0)  # No Pullup
						self.setReg('PORTC', (1 << 4) | (1 << 5))  # I2C Pull-Up
					else:
						for a in ['A', 'B', 'C', 'D']:  # Initialize all inputs
							self.setReg('DDR' + a, 0)
					return
			except Exception as ex:
				print('Failed to connect to ', self.portname, ex.message)

		elif kwargs.get('autoscan', False):  # Scan and pick a port
			portList = getFreePorts()
			for a in portList:
				if portList[a]:
					try:
						self.portname = a
						self.fd, self.version, self.connected = self.connectToPort(self.portname)
						if self.connected:
							# self.fd.setRTS(0)
							if self.nano:
								self.REGS = REGISTERS.VERSIONS[self.version][
									'REGISTERS']  # A map of alphanumeric port names to the 8-bit register locations
								self.REGSTATES = {}  # Store the last written state of the registers
								self.SPECIALS = REGISTERS.VERSIONS[self.version]['SPECIALS']
								for a in ['B', 'C', 'D']:  # Initialize all inputs
									self.setReg('DDR' + a, 0)  # All inputs
									self.setReg('PORT' + a, 0)  # No Pullup
								self.setReg('PORTC', (1 << 4) | (1 << 5))  # I2C Pull-Up
							else:
								for a in ['A', 'B', 'C', 'D']:  # Initialize all inputs
									self.setReg('DDR' + a, 0)  # All inputs
									self.setReg('PORT' + a, 0)  # No Pullup
								self.setReg('PORTC', 3)  # I2C Pull-Up
							return
					except Exception as e:
						print(e)
				else:
					print(a, ' is busy')

	def __get_version__(self, fd):
		fd.flush()
		fd.setRTS(0)
		fd.setDTR(0)
		time.sleep(0.01)
		fd.setRTS(1)
		fd.setDTR(1)
		st = time.time()
		while fd.in_waiting:
			fd.read(fd.in_waiting)
		time.sleep(max(0, 0.25 - (time.time()-st)))
		fd.write(self.GET_VERSION)
		x= fd.read()
		return x

	def get_version(self):
		return self.__get_version__(self.fd)

	def connectToPort(self, portname):
		'''
		connect to a port, and check for the right version
		'''

		try:
			if 'inux' in platform.system():  # Linux based system
				try:
					# try to lock down the serial port
					import socket
					self.blockingSocket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
					self.blockingSocket.bind('\0eyesj2%s' % portname)
					self.blockingSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
					fd = serial.Serial(portname, self.BAUD, timeout=0.2)
					if not fd.isOpen():
						return None, '', False
				except socket.error as e:
					# print ('Port {0} is busy'.format(portname))
					return None, '', False
				# raise RuntimeError("Another program is using %s (%d)" % (portname) )

				'''
				import fcntl
				try:
					fcntl.flock(fd.fileno(), fcntl.LOCK_EX | fcntl.LOCK_NB)
					#print ('locked access to ',portname,fd.fileno())
				except IOError:
					#print ('Port {0} is busy'.format(portname))
					return None,'',False
				'''
			else:
				fd = serial.Serial(portname, self.BAUD, timeout=0.5)
			# print ('not on linux',platform.system())

			if (fd.in_waiting):
				fd.flush()
				fd.readall()

		except serial.SerialException as ex:
			print('Port {0} is unavailable: {1}'.format(portname, ex))
			return None, '', False

		version = self.__get_version__(fd)
		self.nano = False
		if len(version) == 1:
			if ord(version) == ord(self.VERSIONNUM):
				return fd, ord(version), True
			elif ord(version) in [ord(self.VERSIONNUM_168P), ord(self.VERSIONNUM_328P),
			                      ord(self.VERSIONNUM_UNO)]:  # assume it is mega32. will work with glitches
				self.nano = True
				return fd, ord(self.VERSIONNUM), True
		print('version check failed', len(version), ord(version))
		return None, '', False

	def close(self):
		self.fd.close()
		self.portname = None
		self.connected = False
		if self.blockingSocket:
			self.blockingSocket.shutdown(1)
			self.blockingSocket.close()
			self.blockingSocket = None

	def __sendByte__(self, val):
		"""
		transmits a BYTE
		val - byte to send
		"""
		# print (val)
		try:
			if (type(val) == int):
				self.fd.write(Byte.pack(val))
			else:
				self.fd.write(val)
		except:
			self.connected = False

	def __getByte__(self):
		"""
		reads a byte from the serial port and returns it
		"""
		try:
			ss = self.fd.read(1)
		except:
			self.connected = False
			print('No byte received. Disconnected?', time.ctime())
			return 0
		if len(ss):
			return Byte.unpack(ss)[0]
		else:
			print('byte communication error.', time.ctime())
			return None

	def setReg(self, reg, data):
		# print(reg,data)
		if reg not in self.REGS and type(reg) == str: return False
		self.REGSTATES[reg] = data
		self.__sendByte__(self.WRITEB)
		if reg in self.REGS:
			self.__sendByte__(self.REGS[reg])
		else:
			# print('missing register',reg)
			self.__sendByte__(reg)
		self.__sendByte__(data)

	def getReg(self, reg):
		if (reg not in self.REGS) and type(reg) == str:
			print('unknown register', reg)
			return 0
		self.__sendByte__(self.READB)
		if reg in self.REGS:
			self.__sendByte__(self.REGS[reg])
		else:
			# print('missing register',reg)
			self.__sendByte__(reg)
		val = self.__getByte__()
		self.REGSTATES[reg] = val
		return val

	def readADC(self, ch):  # Read the ADC channel
		self.setReg(self.REGS['ADMUX'], 64 | ch)
		self.setReg(self.REGS['ADCSRA'], 197)  # Enable the ADC
		low = self.getReg(self.REGS['ADCL'])
		hi = self.getReg(self.REGS['ADCH'])
		return (hi << 8) | low

	'''
	def writeEEPROM(self,data):
		addr=0
		for a in data:
			timeout=20 #20mS
			while ((self.getReg('EECR') & 2)):
				timeout-=1
				if timeout==0:
					print ('wait timeout!')
					break
				time.sleep(0.001)

			self.setReg('EEARL',addr)
			self.setReg('EEARH',0)
			self.setReg('EEDR',a)
			self.setReg('EECR',4) ##EEMPE master write enable
			self.setReg('EECR',6) # EEPE write
			addr+=1

	def readEEPROM(self,total):
		addr=0; b = []
		for a in range(total):
			timeout=20 #20mS
			while ((self.getReg('EECR') & 2)):
				timeout-=1
				if timeout==0:
					print ('wait timeout!')
					break
				time.sleep(0.001)

			self.setReg('EEARL',addr)
			self.setReg('EEARH',0)
			self.setReg('EECR',1) ##EERE. Read 
			b.append(self.getReg('EEDR'))
			addr+=1
		return b
	'''

	# I2C Calls. Will be replaced with firmware level implementation
	'''
	def initI2C(self): # Initialize I2C
		self.setReg('TWSR',0x00)
		self.setReg('TWBR',0x46)
		self.setReg('TWCR',0x04)

	def startI2C(self): 
		self.setReg('TWCR',(1<<7)|(1<<5) | (1<<2))
		timeout=10 #20mS
		time.sleep(0.001)
		while (not(self.getReg('TWCR') & (1<<7))):
			timeout-=1
			print('waiy')
			if timeout==0:
				print('start timeout')
				break
			time.sleep(0.001)

	def stopI2C(self):
		self.setReg('TWCR',(1<<7) | (1<<4) | (1<<2))
		timeout=10 #20mS
		time.sleep(0.001)

	def writeI2C(self,val):
		self.setReg('TWDR',val)
		self.setReg('TWCR',(1<<7) | (1<<2))
		timeout=20 #20mS
		while (not(self.getReg('TWCR') & (1<<7))):
			timeout-=1
			if timeout==0:
				print ('write timeout')
				break
			time.sleep(0.001)

	def readI2C(self,ack):
		self.setReg('TWCR',(1<<7) | (1<<2) | (ack<<6))
		timeout=20 #20mS
		while (not(self.getReg('TWCR') & (1<<7))):
			timeout-=1
			if timeout==0:
				print ('read timeout')
				break
			time.sleep(0.001)
		if timeout:
			return self.getReg('TWDR')
		else:
			return None
	def I2CWriteBulk(self,address,bytestream): 
		# Individual register write based writing. takes a few hundred milliseconds
		self.startI2C()
		self.writeI2C(address<<1)
		for a in bytestream:
			self.writeI2C(a) 
		self.stopI2C()

	def I2CReadBulk(self,address,register,total): 
		# Individual register write based reading. takes a few hundred milliseconds
		self.startI2C()
		self.writeI2C(address<<1)
		self.writeI2C(register)
		self.startI2C()
		self.writeI2C((address<<1)|1) #Read
		b=[]
		for a in range(total-1):
			b.append(self.readI2C(1) )
		b.append(self.readI2C(0))
		self.stopI2C()
		return b

	# Individual register write based scan. takes a few seconds
	def I2CScan(self):
		found = []
		for a in range(127):
			self.startI2C()
			time.sleep(0.005)
			self.writeI2C(a<<1)
			time.sleep(0.005)
			if self.getReg('TWSR') == 0x18:
				found.append(a)
		self.stopI2C()
		return found
	'''

	def I2CScan(self):
		self.__sendByte__(self.I2C_SCAN)
		addrs = []
		val = self.__getByte__()
		if val is None:
			return []
		while val < 254:
			addrs.append(val)
			val = self.__getByte__()
		if (val == 254): print('timed out')
		# self.setReg('TWBR',0xFF) #I2C speed minimal. testing purposes

		return addrs

	def I2CWriteBulk(self, address, bytestream):
		self.__sendByte__(self.I2C_WRITE)
		self.__sendByte__(address)  # address
		self.__sendByte__(len(bytestream))  # Total bytes to write. <=255
		for a in bytestream:
			self.__sendByte__(Byte.pack(a))
		tmt = self.__getByte__()
		if tmt:
			return True  # Hasn't Timed out.
		else:
			return False  # Timeout occured

	def I2CReadBulk(self, address, register, total):
		self.__sendByte__(self.I2C_READ)
		self.__sendByte__(address)  # address
		self.__sendByte__(register)  # device register address
		self.__sendByte__(total)  # Total bytes to read. <=255
		data = []
		for a in range(total):
			val = self.__getByte__()
			data.append(val)
		tmt = self.__getByte__()
		return data, True if not tmt else False

	class KalmanFilter(object):
		'''
		Credits:http://scottlobdell.me/2014/08/kalman-filtering-python-reading-sensor-input/
		'''

		def __init__(self, var, est, initial_values):  # var = process variance. est = estimated measurement var
			self.var = np.array(var)
			self.est = np.array(est)
			self.posteri_estimate = np.array(initial_values)
			self.posteri_error_estimate = np.ones(len(var), dtype=np.float16)

		def input(self, vals):
			vals = np.array(vals)
			priori_estimate = self.posteri_estimate
			priori_error_estimate = self.posteri_error_estimate + self.var

			blending_factor = priori_error_estimate / (priori_error_estimate + self.est)
			self.posteri_estimate = priori_estimate + blending_factor * (vals - priori_estimate)
			self.posteri_error_estimate = (1 - blending_factor) * priori_error_estimate

		def output(self):
			return self.posteri_estimate

	MPU6050_kalman = 0

	def MPU6050_init(self):
		self.I2CWriteBulk(0x68, [0x1B, 0 << 3])  # Gyro Range . 250
		self.I2CWriteBulk(0x68, [0x1C, 0 << 3])  # Accelerometer Range. 2
		self.I2CWriteBulk(0x68, [0x6B, 0x00])  # poweron
		v, tmt = self.I2CReadBulk(0x68, 0x75, 1)
		self.mag = False
		if v[0] in [0x71, 0x73]:  # MPU9255, MPU9250. Has magnetometer. Enable it.
			self.mag = True
			self.I2CWriteBulk(0x68,
			                  [0x37, 0x22])  # INT_PIN_CFG . I2C passthrough enabled. Rescan to detect magnetometer.

	def MPU6050_gyro_range(self, val):
		self.I2CWriteBulk(0x68,
		                  [0x1B, val << 3])  # Gyro Range . 250,500,1000,2000 -> 0,1,2,3 -> shift left by 3 positions

	def MPU6050_accel_range(self, val):
		print(val)
		self.I2CWriteBulk(0x68,
		                  [0x1C, val << 3])  # Accelerometer Range. 2,4,8,16 -> 0,1,2,3 -> shift left by 3 positions

	def MPU6050_kalman_set(self, val):
		if not val:
			self.MPU6050_kalman = 0
			return
		noise = []
		for a in range(50):
			noise.append(np.array(self.MPU6050_all(disableKalman=True)))
		noise = np.array(noise)
		self.MPU6050_kalman = self.KalmanFilter(1e6 * np.ones(noise.shape[1]) / (10 ** val), np.std(noise, 0) ** 2,
		                                        noise[-1])

	def MPU6050_accel(self):
		b, tmt = self.I2CReadBulk(0x68, 0x3B, 6)
		if tmt: return None
		if None not in b:
			return [(b[x * 2 + 1] << 8) | b[x * 2] for x in range(3)]  # X,Y,Z

	def MPU6050_gyro(self):
		b, tmt = self.I2CReadBulk(0x68, 0x3B + 6, 6)
		if tmt: return None
		if None not in b:
			return [(b[x * 2 + 1] << 8) | b[x * 2] for x in range(3)]  # X,Y,Z

	def MPU6050_all(self, disableKalman=False):
		'''
		returns a 7 element list. Ax,Ay,Az,T,Gx,Gy,Gz
		returns None if communication timed out with I2C sensor
		disableKalman can be set to True if Kalman was previously enabled.
		'''
		b, tmt = self.I2CReadBulk(0x68, 0x3B, 14)
		if tmt: return None
		if None not in b:
			if (not self.MPU6050_kalman) or disableKalman:
				return [np.int16((b[x * 2] << 8) | b[x * 2 + 1]) for x in range(7)]  # Ax,Ay,Az, Temp, Gx, Gy,Gz
			else:
				self.MPU6050_kalman.input([np.int16((b[x * 2] << 8) | b[x * 2 + 1]) for x in range(7)])
				return self.MPU6050_kalman.output()

	######## AK8963 magnetometer attacched to MPU925x #######
	AK8963_ADDRESS = 0x0C
	_AK8963_CNTL = 0x0A

	def AK8963_init(self):
		self.I2CWriteBulk(self.AK8963_ADDRESS, [self._AK8963_CNTL, 0])  # power down mag
		self.I2CWriteBulk(self.AK8963_ADDRESS,
		                  [self._AK8963_CNTL, (1 << 4) | 6])  # mode   (0=14bits,1=16bits) <<4 | (2=8Hz , 6=100Hz)

	def AK8963_all(self, disableKalman=False):
		vals, tmt = self.I2CReadBulk(self.AK8963_ADDRESS, 0x03,
		                             7)  # 6+1 . 1(ST2) should not have bit 4 (0x8) true. It's ideally 16 . overflow bit
		if tmt: return None
		ax, ay, az = struct.unpack('hhh', bytes(vals[:6]))
		if not vals[6] & 0x08:
			return [ax, ay, az]
		else:
			return None

	####### BMP280 ###################
	# https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bme280-ds002.pdf
	## Partly from https://github.com/farmerkeith/BMP280-library/blob/master/farmerkeith_BMP280.cpp
	BMP280_ADDRESS = 118
	BMP280_REG_CONTROL = 0xF4
	BMP280_REG_RESULT = 0xF6
	BMP280_HUMIDITY_ENABLED = False
	_BMP280_humidity_calib = [0] * 6
	BMP280_oversampling = 0
	_BMP280_PRESSURE_MIN_HPA = 0
	_BMP280_PRESSURE_MAX_HPA = 1600
	_BMP280_sea_level_pressure = 1013.25  # for calibration.. from circuitpython library

	def BMP280_init(self):
		b = self.I2CWriteBulk(self.BMP280_ADDRESS, [0xE0, 0xB6])  # reset
		time.sleep(0.1)
		self.BMP280_HUMIDITY_ENABLED = False
		b, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0xD0, 1)
		print(b)
		if b is None: return None
		b = b[0]
		if b in [0x58, 0x56, 0x57]:
			print('BMP280. ID:', b)
		elif b == 0x60:
			self.BMP280_HUMIDITY_ENABLED = True
			print('BME280 . includes humidity')
		else:
			print('ID unknown', b)
		# get calibration data
		b, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0x88, 24)  # 24 bytes containing calibration data
		coeff = list(struct.unpack('<HhhHhhhhhhhh', bytes(b)))
		coeff = [float(i) for i in coeff]
		self._BMP280_temp_calib = coeff[:3]
		self._BMP280_pressure_calib = coeff[3:]
		self._BMP280_t_fine = 0.

		if self.BMP280_HUMIDITY_ENABLED:
			self.I2CWriteBulk(self.BMP280_ADDRESS, [0xF2, 0b101])  # ctrl_hum. oversampling x 16
			# humidity calibration read
			self._BMP280_humidity_calib = [0] * 6
			val, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0xA1, 1)
			self._BMP280_humidity_calib[0] = val[0]  # H1
			coeff, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0xE1, 7)
			coeff = list(struct.unpack('<hBbBbb', bytes(coeff)))
			self._BMP280_humidity_calib[1] = float(coeff[0])
			self._BMP280_humidity_calib[2] = float(coeff[1])
			self._BMP280_humidity_calib[3] = float((coeff[2] << 4) | (coeff[3] & 0xF))
			self._BMP280_humidity_calib[4] = float((coeff[4] << 4) | (coeff[3] >> 4))
			self._BMP280_humidity_calib[5] = float(coeff[5])
			print('calibration data: ', self._BMP280_temp_calib, self._BMP280_humidity_calib)

		self.I2CWriteBulk(self.BMP280_ADDRESS, [0xF4, 0xFF])  # ctrl_meas (oversampling of pressure, temperature)

	def _BMP280_calcTemperature(self, adc_t):
		v1 = (adc_t / 16384.0 - self._BMP280_temp_calib[0] / 1024.0) * self._BMP280_temp_calib[1]
		v2 = ((adc_t / 131072.0 - self._BMP280_temp_calib[0] / 8192.0) * (
					adc_t / 131072.0 - self._BMP280_temp_calib[0] / 8192.0)) * self._BMP280_temp_calib[2]
		self._BMP280_t_fine = int(v1 + v2)
		return (v1 + v2) / 5120.0  # actual temperature.

	def _BMP280_calcPressure(self, adc_p, adc_t):
		self._BMP280_calcTemperature(adc_t)  # t_fine has been set now.
		# Algorithm from the BMP280 driver. adapted from adafruit adaptation
		# https://github.com/BoschSensortec/BMP280_driver/blob/master/bmp280.c
		var1 = self._BMP280_t_fine / 2.0 - 64000.0
		var2 = var1 * var1 * self._BMP280_pressure_calib[5] / 32768.0
		var2 = var2 + var1 * self._BMP280_pressure_calib[4] * 2.0
		var2 = var2 / 4.0 + self._BMP280_pressure_calib[3] * 65536.0
		var3 = self._BMP280_pressure_calib[2] * var1 * var1 / 524288.0
		var1 = (var3 + self._BMP280_pressure_calib[1] * var1) / 524288.0
		var1 = (1.0 + var1 / 32768.0) * self._BMP280_pressure_calib[0]
		if not var1:
			return _BMP280_PRESSURE_MIN_HPA
		pressure = 1048576.0 - adc_p
		pressure = ((pressure - var2 / 4096.0) * 6250.0) / var1
		var1 = self._BMP280_pressure_calib[8] * pressure * pressure / 2147483648.0
		var2 = pressure * self._BMP280_pressure_calib[7] / 32768.0
		pressure = pressure + (var1 + var2 + self._BMP280_pressure_calib[6]) / 16.0
		pressure /= 100
		if pressure < self._BMP280_PRESSURE_MIN_HPA:
			return self._BMP280_PRESSURE_MIN_HPA
		if pressure > self._BMP280_PRESSURE_MAX_HPA:
			return self._BMP280_PRESSURE_MAX_HPA
		return pressure

	def _BMP280_calcHumidity(self, adc_h, adc_t):
		self._BMP280_calcTemperature(adc_t)  # t fine set.
		var1 = float(self._BMP280_t_fine) - 76800.0
		var2 = (self._BMP280_humidity_calib[3] * 64.0 + (self._BMP280_humidity_calib[4] / 16384.0) * var1)
		var3 = adc_h - var2
		var4 = self._BMP280_humidity_calib[1] / 65536.0
		var5 = (1.0 + (self._BMP280_humidity_calib[2] / 67108864.0) * var1)
		var6 = 1.0 + (self._BMP280_humidity_calib[5] / 67108864.0) * var1 * var5
		var6 = var3 * var4 * (var5 * var6)
		humidity = var6 * (1.0 - self._BMP280_humidity_calib[0] * var6 / 524288.0)
		if humidity > 100:
			return 100
		if humidity < 0:
			return 0

		return humidity

	def BMP280_all(self):
		if self.BMP280_HUMIDITY_ENABLED:
			data, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0xF7, 8)
		else:
			data, tmt = self.I2CReadBulk(self.BMP280_ADDRESS, 0xF7, 6)
		# os = [0x34,0x74,0xb4,0xf4]
		# delays=[0.005,0.008,0.014,0.026]
		# self.I2CWriteBulk(self.BMP280_ADDRESS,[self.BMP280_REG_CONTROL,os[self.BMP280_oversampling] ])
		# time.sleep(delays[self.BMP280_oversampling])
		if tmt: return None
		if data is None: return None
		if None not in data:
			# Convert pressure and temperature data to 19-bits
			adc_p = (((data[0] & 0xFF) * 65536.) + ((data[1] & 0xFF) * 256.) + (data[2] & 0xF0)) / 16.
			adc_t = (((data[3] & 0xFF) * 65536.) + ((data[4] & 0xFF) * 256.) + (data[5] & 0xF0)) / 16.
			if self.BMP280_HUMIDITY_ENABLED:
				adc_h = (data[6] * 256.) + data[7]
				return [self._BMP280_calcPressure(adc_p, adc_t), self._BMP280_calcTemperature(adc_t),
				        self._BMP280_calcHumidity(adc_h, adc_t)]
			else:
				return [self._BMP280_calcPressure(adc_p, adc_t), self._BMP280_calcTemperature(adc_t), 0]

		return None

	########## TCS34725 RGB sensor ###########

	_TCS34725_COMMAND_BIT = 0x80
	_TCS34725_REGISTER_STATUS = 0x13
	_TCS34725_REGISTER_CDATA = 0x14
	_TCS34725_REGISTER_RDATA = 0x16
	_TCS34725_REGISTER_GDATA = 0x18
	_TCS34725_REGISTER_BDATA = 0x1a

	_TCS34725_REGISTER_ENABLE = 0x00
	_TCS34725_REGISTER_ATIME = 0x01
	_TCS34725_REGISTER_AILT = 0x04
	_TCS34725_REGISTER_AIHT = 0x06
	_TCS34725_REGISTER_ID = 0x12
	_TCS34725_REGISTER_APERS = 0x0c
	_TCS34725_REGISTER_CONTROL = 0x0f
	_TCS34725_REGISTER_SENSORID = 0x12
	_TCS34725_REGISTER_STATUS = 0x13
	_TCS34725_ENABLE_AIEN = 0x10
	_TCS34725_ENABLE_WEN = 0x08
	_TCS34725_ENABLE_AEN = 0x02
	_TCS34725_ENABLE_PON = 0x01

	_GAINS = (1, 4, 16, 60)
	_CYCLES = (0, 1, 2, 3, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60)
	_INTEGRATION_TIME_THRESHOLD_LOW = 2.4
	_INTEGRATION_TIME_THRESHOLD_HIGH = 614.4

	TCS34725_ADDRESS = 41

	def TCS34725_init(self):
		enable, tmt = self.I2CReadBulk(self.TCS34725_ADDRESS,
		                               self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_ENABLE, 1)
		enable = enable[0]
		self.I2CWriteBulk(self.TCS34725_ADDRESS,
		                  [self._TCS34725_REGISTER_ENABLE, enable | self._TCS34725_ENABLE_PON])  #
		time.sleep(0.003)
		self.I2CWriteBulk(self.TCS34725_ADDRESS, [self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_ENABLE,
		                                          enable | self._TCS34725_ENABLE_PON | self._TCS34725_ENABLE_AEN | self._TCS34725_ENABLE_AIEN])  #
		self.I2CWriteBulk(self.TCS34725_ADDRESS, [self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_APERS, 10])
		self.I2CWriteBulk(self.TCS34725_ADDRESS, [self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_ATIME, 256 - 40])

	def TCS34725_gain(self, g):
		self.I2CWriteBulk(self.TCS34725_ADDRESS,
		                  [self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_CONTROL, g])  # Gain

	def TCS34725_all(self):
		R, tmt = self.I2CReadBulk(self.TCS34725_ADDRESS, self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_RDATA, 2)
		G, tmt = self.I2CReadBulk(self.TCS34725_ADDRESS, self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_GDATA, 2)
		B, tmt = self.I2CReadBulk(self.TCS34725_ADDRESS, self._TCS34725_COMMAND_BIT | self._TCS34725_REGISTER_BDATA, 2)

		if tmt: return None
		return [R[0] | (R[1] << 8), G[0] | (G[1] << 8), B[0] | (B[1] << 8)]

	def TCS34725_range(self):
		pass

	####### MS5611 Altimeter ###################
	MS5611_ADDRESS = 119

	def MS5611_init(self):
		self.I2CWriteBulk(self.MS5611_ADDRESS, [0x1E])  # reset
		time.sleep(0.5)
		self._MS5611_calib = np.zeros(6)

		# calibration data.
		# pressure gain, offset . T coeff of P gain, offset. Ref temp. T coeff of T. all unsigned shorts.
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xA2, 2)
		if tmt: return
		self._MS5611_calib[0] = struct.unpack('!H', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xA4, 2)
		self._MS5611_calib[1] = struct.unpack('!H', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xA6, 2)
		self._MS5611_calib[2] = struct.unpack('!H', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xA8, 2)
		self._MS5611_calib[3] = struct.unpack('!H', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xAA, 2)
		self._MS5611_calib[4] = struct.unpack('!H', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0xAC, 2)
		self._MS5611_calib[5] = struct.unpack('!H', bytes(b))[0]
		print('Calibration for MS5611:', self._MS5611_calib)


	#BMP180 pressure sensor
	BMP180 = None

	def BMP180_init(self):
		import BMP180
		self.BMP180 = BMP180(self.I2CReadBulk, self.I2CWriteBulk)

	def BMP180_all(self):
		if self.BMP180 is not None:
			return self.BMP180.getRaw()

	def MS5611_all(self):
		self.I2CWriteBulk(self.MS5611_ADDRESS, [0x48])  # 0x48 Pressure conversion(OSR = 4096) command
		time.sleep(0.01)  # 10mS
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0x00, 3)  # data.
		D1 = b[0] * 65536 + b[1] * 256 + b[2]  # msb2, msb1, lsb

		self.I2CWriteBulk(self.MS5611_ADDRESS, [0x58])  # 0x58 Temperature conversion(OSR = 4096) command
		time.sleep(0.01)
		b, tmt = self.I2CReadBulk(self.MS5611_ADDRESS, 0x00, 3)  # data.
		D2 = b[0] * 65536 + b[1] * 256 + b[2]  # msb2, msb1, lsb

		dT = D2 - self._MS5611_calib[4] * 256
		TEMP = 2000 + dT * self._MS5611_calib[5] / 8388608
		OFF = self._MS5611_calib[1] * 65536 + (self._MS5611_calib[3] * dT) / 128
		SENS = self._MS5611_calib[0] * 32768 + (self._MS5611_calib[2] * dT) / 256
		T2 = 0;
		OFF2 = 0;
		SENS2 = 0
		if TEMP >= 2000:
			T2 = 0
			OFF2 = 0
			SENS2 = 0
		elif TEMP < 2000:
			T2 = (dT * dT) / 2147483648
			OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2
			SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4
			if TEMP < -1500:
				OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500))
				SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2

		TEMP = TEMP - T2
		OFF = OFF - OFF2
		SENS = SENS - SENS2
		pressure = ((((D1 * SENS) / 2097152) - OFF) / 32768.0) / 100.0
		cTemp = TEMP / 100.0
		return [pressure, cTemp, 0]

	### INA3221 3 channel , high side current sensor #############
	INA3221_ADDRESS = 0x41
	_INA3221_REG_CONFIG = 0x0
	_INA3221_SHUNT_RESISTOR_VALUE = 0.1
	_INA3221_REG_SHUNTVOLTAGE = 0x01
	_INA3221_REG_BUSVOLTAGE = 0x02

	def INA3221_init(self):
		self.I2CWriteBulk(self.INA3221_ADDRESS, [self._INA3221_REG_CONFIG, 0b01110101, 0b00100111])  # cont shunt.

	def INA3221_all(self):
		I = [0., 0., 0.]
		b, tmt = self.I2CReadBulk(self.INA3221_ADDRESS, self._INA3221_REG_SHUNTVOLTAGE, 2)
		if tmt: return None
		b[1] &= 0xF8;
		I[0] = struct.unpack('!h', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.INA3221_ADDRESS, self._INA3221_REG_SHUNTVOLTAGE + 2, 2)
		if tmt: return None
		b[1] &= 0xF8;
		I[1] = struct.unpack('!h', bytes(b))[0]
		b, tmt = self.I2CReadBulk(self.INA3221_ADDRESS, self._INA3221_REG_SHUNTVOLTAGE + 4, 2)
		if tmt: return None
		b[1] &= 0xF8;
		I[2] = struct.unpack('!h', bytes(b))[0]
		return [0.005 * I[0] / self._INA3221_SHUNT_RESISTOR_VALUE, 0.005 * I[1] / self._INA3221_SHUNT_RESISTOR_VALUE,
		        0.005 * I[2] / self._INA3221_SHUNT_RESISTOR_VALUE]

	### SHT21 HUMIDITY TEMPERATURE SENSOR #############
	SHT21_ADDRESS = 0x41
	_SHT21_TEMP = 0xf3
	_SHT21_HUM = 0xf5
	_SHT21_RESET = 0xFE
	_INA3221_REG_SHUNTVOLTAGE = 0x01
	_INA3221_REG_BUSVOLTAGE = 0x02

	def SHT21_init(self):
		self.I2CWriteBulk(self.SHT21_ADDRESS, [self._SHT21_RESET])  # reset
		time.sleep(0.1)

	def SHT21_all(self):
		self.I2CWriteBulk(self.SHT21_ADDRESS, [self._SHT21_TEMP])
		time.sleep(.1)
		self.startI2C()
		self.writeI2C((self.SHT21_ADDRESS << 1) | 1)  # Read
		b = []
		for a in range(2): b.append(self.readI2C(1))
		b.append(self.readI2C(0))
		self.stopI2C()
		temperature, checksum = struct.unpack('>HB', bytes(b))
		return [temperature * 175.72 / 65536.0 - 46.85, 0]

	####### TSL2561 LIGHT SENSOR ###########
	TSL_GAIN = 0x00  # 0x00=1x , 0x01 = 16x
	TSL_TIMING = 0x00  # 0x00=3 mS , 0x01 = 101 mS, 0x02 = 402mS

	def TSL2561_init(self):
		self.I2CWriteBulk(0x39, [0x80, 0x03])  # poweron
		self.I2CWriteBulk(0x39, [0x80 | 0x01, self.TSL_GAIN | self.TSL_TIMING])
		return self.TSL2561_all()

	def TSL2561_gain(self, gain):
		self.TSL_GAIN = gain << 4
		self.TSL2561_config(self.TSL_GAIN, self.TSL_TIMING)

	def TSL2561_timing(self, timing):
		self.TSL_TIMING = timing
		self.TSL2561_config(self.TSL_GAIN, self.TSL_TIMING)

	def TSL2561_rate(self, timing):
		self.TSL_TIMING = timing
		self.TSL2561_config(self.TSL_GAIN, self.TSL_TIMING)

	def TSL2561_config(self, gain, timing):
		self.I2CWriteBulk(0x39,
		                  [0x80 | 0x01, gain | timing])  # Timing register 0x01. gain[1x,16x] | timing[13mS,100mS,400mS]

	def TSL2561_all(self):
		'''
		returns a 2 element list. total,IR
		returns None if communication timed out with I2C sensor
		'''
		b, tmt = self.I2CReadBulk(0x39, 0x80 | 0x20 | 0x0C, 4)
		if tmt: return None
		if None not in b:
			return [(b[x * 2 + 1] << 8) | b[x * 2] for x in range(2)]  # total, IR

	def MLX90614_init(self):
		pass

	def MLX90614_all(self):
		'''
		return a single element list.  None if failed
		'''
		vals, tmt = self.I2CReadBulk(0x5A, 0x07, 3)
		if tmt: return None
		if vals:
			if len(vals) == 3:
				return [((((vals[1] & 0x007f) << 8) + vals[0]) * 0.02) - 0.01 - 273.15]
			else:
				return None
		else:
			return None

	MCP5725_ADDRESS = 0x60

	def MCP4725_init(self):
		pass

	def MCP4725_set(self, val):
		'''
		Set the DAC value. 0 - 4095
		'''
		self.I2CWriteBulk(self.MCP5725_ADDRESS, [0x40, (val >> 4) & 0xFF, (val & 0xF) << 4])

	####################### HMC5883L MAGNETOMETER ###############

	HMC5883L_ADDRESS = 0x1E
	HMC_CONFA = 0x00
	HMC_CONFB = 0x01
	HMC_MODE = 0x02
	HMC_STATUS = 0x09

	# --------CONFA register bits. 0x00-----------
	HMCSamplesToAverage = 0
	HMCSamplesToAverage_choices = [1, 2, 4, 8]

	HMCDataOutputRate = 6
	HMCDataOutputRate_choices = [0.75, 1.5, 3, 7.5, 15, 30, 75]

	HMCMeasurementConf = 0

	# --------CONFB register bits. 0x01-----------
	HMCGainValue = 7  # least sensitive
	HMCGain_choices = [8, 7, 6, 5, 4, 3, 2, 1]
	HMCGainScaling = [1370., 1090., 820., 660., 440., 390., 330., 230.]

	def HMC5883L_init(self):
		self.__writeHMCCONFA__()
		self.__writeHMCCONFB__()
		self.I2CWriteBulk(self.HMC5883L_ADDRESS, [self.HMC_MODE, 0])  # enable continuous measurement mode

	def __writeHMCCONFB__(self):
		self.I2CWriteBulk(self.HMC5883L_ADDRESS, [self.HMC_CONFB, self.HMCGainValue << 5])  # set gain

	def __writeHMCCONFA__(self):
		self.I2CWriteBulk(self.HMC5883L_ADDRESS, [self.HMC_CONFA,
		                                          (self.HMCDataOutputRate << 2) | (self.HMCSamplesToAverage << 5) | (
			                                          self.HMCMeasurementConf)])

	def HMC5883L_getVals(self, addr, bytes):
		vals = self.I2CReadBulk(self.ADDRESS, addr, bytes)
		return vals

	def HMC5883L_all(self):
		vals = self.HMC5883L_getVals(0x03, 6)
		if vals:
			if len(vals) == 6:
				return [np.int16(vals[a * 2] << 8 | vals[a * 2 + 1]) / self.HMCGainScaling[self.HMCGainValue] for a in
				        range(3)]
			else:
				return False
		else:
			return False

	PCA9685_address = 64

	def PCA9685_init(self):
		prescale_val = int((25000000.0 / 4096 / 60.) - 1)  # default clock at 25MHz
		# self.I2CWriteBulk(self.PCA9685_address, [0x00,0x10]) #MODE 1 , Sleep
		print('clock set to,', prescale_val)
		self.I2CWriteBulk(self.PCA9685_address, [0xFE, prescale_val])  # PRESCALE , prescale value
		self.I2CWriteBulk(self.PCA9685_address, [0x00, 0x80])  # MODE 1 , restart
		self.I2CWriteBulk(self.PCA9685_address, [0x01, 0x04])  # MODE 2 , Totem Pole

		pass

	CH0 = 0x6  # LED0 start register
	CH0_ON_L = 0x6  # channel0 output and brightness control byte 0
	CH0_ON_H = 0x7  # channel0 output and brightness control byte 1
	CH0_OFF_L = 0x8  # channel0 output and brightness control byte 2
	CH0_OFF_H = 0x9  # channel0 output and brightness control byte 3
	CHAN_WIDTH = 4

	def PCA9685_set(self, chan, angle):
		'''
		chan: 1-16
		Set the servo angle for SG90: angle(0 - 180)
		'''
		Min = 180
		Max = 650
		val = int(((Max - Min) * (angle / 180.)) + Min)
		print(chan, angle, val)
		self.I2CWriteBulk(self.PCA9685_address, [self.CH0_ON_L + self.CHAN_WIDTH * (chan - 1), 0])  #
		self.I2CWriteBulk(self.PCA9685_address,
		                  [self.CH0_ON_H + self.CHAN_WIDTH * (chan - 1), 0])  # Turn on immediately. At 0.
		self.I2CWriteBulk(self.PCA9685_address, [self.CH0_OFF_L + self.CHAN_WIDTH * (chan - 1),
		                                         val & 0xFF])  # Turn off after val width 0-4095
		self.I2CWriteBulk(self.PCA9685_address, [self.CH0_OFF_H + self.CHAN_WIDTH * (chan - 1), (val >> 8) & 0xFF])

	## ADS1115
	REG_POINTER_MASK = 0x3
	REG_POINTER_CONVERT = 0
	REG_POINTER_CONFIG = 1
	REG_POINTER_LOWTHRESH = 2
	REG_POINTER_HITHRESH = 3

	REG_CONFIG_OS_MASK = 0x8000
	REG_CONFIG_OS_SINGLE = 0x8000
	REG_CONFIG_OS_BUSY = 0x0000
	REG_CONFIG_OS_NOTBUSY = 0x8000

	REG_CONFIG_MUX_MASK = 0x7000
	REG_CONFIG_MUX_DIFF_0_1 = 0x0000  # Differential P = AIN0, N = AIN1 =default)
	REG_CONFIG_MUX_DIFF_0_3 = 0x1000  # Differential P = AIN0, N = AIN3
	REG_CONFIG_MUX_DIFF_1_3 = 0x2000  # Differential P = AIN1, N = AIN3
	REG_CONFIG_MUX_DIFF_2_3 = 0x3000  # Differential P = AIN2, N = AIN3
	REG_CONFIG_MUX_SINGLE_0 = 0x4000  # Single-ended AIN0
	REG_CONFIG_MUX_SINGLE_1 = 0x5000  # Single-ended AIN1
	REG_CONFIG_MUX_SINGLE_2 = 0x6000  # Single-ended AIN2
	REG_CONFIG_MUX_SINGLE_3 = 0x7000  # Single-ended AIN3

	REG_CONFIG_PGA_MASK = 0x0E00  # bits 11:9
	REG_CONFIG_PGA_6_144V = (0 << 9)  # +/-6.144V range = Gain 2/3
	REG_CONFIG_PGA_4_096V = (1 << 9)  # +/-4.096V range = Gain 1
	REG_CONFIG_PGA_2_048V = (2 << 9)  # +/-2.048V range = Gain 2 =default)
	REG_CONFIG_PGA_1_024V = (3 << 9)  # +/-1.024V range = Gain 4
	REG_CONFIG_PGA_0_512V = (4 << 9)  # +/-0.512V range = Gain 8
	REG_CONFIG_PGA_0_256V = (5 << 9)  # +/-0.256V range = Gain 16

	REG_CONFIG_MODE_MASK = 0x0100  # bit 8
	REG_CONFIG_MODE_CONTIN = (0 << 8)  # Continuous conversion mode
	REG_CONFIG_MODE_SINGLE = (1 << 8)  # Power-down single-shot mode =default)

	REG_CONFIG_DR_MASK = 0x00E0
	REG_CONFIG_DR_8SPS = (0 << 5)  # 8 SPS
	REG_CONFIG_DR_16SPS = (1 << 5)  # 16 SPS
	REG_CONFIG_DR_32SPS = (2 << 5)  # 32 SPS
	REG_CONFIG_DR_64SPS = (3 << 5)  # 64 SPS
	REG_CONFIG_DR_128SPS = (4 << 5)  # 128 SPS
	REG_CONFIG_DR_250SPS = (5 << 5)  # 260 SPS
	REG_CONFIG_DR_475SPS = (6 << 5)  # 475 SPS
	REG_CONFIG_DR_860SPS = (7 << 5)  # 860 SPS

	REG_CONFIG_CMODE_MASK = 0x0010
	REG_CONFIG_CMODE_TRAD = 0x0000
	REG_CONFIG_CMODE_WINDOW = 0x0010

	REG_CONFIG_CPOL_MASK = 0x0008
	REG_CONFIG_CPOL_ACTVLOW = 0x0000
	REG_CONFIG_CPOL_ACTVHI = 0x0008

	REG_CONFIG_CLAT_MASK = 0x0004
	REG_CONFIG_CLAT_NONLAT = 0x0000
	REG_CONFIG_CLAT_LATCH = 0x0004

	REG_CONFIG_CQUE_MASK = 0x0003
	REG_CONFIG_CQUE_1CONV = 0x0000
	REG_CONFIG_CQUE_2CONV = 0x0001
	REG_CONFIG_CQUE_4CONV = 0x0002
	REG_CONFIG_CQUE_NONE = 0x0003

	ADS1115_gains = OrderedDict([('GAIN_TWOTHIRDS', REG_CONFIG_PGA_6_144V), ('GAIN_ONE', REG_CONFIG_PGA_4_096V),
	                             ('GAIN_TWO', REG_CONFIG_PGA_2_048V), ('GAIN_FOUR', REG_CONFIG_PGA_1_024V),
	                             ('GAIN_EIGHT', REG_CONFIG_PGA_0_512V), ('GAIN_SIXTEEN', REG_CONFIG_PGA_0_256V)])
	ADS1115_gain_scaling = OrderedDict(
		[('GAIN_TWOTHIRDS', 0.1875), ('GAIN_ONE', 0.125), ('GAIN_TWO', 0.0625), ('GAIN_FOUR', 0.03125),
		 ('GAIN_EIGHT', 0.015625), ('GAIN_SIXTEEN', 0.0078125)])
	ADS1115_scaling = 0.125
	ADS1115_channels = OrderedDict(
		[('UNI_0', 0), ('UNI_1', 1), ('UNI_2', 2), ('UNI_3', 3), ('DIFF_01', '01'), ('DIFF_23', '23')])
	ADS1115_rates = OrderedDict(
		[(8, REG_CONFIG_DR_8SPS), (16, REG_CONFIG_DR_16SPS), (32, REG_CONFIG_DR_32SPS), (64, REG_CONFIG_DR_64SPS),
		 (128, REG_CONFIG_DR_128SPS), (250, REG_CONFIG_DR_250SPS), (475, REG_CONFIG_DR_475SPS),
		 (860, REG_CONFIG_DR_860SPS)])  # sampling data rate
	ADS1115_DATARATE = 250  # 250SPS [ 8, 16, 32, 64, 128, 250, 475, 860 ]
	ADS1115_GAIN = REG_CONFIG_PGA_4_096V  # +/-4.096V range = Gain 1 . [+-6, +-4, +-2, +-1, +-0.5, +- 0.25]
	ADS1115_CHANNEL = 0  # ref: type_selection
	ADS1115_ADDRESS = 0x48

	def ADS1115_init(self):
		self.I2CWriteBulk(self.ADS1115_ADDRESS, [0x80, 0x03])  # poweron

	def ADS1115_gain(self, gain):
		'''
		options : 'GAIN_TWOTHIRDS','GAIN_ONE','GAIN_TWO','GAIN_FOUR','GAIN_EIGHT','GAIN_SIXTEEN'
		'''
		print('setting gain:', str(gain))
		if (type(gain) == int):  # From the UI selectors which return index
			self.ADS1115_GAIN = list(self.ADS1115_gains.items())[gain][1]
			print('set gain with index selection:', self.ADS1115_GAIN)
			self.ADS1115_scaling = list(self.ADS1115_gain_scaling.items())[gain][1]
			print('Scaling factor:', self.ADS1115_scaling)
		else:
			self.ADS1115_GAIN = self.ADS1115_gains.get(gain, self.REG_CONFIG_PGA_4_096V)
			self.ADS1115_scaling = self.ADS1115_gain_scaling.get(gain)
			print('set gain type B:', str(gain), self.ADS1115_GAIN, self.ADS1115_scaling)

	def ADS1115_channel(self, channel):
		'''
		options 'UNI_0','UNI_1','UNI_2','UNI_3','DIFF_01','DIFF_23'
		'''
		self.ADS1115_CHANNEL = int(channel)
		print('channel', channel, self.ADS1115_CHANNEL)

	def ADS1115_rate(self, rate):
		'''
		data rate options 8,16,32,64,128,250,475,860 SPS . string.
		'''
		opts = [8, 16, 32, 64, 128, 250, 475, 860]
		rate = int(rate)
		if rate < len(opts):
			self.ADS1115_DATARATE = opts[rate]

		print('rate:', rate, self.ADS1115_DATARATE)

	def ADS1115_read(self):
		'''
		returns a voltage from ADS1115 channel selected using ADS1115_channel. default UNI_0 (Unipolar from channel 0)
		'''
		if self.ADS1115_CHANNEL in [0, 1, 2, 3]:
			config = (self.REG_CONFIG_CQUE_NONE  # Disable the comparator (default val)
			          | self.REG_CONFIG_CLAT_NONLAT  # Non-latching (default val)
			          | self.REG_CONFIG_CPOL_ACTVLOW  # Alert/Rdy active low   (default val)
			          | self.REG_CONFIG_CMODE_TRAD  # Traditional comparator (default val)
			          | (self.ADS1115_rates.get(self.ADS1115_DATARATE,
			                                    self.REG_CONFIG_DR_250SPS))  # 250 samples per second (default)
			          | (self.REG_CONFIG_MODE_SINGLE)  # Single-shot mode (default)
			          | self.ADS1115_GAIN)
			if self.ADS1115_CHANNEL == 0:
				config |= self.REG_CONFIG_MUX_SINGLE_0
			elif self.ADS1115_CHANNEL == 1:
				config |= self.REG_CONFIG_MUX_SINGLE_1
			elif self.ADS1115_CHANNEL == 2:
				config |= self.REG_CONFIG_MUX_SINGLE_2
			elif self.ADS1115_CHANNEL == 3:
				config |= self.REG_CONFIG_MUX_SINGLE_3
			# Set 'start single-conversion' bit
			config |= self.REG_CONFIG_OS_SINGLE
			self.I2CWriteBulk(self.ADS1115_ADDRESS, [self.REG_POINTER_CONFIG, (config >> 8) & 0xFF, config & 0xFF])
			time.sleep(1. / self.ADS1115_DATARATE + .002)  # convert to mS to S

			b, tmt = self.I2CReadBulk(self.ADS1115_ADDRESS, self.REG_POINTER_CONVERT, 2)
			if tmt: return None
			if b is not None:
				x = ((b[0] << 8) | b[1]) * self.ADS1115_scaling * 1e-3
				return [((b[0] << 8) | b[1]) * self.ADS1115_scaling * 1e-3]  # scale and convert to volts

		elif self.ADS1115_CHANNEL in ['01', '23']:
			return [0]


if __name__ == '__main__':
	a = connect(autoscan=True)
	print('version', a.version)
	print('------------')
	if not a.connected:
		sys.exit(1)
	time.sleep(0.01)
	a.setReg('DDRC', 3)
	a.setReg('PORTC', 2)
	time.sleep(1)
	a.setReg('PORTC', 3)
	a.setReg('DDRC', 0)
	print(a.I2CScan())
	'''
	a.PCA9685_init()
	a.PCA9685_set(1,650)

	for x in range(180):
		a.PCA9685_set(1,x)
		time.sleep(0.01)

	
	a.TSL2561_init()
	s=time.time()
	for x in range(1000):
		print(a.TSL2561_all())
	print(time.time()-s)

	a.MPU6050_init()
	s=time.time()
	for x in range(1000):
		print(a.MPU6050_all()[0])
	print(time.time()-s)
	'''