#! /usr/bin/env python
#
# created: 2016-Nov-29 by KI
# This is a script for automatically adjusting the green QPD offset.
# When executing this script, the interferometer should be locked with IR sensors.
#
import cdsutils as cds
import ezca, time
import numpy as np

# initialize ezca
ezca = ezca.Ezca()

# some constants
Pgood = 0.5 # Trigger thrshold for arm power
Pperfect = 0.9 # arm power criteria
WfsPerfect = 0.1 # Wfs criteria

# actuation points
QpdOffsets = ['ALS-Y_QPD_A_PIT_OFFSET',
                'ALS-Y_QPD_B_PIT_OFFSET',
                'ALS-Y_QPD_A_YAW_OFFSET',
                'ALS-Y_QPD_B_YAW_OFFSET']

# sensors channel names
Wfs = ['ALS-Y_WFS_A_I_PIT_OUTPUT',
        'ALS-Y_WFS_B_I_PIT_OUTPUT',
        'ALS-Y_WFS_A_I_YAW_OUTPUT',
        'ALS-Y_WFS_B_I_YAW_OUTPUT']

# gains for individual loops, emperically adjusted
Gains = [6e-6, -6e-6, 6e-6, -6e-6]
# Flags to check the completenes
WfsFlags = [False, False, False, False]

TR = 'ALS-Y_TR_A_LF_OUT16' # TR channel name

# set ramping times to zero
for jj in range(0, len(QpdOffsets)):
    Tramp = QpdOffsets[jj].replace('OFFSET', 'TRAMP')
    ezca[Tramp] = 0

# hop into loop.
while True:
    if ezca[TR] > Pgood:
        for jj in range(0, len(QpdOffsets)): 
            # do the cds servo
            cds.servo(ezca, QpdOffsets[jj],
                readback=Wfs[jj],
                gain = Gains[jj],
                timeout = 1)

            # clipping the output
            temp = ezca[QpdOffsets[jj]] 
            if np.abs(temp) > 1:
                ezca[QpdOffsets[jj]] = np.sign(temp)

           # get some samples
            ArmPowerFlag = ezca[TR] < Pperfect
            WfsFlags[jj] = np.abs( ezca[ Wfs[jj] ] ) < WfsPerfect
           
        # check if it's good enough 
        if ArmPowerFlag and np.sum(WfsFlags) == 4:
            print("=============================")
            print("Good. You can exit by Cotrl-C")
            print("=============================")
            time.sleep(0.4)
    else:
        print("%s too low. sleep for 1 sec"%TR)
        time.sleep(1)