from faultmodel.fdiptypes import Model
#from ezservo.MassServo import MassServo
#from ezservo.ServoConfig import ServoConfig
from faultmodel.mediator import MassMediator
from daq.fp_cdaq import CDaq
from microstrain import gx5_25
import datetime
import time
import h5py
import numpy as np

import re


def ConstantAcquisitionAndModel():
    """
    Configured for use on the subsea DAQ implementation on LRAUV
    """


    def process_data(data: np.ndarray):
        voltage_motorcurrent = np.mean(data[0, :])
        voltage_motorvoltage = np.mean(data[1, :])
        voltage_stringpot    = np.mean(data[2, :])
        return (voltage_motorcurrent, voltage_motorvoltage, voltage_stringpot)

    def process_data_dig(data: np.ndarray):
        angle_motor = np.mean(data)
        return (angle_motor)
    
    def process_ahrs_data(data: np.ndarray):
        time_ns = data[0]
        roll    = data[1]
        pitch    = data[2]
        yaw    = data[3]
        #rolld    = data[4]
        #pitchd   = data[5]
        #yawd    = data[6]
        return (time_ns, roll, pitch, yaw)
    
    # Initialize the Model
    model = MassMediator(Model.V1)
    cmd_position = 0;

    # Initialize the AHRS
    ahrs = gx5_25.GX5_25()
    ahrs.open()
    

    # Initialize the Data Acquisition
    with CDaq() as cdaq:

        # Setup Acquisition
        cdaq.add_channel('cDAQ1Mod1/ai0', 'voltage', 'MotorCurrent')
        cdaq.add_channel('cDAQ1Mod1/ai1', 'voltage', 'MotorVoltage')
        cdaq.add_channel('cDAQ1Mod1/ai2', 'voltage', 'StringPotVoltage')
        cdaq.add_channel('cDAQ1Mod3/ctr0', 'ang_encoder','MotorPosition')
        
        n_samples = 160
        sample_rate = 1600.0
        cdaq.setup_task(samplerate=sample_rate, nsamples=n_samples)

        # Enter in conversion constants (from mkemp email 3/26/18)
        conv_motor_current = lambda x: x*1000.0
        conv_motor_voltage = lambda x: x*2.0
        conv_mass_position = lambda x: x*13.7


        # Create a data file
        time_str = datetime.datetime.now().strftime("%Y%m%d%H%M%S")
        hf = h5py.File("SingleExecution_{}.hdf5".format(time_str), 'w')

        # Analog Data Setup
        hf_group_analog = hf.create_group('analog')
        dset_rmv = hf_group_analog.create_dataset('raw_voltage_motor', (0, 1), maxshape=(None, 1))
        dset_rmc = hf_group_analog.create_dataset('raw_current_motor', (0, 1), maxshape=(None, 1))
        dset_rmp = hf_group_analog.create_dataset('raw_voltage_string_pot', (0, 1), maxshape=(None, 1))
        dset_mmv = hf_group_analog.create_dataset('mean_voltage_motor', (0, 1), maxshape=(None, 1))
        dset_mmc = hf_group_analog.create_dataset('mean_current_motor', (0, 1), maxshape=(None, 1))
        dset_mms = hf_group_analog.create_dataset('mean_voltage_string_pot', (0, 1), maxshape=(None, 1))
        dset_mmp = hf_group_analog.create_dataset('mean_position_string_pot', (0, 1), maxshape=(None, 1))

        dset_rma = hf_group_analog.create_dataset('raw_angle_motor', (0, 1), maxshape=(None, 1))
        dset_mma = hf_group_analog.create_dataset('mean_angle_motor', (0, 1), maxshape=(None, 1))
        
        hf_group_analog.create_dataset('sample_rate_hertz', data=sample_rate)
        hf_group_analog.create_dataset('samples_per_loop', data=n_samples)

        # EZServo data setup
        #hf_group_servo = hf.create_group('ezservo')

        # AHRS data setup
        hf_group_ahrs = hf.create_group('ahrs')
        dset_ahrs_time = hf_group_ahrs.create_dataset('time', (0, 1), maxshape=(None, 1))
        dset_ahrs_roll = hf_group_ahrs.create_dataset('roll_radians', (0, 1), maxshape=(None, 1))
        dset_ahrs_pitch = hf_group_ahrs.create_dataset('pitch_radians', (0, 1), maxshape=(None, 1))
        dset_ahrs_yaw = hf_group_ahrs.create_dataset('yaw_radians', (0, 1), maxshape=(None, 1))
        dset_ahrs_rolld = hf_group_ahrs.create_dataset('roll_degrees', (0, 1), maxshape=(None, 1))
        dset_ahrs_pitchd = hf_group_ahrs.create_dataset('pitch_degrees', (0, 1), maxshape=(None, 1))
        dset_ahrs_yawd = hf_group_ahrs.create_dataset('yaw_degrees', (0, 1), maxshape=(None, 1))
        
        
        #TODO need to add the LCM receiver to this datagroup. 
        #dset_ezpos = hf_group_servo.create_dataset('position', (0, 1), maxshape=(None, 1))
        #dset_ezvel = hf_group_servo.create_dataset('velocity', (0, 1), maxshape=(None, 1))

        # Model Data setup
        hf_group_model = hf.create_group('model')
        str_datatype = h5py.special_dtype(vlen=str)
        dset_model_state = hf_group_model.create_dataset('state', (0, 1), dtype=str_datatype, maxshape=(None, 1))
        dset_model_health = hf_group_model.create_dataset('health', (0, 1), dtype=str_datatype, maxshape=(None, 1))
        dset_model_fault = hf_group_model.create_dataset('fault_type', (0, 1), dtype=str_datatype, maxshape=(None, 1))
        dset_model_ttf = hf_group_model.create_dataset('time_to_failure', (0, 1), maxshape=(None, 1))

        # Contextual Data setup and population
        hf_group_context = hf.create_group('info')
        hf_group_context.create_dataset('version',data='0.1')
        hf_group_context.create_dataset('start_date',data=datetime.datetime.now().strftime("%Y/%m%d"))
        hf_group_context.create_dataset('start_time',data=time.time())

        # Set initial state 0 will rehome, 1 will start a movement
        state = 1
        start_time = time.process_time()

        cdaq.start_acquisition()
        n_acqs = 0

        ahrs.start()
        yaw = 0.0
        n_acqs_ahrs = 0
        

        while state < 4:
            # log the ahrs data
            while ahrs.data_available():
                data = ahrs.data_buff.get()
                if data is not None:
                    # Log AHRS Data
                    (time_ns, roll, pitch, yaw) = process_ahrs_data(data)
                    #print(f"\t\t\t\t\t\t\t\tR:{roll} P:{pitch} Y:{yaw}")
                    dset_ahrs_time.resize(dset_ahrs_time.shape[0] + 1, axis=0)
                    dset_ahrs_roll.resize(dset_ahrs_roll.shape[0] + 1, axis=0)
                    dset_ahrs_pitch.resize(dset_ahrs_pitch.shape[0] + 1, axis=0)
                    dset_ahrs_yaw.resize(dset_ahrs_yaw.shape[0] + 1, axis=0)

                    dset_ahrs_time[n_acqs_ahrs, 0] = time_ns
                    dset_ahrs_roll[n_acqs_ahrs, 0] = roll
                    dset_ahrs_pitch[n_acqs_ahrs, 0] = pitch
                    dset_ahrs_yaw[n_acqs_ahrs, 0] = yaw
                    
                    n_acqs_ahrs += 1

            #check for data from the daq
            if cdaq.databuff.empty() or cdaq.databuff_dig.empty():
                time.sleep(0.01)
                continue


            # log & Process DAQ Data
            data = cdaq.databuff.get()

            data_dig = cdaq.databuff_dig.get()

            

            # Increment counter
            loop_time = time.process_time()
            str_time = "Elapsed: {:0.2f} ".format(loop_time - start_time)

            # Generate Means
            (voltage_motorcurrent, voltage_motorvoltage, voltage_stringpot) = process_data(data)
            (angle_servo) = process_data_dig(data_dig)

            voltage_servo = conv_motor_voltage(voltage_motorvoltage)
            current_servo = conv_motor_current(voltage_motorcurrent)
            position_string = conv_mass_position(voltage_stringpot)
            #TODO conversions on means for angular position??

            str_daq = "Vservo: {:5.2f} Vstring: {:5.2f} Iservo: {:5.2f} Lservo: {:5.2f} Pstring: {:5.2f}".format(voltage_servo, voltage_stringpot, current_servo, angle_servo, position_string)
            # Log Motor Current
            dset_rmc.resize(dset_rmc.shape[0] + n_samples, axis=0)
            dset_rmc[-n_samples:, :] = np.reshape(data[0, :], (n_samples, 1))

            # Log Motor Voltage
            dset_rmv.resize(dset_rmv.shape[0] + n_samples, axis=0)
            dset_rmv[-n_samples:, :] = np.reshape(data[1, :], (n_samples, 1))

            # Log Motor Position Voltage
            dset_rmp.resize(dset_rmp.shape[0] + n_samples, axis=0)
            dset_rmp[-n_samples:, :] = np.reshape(data[2, :], (n_samples, 1))

            # Log Motor Angle
            dset_rma.resize(dset_rma.shape[0] + n_samples, axis=0)
            dset_rma[-n_samples:, :] = np.reshape(data_dig, (n_samples, 1))



            # Log Mean values
            dset_mmv.resize(dset_mmv.shape[0] + 1, axis=0)
            dset_mms.resize(dset_mms.shape[0] + 1, axis=0)
            dset_mmp.resize(dset_mmp.shape[0] + 1, axis=0)
            dset_mmc.resize(dset_mmc.shape[0] + 1, axis=0)
            dset_mma.resize(dset_mma.shape[0] + 1, axis=0)
            dset_mmv[n_acqs, 0] = voltage_servo
            dset_mms[n_acqs, 0] = voltage_stringpot
            dset_mmp[n_acqs, 0] = position_string
            dset_mmc[n_acqs, 0] = current_servo
            dset_mma[n_acqs, 0] = angle_servo

            # Get Position & Velocity from EZServo
            #(timestamp, ez_position) = massServo.get_position_mm()
            #(timestamp, ez_velocity) = massServo.get_velocity_actual()

            # Log Position & Velocity from EZServo
            #dset_ezpos.resize(dset_ezpos.shape[0] + 1, axis=0)
            #dset_ezvel.resize(dset_ezvel.shape[0] + 1, axis=0)
            #dset_ezpos[n_acqs, 0] = ez_position
            #dset_ezvel[n_acqs, 0] = ez_velocity
            #str_ez = "EZPos: {:5.2f} EZVelocity: {:5.2f}".format(ez_position, ez_velocity)

            # Feed the Model
            # TODO need to add pitch eventually and position from LCM
            #model_state = model.run_state_machine(ez_position, cmd_position, current_servo, ez_velocity, 0.0)

            # Log the model input/output
            dset_model_state.resize(dset_model_state.shape[0] + 1, axis=0)
            dset_model_health.resize(dset_model_health.shape[0] + 1, axis=0)
            dset_model_fault.resize(dset_model_fault.shape[0] + 1, axis=0)
            dset_model_ttf.resize(dset_model_ttf.shape[0] + 1, axis=0)
            print(str_time, str_daq)

            #dset_model_state[n_acqs, 0] = model_state['state']
            #dset_model_health[n_acqs, 0] = model_state['health']
            #dset_model_fault[n_acqs, 0] = model_state['faulttype']
            #dset_model_ttf[n_acqs, 0] = model_state['ttf']

            # Run the movement script
            #servostate = massServo.query_status()
            #if servostate is 'READY':

            #    if state is 0:
            #        print('Homing')
            #        massServo.run_servo('home')
            #        cmd_position = 0
            #        state += 1
            #    elif state is 1:
            #        print('Move To 10')
            #        cmd_position = 10
            #        massServo.run_servo('run', cmd_position)
            #        state += 1
            #    elif state is 2:
            #        print('Move To 0')
            #        cmd_position = 0
            #        massServo.run_servo('run', cmd_position)
            #        end_state = 0
            #        state += 1
            #    elif state is 3:
            #        if end_state < 20:
            #            end_state += 1
            #        else:
            #            state += 1

            # time.sleep(thread_pacer(loop_time))
            n_acqs += 1

        hf.close()


if __name__ == '__main__':
    try:
        ConstantAcquisitionAndModel()
    except KeyboardInterrupt:
        pass
