#!/usr/bin/env python

import sys, time
import tos

import signal, os


AM_OSCILLOSCOPE = 0x93

if '-f' in sys.argv:
    dataout = open(sys.argv[sys.argv.index('-f')+1],'w')
else:
    dataout = sys.stdout;

_decode=False
if '-d' in sys.argv:
    _decode=True

_interval = None
if '-i' in sys.argv:
    _interval = int(sys.argv[sys.argv.index('-i')+1])
    print >>dataout, 'm_interval=', _interval

_oversample = None
if '-o' in sys.argv:
    _oversample = int(sys.argv[sys.argv.index('-o')+1])
    print >>dataout, 'm_oversample=', _oversample

_calibrated = '-c' in sys.argv

ck = {'Xa': 2./-487.25, 'Ya': 2./-512, 'Za':2./499.5,
      'Xd': 2./524.95,  'Yd': 2./-528, 'Zd':2./498.67}
cd = {'Xa': -509.38,    'Ya':-506,     'Za':-516.68,
      'Xd': -0.28,      'Yd':-3.4,     'Zd': -7.67}

class OscilloscopeMsg(tos.Packet):
    def __init__(self, packet = None):
        tos.Packet.__init__(self,
                            [('version',  'int', 2),
                             ('interval', 'int', 2),
                             ('id',       'int', 2),
                             ('count',    'int', 2),
                             ('oversample','int', 1),
                             ('channels', 'int', 1),
                             # readings
                             ('acca_X',  'int', 2),
                             ('acca_Y',  'int', 2),
                             ('acca_Z',  'int', 2),
                             ('accd_X', 'sint', 2),
                             ('accd_Y', 'sint', 2),
                             ('accd_Z', 'sint', 2),
                             ('temp',   'sint', 2),
                             ('readings', 'blob', None)],
                            packet)
if '-h' in sys.argv:
    print >>sys.stderr, "Usage:", sys.argv[0], "serial@/dev/ttyUSB0:57600 [-i interval] [-o oversample] [-d [-c]]"
    sys.exit()

am = tos.AM()

if dataout != sys.stdout:
    print >>dataout, "m_starttime=", time.time(), ";"
    print >>dataout, "m_=["
while True:
    try:
        p = am.read()
        if p and p.type == AM_OSCILLOSCOPE:
            msg = OscilloscopeMsg(p.data)
            if _decode and (msg.oversample > 0):
                temp = msg.temp/256.0/msg.oversample
                aax = float(msg.acca_X)/msg.oversample
                aay = float(msg.acca_Y)/msg.oversample
                aaz = float(msg.acca_Z)/msg.oversample
                adx = float(msg.accd_X)/msg.oversample
                ady = float(msg.accd_Y)/msg.oversample
                adz = float(msg.accd_Z)/msg.oversample
                if _calibrated:
                    aax = ck['Xa']*(aax+cd['Xa'])
                    aay = ck['Ya']*(aay+cd['Ya'])
                    aaz = ck['Za']*(aaz+cd['Za'])
                    adx = ck['Xd']*(adx+cd['Xd'])
                    ady = ck['Yd']*(ady+cd['Yd'])
                    adz = ck['Zd']*(adz+cd['Zd'])
                    print >>dataout, "%10f" % time.time(), msg.id, msg.count, aax, aay, aaz, adx, ady, adz, temp
                else:
                    readings = [i<<8 | j for (i,j) in zip(msg.readings[::2], msg.readings[1::2])]
                    print >>dataout, "%10f" % time.time(), msg.id, msg.count, aax, aay, aaz, adx, ady, adz, temp
            else:
                readings = [i<<8 | j for (i,j) in zip(msg.readings[::2], msg.readings[1::2])]
                print >>dataout, "%10f" % time.time(), msg.id, msg.count,msg.acca_X, msg.acca_Y, msg.acca_Z,msg.accd_X, msg.accd_Y, msg.accd_Z, msg.temp, readings[0], readings[1], "% os=", msg.oversample

            if dataout != sys.stdout:
                sys.stdout.write('.')
                sys.stdout.flush()

            dataout.flush()
            _send = False

            if _interval and msg.interval != _interval:
                msg.interval = _interval
                _interval = None
                _send = True

            if _oversample and msg.oversample != _oversample:
                msg.oversample = _oversample
                _oversample = None
                _send = True

            if _send:
                if (msg.interval % msg.oversample != 0):
                    print >>sys.stderr, "Warning: interval (", msg.interval, ") not evenly divisible by oversample (", msg.oversample, ")"
                msg.version += 1
                print >>sys.stderr, "Sleeping..."
                time.sleep(2.0)
                print >>sys.stderr, "Sending updated parameters"
                am.write(msg, p.type)
            
        else:
            print >>dataout, '%?', p
    except KeyboardInterrupt:
        break;

# exit code
if dataout != sys.stdout:
    print >>dataout, "];"
    dataout.close()