import subprocess

from guardian import GuardState, GuardStateDecorator

import ISC_library
import cdsutils as cdu
import time
import fast_ezca as fez
import math

##################################################
# Take care of WFS centering
##################################################

# need to include POP?
def gen_WFS_DC_CENTERING(dof,port):
    class WFS_DC_CENTERING(GuardState):
        request = False

        @ISC_library.assert_dof_locked_gen(['IMC',dof])
        def main(self):
            log('Port is: %s'%port)
            log('DOF is: %s'%dof)
            if port == 'REFL':
                self.servos = [1,2]
                self.sus    = ['RM1','RM2']
            elif port == 'AS':
                self.servos = [3,4]
                self.sus    = ['OM1','OM2']
            elif port == 'REFL_AS':
                self.servos = [1,2,3,4]
                self.sus    = ['RM1','RM2','OM1','OM2']
            log(self.servos)

            if 'AS' in port:
                if ezca['SYS-MOTION_C_FASTSHUTTER_A_STATE'] == 1:
                    notify('Toast is ready!')
                    ezca['ISI-HAM6_SATCLEAR'] = 1  #reset saturation counter
                    ezca['ISI-HAM6_WD_RSET'] = 1
                    time.sleep(.1)
                    ezca['SYS-MOTION_C_FASTSHUTTER_A_UNBLOCK'] = 1 # open fast shutter

            log('turning on DC centering')
            for py in ['P','Y']:
                for servo in self.servos:
                    ezca.get_LIGOFilter('ASC-DC%s_%s'%(servo,py)).switch_on('INPUT')

        @ISC_library.assert_dof_locked_gen(['IMC',dof])
        def run(self):
            if not ISC_library.WFS_DC_centering_servos_OK(port):
                notify('WFS DC centering railed')
                if 'AS' in port:
                    if ezca['SYS-MOTION_C_FASTSHUTTER_A_STATE'] == 1:
                        notify('Toast is ready!')
                        ezca['ISI-HAM6_SATCLEAR'] = 1  #reset saturation counter
                        ezca['ISI-HAM6_WD_RSET'] = 1
                        time.sleep(.1)
                        ezca['SYS-MOTION_C_FASTSHUTTER_A_UNBLOCK'] = 1 # open fast shutter
                for py in ['P','Y']:
                    for servo in self.servos:
                        ezca.get_LIGOFilter('ASC-DC%s_%s'%(servo,py)).switch_off('INPUT')
                        ezca['ASC-DC%s_%s_RSET'%(servo,py)] = 2
                    for sus in self.sus:
                        ezca['SUS-%s_M1_LOCK_%s_RSET'%(sus,py)] = 2
                time.sleep(.1)
                for py in ['P','Y']:
                    for servo in self.servos:
                        ezca.get_LIGOFilter('ASC-DC%s_%s'%(servo,py)).switch_on('INPUT')

            servolist = []
            for py in ['P','Y']:
                for servo in self.servos:
                    servolist.append('ASC-DC%s_%s_INMON'%(servo,py))
            inmonvals = cdu.avg(2,servolist)
            if any(x >= 0.2 for x in inmonvals):
                return False
            else:
                return True

    return WFS_DC_CENTERING


##################################################
# Offload alignment to sliders
##################################################


