#
# $Id$
#

import glob
import pickle
import pylab
import math

def bmp085_calc(UT=0.0, UP=0.0, BMPCAL=[], oss=1, **kwargs):
	""" algorithm from datasheet BMP085 Rev1.2 """
	AC1=BMPCAL[0]
	AC2=BMPCAL[1]
	AC3=BMPCAL[2]
	AC4=BMPCAL[3]
	AC5=BMPCAL[4]
	AC6=BMPCAL[5]
	B1=BMPCAL[6]
	B2=BMPCAL[7]
	MB=BMPCAL[8]
	MC=BMPCAL[9]
	MD=BMPCAL[10]

	# calculate true temperature
	X1 = (UT - AC6) * AC5 / 32768
	X2 = MC * 2048 / (X1 + MD)
	B5 = X1 + X2
	T = (B5 + 8) / 16 / 10
	
	# calculate true pressure
	B6 = B5 - 4000
	X1 = (B2 * (B6 * B6 / 4096)) / 2048
	X2 = AC2 * B6 / 2048
	X3 = X1 + X2
	factor = pow(2, oss)
	B3 = ((AC1 * 4 + X3) * factor + 2) / 4
	X1 = AC3 * B6 / 8192					# left shift once mult by 2
	X2 = (B1 * (B6 * B6 / 4096)) / 65536
	X3 = ((X1 + X2) + 2) / 4
	B4 = AC4 * (X3 + 32768) / 32768
	B7 = (UP - B3) * (50000 / factor)
	if B7 < 0x80000000:
		p = 2 * B7 / B4
	else:
		p = B7 / B4 * 2
	X1 = p * p / 65536
	X1 = X1 * 3038 / 65536
	X2 = (-7357 * p) / 65536
	p = p + (X1 + X2 + 3791) / 16				# pressure in Pa

	return (T, p)

def betrag(x,y,z):
	return math.pow(x**2 + y**2 + z**2, 0.5)
	
def absg(x, y, z):
	return betrag(*map(lambda x:x/16*0.012, [x,y,z]))

def absheight(p, p0=101325.0):
	if p>0:
		return 44330*(1-pow(p/p0,1/5.255))
	else:
		return 0
	
def procline(ln):
	d={}

	if ln.find('=~') == -1:	# filter Putty log line
		if ln.find('=') > 0: # filter host prints

			if ln[0] == "#":
				d['type']='slow'
			else:
				d['type']='fast'

			l=ln.lstrip('#').strip().split(' ')

			if len(l) > 2:
				for tpl in l:
					k,v = tpl.split('=')
					if k in ['AX', 'AY', 'AZ', 'CX', 'CY', 'CZ', 'T', 'UP', 'UT', 'ED']:
						i = int(v,16)
						if k in ['AX', 'AY', 'AZ', 'CX', 'CY', 'CZ']:
							if i > 32767: i=i-65536
						d.update({k:float(i)})
					elif k in ['BMPCAL']:
						l=[]
						for i in range(len(v)/4):
							x = int(v[i*4:i*4+4],16)
							if i not in [3,4,5] and x > 32767: x=x-65536
							l.append(x)
						d.update({k:l})
					else:
						d.update({k:v})
			return d
	return None
	
def procfile(data):
	a=[]
	for idx, ln in enumerate(data):
		d=procline(ln)
		if d is not None:
			d['idx']=idx
			a.append(d.copy())
	return a

def unwrap(a):
	tstamp = 0
	old = 0	
	for i in a:
		if i['type'] == 'fast':
			if i['T'] > old:
				tstamp += i['T'] - old
			else:
				tstamp += i['T'] - old + 65536
			old = i['T']
			i.update({'t [sec]':tstamp/1e3})
	return a

def values(a, key):
	""" Get all values of a given key for a list of dictionaries """
	l=[]
	for i in a:
		if i[key] not in l: l.append(i[key])
	return l

def procset(dir='.', set=''):
	fns = [i for i in glob.glob(dir+set+'*')]
	a=[]
	print '|', dir
	for fn in fns:
		d = [i for i in open(fn)]
		p = procfile(d)
		print '+', fn, 'Lines:', len(d)
		print '++ slow frames', len([i for i in p if i['type']=='slow'])
		print '++ fast frames', len([i for i in p if i['type']=='fast'])
		print '++ unwrapping'
		[i.update({'filename':fn, 'set':set}) for i in p]
		a += unwrap(p)
	return a

def plot(a):
	pylab.figure()
	for row, fn in enumerate(values(a, 'filename')):
		pylab.subplot(310+row)
		xs, ys = zip(*[ (i['t [sec]'], absg(i['AX'], i['AY'], i['AZ'])) for i in a if i['type']=='fast' and i['filename']==fn])
		pylab.plot(xs, ys, '.')
		pylab.xlim(0, 300)
		pylab.ylim(0, 20)
		pylab.ylabel('ACC [g]')
		pylab.title(fn)
	pylab.subplots_adjust(hspace=1.0)
	pylab.suptitle(a[0]['set'])

	pylab.figure()
	for row, fn in enumerate(values(a, 'filename')):
		pylab.subplot(310+row)
		xs, ys = zip(*[ (i['t [sec]'], [i['CX'], i['CY'], i['CZ']]) for i in a if i['type']=='fast' and i['filename']==fn])
		pylab.plot(xs, ys, '.')
		pylab.xlim(0, 300)
		#pylab.ylim(0, 20)
		pylab.ylabel('C [gauss]')
		pylab.title(fn)
	pylab.subplots_adjust(hspace=1.0)
	pylab.suptitle(a[0]['set'])

	pylab.figure()
	for row, fn in enumerate(values(a, 'filename')):
		ut = [i['UT'] for i in a if i['type']=='slow' and i['filename']==fn][0]
		bmpcal = [i['BMPCAL'] for i in a if i['type']=='slow' and i['filename']==fn][0]
		pylab.subplot(310+row)
		#xs, ys = zip(*[ (i['T'], bmp085_calc(**i)[1]) for i in a if i['type']=='slow' and i['filename']==fn])
		xs, ys = zip(*[ (i['t [sec]'], bmp085_calc(UP=i['UP'], UT=ut, BMPCAL=bmpcal)[1]) for i in a if i['type']=='fast' and i['filename']==fn])
		pylab.plot(xs, ys, '.')
		pylab.xlim(0, 300)
		pylab.ylim(25000, 40000)
		pylab.title(fn)
	pylab.subplots_adjust(hspace=1.0)
	pylab.suptitle(a[0]['set'])

	pylab.figure()
	for row, fn in enumerate(values(a, 'filename')):
		pylab.subplot(310+row)
		xs, ys = zip(*[ (i['idx'], i['t [sec]']) for i in a if i['type']=='fast' and i['filename']==fn])
		pylab.plot(xs, ys, '.')
		pylab.xlabel('idx')
		pylab.ylabel('t [sec]')
		pylab.title(fn)
	pylab.subplots_adjust(hspace=1.0)
	pylab.suptitle(a[0]['set'])
	
	pylab.show()

	
if __name__ == "__main__":
	dir = './RJD2011/'

	#a = procset(dir=dir,set='20110604_hurrikan_andreas')
	#a = procset(dir=dir,set='20110604_infected')
	#a = procset(dir=dir,set='20110604_spacetaxi_jan')
	#a = procset(dir=dir,set='20110604_usaf_jan')
	a = procset(dir=dir,set='20110604_video_david')
	#a = procset(dir=dir,set='20110604_warthog_andreas')
	#a = procset(dir=dir,set='20110605_warthog_andreas2')

	plot(a)

# EOF
