#!/usr/bin/env python

import sys
from ezca import Ezca
from math import sqrt, pow
import time 
import subprocess
import cdsutils

sys.path.append('/ligo/cdscfg')
import stdenv as cds;

cds.INIT_ENV()

Ezca().export()

sys.path.append('/opt/rtcds/userapps/release/isc/common/scripts/decoup/isclib')
import isclib.matrices as matrix
#import stop_osc
#
# quad dither settings
pr3_p_freq = 13.2
etmx_p_freq = 6.7
etmy_p_freq = 7.4

pr3_y_freq = 14.8
etmx_y_freq = 8.1
etmy_y_freq = 8.8


if cds.SITE == 'LLO':
    quad_clk_gain = 100
else:
    #quad_clk_gain = 300  # LHO has been using 300cts
    #quad_clk_gain = 2000  # use during DC readout, 2W
    quad_clk_gain = {'PIT7':10,'PIT8':200,'PIT9':200,'YAW7':30, 'YAW8':200,'YAW9':200} # High noise 2W, July2016
print 'Nominal value of osc clock gain is 300, in NomLowNoise, 2000 for DC readout at 2W'
print 'Current oscillator amplitude is:'
print quad_clk_gain

quad_sin_gain = 1
quad_cos_gain = 1

t_ramp = 5

# demod phases
if cds.SITE == 'LLO':
    pit1_ph = 45
    pit2_ph = 45
    pit3_ph = 45
    pit4_ph = 45
    yaw1_ph = 45
    yaw2_ph = 45
    yaw3_ph = 45
    yaw4_ph = 45
else:
    pit7_ph = -9  
    pit8_ph = -105
    pit9_ph = -5
    yaw7_ph = -10
    yaw8_ph = -16
    yaw9_ph = -14

#########################################
# Ramp down the dither lines
print 'Turning off beam spot monitor dither lines, leaveing PIT1 and YAW1 on'
for dof in ['PIT7','PIT8','PIT9','YAW7','YAW8','YAW9']:
    ezca['ASC-ADS_' + dof + '_OSC_TRAMP'] = 5
    time.sleep(0.3)
    ezca['ASC-ADS_' + dof + '_OSC_CLKGAIN'] = 0
    ezca['ASC-ADS_' + dof + '_OSC_SINGAIN'] = 0
    ezca['ASC-ADS_' + dof + '_OSC_COSGAIN'] = 0
time.sleep(6)

#########################################
# Set up matrices

#zero everything except SRM loop
dither_act_list= matrix.asc_ads_actuators_dither
control_act_list= matrix.asc_ads_actuators
sensor_list=matrix.asc_ads_sensors
dofs_pit_list=['PIT7', 'PIT8','PIT9']
dofs_yaw_list=['YAW7', 'YAW8','YAW9']
for osc in ['OSC7','OSC8','OSC9']:
    for optic in dither_act_list:
        matrix.asc_ads_lo_pit[optic,osc]=0
        matrix.asc_ads_lo_yaw[optic,osc]=0
'''for optic in control_act_list:
    for dof in dofs_pit_list:
        matrix.asc_ads_output_pit[optic,dof]=0
    for dof in dofs_yaw_list:
        matrix.asc_ads_output_yaw[optic,dof]=0
for dof in dofs_yaw_list:
    for sensor in sensor_list:
        matrix.asc_ads_input[dof,sensor]=0
for dof in dofs_pit_list:
    for sensor in sensor_list:
        matrix.asc_ads_input[dof,sensor]=0'''


print 'Setting oscillator matrices'
#pit oscillator matrix
matrix.asc_ads_lo_pit['PR3','OSC7'] = 1
matrix.asc_ads_lo_pit['ETMX','OSC8'] = 1
matrix.asc_ads_lo_pit['ETMY','OSC9'] = 1

