#!/usr/bin/env python

import os
import sys
from ezca import Ezca
import time 
import subprocess
import cdsutils
import numpy

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


cds.INIT_ENV()

Ezca().export()

# a script to be used for doing charge measurements in lock.

###useful functions copied from G Vajente's ASC sensing matrix script

def gpsnow():
    return int(subprocess.check_output(["tconvert", "now"]))

def make_excitation_file(frequency, T, Tramp):
    # generate excitation file
    t = numpy.arange((T+2*Tramp)*fsample)/numpy.double(fsample)
    s = numpy.sin(2*numpy.pi*frequency*t)
    e = numpy.ones(numpy.shape(t))
    e[t<Tramp] = t[t<Tramp]/Tramp
    e[t>T+Tramp] = (T+2*Tramp - t[t>T+Tramp])/Tramp
    se = s * e
    ff = open('excitation.txt', 'w')
    for x in se:
        ff.write('%.10f '% x)
    ff.close()

def excitation(channel, frequency, amplitude, T, Tramp):
    make_excitation_file(frequency, T, Tramp)  
    #record time when the excitation has ramped up to full amplitude
    gps_start = gpsnow()+Tramp
    # start it
    with open(os.devnull, 'w') as fp:
        subprocess.Popen(["awgstream", channel, str(fsample), 'excitation.txt', str(amplitude)])
    time.sleep(Tramp+T)
    #record end of time when excitation is at full amplitude
    gps_stop= gpsnow()
    #wait for excitation to fully ramp down before returning
    time.sleep(Tramp)
    return [gps_start, gps_stop]

# other functions

def store_initial_values():
    #note inital values to restore when script is done
    BIAS_gain = ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD]
    BIAS_Tramp = ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD]
    BIAS_value = ezca['SUS-%s_L3_LOCK_INBIAS'%QUAD]
    DA_L2L_gain = ezca['SUS-%s_L3_DRIVEALIGN_L2L_GAIN'%QUAD]
    DA_L2L_Tramp = ezca['SUS-%s_L3_DRIVEALIGN_L2L_TRAMP'%QUAD]
    linearization_on_to_start = False

def set_up_DAL2L():
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_TRAMP'%QUAD] = 1
    time.sleep(0.1)
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_GAIN'%QUAD] = 1
    ezca.get_LIGOFilter('SUS-%s_L3_DRIVEALIGN_L2L'%QUAD).only_on('INPUT', 'OUTPUT', 'DECIMATION')
    time.sleep(3)

def check_for_L2A():
    if ezca['SUS-%s_L3_DRIVEALIGN_L2P_GAIN'%QUAD] != 0 or ezca['SUS-%s_L3_DRIVEALIGN_L2Y_GAIN'%QUAD] != 0:
        time.sleep(0.1)
        ezca['SUS-%s_L3_DRIVEALIGN_L2P_GAIN'%QUAD]=0
        ezca['SUS-%s_L3_DRIVEALIGN_L2Y_GAIN'%QUAD]=0
        return True
    else:
        return False

def check_no_DARM_feedback():
    #FIXME: need to check for DAC crossing offsets
    #first check if DARM is fed back to this ESD, and if linearization is on
    if cdsutils.avg(5,'SUS-%s_L3_DRIVEALIGN_L_OUTMON'%QUAD) != 0:
        print 'DARM seems to be feeding back to this ESD, you need to move DARM control before running this measurement'
        return False
    else:   
        return True

def linearization_check():
    # then check if the ESD linearization is on
    if ezca['SUS-%s_L3_ESDOUTF_LIN_BYPASS_SW'%QUAD] == 0:
        linearization_on_to_start = True
        print 'ESD linearization is on, turning it off'
        slowly_ramp_off_bias()
        ezca['SUS-%s_L3_ESDOUTF_LIN_BYPASS_SW'%QUAD] = 1
        print 'linearization bypassed, ramping bias back on'
        slowly_ramp_on_bias()
        return True

def slowly_ramp_off_bias():
    print 'Ramping off bias on %s ESD'%QUAD
    ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD] = 60
    time.sleep(0.1)
    ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD] = 0
    time.sleep(62)


def slowly_ramp_on_bias():
    print 'Ramping on bias on %s ESD'%QUAD
    #turn the gain to zero before you start (this only works if the BIAS is 0 before you run this function)
    ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD] = 2
    time.sleep(0.1)
    ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD] = 0
    time.sleep(2.2)
    ezca['SUS-%s_L3_LOCK_INBIAS'%QUAD] = BIAS_value
    ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD] = 60
    time.sleep(1)
    ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD] = BIAS_gain
    time.sleep(60)

def run_bias_excitation(exc_name):
    print 'Exciting bias path on %s'%QUAD
    exc_channel = IFO + ':SUS-'+QUAD+'_L3_LOCK_BIAS_EXC'
    #sys.stdout.write("[%d] start excitation for channel %s\n" % (gpsnow(), exc))
    [gps_start, gps_stop] = excitation(exc_channel, drive_freq, exc_amp_bias, exc_duration, exc_Tramp)
    '''### wait for the excitation to start
    time.sleep(1 + exc_Tramp)
    start_gps = gpsnow()
    print "  GPS start: %d " % start_gps
    ### wait the desired time
    time.sleep(exc_duration)
    stop_gps = gpsnow()
    print "  GPS stop: %d " % stop_gps
    time.sleep(exc_Tramp)'''
    ### append times to the log file
    logfile.append([gps_start, gps_stop, exc_name])

