import logging
import math as m
import time
from hdf5.hdf5_fdi import HDF5_fdi
from .model_interface import Model
# this pulls in all the variables declared in model_config into the namespace of this module
from .model_config import * 

logger = logging.getLogger(__name__)

class Model_v1(Model):
    ''' 
    The work here is done inside two functions: 
    * Detection is performed in detection_algorithm() which is installed as an 'after' callback on two triggers 
    (getting out of the transient start period, and the reflexive transition based on the main loop timer.) This 
    was implemented this way because there is another trigger (getting out of isolation) that should not trigger it, 
    and this selectivity is not possible with a state-attached callback.

    * Isolation is performed in isolation_algorithm(), which is installed as a state-attached 'on_enter' callback.

    '''
    
    def __init__(self, ahrs, daq, log_folder):
        super().__init__(ahrs, daq, log_folder)

        self.x_motor_previous = None          # state variables: holds the previous motor position (from the observer)
        self.time_since_start_previous = None # holds the previous iteration's time since start
        self.fault_signature = None
        self.hdf = HDF5_fdi(self.log_folder)
        self.hdf.new_log()

        
    def reset(self):
        super().reset()
        self.hdf.stop()
        self.hdf.new_log()
        
        
#### this is where the real work happens:            
    def detection_algorithm(self):
        ''' Returns true if there has been a detection, false otherwise. '''

        print('running detection algorithm')

        # read & pre-process the time since start & data
        time_since_start, raw_data = self.daq.get_time_and_data()

        # it's possible for two invocations of this function to occur extremely closely together, due to async,
        # one from a timer ending and one from the main loop. Check if this has happened by making sure we're
        # not seeing the same data for a second time. If we are, just return.
        if self.time_since_start_previous:
            if self.time_since_start_previous == time_since_start:
                logger.debug(f'Got repeated timestamp {time_since_start} from DAQ')
                return
        
        sensor = raw_data.mean(axis=1)  # compute the mean over the last block

        self.hdf.append_raw_motor_data(time_since_start, sensor) # log the raw sensor data
        voltage = -2.15    * (sensor[0] -0.0)      # rescale sensor reading to V.  NB: those factors aren't expected to change after the initial calibration
        current = -0.1075  * (sensor[1] - 0.015)    # ... to A
        str_pot = -6.8   * (sensor[2] +3.7)        # ... to mm
        encoder = -0.01309 * sensor[3]             # ... to radians ( = 2*pi radians per 4*32 tks)       

        self.hdf.append_motor_data(time_since_start, [voltage, current, str_pot, encoder, self.x_motor_previous or float('nan')]) # log the sensor data in physical units

        pitch = self.ahrs.get_data()[1]           # vehicle pitch, in rad
        self.hdf.append_pitch(time.time()-self.start_time, pitch)              # log the pitch
        
        # estimate the state
        x_motor = encoder                  # motor position, rad
        x_battery = str_pot                # battery position, mm
        i = current                        # motor current, A
        omega = (voltage - Rm*current)/Km  # motor speed, rad/s

        r0 = alpha*x_motor - x_battery     # encoder - string pot, rad
        if self.x_motor_previous:
            dT = time_since_start - self.time_since_start_previous
            omega_mech = (x_motor - self.x_motor_previous)/dT
            r1 = alpha*(omega - omega_mech)               # electrical - mechanical speed, mm/s
            if omega_mech >= 0:
                r3 = alpha*(omega_mech - omega_nominal)   # mechanical speed - nominal, mm/s
            else:
                r3 = alpha*(omega_mech + omega_nominal)
        else:
            r1 = 0
            r3 = 0

        if omega >= 0.0:
            r2 = i - max(i_cutoff, i_0 + Kpitch*pitch)   # current - nominal, A
        else:
            r2 = i - min(-i_cutoff, -i_0 + Kpitch*pitch)
            
        self.hdf.append_detection_residuals(time_since_start, [r0, r1, r2, r3]) # log the residuals
        
        # test for faults
        fault0 = m.fabs(r0) > threshold0
        fault1 = m.fabs(r1) > threshold1
        fault2 = m.fabs(r2) > threshold2
        fault3 = m.fabs(r3) > threshold3
        fault_signature = [fault0, fault1, fault2, fault3]
        print(r0, r1, r2, r3)
        
        self.x_motor_previous = x_motor
        self.time_since_start_previous = time_since_start
        self.fault_signature = fault_signature

        # isolation can be triggered as a task, to be scheduled in
        # parallel, or as as a state transition to go into and out of.
        # uncomment this line for the second scenario, i.e. to
        # transition to the isolation state.
        if any(fault_signature):
#            self.fault_detected()
            print('Fault detected *************************')
            print(fault_signature)

    
    def isolation_algorithm(self):
        ''' Emits a message and returns True if a fault is isolated, returns
        False otherwise. '''
        # use self.fault_detected
        pass


    def on_exit_detection(self):
        self.hdf.flush()
        self.x_motor_previous = None
        self.time_since_start_previous = None
