import time

from re       import compile
from serial   import Serial
from datetime import datetime
from struct   import *

class State:                                    #information stored
	acc  = [None]*3
	mag  = [None]*3
	lgt  = [None]*1
	tch  = [None]*2
	a2d  = [None]*6
	time = None

	def __init__(self):
		self.time = datetime.utcnow()

class Frame:
	# Sensor types
	ACC_SNS    = 0x00
	MAG_SNS    = 0x01
	LGT_SNS    = 0x02
	TCH_SNS    = 0x03
	A2D_SNS    = 0x04

	SNS_NUM    = 0x05
	SNS_SHIFT  = 4
	SNS_MASK   = 0xF0

	# Data types
	INT        = 0
	LONG       = 1
	FLOAT      = 2
	DOUBLE     = 3

	TYP_NUM    = 4
	TYP_SHIFT  = 0
	TYP_MASK   = 0x0F

	# Command codes
	STOP       = 0
	START      = 1
	RATE       = 2

	OPER_NUM   = 3
	OPER_SHIFT = 0
	OPER_MASK  = 0x0F

	# Frame information
	HEADER     = 0x02
	TAIL       = 0x0A

	HEADER_POS = 0
	BITS_POS   = 1
	COUNT_POS  = 2
	DATA_POS   = 3

	# Maps
	snsMap   = {ACC_SNS: 'acc',
	            MAG_SNS: 'mag',
	            LGT_SNS: 'lgt',
	            TCH_SNS: 'tch',
	            A2D_SNS: 'a2d'}

	cmdMap   = {START,   'start',
	            STOP,    'stop',
	            RATE,    'rate'}

	sizeMap  = {INT:      2,
	            LONG:     4,
	            FLOAT:    4,
	            DOUBLE:   8}

	fmtMap   = {INT:     'h',
	            LONG:    'i',
	            FLOAT:   'f',
	            DOUBLE:  'd'}

	# Parser data
	index    = 0   # read index
	count    = 0   # number of items in frame
	length   = 0   # length of frame (in bytes)
	bits_sns = 0   # sensor type
	bits_typ = 0   # data type
	binary   = ""  # binary read-in
	values   = []  # converted numeric data

	# Constructor
	def __init__(self):
		pass

	# Converters
	@staticmethod
	def findCode(dataMap, name):
		for code in dataMap:
			if dataMap[code] == name:
				return code
		print("[ERROR] No code found")

	# Parse frame
	def parse(self, byte):
		# save current pos and increment read index
		# if we have an error we cna reset index below
		pos = self.index
		self.index += 1

		if pos == Frame.HEADER_POS:
			if ord(byte) != Frame.HEADER:
				self.index = 0
			#print('parse: header  %02x' % ord(byte))

		elif pos == Frame.BITS_POS:
			self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT
			self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT
			if self.bits_sns >= Frame.SNS_NUM:
				self.index = 0
			if self.bits_typ >= Frame.TYP_NUM:
				self.index = 0
			#print('parse: bits    sns=%d typ=%d' %
			#	(self.bits_sns, self.bits_typ))

		elif pos == Frame.COUNT_POS:
			wordsize    = Frame.sizeMap[self.bits_typ]
			self.count  = ord(byte)
			self.length = Frame.DATA_POS + self.count*wordsize + 1
			#print('parse: count   cnt=%d len=%d' %
			#	(self.count, self.length))

		elif pos < self.length-1:
			self.binary += byte
			#print('parse: data    %02x @%d' %
			#	(ord(byte), pos-Frame.DATA_POS))

		elif pos == self.length-1:
			#print('parse: tail    %02x' % ord(byte))
			if ord(byte) == Frame.TAIL:
				state = self.convert()
			else:
				state = None
			self.binary = ""
			self.index  = 0
			return state

		elif pos > self.length-1:
			print('Error parsing')

	# Convert frame to state
	def convert(self):
		# Covnert data
		fmt = Frame.fmtMap[self.bits_typ] * self.count
		sns = Frame.snsMap[self.bits_sns]
		self.values = unpack('<'+fmt, self.binary)
		print('convert: %3s = \'%3s\'%%[%s] -> [%s]' %
			(sns, fmt, hexDump(self.binary), fltDump(self.values)))

		# Create state
		state = State()
		setattr(state, sns, self.values)
		return state

class Device:
	# Constructors
	def __init__(self, config):
		print('Defice.__init__')
		self.config = config
		self.serial = None
		self.frame  = Frame()

	# Methods
	def connect(self):
		print('Device.connect')
		buildingFrame = 0
		try:
			self.inbuf  = []
			self.serial = Serial(self.config.device, \
				baudrate = self.config.baudrate, \
				parity   = self.config.parity,   \
				bytesize = self.config.databits, \
				stopbits = self.config.stopbits, \
				timeout  = 0)
			for sns in self.config.rate:
				self.set_rate(sns, self.config.rate[sns])
			for sns in self.config.enable:
				self.set_enable(sns, self.config.enable[sns])
			self.serial.flushInput()
		except Exception as ex:
			return str(ex)

	def disconnect(self):
		print('Device.disconnect')
		if self.serial and self.serial.isOpen():
			self.serial.close()

	def running(self):                                      # isRunning
		if self.serial == None:
			return False
		if self.serial.isOpen() == False:
			return False
		return True

	def set_rate(self, sensor, interval):
		sns   = Frame.findCode(Frame.snsMap, sensor)
		bits  = (sns        << Frame.SNS_SHIFT) | \
		        (Frame.RATE << Frame.OPER_SHIFT)
		self._write_binary('Bf', bits, interval)

	def set_enable(self, sensor, enabled):
		sns   = Frame.findCode(Frame.snsMap, sensor)
		oper  = Frame.START if enabled else Frame.STOP
		bits  = (sns  << Frame.SNS_SHIFT) | \
		        (oper << Frame.OPER_SHIFT)
		self._write_binary('B', bits)

	def process(self):
		items = []
		count = 0
		limit = 1000

		if not self.running():
			return items

		while self.serial.readable():
			try:
				byte  = self.serial.read()
			except Exception as ex:
				# Not sure why this is excepting
				# if it says it's readable
				break
			if len(byte) == 0:
				break

			state = self.frame.parse(byte)
			if state:
				items.append(state)
			if count > limit:
				print('[ERROR] Exceeded Read Limit')
				break

			count += 1

		return items


	# auxilary function
	def frame_len(self, frame):
		if len(frame) < 3:
			return -1
		dataType_snsType = ord(frame[1])
		dataNum  = ord(frame[2])
		dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT
		snsType  = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT
		dataSize = Frame.sizeMap[dataType]
		return (dataSize*dataNum+4)

	def printHex(self, frame):
		frameLen = self.frame_len(frame)
		i = 0
		while i<frameLen:
			print(hex(ord(frame[i])))
			i+=1

	# Private methods
	def _write_binary(self, fmt, *args):
		#print('Device._write_binary')
		if self.serial:
			fmt   = 'B' + fmt + 'B'
			args  = [Frame.HEADER] + list(args) + [Frame.TAIL]
			frame = pack('<'+fmt, *args)
			print('write: bin:[' + hexDump(frame) + ']')
			self.serial.write(frame)
			self.serial.flush()
			time.sleep(0.1)

def hexDump(frame):
	digits = ['%02x' % ord(byte) for byte in frame]
	return ' '.join(digits)

def fltDump(data):
	digits = ['%5.2f' % flt for flt in data]
	return ' '.join(digits)