# $Id$

# execfile("python\\dump_data_plot.py")

from scipy import integrate
import pylab

dateipfad="data\\hurrikan_andreas.txt"
#dateipfad="data\\infected.txt"
dateipfad="data\\warthog_andreas1.txt"
dateipfad="data\\interceptor_jan.txt"
dateipfad="data\\space_taxi_jan1.txt"
datei=open(dateipfad,"r")
time = [0]
addr = [0]						# sensor address
energy = [0]
ax = [0]					# accelerometer
ay = [0]
az = [0]
alt = [0]
cmpx = [0]						# compass
cmpy = [0]
cmpz = [0]
press = [41000]						# pressure sensor
calib = ['']						# pressure calibration
temperature = ['']
accx0 = 0
accy0 = 0
accz = 0
p0 = 101325.0					# pressure at sea level
h0 = 360						# altitude of Hartha
press_alt = []
index = []

AC1 = 408
AC2 = -72
AC3 = -14383
AC4 = 32741
AC5 = 32757
AC6 = 23153
B1 = 6190
B2 = 4
MB = -32768
MC = -8711
MD = 2868
UP = 23843
UT = 27898 * 1.05

def data(t, list):
    fs = 12e-3		 			# 3e-3@6g, 6e-3@12g, 12e-3@24g
    if list[0] == '#':
        offset = 1
        delta_t = 1.0
    else:
        offset = 0
        delta_t = 0.01
    if lst[offset][:2] == 'T=':
        idx = '0x' + lst[offset][2:6]
        idx = eval(idx)
        t = t + delta_t
    else:
        pass
    if lst[2 + offset][:3] == 'ED=':
        ed = '0x' + lst[2 + offset][3:5]
        ed = eval(ed)
    else:
        ed = 0
    if lst[3 + offset][:3] == 'AX=':
        ax = '0x' + lst[3 + offset][3:7]
        ax = eval(ax)
        if ax > 32767:				# 2's complement calculation for negative values
            ax = ax - 65536
        else:
            pass 
    else:
        ax = -40000
    ax = ax/16.0 * fs				# division by 16 due to 12 bit values
    if lst[4 + offset][:3] == 'AY=':
        ay = '0x' + lst[4 + offset][3:7]
        ay = eval(ay)
        if ay > 32767:
            ay = ay - 65536
        else:
            pass
    else:
        ay = -40000
    ay = ay/16.0 * fs
    if lst[5 + offset][:3] == 'AZ=':
        az = '0x' + lst[5 + offset][3:7]
        az = eval(az)
        if az > 32767:
            az = az - 65536
        else:
            pass
    else:
        az = -40000
    az = az/16.0 * fs

#    acc = sqrt( pow(ax, 2) + pow(ay, 2) + pow(az, 2) )	# calculate length of vector 1g during idle
    if lst[6 + offset][:3] == 'CX=':							# only raw data for hall sensor
        cx = '0x' + lst[6 + offset][3:7]
        cx = eval(cx)
        if cx > 32767:
            cx = cx - 65536
        else:
            pass
    else:
        cx = -40000
    if lst[7 + offset][:3] == 'CY=':
        cy = '0x' + lst[7 + offset][3:7]
        cy = eval(cy)
        if cy > 32767:
            cy = cy - 65536
        else:
            pass
    else:
        cy = -40000
    if lst[8 + offset][:3] == 'CZ=':
        cz = '0x' + lst[8 + offset][3:7]
        cz = eval(cz)
        if cz > 32767:
            cz = cz - 65536
        else:
            pass
    else:
        cz = -40000
    if lst[9 + offset][:3] == 'UP=':
        p = '0x' + lst[9 + offset][3:7]							# only raw data for pressure
        p = eval(p)										# have a look into datasheet
    else:
        p = -40000
    if offset:	
        if lst[10 + offset][:7] == 'BMPCAL=':					# 44x4bit-->176bit
            data = lst[10 + offset][7:51]					# only raw data for pressure										# have a look into datasheet
        else:
            data = ''
        cal = []
        for i in range(11):
            element = '0x' + data[(4 * i):(4 + 4 * i)]
            element = eval(element)
            if i in [3, 4, 5]:
                pass
            else:
                if element > 32767:
                    element = element - 65536
                else:
                    pass
            cal.append(element)
#        print cal
        if lst[12 + offset][:3] =='UT=':
            temp = '0x' + lst[12 + offset][3:7]
            temp = eval(temp)
        else:
            pass
        ax0 = ax
        ay0 = ay
        az0 = az
#        print ax, ay, az
#        print ax0, ay0, az0
#        raw_input()
    else:
        az0 = ''
        ay0 = ''
        ax0 = ''
        cal = ''
        temp = ''
    return (t, ed, ax, ax0, ay, ay0, az, az0, cx, cy, cz, p, cal, temp, idx)

t = 0
text=datei.readline()
while (text<>""):										# change it
#for i in range(10):
    text = text.strip('\n')
    lst = text.split(' ')
    if len(lst)<10:
        pass
    else:
        (t, ed, accx, accx0, accy, accy0, accz, accz0 , cx, cy, cz, p, cal, temp, idx) = data(t, lst)