#yaw oscillator matrix
matrix.asc_ads_lo_yaw['PR3','OSC7'] = 1
matrix.asc_ads_lo_yaw['ETMX','OSC8'] = 1
matrix.asc_ads_lo_yaw['ETMY','OSC9'] = 1

print 'Setting demod input matrix'
matrix.asc_ads_input_pit['PIT7','PRCL_CTRL'] = 1
matrix.asc_ads_input_pit['PIT8','DARM_CTRL'] = 1
matrix.asc_ads_input_pit['PIT9','DARM_CTRL'] = 1
matrix.asc_ads_input_yaw['YAW7','PRCL_CTRL'] = 1
matrix.asc_ads_input_yaw['YAW8','DARM_CTRL'] = 1
matrix.asc_ads_input_yaw['YAW9','DARM_CTRL'] = 1




#########################################
#Set up dither lines

print 'Send the dither lines'
# quads
ezca['ASC-ADS_PIT7_OSC_FREQ'] = pr3_p_freq
ezca['ASC-ADS_YAW7_OSC_FREQ'] = pr3_y_freq

ezca['ASC-ADS_PIT8_OSC_FREQ'] = etmx_p_freq
ezca['ASC-ADS_YAW8_OSC_FREQ'] = etmx_y_freq

ezca['ASC-ADS_PIT9_OSC_FREQ'] = etmy_p_freq
ezca['ASC-ADS_YAW9_OSC_FREQ'] = etmy_y_freq

for dof in ['PIT7','PIT8','PIT9','YAW7','YAW8','YAW9']:
    ezca['ASC-ADS_' + dof + '_OSC_TRAMP'] = t_ramp
    time.sleep(0.3)
    ezca['ASC-ADS_' + dof + '_OSC_CLKGAIN'] = quad_clk_gain[dof]
    ezca['ASC-ADS_' + dof + '_OSC_SINGAIN'] = quad_sin_gain
    ezca['ASC-ADS_' + dof + '_OSC_COSGAIN'] = quad_cos_gain

#########################################
# Set up demods

# Set the phases
ezca['ASC-ADS_PIT7_DEMOD_PHASE'] = pit7_ph
ezca['ASC-ADS_PIT8_DEMOD_PHASE'] = pit8_ph
ezca['ASC-ADS_PIT9_DEMOD_PHASE'] = pit9_ph
ezca['ASC-ADS_YAW7_DEMOD_PHASE'] = yaw7_ph
ezca['ASC-ADS_YAW8_DEMOD_PHASE'] = yaw8_ph
ezca['ASC-ADS_YAW9_DEMOD_PHASE'] = yaw9_ph
'''
# Choose the right BP filter
ezca.switch('ASC-ADS_PIT2_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT3_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT4_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT5_DEMOD_SIG','FM1','OFF','FM2','ON')
       
ezca.switch('ASC-ADS_YAW2_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW3_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW4_DEMOD_SIG','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW5_DEMOD_SIG','FM1','OFF','FM2','ON')

# Choose the right LP filter
ezca.switch('ASC-ADS_PIT2_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT3_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT4_DEMOD_I','FM1','OFF','FM2','ON') 
ezca.switch('ASC-ADS_PIT5_DEMOD_I','FM1','OFF','FM2','ON') 
ezca.switch('ASC-ADS_PIT2_DEMOD_Q','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT3_DEMOD_Q','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_PIT4_DEMOD_Q','FM1','OFF','FM2','ON') 
ezca.switch('ASC-ADS_PIT5_DEMOD_Q','FM1','OFF','FM2','ON') 
 
ezca.switch('ASC-ADS_YAW2_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW3_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW4_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW5_DEMOD_I','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW2_DEMOD_Q','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW3_DEMOD_Q','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW4_DEMOD_Q','FM1','OFF','FM2','ON')
ezca.switch('ASC-ADS_YAW5_DEMOD_Q','FM1','OFF','FM2','ON')

'''