def run_L_excitation(exc_name):
    print 'Exciting signal path on %s'%QUAD
    exc_channel = IFO + ':SUS-'+QUAD+'_L3_DRIVEALIGN_L2L_EXC'
    [gps_start, gps_stop] = excitation(exc_channel, drive_freq, exc_amp_DAL2L, exc_duration, exc_Tramp)
    #sys.stdout.write("[%d] start excitation for channel %s\n" % (gpsnow(), exc))
    '''### wait for the excitation to start
    time.sleep(1 + exc_Tramp)
    start_gps = gpsnow()
    print "  GPS start: %d " % start_gps
    ### wait the desired time
    time.sleep(exc_duration)
    stop_gps = gpsnow()
    print "  GPS stop: %d " % stop_gps
    time.sleep(exc_Tramp)'''
    ### append times to the log file
    logfile.append([gps_start, gps_stop, exc_name])



################################################
#                                              #
#   Configuration and initialisation section   #
#                                              #
################################################
IFO = 'H1'
QUAD='ETMY'

# prefix of the output file names (logfile and html file)
start_time = gpsnow()
filename = QUAD + '_' + str(start_time) + '_log.txt'
# Set up all signal injection properties first
fsample = 16384.0							# Data rate
exc_duration = 60							# Length of each individual drive
exc_Tramp = 2
if QUAD == 'ETMX' or QUAD == 'ETMY':
    drive_freq =  12.0								# Drive frequency
    exc_amp_bias = 0.01		#  in volts to driver 
    exc_amp_DAL2L=100
else:
    drive_freq =  12.0								# Drive frequency
    exc_amp_bias = 0.01		#  in volts to driver 
    exc_amp_DAL2L=30


#######  actualy do it

#store_initial_values()

#note inital values to restore when script is done
BIAS_gain = ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD]
BIAS_Tramp = ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD]
BIAS_value = ezca['SUS-%s_L3_LOCK_INBIAS'%QUAD]
DA_L2L_gain = ezca['SUS-%s_L3_DRIVEALIGN_L2L_GAIN'%QUAD]
DA_L2L_Tramp = ezca['SUS-%s_L3_DRIVEALIGN_L2L_TRAMP'%QUAD]
L2P_gain=ezca['SUS-%s_L3_DRIVEALIGN_L2P_GAIN'%QUAD]
L2Y_gain=ezca['SUS-%s_L3_DRIVEALIGN_L2Y_GAIN'%QUAD]
linearization_on_to_start = False

print 'starting in lock charge measurements for %s'%QUAD
if check_no_DARM_feedback():
    #check if linearization is on, if so turn it off
    linearization_on_to_start = linearization_check()
    # if everything is OK, start excitations
    logfile = []
    #exc_types=[bias_drive_bias_on, bias_drive_bias_off, L_drive_bias_off, L_drive_L_offset, L_drive_bias_on]
    # run the first excitation
    run_bias_excitation('bias_drive_bias_on')
    slowly_ramp_off_bias()
    #turn off inbias and turn gain back on to allow excitations through bias path
    ezca['SUS-%s_L3_LOCK_INBIAS'%QUAD] = 0
    ezca['SUS-%s_L3_LOCK_BIAS_TRAMP'%QUAD] = 2
    time.sleep(0.1)
    ezca['SUS-%s_L3_LOCK_BIAS_GAIN'%QUAD] = BIAS_gain
    time.sleep(2)
    run_bias_excitation('bias_drive_bias_off')
    time.sleep(1)
    #get ready to start the signal path excitations
    set_up_DAL2L()
    reset_L2A_flag = check_for_L2A()
    run_L_excitation('L_drive_bias_off')
    #time.sleep(1)
    print 'Ramping on offset on signal electrodes for %s'%QUAD
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_TRAMP'%QUAD]=20
    ezca.get_LIGOFilter('SUS-%s_L3_DRIVEALIGN_L2L'%QUAD).switch_on('OFFSET')
    time.sleep(0.1)
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_OFFSET'%QUAD]=50000
    time.sleep(20)
    run_L_excitation('L_drive_L_offset')
    print 'ramping L offset off and bias back to original value'
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_OFFSET'%QUAD]=0
    slowly_ramp_on_bias()
    time.sleep(1)
    run_L_excitation('L_drive_bias_on')
    print 'Finished with all excitations on %s'%QUAD
    ### Save the logfile 	
    f = open(filename+".log", 'w')
    for l in logfile:
        f.write("%d %d %s\n" % (l[0], l[1], l[2]))
    f.close()
    print "Saved GPS times in logfile: %s" % filename

    print 'Restoring things to the way they were before the measurement'
    if linearization_on_to_start:
        print 'turning linearization back on for %s'%QUAD
        slowly_ramp_off_bias()
        ezca['SUS-%s_L3_ESDOUTF_LIN_BYPASS_SW'%QUAD] = 0
        slowly_ramp_on_bias()
    if reset_L2A_flag:
        ezca['SUS-%s_L3_DRIVEALIGN_L2P_GAIN'%QUAD] = L2P_gain
        ezca['SUS-%s_L3_DRIVEALIGN_L2Y_GAIN'%QUAD] = L2Y_gain  
    #bias gain should already have been restored with an appropriate ramp
    ezca.get_LIGOFilter('SUS-%s_L3_DRIVEALIGN_L2L'%QUAD).switch_off('OFFSET')
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_GAIN'%QUAD] = DA_L2L_gain
    ezca['SUS-%s_L3_DRIVEALIGN_L2L_TRAMP'%QUAD] = DA_L2L_Tramp
    print 'all done'

else:
    print 'Not running the measurement since DARM is locked on this optic'