#        print "%10.2f s\t%3.2f m/s^2" % (time[-1], acc)
        time.append(t)
        energy.append(ed)
        ax.append(accx)
        ay.append(accy)
        az.append(accz)
        cmpx.append(cx)
        cmpy.append(cy)
        cmpz.append(cz)
        press.append(p)
        calib.append(cal)
        temperature.append(temp)
        index.append(idx)
#        print t, acc
    text=datei.readline()

datei.close()

### Calculation of altitude based on pressure sensor ############################################

oss = 1
for i in range(len(press)):
    data = calib[i]
    cal_temp = temperature[i]
    factor = pow(2, (8 - oss))
    unsgnUP = press[i] * 256 / factor	# UP = (press << 8)	consideration of XLSB register
										# UP = UP >> (8-oss); oss=1
										# long = 32bit; 2^32=4294967296
#    unsgnUP = 23843
    if unsgnUP > 2147483647:			# (unsigned long) used to calculate B7
        UP = unsgnUP - 4294967296
    else:
        UP = unsgnUP
#    print UP, unsgnUP

    if data == '':
        pass			# use old values
    else:
        AC1=data[0]
        AC2=data[1]
        AC3=data[2]
        AC4=data[3]
        AC5=data[4]
        AC6=data[5]
        B1=data[6]
        B2=data[7]
        MB=data[8]
        MC=data[9]
        MD=data[10]

    if cal_temp == '':
        pass
    else:
        UT = cal_temp
        if UT > 32767:
            UT = UT - 65537
        else:
            pass		
#    UT = 27898 * 1.05						# read temperature!!!
    X1 = (UT - AC6) * AC5 / 32768			# found in temp section
    X2 = MC * 2048 / (X1 + MD)
    B5 = X1 + X2
    T = (B5 + 8) / 16 / 10
    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		# oss set to 1 -> datasheet
    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 = (unsgnUP - 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
#    print p
    p = p / p0									# normalized
    h = 44330 * (1 - pow(p , 0.1903))
    if data == '':
        pass
    else:
        h0 = h
    h = h - h0
    press_alt.append(h)

### Calculation based on acceleration sensor #####################################################	
#g_offset = average(acc_scalar)
#g_offset = acc_scalar[0]
g_offset = 10
#print "g_offset = %2.2f" % g_offset
ax1 = [ (1 * (d - 0)) for d in ax]		# acceleration [m/s^2] corrected by 1g
ay1 = [ (1 * (d - 0)) for d in ay]
az1 = [ (1 * (d - 0)) for d in az]
ax1[0] = 0
ay1[0] = 0
az1[0] = 0
a_scalar = [1 * sqrt( ax1[i] * ax1[i] + ay1[i] * ay1[i] + az1[i] * az1[i] ) - 0 for i in range(len(ax))]
																	# calculate length of vector
### velocity																	
vx = integrate.cumtrapz(ax1, time)
vy = integrate.cumtrapz(ay1, time)
vz = integrate.cumtrapz(az1, time)
v_scalar = [sqrt( vx[i] * vx[i] + vy[i] * vy[i] + vz[i] * vz[i] ) for i in range(len(vx))]

### altitude
sx = integrate.cumtrapz(vx, time[1:])
sy = integrate.cumtrapz(vy, time[1:])
sz = integrate.cumtrapz(vz, time[1:])
s_scalar = [sqrt( sx[i] * sx[i] + sy[i] * sy[i] + sz[i] * sz[i] ) for i in range(len(sx))]

pylab.figure(figsize=(12,9), facecolor='w', edgecolor='b')
pylab.subplots_adjust(left = 0.05, bottom = 0.04, right = 0.96, top = 0.96, wspace = 0.4, hspace = 0.4)


pylab.subplot(5, 1, 1)
pylab.plot(time[1:], a_scalar[1:], label = 'scalar')
pylab.plot(time[1:], ax1[1:], label = 'x')
pylab.plot(time[1:], ay1[1:], label = 'y')
pylab.plot(time[1:], az1[1:], label = 'z')
pylab.ylabel('a[g]')
pylab.xlabel('t[s]')
pylab.title('Acceleration')
pylab.legend(loc = 'upper right')

pylab.subplot(5, 1, 2)
pylab.plot(time[1:], v_scalar)
#pylab.plot(time[1:], vx)
#pylab.plot(time[1:], vy)
#pylab.plot(time[1:], vz)

pylab.subplot(5, 1, 3)
pylab.plot(time[2:], s_scalar)
pylab.plot(time[1:], press_alt[1:])
#pylab.plot(time[2:], sx)
#pylab.plot(time[2:], sy)
#pylab.plot(time[2:], sz)

pylab.subplot(5, 1, 4)
pylab.plot(time[1:], press_alt[1:])

pylab.subplot(5, 1, 5)
pylab.plot(time, energy)
pylab.plot(time[1:], index)

dateipfad="data\\back1.txt"
datei=open(dateipfad,"w")

i = 0
while (i < len(time)):
    if i < 1:
        v = 0
        h = 0		
    elif i < 2:
        h = 0
    else:
        v = 0
        h = 0
#    datei.write("%4.2f\t%2.2f\t%.2f\t%4.2f\n" % (time[i], acc_up[i], v, h))
    i = i + 1

datei.close()