import ch.psi.pshell.epics.ControlledVariable as ControlledVariable
#This scan is dedicated to scan either one parameter (e.g. a quadrupole) and take a certain amount of images at every location
CAMERA = "SARCL01-DSCR170"  #Specify the camera we want to use
#CAMERA = "SINDI02-DLAC055"   
#CAMERA = "SINBC02-DSRM310"
#QUADRUPOLE = "S20SY01-MQUA080" # quadrupole for the scan with S10DI01-DSCR020: S10CB02-MQUA230
QUADRUPOLE = "S10BC01-MQUA100"
CHARGE_BPM = "SARCL01-DBPM120:Q1" #reading out the charge also
CHARGE_ICT = "S10DI01-DICT025:B1_CHARGE-OP" #reading out the charge also
#RANGE = (6.226,7.226) #range for the quadrupole
RANGE = None  #if range None, just NUM_IMAGES are recorded
STEPS = 20 # number of steps for the quadrupole scan range (scan will take STEPS+1 measurements)
SETTLING_TIME = 0.1
NUM_IMAGES = 50 #number of images for every step

print cam_server.cameras
cam_server.start(CAMERA + "_sp1", True)
wait_cam_server_message()
print cam_server.value.identifiers
m=cam_server.getDataMatrix()
x = cam_server.stream.getChild("x_fit_mean")
y = cam_server.stream.getChild("y_fit_mean")

"""
ax = create_averager(x, 5, -1, "X Fit")
ay = create_averager(y, 5, -1, "Y Fit")
ay.monitored = True
"""
set_device_alias(m,"image")

#Create quadrupole device
quad = ControlledVariable(QUADRUPOLE, QUADRUPOLE + ":I-SET", QUADRUPOLE + ":I-READ")
quad.config.minValue   =-10.0
quad.config.maxValue   = 10.0
quad.config.precision  = 3
quad.config.resolution = 0.007
quad.config.save()
quad.initialize()

bpm = Channel(CHARGE_BPM, 'd', alias = "Charge BPM")
ict = Channel(CHARGE_ICT, 'd', alias = "Charge ICT")

#Metadata

try:
    set_attribute("/", "Camera", CAMERA)
    set_attribute("/", "Quadrupole", QUADRUPOLE)
    set_attribute("/", "Scan Parameters", RANGE + (STEPS,))
    set_attribute("/", "Screen Position", caget(CAMERA + ":GET_SCREEN1_POS", 's'))  
    set_attribute("/", "Filter Position", caget(CAMERA + ":GET_FILTER", 's'))  
except:
    pass


#cam_server.captureBackground(10)
#cam_server.setBackgroundImage(img)
#cam_server.setBackgroundSubtraction(False)
former_value = quad.read()    

try:    
    laser_off()
    #save_dataset("/Background", m.read())
    #mscan (cam_server.stream, m, 10)        #Saves 10 next frames     -> For machine at 10 -> 100Hz 
    tscan(m, 10, 1.0)                     # 10 samples every 0.2 s  -> For machine at 1Hz 
    laser_on()    
    
    #tscan((m, x, y), 10, 1.0) # 10 samples every 1.0 s
    #mscan (cam_server.stream,(m, x, y), -1, 5.0) #Saves all frames received in 5s
    #mscan (cam_server.stream,(m, x, y), 50) # Saves firs 50 frames    
    #lscan(quad, (m, x, y), RANGE[0], RANGE[1], STEPS, latency=SETTLING_TIME)
    
    readables = cam_server.stream.getReadables().clone() + [bpm, ict]
    readables.remove(cam_server.stream.getChild("image")) 
    readables.insert(0,m)
    
    if RANGE is None:
        mscan(cam_server.stream, readables, NUM_IMAGES)
    else:
        if NUM_IMAGES<=1:
            def wait_next():
                cam_server.stream.waitCacheChange(10000) 
            lscan(quad, readables, RANGE[0], RANGE[1], STEPS, latency=SETTLING_TIME, before_read = wait_next)    
        else:
            class ImageSampler(Writable):
                def write(self, value):
                    cam_server.stream.waitCacheChange(10000)   
            image_sampler = ImageSampler()    
            ascan([quad, image_sampler], readables, [RANGE[0], 0], [RANGE[1], NUM_IMAGES-1], [STEPS, NUM_IMAGES-1], latency=SETTLING_TIME)    

            #vector=[]
            #for pos in frange(RANGE[0], RANGE[1], STEP_SIZE):
            #    for img in range(NUM_IMAGES):
            #        vector.append(pos)
            #vscan(quad, readables, vector)
            
finally:
    quad.write(former_value)    
    quad.close()
    cam_server.stop()