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

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

bph = ControlledVariable("Beam phase", "SINEG01-RSYS:SET-BEAM-PHASE-SIM", "SINEG01-RSYS:SET-BEAM-PHASE-SIM")
bph.config.minValue   =-180.0
bph.config.maxValue   = 360.0
bph.config.precision  = 3
bph.config.resolution = 1.0
bph.config.save()
bph.initialize()
rph  = Channel("SINEG01-RSYS:SET-VSUM-PHASE-SIM", type = 'd', alias = 'RF phase')
q    = Channel("SINEG01-DICT215:B1_CHARGE-SIM",   type = 'd', alias = 'ICT-Q')

rph0 = rph.read()

try:
    q_averager   = create_averager(q, nb, 0.100)
    rph_averager = create_averager(rph, nb, 0.100)
    r = lscan(bph, (rph_averager, q_averager), start, stop, step, latency=lat)
    beamphase  = r.getPositions(0)
    rfphase    = [val.mean for val in r.getReadable(0)]
    rfphaserms = [val.stdev for val in r.getReadable(0)]
    charge     = [val.mean for val in r.getReadable(1)]
    chargerms  = [val.stdev for val in r.getReadable(1)]
finally:
    rph.write(rph0)
    bph.close()
    rph.close()
    q.close()

#Setting the return value
index_max = charge.index(max(charge))
bph_ref = beamphase[index_max] - 80
rph_ref = rfphase[index_max] - 80

print bph_ref
print rph_ref

set_return(bph_ref, rph_ref)