import time, traceback, sys

from .lcm_patterns import Subscriber
import lcmMessages

def translate_state(v):
    if v == 1.0:
        return 'homing'
    elif v == 2.0:
        return 'idling'
    elif v == 3.0:
        return 'moving'


def decode_dv(msg, f, c):
    fdi_state = None
    delta = None
    run = None
    vars = msg.floatVector
    for v in vars:
        if v.name == 'fdi_state':
            fdi_state = translate_state(v.val[0])
        elif v.name == 'platform_mass_position_delta':
            delta = v.val[0]
        elif v.name == 'fdi_control_run':
            run = v.val[0]
    f.write(f'{c} {msg.seqNo} {fdi_state} {delta} {run}\n')


def dump_dv(msg, f, c):
    f.write(f'{c} {msg.seqNo} {time.time()} ')
    if msg.nIntVectors:
#        f.write('ints ')
        dump_v(msg.intVector, f)
    if msg.nFloatVectors:
#        f.write('floats ')
        dump_v(msg.floatVector, f)
    if msg.nDoubleVectors:
        pass
    if msg.nStringVectors:
        pass
    f.write('\n')

def dump_v(v, f):
    for e in v:
        f.write(f'{e.name} = {e.val} ')
        
def msg_handler(channel_name, data):   # message callback
#    f = open('foo.txt', 'a')
    f = sys.stdout
    #print(channel_name)
    if channel_name == "MassServo":
        try:
            msg = lcmMessages.DataVectors.decode(data)
            dump_dv(msg, f, channel_name)
        except Exception:
            print("failed to decode ", data)
            traceback.print_exc()
    elif channel_name == "FDI" or channel_name == "FDI_CONTROL":
        try:
            msg = lcmMessages.DataVectors.decode(data)
            decode_dv(msg, f, channel_name)
#            dump_dv(msg, f, channel_name)
        except Exception:
            print("failed again ", data)
            traceback.print_exc()
#    f.close()

channels = [".*"]
#channels = ["FDI"]
with Subscriber(channels, msg_handler) as lcm:

    while True: 
        rc = lcm.get() # non-blocking read.  False = no messages
        time.sleep(0.1)    # 
