###################################################################################################
#  Deployment specific global definitions - executed after startup.py
###################################################################################################

BEAMLINE_XNAME = "X06DA"

dtor = math.pi /180.0

for d in [pvs, pvc, phs, phc, ths, thc, tvs, tvc, qhs, qhc, qvs, qvc]:
    d.setBlockingWrite(True)


def a2e(t,h=1.0,k=1.0,l=1.0, deg =True, ln = False):
    lncorr= 2.e-4 if ln else 0.0    
    d0=2 * 5.43102 *  (1.0-lncorr) / math.sqrt(h**2+k**2+l**2)
    tt= (t * dtor) if (deg or (t>1.0)) else t
    return 12.39842 / (d0*math.sin(tt))

def angle(e,h=1.0,k=1.0,l=1.0, deg =True, ln = False,bent = False):
    lncorr = 2.e-4 if ln else 0.0    
    d0 =2 * 5.43102 *  (1.0-lncorr) / math.sqrt(h**2+k**2+l**2)
    a =	math.asin(12.39842/d0/e)
    if bent:
        rho	= 2*19.65*8.35/28*math.sin(a)
        dt	= 0.2e-3/rho*0.279
        d0	= 2*5.43102*(1+dt)/math.sqrt(h**2+k**2+l**2)
        a   = math.asin(12.39842/d0/e)   
    a = a/dtor if deg else a
    return a

def get_energy(debug_msg = True):
    t2 = th2.position
    e = a2e(abs(t2))
    if debug_msg:
        print 'Energy [keV]:'+ str(e) + '   Wavelength [A]:' + str(12.39842/e)
    return e

def set_energy(e, debug_msg = True):
    e = float(e)
    if e< 4.7  or e > 18.0:
        raise Exception ("please select an energy between 4.7 an 18 keV")
    t1e = th1_encoder.read()
    t2e = th2_encoder.read()
    t1v = th1.read()
    t2v = th2.read()
    t1s = angle(e,1,1,1,ln=True)
    t2s = angle(e,1,1,1)

    # calculate the corrections later, after 
    # calibration of encoders
    dt1 = 0. 
    dt2 = 0.

    # it is better to move both crystals by the same amount. however,
    # if one motor got stuck and the other did not, we should not move
    # them by the same amount but each according to its calculated deltha theta
    t1p = -t1s
    t2p = t2s

    if debug_msg:
        print 'set angles th1=' + str(t1p) + " th2=" + str(t2p)
    th1.move(t1p)    #TODO: simultaneous move?
    th2.move(t2p)
    th1.waitReady(-1)
    th2.waitReady(-1)
    if debug_msg:
        print 'done set energy'


def shopen():
    exp_shutter.write("OPEN")
        
def shclose():
    exp_shutter.write("CLOSED")    

def vfoc (dx=0.1):
    #scan,'dfy',0,2*dx,25,/fit,det='des',dat=d        
    motor = cy #TODO: Is right? original was not relative; motor channel was X06DA-ES-DF1:TRY-VAL
    sensor = mono_beam #TODO: sensor is not clear since set det to des(X06DA-ES-KBOX:SIG-RAW) but parameter is called 'detector' in scan: is default mono_beam used?
    
    result = lscan(motor, sensor, -dx, dx, 25, latency = 0.3, relative = True) #TODO: range is 0, 2*dx?
    (ydata, xdata) = (result.getReadable(0), result.getPositions(0))

    from mathutils import deriv, fit_gaussians, Gaussian, get_values    
    d = [x*1e6 for x in deriv(ydata, xdata)]
    gaussians = fit_gaussians(d, xdata , [d.index(max(d)) ,])
    (n, m, s) = gaussians[0]    
    fitted_gaussian_function = Gaussian(n, m, s)    
    g = get_values(fitted_gaussian_function,xdata)    
    plot((ydata, d, g), ("Sensor", "Deriv", "Gauss Fit"), (xdata,xdata,xdata))
    #PLOT,d(0,*),deriv(reform(d(1,*))*1e6)
    #oplot,d(0,*),gaussfit(reform(d(0,*)),deriv(reform(d(1,*))*1e6),a,nterm=5),ps=-6            
    print 'CENTRE ',m
    print 'FWHM microns',s*2354.    
    motor.move(m)    #?motor.move(-dx+a(1))    
    #WM,'mb1' #WHAT for?    
    #WM,'mb2'
    return (n, m, s) 

def knife():
    pvc0 = pvc.read()
    pvs0 = pvs.read()
    try:
        step    =   0.175
        pvc.write(pvc0 - 3.*step)
        pvs.write(0.1)    
        c = []
        for i in range(5):
            pvc.write(pvc0 + step * i)
            a = vfoc()
            c.apppend(a[1])    
        print 'mean',mean(c)
        print 'stdev',stdev(c)
    finally:
        pvc.write(pvc0)
        pvs.write(pvs0)
    
def knifescan(motor, ll, hl, n=25):
    try:
        shopen()
        time.sleep(1.0)
        result = lscan(motor, marcover, ll, hl, n, latency = 0.3, relative = True)
        #scan, motor, ll, hl, n,det='marcover',dat=d
        (ydata, xdata) = (result.getReadable(0), result.getPositions(0))    
        d =  deriv(ydata, xdata)    
        #inversed    #IF MOTOR EQ 'dfx' THEN sign = -1. ELSE sign = 1 #TODO    
        if motor in []:
            d = [x*-1.0 for x in d]   
        gaussians = fit_gaussians(d, xdata , [d.index(max(d)) ,])
        (n, m, s) = gaussians[0] 
    
        #PLOT,D(0,*), SIGN*DERIV(d(1,*)), THICK=2 
        #OPLOT, d(0,*), GAUSSFIT(d(0,*),SIGN*DERIV(d(1,*)),a),col=150
        plot((ydata, d, g), ("Sensor", "Deriv", "Gauss Fit"), (xdata,xdata,xdata))
        print 'Centre [scanunits]',m
        print 'FWHM   [scanunits]',s*2.3542    #sigma to FWHM  factor is 2.3542
    finally:
        shclose()

run("fit")
run("rock")
#run("sete")
#run("seteq")



def fbm():
    """
    """
    run("fbm")

def rocknroll():
    """
    """
    run("rocknroll")

def censlits():
    """
    """
    run("censlits")

def flux_diode():
    """
    """
    run("flux_diode")

def sete(e, all=True, wavelength=False):   
    run("sete", {"e":e, "all":all, "wavelength":wavelength})

def seteq(e):   
    run("seteq", {"e":e})


def check_rock():
    run("check_rock")
     

import ch.psi.pshell.device.ReadonlyRegisterBase as ReadonlyRegisterBase
class EnergyReader(ReadonlyRegisterBase):
    def doRead(self):
        return get_energy(False)

add_device(EnergyReader("energy",3), True)   

energy.polling = 1000