def gen_OFFLOAD_ALIGNMENT_MANY(dof,ramptime,opticList):
    class OFFLOAD_ALIGNMENT(GuardState):
        request = False
        redirect = False

        #@ISC_library.assert_dof_locked_gen(['IMC',dof])
        # SED note July 22nd 2018.  This was causing the green WFS to not get offloaded because it would fail this check even when the arm stayed locked.  Since this is a redirect = fasle state, I'm not sure if we want this decorator (on this state, because if we loose lock in the midlle of offlaoding, we still want to finish the offloading not stop partway through.
        def main(self):
            log('starting smooth offload') # formerly separated out as smooth_offload_fast function
            log(opticList)

            # set up names for each optic and DOF to offload
            numOffload = 2 * len(opticList)
            n_dof = []
            n_optic = []
            n_top_stage = []

            for index in range(len(opticList)):
                optic = opticList[index]       # this optic

                # the top stage for this optic
                if (optic[:3] == ('ETM')) or (optic[:3] == ('ITM')):
                    top_stage = 'M0'
                else:
                    top_stage = 'M1'

                # the optic, dof, and top stage for each index
                n_dof += ['P', 'Y']
                n_optic += [optic, optic]
                n_top_stage += [top_stage, top_stage]

            # set up LIGOFilters
            lock_fm = []
            drivealign_fm = []
            opticalign_fm = []

            for index in range(numOffload):
                dof = n_dof[index]
                optic = n_optic[index]
                top_stage = n_top_stage[index]

                lock_fm.append(ezca.get_LIGOFilter('SUS-{0}_{1}_LOCK_{2}'.format(optic, top_stage, dof)))
                drivealign_fm.append(ezca.get_LIGOFilter('SUS-{0}_{1}_DRIVEALIGN_{2}2{2}'.format(optic, top_stage, dof)))
                opticalign_fm.append(ezca.get_LIGOFilter('SUS-{0}_{1}_OPTICALIGN_{2}'.format(optic, top_stage, dof)))

            # record initial state
            lock_gain = []
            lock_tramp = []
            opticalign_tramp = []

            for index in range(numOffload):
                lock_gain.append(lock_fm[index].GAIN.get())
                lock_tramp.append(lock_fm[index].TRAMP.get())
                opticalign_tramp.append(opticalign_fm[index].TRAMP.get())

            # simultaneously ramp LOCK gain to zero, and
            doList = []
            for index in range(numOffload):
                desired_bias = opticalign_fm[index].OFFSET.get()
                desired_bias += round((drivealign_fm[index].OUTPUT.get()/opticalign_fm[index].GAIN.get()), 4)
                doList.append([('off', lock_fm[index], 'INPUT'), ('ramp_gain', lock_fm[index], 0, ramptime), \
                  ('ramp_offset', opticalign_fm[index], desired_bias, ramptime)])
            fez.do_many(ezca, doList)

            # wait for all ramps to complete
            log('waiting for ramps to finish...')
            fez.wait_many(ezca, lock_fm + opticalign_fm)
            log('done waiting')

            # restore initial state
            doList = []
            for index in range(numOffload):
                doList.append([('clear', lock_fm[index]), ('on', lock_fm[index], 'INPUT'), \
                  ('ramp_gain', lock_fm[index], lock_gain[index], lock_tramp[index]), \
                  ('write', opticalign_fm[index].filter_name + '_TRAMP', opticalign_tramp[index])])
            fez.do_many(ezca, doList)

        #@ISC_library.assert_dof_locked_gen(['IMC',dof])
        def run(self):
            return True

    return OFFLOAD_ALIGNMENT




##################################################
# ALS fine tune IR (used for COMM and DIFF)
##################################################

def gen_TUNE_IR_BETTER(SweepWidth, StepSize, pause, Thresh, TransChan, SweepChan, SleepTime, dof):
    #dof should be 1 for comm, 2 for diff
    class TUNE_IR_BETTER(GuardState):
        request = False

        @ISC_library.assert_dof_locked_gen(['IMC', 'XARM_GREEN', 'YARM_GREEN'])
        def main(self):
            self.SweepWidth = SweepWidth  # width of the sweep is +/- this number
            self.StepSize = StepSize
            self.SleepTime = SleepTime
            self.pause = pause # time to pause for each step (seconds)
            self.threshHigh = Thresh   # threshold on transmitted power to jump to next state
            self.threshLow = 0.02
            self.NumSteps = 2*math.floor(self.SweepWidth/self.StepSize) # total number of steps
            self.counter = 0
            self.StartPoint = ezca[SweepChan]
            self.MaxTrans = ezca[TransChan]
            self.transValueOld = ezca[TransChan]
            self.transValueNew = ezca[TransChan]
            self.direction = 1 # positive or negative
            self.MaxTransOffset = ezca[SweepChan]
            self.timer['pause1'] = 5
            self.timer['pause'] = self.pause
            self.timer['threshWait'] = 4
            self.skip = False


        @ISC_library.assert_dof_locked_gen(['IMC', 'XARM_GREEN', 'YARM_GREEN'])
        def run(self):
            #check if you don't need a fine tune IR
            #if ezca[TransChan] >= self.threshHigh and ezca['ALS-C_TRX_A_LF_OUTMON'] > 0.95 and self.timer['threshWait']:
            #    log(TransChan+' is already above threshold')
            #    self.skip = True

            # check gren arm build ups before finding IR
            if ezca['ALS-C_TRX_A_LF_OUTMON'] < 0.85:
                notify('X arm alignment is bad, not searching')
                time.sleep(1)
            # also check Y arm if serarching for diff offset
            elif dof == 2 and ezca['ALS-C_TRY_A_LF_OUTMON'] < 0.85:
                notify('Y arm alignment is bad, not searching')
                time.sleep(1)
            else:
                if self.skip == True:
                    return True
                self.transValueNew = cdu.avg(1, TransChan)
                log('Transmitted value is {}'.format(self.transValueNew))
                if self.transValueNew < self.threshLow:
                    log('Transmitted power too low for FINE_TUNE_IR.')
                    return 'NO_IR_FOUND'
                elif self.transValueNew >= self.threshHigh:
                    log('Transmitted power sufficient.')
                    return True
                elif self.counter < self.NumSteps:
                    if self.transValueNew < self.transValueOld:
                        self.direction *= -1
                    ezca[SweepChan] += self.StepSize * self.direction
                    self.transValueOld = self.transValueNew
                    time.sleep(self.SleepTime)
                else:
                    log('Fine tuning failed.')
                    return 'NO_IR_FOUND'
    return TUNE_IR_BETTER





