#
# $Id$
#

import glob
import pylab
import math

def bmp085_calc(UT, UP, BMPCAL, oss=1):
	""" 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)
	p = 2 * B7 / B4
	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 scale_acc(raw):
	return raw/16*0.012
def absg(x, y, z):
	return betrag(*map(scale_acc, [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):
	""" Process one line, parse k=v tuples into dictionary
	"""
	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', 
						'CPX', 'CPY', 'CPZ', 'CNX', 'CNY', 'CNZ', 
						'T', 'UP', 'UT', 'ED']:
						i = int(v,16)
						if k in ['AX', 'AY', 'AZ', 'CX', 'CY', 'CZ', 'CPX', 'CPY', 'CPZ', 'CNX', 'CNY', 'CNZ']:
							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):
	""" Process a single file
		 - read all lines
		 - process one line (parse into dict)
	"""
	a=[]
	state=''
	statecnt=0
	for idx, ln in enumerate(data):
		d=procline(ln)
		if d is not None:
			if state != d['type']:
				if statecnt == 2:
					state=d['type']
					statecnt=0
					d=None # skip the first fast / slow frame
				else:
					statecnt += 1
			else:
				d['idx']=idx
				a.append(d.copy())
	return a

def unwrap(a):
	""" Extract the absolute time from indices
		Frames can be lost during flight and index of fast frames
		wraps during one fast frame phase, try to unwrap and map
		to absolute time here
	"""
	tstamp = 0
	old = 0	
	for i in a:
		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(files):
	""" Process a set of files containing the same flight distributed over
		multiple files. This is the case if more than one receiver sniffed
		air traffic as well as local flash data was read and should be 
		considered for processing
	"""
	data=[]
	for fn in files:
		d = [i for i in open(fn)] # read entire file into array
		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}) for i in p]
		data += unwrap(p)
	return data

def cal_hmc5883l(axis, raw, calpos, calneg, gain=1):
	# LSB/Gauss
	HMC5883L_GAINCAL = 1 # fixed
	gaintable = [
			1370,
			1090,
			820,
			660,
			440,
			390,
			330,
			230 ]
	bias_axes = {'CX':1.16, 'CY':1.16, 'CZ':1.08}
	return( bias_axes[axis] * gaintable[HMC5883L_GAINCAL]/gaintable[gain] * raw/calpos )

def scale_hmc5883l(raw):
	return raw/1090.0 # fixed gain setting

def plot_rjd2011(a):
	plotargs = {'alpha':0.3}

	fns = values(a, 'filename')
	fig, axs = pylab.subplots(nrows=len(fns), ncols=1, sharex=True)
	if type(axs) != 'list': axs = [axs]
	for ax, fn in zip(axs, fns):
		xs, ys = zip(*[ (i['t [sec]'], (i['AX'], i['AY'], i['AZ'])) for i in a if i['type']=='fast' and i['filename']==fn])
		ax.plot(xs, ys, '.', **plotargs)
		ax.set_ylabel('ACC [g]')
		ax.set_xlabel('t [sec]')
		ax.set_title(fn)
	fig.suptitle(a[0]['set'])

	fns = values(a, 'filename')
	fig, axs = pylab.subplots(nrows=len(fns), ncols=1, sharex=True)
	if type(axs) != 'list': axs = [axs]
	for ax, fn in zip(axs, fns):
		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]
		#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])
		ax.plot(xs, ys, '.', **plotargs)
		ax.set_ylabel('P [Pa]')
		ax.set_xlabel('t [sec]')
		ax.set_title(fn)
	fig.suptitle(a[0]['set'])

def scale_physical(data):
	""" Scale values to physical dimensions """

	# Acceleration Sensor
	# dimension: g (Force, 1g = earth gravity)
	[i.update({'ax[g]':scale_acc(i['AX'])}) for i in data]
	[i.update({'ay[g]':scale_acc(i['AY'])}) for i in data]
	[i.update({'az[g]':scale_acc(i['AZ'])}) for i in data]
	[i.update({'a[g]':betrag(*map(scale_acc,[i['AX'],i['AY'],i['AZ']]))}) for i in data]

    # Integrate acceleration to get speed
	data[0].update({'v[ms-1]':0})
	for i,d in enumerate(data[1:]):
		data[i+1].update({'v[ms-1]':((data[i+1]['a[g]']-1.0)*(data[i+1]['t[sec]']-data[i]['t[sec]'])+data[i]['v[ms-1]'])})

	# Magnetic Sensor
	# dimension: Gs (Gauss) (Old magnetic unit, replaced by Tesla: 1 Gs=1e-4 Tesla)
	# earth magnetic field @ Meridian 50 = 48 uT = 0.48 Gs
	[i.update({'cx[Gs]':scale_hmc5883l(i['CX'])}) for i in data]
	[i.update({'cy[Gs]':scale_hmc5883l(i['CY'])}) for i in data]
	[i.update({'cz[Gs]':scale_hmc5883l(i['CZ'])}) for i in data]
	[i.update({'c[Gs]':betrag(*map(scale_hmc5883l,[i['CX'],i['CY'],i['CZ']]))}) for i in data]
	
	# Pressure Sensor
	bmpcal = [(i['BMPCAL']) for i in data if i['type']=='slow'][0]
	ut = [(i['UT']) for i in data if i['type']=='slow'][0]

	return data

	
def plotset(data):
	linestyle='.-'
	xkey = 't[sec]'
	ykeys = [
		['ax[g]','ay[g]','az[g]'], 
		#'v[ms-1]',
		#'a[g]',
		['cx[Gs]','cy[Gs]','cz[Gs]'], 
		#'c[Gs]',
		#'UP', 
		#'UT',
		#'ED',
		]
	
	plotargs = {'alpha':0.5}
	highlightargs = {'facecolor':'lightgreen', 'edgecolor':'green', 'alpha':0.2}
	
	fig, axs = pylab.subplots(nrows=len(ykeys), ncols=1, sharex=True)

	highlight_min = [i for i in data if i['type']=='fast'][0]['t[sec]']
	highlight_max = [i for i in data if i['type']=='fast'][-1]['t[sec]']

	[ax.grid(True, linestyle='-', color='lightgrey') for ax in axs]

	for ykey, ax in zip(ykeys, axs):
		if type(ykey) != list:
			ykey = [ykey]
		for yk in ykey:
			xs, ys = zip(*[ (i[xkey], i[yk]) for i in data])
			ax.plot(xs, ys, linestyle,label=str(yk), **plotargs)
		ax.set_xlabel(xkey)
		ax.set_ylabel(ykey)
		ax.legend(loc=(1.,0))
		ax.axvspan(xmin=highlight_min,xmax=highlight_max, **highlightargs)

	fig.suptitle(data[0]['filename'])
	
if __name__ == "__main__":
	pylab.rcParams['font.size']=10

	for fn in glob.glob('./RJD2012/*'):
		data = procset(files=[fn])
		data = scale_physical(data)
		plotset(data)

	pylab.show()

	#hmc()
	
# EOF
