''' 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.gx5_25 import GX5_25
from compact_daq.compact_daq import CompactDAQ
from config.config import get_config

# relative local imports
from .state_machine import setup_state_machine
from .model_v1 import Model_v1 as Model
from .fdi_channels import app_control_t, context_t

# State management
trigger = None  # all lcm context msgs populate this
distance = None # lcm 'distance' context msgs populate this
run_flag = True # lcm app control msgs populate this

# Parameters
loop_period = 0.25    # FDI periodicity, in seconds
start_delay = 1 # start transient duration, in seconds
stop_delay = 1  # stop transient duration, in seconds
distance_threshold = 1000 # ticks from target at which to fire stop timer

def msg_handler(channel_name, data): # lcm message handler
    global trigger, distance, run_flag
    if channel_name == "CONTEXT":
        msg = context_t.decode(data)
        words = msg.state.split()
        trigger = words[0]
        if len(words) > 1:
            distance = int(words[1])
        else: distance = None
    elif channel_name == "APP_CONTROL":
        msg = app_control_t.decode(data)
        run_flag = msg.run_flag
    else:
        print("unknown msg")

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

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

# timer management
start_timer = None
stop_timer = None

def clear_timers():
    global start_timer, stop_timer
    if start_timer: 
        start_timer.cancel()
        start_timer = None
    if stop_timer: 
        stop_timer.cancel()
        stop_timer = None


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

async def check_context(lcm, model):
    global trigger, distance, run_flag, start_timer, stop_timer
    while run_flag:
        await asyncio.sleep(loop_period)
        new_msg = lcm.get()
#        print(f"context {trigger} {time.strftime('%X')}")
        if trigger:
            if trigger == 'distance': 
                # handle it or sweep it under the rug, depending on distance.
                # if distance below threshold, and the stop timer isn't 
                # already running, issue 'at_target' to create stop timer.
                if distance < distance_threshold and not stop_timer:
                    trigger = 'at_target'
                else: 
                    trigger = 'moving'
                    new_msg = None

            trigger_method = getattr(model, trigger)
            trigger_method()
        if new_msg:
            # cancel any pending timers
            clear_timers()
            # start any timers that should be started
            if trigger == 'moving': #TODO: triggered in model?
                start_timer = asyncio.create_task( timer(start_delay, model.timer_expiration, description='start') )
            elif trigger == 'at_target':
                stop_timer = asyncio.create_task( timer(stop_delay, model.timer_expiration, description='stop') )

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

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

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


asyncio.run(main())