import ch.psi.pshell.epics.Positioner as Positioner

USE_SCREEN_PANEL = False

start   = 85.0
stop    = 90.0
step    = 0.5
nb      = 3
lat     = 0.1
disp    = -0.387
p0      = 7.1
plt     = plot(None, title="Output")[0]
thr     = 500

A = p0 / disp / 1e6
B = p0

#Plot setup
plt.clear()
plt.removeMarker(None)
plt.setStyle(plt.Style.ErrorY)
plt.addSeries(LinePlotErrorSeries("Momentum", Color.red))
plt.addSeries(LinePlotErrorSeries("Momentum Spread", Color.yellow, 2))
plt.getAxis(plt.AxisId.X).setLabel("Gun Beam Phase (deg)")
plt.getAxis(plt.AxisId.Y).setLabel("Momentum (MeV/c)")
plt.getAxis(plt.AxisId.Y2).setLabel("Momentum Spread (MeV/c)")
plt.setLegendVisible(True)


phase = DummyPositioner("Gun Phase")
camera_name  = "simulation"

phase.config.minValue   = -90.0
phase.config.maxValue   = 360.0
phase.config.precision  = 4
phase.config.deadband = 0.5
phase.config.rotation   = True
phase.config.save()
phase.initialize()
phase0 = phase.read() % 360

#Camera setup
cam_server.start(camera_name+"_sp", USE_SCREEN_PANEL)    

wait_cam_server_message()
x   = cam_server.stream.getChild("x_center_of_mass")
dx  = cam_server.stream.getChild("x_rms")
cam_server.setThreshold(thr)
cam_server.setBackgroundSubtraction(False)

#Creating averagers
x_averager  = create_averager(x,  nb, -1)    # -1 event based, waits for the next value
dx_averager = create_averager(dx, nb, -1)
dx_averager.monitored = True                 # not blocking, will return last nb values

#Record callback: uptate of output plot
def after_sample(record, scan):
    global A, B, plt
    x_pos_mean,   x_pos_stdev   = record[0].mean, record[0].stdev
    x_width_mean, x_width_stdev = record[1].mean, record[1].stdev
    p_mean,  p_stdev  = A * x_pos_mean + B,    abs(A) * x_pos_stdev
    dp_mean, dp_stdev = abs(A) * x_width_mean, abs(A) * x_width_stdev
    phase = ((record.positions[0] + 90) % 360) - 90 if start < 0 else record.positions[0]
    plt.getSeries(0).appendData(phase, p_mean, p_stdev)
    plt.getSeries(1).appendData(phase, dp_mean, dp_stdev)

#The scan loop
try:
    phase.write(start)
    time.sleep(1.0)
    r = lscan(phase, [x_averager, dx_averager], start, stop, step , latency=lat, after_read = after_sample)
finally:
    phase.write(phase0)
    phase.close()
    cam_server.stop()   # stops cam_server but does not close it cam_server is a global object

ph = r.getPositions(0)
if start < 0:
    ph = [((val + 90) % 360) -90 for val in ph ]
p  = [A * val.mean + B  for val in r.getReadable(0)]
dp = [abs(A) * val.mean for val in r.getReadable(1)]