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

if get_exec_pars().source == CommandSource.ui:
    start   = 35.0
    stop    = 55.0
    step    = 1.0
    nb      = 3
    lat     = 0.3
    disp    = -0.32
    energy0 = 7.1
else:
    start   = args[0]
    stop    = args[1]
    step    = args[2]
    nb      = int(args[3])
    lat     = args[4]
    disp    = args[5]
    energy0 = args[6]

phase = ControlledVariable("Phase", "SINEG01-RSYS:SET-BEAM-PHASE", "SINEG01-RSYS:GET-BEAM-PHASE")
#phase = ControlledVariable("Phase", "STEST01-RSYS:SET-BEAM-PHASE", "STEST01-RSYS:GET-BEAM-PHASE")
phase.config.minValue   =-180.0
phase.config.maxValue   = 360.0
phase.config.precision  = 3
phase.config.resolution = 1.0
phase.config.save()
phase.initialize()

phase0 = phase.read()

kill_camtool()
check_camtool()
camtool.start("SINBD01-DSCR010")
wait_camtool_message()

x   = camtool.stream.getChild("gr_x_fit_mean")
dx  = camtool.stream.getChild("gr_x_fit_standard_deviation")

try:
    xb  = create_averager(x,  nb, -1)    # -1 event based, waits for the next value
    dxb = create_averager(dx, nb, -1)
    dxb.monitored=True     # not blocking, will return last nb values
    r = lscan(phase, [xb, dxb], start, stop, step , latency=lat)
    rf_phase = r.getPositions(0)
    E   = [energy0 * (1 + val.mean / 1e6 / disp) for val in r.getReadable(0)]
    dE  = [energy0 * (    val.mean / 1e6 / disp) for val in r.getReadable(1)]
finally:
    phase.write(phase0)
    phase.close()
    camtool.stop()   # stops camtool but does not close it camtool is a global object

p = plot(None, title="Output")[0]
p.clear()
p.addSeries(LinePlotSeries("Energy"))
p.addSeries(LinePlotSeries("Energy spread"))
p.getSeries(0).setData(to_array(rf_phase, 'd'), E)    
p.getSeries(1).setData(to_array(rf_phase, 'd'), dE)    
p.setLegendVisible(True)

set_return([E,dE])