#Interface to CameraTool  application

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

class CamToolImage(ReadableMatrix):
    def __init__(self, camtool):
        self.camtool = camtool
        shape = camtool.shape.read()
        self._width = shape[1] #len(camtool.profile_x.read())  
        self._height = shape[0] #len(camtool.(profile_y.read())  
        
    def read(self):                
        raw = self.camtool.data.read()
        #return Matrix(toDouble(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
        

class CamTool(DeviceBase):
    def __init__(self, name, prefix = "cam:", latch = False, camera = "SINEG01-DSCR190", egu = True, gauss = True):
        DeviceBase.__init__(self, name)
        run_channel = prefix + "camera.run"         

        caget(run_channel)
        #try:
        #    caget(run_channel)
        #except:
        #    cmd = "camtool --casprefix " + prefix + " --run -1 " + camera #--nogui   
        #    print cmd
        #    fork( (exec_cmd,(cmd,)),)                        
        #    self.waitForChannel(run_channel, 20)
        #    time.sleep(5.0)
        self.prefix = prefix
        self.latch = latch
        self.egu = egu
        self.gauss=gauss
        self.mode = "latch" if latch else "pipeline"
        self.data_prefix = prefix + self.mode + "."
        self.cam_run = create_channel_device(prefix + "camera.run", type='i', size=None, deviceName=name + " run") # Channel(prefix + "camera.run", alias = name + " run")
        self.cam_latch = create_channel_device(prefix + "latch.capture", type='l', size=None, deviceName=name + " latch") # Channel(prefix + "latch.capture", 'l', alias = name + " latch")        
        

        if self.latch:
            self.start()
            time.sleep(3.0)
            self.cam_latch.write(1)                     
            self.waitForChannel(self.data_prefix + "timestamp",20)
        else:
            self.cam_run.write(1) 
            time.sleep(5.0)       
            self.waitForChannel(self.data_prefix + "timestamp",20)
            self.waitForChannel(self.data_prefix + "image.shape",20)
            self.stop()
        self.timestamp = create_channel_device(self.data_prefix + "timestamp", type='d', size=None, deviceName=name + " timestamp")  # Channel(self.data_prefix + "timestamp",type = 'd', alias = name + " timestamp") #[D1, D2, ...]        
        self.timestamp.setMonitored(True)#self.timestamp.set_monitored(True)
        if egu:
            if gauss:
                self.com_x = create_channel_device(self.data_prefix + "x_stats.g_center_egu", type='d', size=None, deviceName=name + " gc_egu x")  
                self.com_y = create_channel_device(self.data_prefix + "y_stats.g_center_egu", type='d', size=None, deviceName=name + " gc_egu y")  
                caput(self.data_prefix + "x_stats.gauss_en", 1)
                caput(self.data_prefix + "y_stats.gauss_en", 1)
            else:
                self.com_x = create_channel_device(self.data_prefix + "x_stats.com_egu", type='d', size=None, deviceName=name + " com_egu x")  #Channel(self.data_prefix + "x_stats.com_egu", alias = name + " com_egu x")
                self.com_y = create_channel_device(self.data_prefix + "y_stats.com_egu", type='d', size=None, deviceName=name + " com_egu y")  #Channel(self.data_prefix + "y_stats.com_egu", alias = name + " com_egu y")
        else:
            self.com_x = create_channel_device(self.data_prefix + "x_stats.com", type='d', size=None, deviceName=name + " com x")  #Channel(self.data_prefix + "x_stats.com", alias = name + " com x")
            self.com_y = create_channel_device(self.data_prefix + "y_stats.com", type='d', size=None, deviceName=name + " com x")  #Channel(self.data_prefix + "y_stats.com", alias = name + " com y")
        self.profile_x = create_channel_device(self.data_prefix + "profile.x", type='d', size=None, deviceName=name + " profile x")  #Channel(self.data_prefix + "profile.x", alias = name + " profile x")
        self.profile_y = create_channel_device(self.data_prefix + "profile.y", type='d', size=None, deviceName=name + " profile y")  #Channel(self.data_prefix + "profile.y", alias = name + " profile y")
        self.data = Channel(self.data_prefix + "image", alias = name + " data")
        self.shape = Channel(self.data_prefix + "image.shape", alias = name + " shape") #[D1, D2, ...]
        self.bg_en = create_channel_device(prefix + "pipeline.bg.enabled", type='l', size=None, deviceName=name + " bg enabled")  #self.bg_en = Channel(prefix + "pipeline.bg.enabled", type = 'l', alias = name + " bg enabled")
        self.bg_capture = create_channel_device(prefix + "pipeline.bg.capture", type='l', size=None, deviceName=name + " bg capture")  #self.bg_capture = Channel(prefix + "pipeline.bg.capture", type = 'l', alias = name + " bg capture")
        self.bg_capture_remain = create_channel_device(prefix + "pipeline.bg.capture_remain", type='l', size=None, deviceName=name + " capture remain")  #self.bg_capture_remain = Channel(prefix + "pipeline.bg.capture_remain", alias = name + " capture remain")                
        self.num_images = 1
        self.grab_timeout = 5.0
        self.image = CamToolImage(self)
        set_device_alias(self.image, name + " image")
        self.com_x_samples, self.com_y_samples = [], []
        class CamToolComX(Readable):
            def read(self):     
                return mean(self.camtool.com_x_samples)
        self.com_x_mean = CamToolComX(); self.com_x_mean.camtool = self
        class CamToolComY(Readable):
            def read(self):     
                return mean(self.camtool.com_y_samples)
        self.com_y_mean = CamToolComY(); self.com_y_mean.camtool = self
        class CamToolComXVar(Readable):
            def read(self):     
                return stdev(self.camtool.com_x_samples)
        self.com_x_stdev = CamToolComXVar(); self.com_x_stdev.camtool = self
        class CamToolComXVar(Readable):
            def read(self):     
                return stdev(self.camtool.com_y_samples)
        self.com_y_stdev = CamToolComXVar(); self.com_y_stdev.camtool = self
        set_device_alias(self.com_x_mean, name + " com x mean")
        set_device_alias(self.com_y_mean, name + " com y mean")
        set_device_alias(self.com_x_stdev, name + " com x stdev")
        set_device_alias(self.com_y_stdev, name + " com y stdev")        

    def waitForChannel(self, channel, timeout):
        print "Waiting for:", channel
        start = time.time()
        while(True):
            try:
                caget(channel)
                print "Found channel:", channel
                break
            #except Exception as e:
            except:                
                if (time.time() - start) > timeout:
                    raise Exception("Timeout waiting for channel: " + channel)                
            time.sleep(0.5)
        

    def doInitialize(self):        
        if self.latch:
            self.start()            
        else:
            self.stop()
    
    def start(self):
        #print "Start"
        self.cam_run.write(-1)

    def stop(self):
        #print "Stop"
        self.cam_run.write(0)    
    
    def capture(self):
        retries = 3
        while(True):
            try:
                timestamp = self.timestamp.read()
                #print "Current timestamp: ", timestamp
                if not self.latch:
                    self.cam_run.write(1)                                
                else:
                    self.cam_latch.write(1)
                start = time.time()        
                while(True):
                    val = self.timestamp.read()
                    if timestamp != val:
                        #print "New timestamp: ", val
                        self.setCache(val)
                        return                    
                    if (time.time()-start) > self.grab_timeout:
                        raise Exception("Frame timeout")
                    time.sleep(0.005)    
            except Exception, e:
                retries = retries -1
                print "----------------    Exception     ------------------------" 
                print e
                if retries<=0:
                    print "Failure "
                    raise e
                else:
                    print "Retrying ",  retries
    
    def doUpdate(self):
        self.com_x_samples, self.com_y_samples = [], []
        for i in range(self.num_images):
            self.capture()
            self.com_x_samples.append(self.com_x.read())
            self.com_y_samples.append(self.com_y.read())

    def setNumberOfImages(self, value):
        self.num_images = value

    def enableBackground(self, value):
        self.bg_en.write(1 if value else 0)
        
    def captureBackground(self, images):        
        self.start()
        self.bg_capture.write(images)
        sleep(0.2)
        while(True):
            finished = True
            for retry in range(10):
                if self.bg_capture_remain.read() > 0:
                    finished = False
                    break
                sleep(0.05)
            if finished:
                break
            time.sleep(0.1)                
        #self.stop()        
        #self.bg_capture.write(images)
        #while( self.bg_capture_remain.read() > 0):
        #    self.capture()
        self.doInitialize()
    
    def doClose(self):        
        self.cam_run.close()
        self.cam_latch.close()
        self.timestamp.close()        
        self.com_x.close()
        self.com_y.close()
        self.profile_x.close()
        self.profile_y.close()
        self.data.close()
        self.shape.close()
        #self.bg_image.close()        
        self.bg_en.close()
        self.bg_capture.close()
        self.bg_capture_remain.close()
        #self.image.close()              

"""
try:
    if get_device("camtool") is None:
        add_device(CamTool("camtool", prefix = "SINEG01-DSCR190:",  latch = True, camera = "SINEG01-DSCR190"), True)
    camtool.enableBackground(True)
    for i in range (1000):        
        camtool.capture()        
        print i, camtool.take(), camtool.com_x.read(), camtool.com_y.read() 

finally:
    print ""        

"""

"""
if __name__ == "__builtin__":
    #image_stats.enableBackground(False)    
    image_stats.enableBackground(False)
    #image_stats.captureBackground(5)
    for i in range (1000):        
        image_stats.capture()        
        print i, image_stats.take(), image_stats.com_x.read(), image_stats.com_y.read() 
    #from camtool import CamTool
    #camera_tool = CamTool("camtool", latch = True)
    #add_device(camera_tool, True)
    #camera_tool.enableBackground(True)          
    #camera_tool.capture()     
    #print camera_tool.take(), camera_tool.com_x.read(), camera_tool.com_y.read()    

    
"""