import  ch.psi.pshell.epics.ControlledVariable as ControlledVariable

if get_exec_pars().source == CommandSource.ui:
    start   = 0.0
    stop    = 360.0
    step    = 5.0
    nb      = 1
    lat     = 0.100
else:
    start = args[0]
    stop  = args[1]
    step =  args[2]
    nb   =  int(args[3])
    lat  =  args[4]
    

phase =  ControlledVariable("Phase", "SINEG01-RSYS:SET-VSUM-PHASE-SIM", "SINEG01-RSYS:GET-VSUM-PHASE-SIM")
phase.config.minValue   =-45.0
phase.config.maxValue   = 360.0
phase.config.resolution = 0.5
phase.initialize()

V  =  Channel("SINEG01-RSYS:GET-VSUM-AMPLT-SIM", type = 'd', alias='Amplitude Readback')
P  =  Channel("SINEG01-RSYS:GET-KLY-POWER-SIM",  type = 'd', alias='Power Readback')


#camtool = Camtool(url)
#camtool.startPipeline(camera_name)
#camtool.startReceiver()
#x = camtool.getValue("x_fit_mean")
x   =  Channel("SINEG01-RSYS:GET-VSUM-AMPLT-SIM", type = 'd', alias='Amplitude Readback')
dx  =  Channel("SINEG01-RSYS:GET-KLY-POWER-SIM",  type = 'd', alias='Power Readback')


try:
    xb  = create_averager(x,  nb, 0.100)
    dxb = create_averager(dx, nb, 0.100)
    r = lscan(phase, [xb, dxb], start, stop, step , latency=lat, after_read = after)
    rf_phase = r.getPositions(0)
    E   = [val.mean/1000.0/disp*energy0 for val in r.getReadable(0)]
    dE  = [val.mean/1000.0/disp*energy0 for val in r.getReadable(1)]
    caput("SINEG01-RSYS:GET-PHASE-ARRAY",  to_array(rf_phase,'d'))
    caput("SINEG01-RSYS:GET-CHARGE-ARRAY", to_array(charge,  'd'))
    phase_fit_max = None
    try:
        (energy_max, angular_frequency, phase0, in_range, phase_fit_max, fit_x, fit_y) = hfit(energy , xdata = rf_phase)
    except:
        raise Exception("Fit failure")
    caput("SINEG01-RSYS:GET-ONCREST-VSUM-PHASE", phase_fit_max)
    caput("SINEG01-RSYS:GET-ONCREST-E-GAIN",     energy_max)
    caput("SINEG01-RSYS:GET-FIT-PHASE-ARRAY",    fit_x)
    caput("SINEG01-RSYS:GET-FIT-ENERGY-ARRAY",   fit_y)
    phase_min, phase_max = min(rf_phase), max(rf_phase)
    if not (phase_min <= phase_fit_max <= phase_max):
        raise Exception("Fit maximum outside scan range")
    phase.write(phase_fit_max)
    time.sleep(lat)
    Ampl  = V.read()
    Power = P.read()
    caput("SINEG01-RSYS:GET-ONCREST-VSUM-AMPLT", Ampl)
    caput("SINEG01-RSYS:GET-ONCREST-KLY-POWER",  Power)
finally:
    phase.close()
    V.close()
    P.close()
    st.close()

print ("------------------------------------")
print ("Valid fit")

energy_gain     = energy_max
phase_offset    = 90 - phase_fit_max
amplitude_scale = energy_gain / Ampl
power_scale     = Power / math.pow(Ampl,2)

caput("SINEG01-RSYS:SET-VSUM-PHASE-OFFSET-BASE", phase_offset)
caput("SINEG01-RSYS:SET-VSUM-AMPLT-SCALE",       amplitude_scale)
caput("SINEG01-RSYS:SET-VOLT-POWER-SCALE",       power_scale)