'''
Usage: 
python -m applications.fault_detection.fdi [control_override]
'''

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

# 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
import lcmMessages

logger = logging.getLogger(__name__)

# Parameters
loop_period = 0.1    # FDI periodicity, in seconds
start_delay = 0.5 # start transient duration, in seconds
stop_delay = 0.5  # stop transient duration, in seconds
distance_threshold = 0.001 # meters from target at which to fire stop timer

# engine run management
run_flag = False # lcm app control msgs populate this
control_override = False # commandline override of the control channel

# State management
trigger = None  # all lcm context msgs populate this
distance = None # lcm 'distance' context msgs populate this
trigger_changed = False

# 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

        
def reset_state(model):
    global trigger, distance, trigger_changed
    logger.info('Resetting FDI engine state.')
    trigger = None
    distance = None
    trigger_changed = None
    clear_timers()
    model.reset()
    

def translate_int_to_trigger(i):
    FDI_STATE_HOMING = 1
    FDI_STATE_IDLING = 2
    FDI_STATE_MOVING = 3
    if i == FDI_STATE_HOMING:
        return 'homing'
    elif i == FDI_STATE_IDLING:
        return 'idling'
    elif i == FDI_STATE_MOVING:
        return 'moving'

    
def extract_control(msg):
    if msg.nFloatVectors:
        for v in msg.floatVector:
            if v.name == 'fdi_control_run':
                ret = v.val[0]
                if ret > 0:
                    print('Received control 1')
                    return True
                else:
                    print('Received control 0')
                    return False

                
def extract_msg(msg):
    trigger = None
    distance = None
    if msg.nFloatVectors:
        for v in msg.floatVector:
            if v.name == 'fdi_state':
                trigger = translate_int_to_trigger(v.val[0])
            elif v.name == 'platform_mass_position_delta':
                distance = v.val[0]
        # discard the distance unless we're moving
        if trigger != 'moving':
            distance = None
    return (trigger, distance)

                
def msg_handler(channel_name, data): # lcm message handler
    global trigger, trigger_changed, distance, run_flag
    if channel_name == "FDI":
        msg = lcmMessages.DataVectors.decode(data)
        (new_trigger, distance) = extract_msg(msg)

        if new_trigger == trigger:
            trigger_changed = False
        else:
            trigger = new_trigger
            trigger_changed = True
    elif channel_name == "FDI_CONTROL":
        msg = lcmMessages.DataVectors.decode(data)
        run_flag = extract_control(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 = ["FDI", "FDI_CONTROL"]


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, trigger_changed, distance, run_flag, \
        start_timer, stop_timer, control_override

    while run_flag or control_override:
        await asyncio.sleep(loop_period)
        new_msg = lcm.get()
        if not new_msg: continue
        # triggers are arriving at 4 Hz, so we care about changes
        
        # if distance below threshold, and the stop timer isn't 
        # already running, issue an 'at_target' trigger.
        if trigger and trigger == 'moving' and \
               distance < distance_threshold and not stop_timer:
            trigger = 'at_target'
            trigger_changed = True

        if trigger:
            print(f"{trigger} {distance} {time.strftime('%X')}")

            trigger_method = getattr(model, trigger)
            trigger_method()

        if trigger and trigger_changed:
            # 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':
                # reset the trigger back to moving, so that comparisons work
                trigger = 'moving'
                stop_timer = asyncio.create_task( timer(stop_delay, model.timer_expiration, description='stop') )

    logger.info('Stopping FDI.')
    reset_state(model)
            

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

        logger.info('Initializing state machine.')
        model = Model(ahrs, daq, data_folder)
        SM = setup_state_machine(model)

        while True:
            if run_flag or control_override:
                # state machine initialization
                logger.info('Starting FDI.')
                context_task = asyncio.create_task(check_context(lcm, model))
                await context_task
            else:
                time.sleep(loop_period)
                new_msg = lcm.get()
    logger.info('Exiting FDI main.')

    
def signal_handler(signum, frame):
    logger.debug(f'Signal raised: {signal.Signals(signum).name}.')
    signal.default_int_handler(signum, frame)

signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGABRT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
signal.signal(signal.SIGSEGV, signal_handler)
signal.signal(signal.SIGILL, signal_handler)
signal.signal(signal.SIGFPE, signal_handler)

logging.basicConfig(
    level=logging.DEBUG, # logging.INFO
    format="[%(asctime)s] %(levelname)s [%(name)s] %(message)s",
    datefmt="%H:%M:%S",
    filename=os.path.join(data_folder, 'fdi.log'))

if len(sys.argv) > 1 and sys.argv[1] == 'control_override':
    print('Running with control override')
    logger.info('Running with control override')
    
    control_override = True
    
asyncio.run(main())
