''' Known issues: 
- at_target cannot be tested, requires timer to expire *before* motor driver is done with the move.
'''

# standard library imports
import asyncio, os, sys, time

# external package imports
import numpy as np

# absolute local imports
from lcm_wrapper.lcm_patterns import Subscriber
from ahrs.dummy import GX5_25_dummy as GX5_25
from compact_daq.dummy import CompactDAQ_dummy as CompactDAQ
from hdf5.hdf5 import HDF5
from config.config import get_config

from ..state_machine import setup_state_machine
from .model_simulation import Model

# relative local imports
from ..fdi_channels import app_control_t, context_t


trigger = None
run_flag = True
loop_period = 0.5    # FDI periodicity, in seconds
start_delay = 5
stop_delay = 5

def msg_handler(channel_name, data): # lcm message handler
    global trigger, run_flag # TODO: is there a better way?
    if channel_name == "CONTEXT":
        msg = context_t.decode(data)
        trigger = msg.state 
    elif channel_name == "APP_CONTROL":
        msg = app_control_t.decode(data)
        run_flag = msg.run_flag
    else:
        print("unknown msg")


# --  INITIALIZATION -------------------------------
cfg = get_config(validate_present=['ahrs_com', 'data_folder'])
if cfg:
    ahrs_com = cfg['ahrs_com']
    data_folder = cfg['data_folder']
else:
    print('Problem loading configuration. Exiting.')
    exit()

# LCM channels
channels = ["CONTEXT", "APP_CONTROL"]

async def timer(delay, end_func):
    print(f"timer on {time.strftime('%X')}")
    await asyncio.sleep(delay)
    print(f"timer off {time.strftime('%X')}")
    end_func()
    

async def check_context(lcm, model):
    global trigger, run_flag
    while run_flag:
        await asyncio.sleep(loop_period)
        new_msg = lcm.get()
        print(f"context {trigger} {time.strftime('%X')}")
        if trigger:
            trigger_method = getattr(model, trigger)
            trigger_method()
        if new_msg and trigger == 'moving': #TODO: triggered in model?
            timer_task = asyncio.create_task( timer(start_delay, model.timer_expiration) )
        elif new_msg and trigger == 'at_target':
            timer_task = asyncio.create_task( timer(stop_delay, model.timer_expiration) )

            
async def main():
    # start the DAQ, AHRS, processed data logging, LCM
    with CompactDAQ(data_folder) as daq, \
         GX5_25(COM=ahrs_com) as ahrs, \
         HDF5(data_folder) as hdf, \
         Subscriber(channels, msg_handler) as lcm:


        # state machine initialization
        model = Model(ahrs, daq, hdf)
        SM = setup_state_machine(model)

        context_task = asyncio.create_task(check_context(lcm, model))
        await context_task


asyncio.run(main())