# Tool to align the solenoid on the gun.
# S. Bettoni, A. Gobbo, D. Voulot
# 06/06/2016


# Procedure:
    # switch off all the magnets between the gun solenoid and the screen or BPM used for the measurement
    # change the current of the gun soleoid
    # look at the centroid position (BPM or screen) downstream of the gun.



import org.apache.commons.math3.linear.Array2DRowRealMatrix as Matrix
import ch.psi.utils.Convert.toBidimensional as mono_to_bidi
    

I_set = Channel("SINEG01-MSOL130:I-SET", alias = "I_set"); 
I_get = Channel("SINEG01-MSOL130:I-READ", alias = "I_get")
# cam_x, cam_y = ...       # some camtool command / channel?
bpm_x = Channel("SINEG01-DBPM340:X1")
bpm_y = Channel("SINEG01-DBPM340:Y1")

cam_x = Channel("SINEG01-DSCR190:profile.X_stats.com", alias = "cam_x")
cam_y = Channel("SINEG01-DSCR190:profile.Y_stats.com", alias = "cam_y")

profile_x = Channel("SINEG01-DSCR190:profile.X", alias = "profile_x")
profile_y = Channel("SINEG01-DSCR190:profile.Y", alias = "profile_y")

cam_raw_data = Channel("SINEG01-DSCR190:data")


class CameraImage(ReadableMatrix):
    def read(self):                
        raw = cam_raw_data.read()
        return Matrix(mono_to_bidi(raw, self.getHeight(), self.getWidth())).transpose().getData()    #data is transposed
        #return mono_to_bidi(raw, self.getWidth(), self.getHeight())

    def getWidth(self):
        return self._width

    def getHeight(self):
        return self._height
        
cam_img = CameraImage()     
cam_img._width = len(profile_x.read())  
cam_img._height = len(profile_y.read())  


def ccr(mag):
    n = 1
    while n > 0:
        sleep(0.5)
        n = caget(mag + ":I-COMP")
def laser_on():
    caput("SIN-TIMAST-TMA:Beam-Las-Delay-Sel", 0)
    
def laser_off():
    caput("SIN-TIMAST-TMA:Beam-Las-Delay-Sel", 1)


def run_pipeline():
    caput("camtool...:start")

    while !caget("camtool...:done"):
        sleep(0.1) 

    caget("camtool....:data")
    #caget("camtool....:X_stats.com")
    #caget("camtool....:X_stats.com")
    
    
I1 = 20.0
I2 = 150.0
dI = 1.0
settling_time = 1.0

# Switch off magnets
mag = [ "SINEG01-MCRX120","SINEG01-MCRY120",
        "SINEG01-MQUA140",
        "SINEG01-MQUA150",
        "SINEG01-MCRX160","SINEG01-MCRY160",
        "SINEG01-MCRX180","SINEG01-MCRY180",
        "SINEG01-MCRX200","SINEG01-MCRY200",
        "SINEG01-MCRX220","SINEG01-MCRY220",
        "SINEG01-MQUA310",
        "SINEG01-MQUA320" ]
for m in mag:
    caput(m + ":I-SET", 0.0)
for m in mag:
    ccr(m)

laser_on()
# Scan using the screen
#r = lscan(I_set, [I_read, cam_x, cam_y], I1, I2, dI, 1.0, passes = 2, zigzag = True)
# Scan using the BPM
try:
    #r = lscan(I_set, [I_get, cam_x, cam_y], I1, I2, dI, 1.0, before_read = ccr)
    r = lscan(I_set, [I_get, cam_x, cam_y, cam_img], I1, I2, dI, settling_time)
finally:
    laser_off()

# take the result of the scan and do the plots
p = plot(r.getReadable(2), xdata=r.getReadable(1), title = "Centroid excursion")[0]
#add_convex_hull_plot ("Centroid excursion", r.getReadable(1),r.getReadable(2), "Name")

# save the entry in the logbook
msg = str(r)
msg = msg + "\nFile: " + get_context().path
msg = msg + "\n\n" + r.print()
elog("Gun solenoid current scan", msg , get_plot_snapshots())