#import ch.psi.pshell.device.DeviceBase as DeviceBase
from startup import *
import ch.psi.utils.Convert.toBidimensional as mono_to_bidi
import org.apache.commons.math3.linear.Array2DRowRealMatrix as Matrix


class CamToolImage(ReadableMatrix):
    def __init__(self, camtool):
        self.camtool = camtool
        shape = camtool.cam_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.cam_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
        


class CamTool(DeviceBase):
    def __init__(self, name, prefix = "cam:", latch = False):
        DeviceBase.__init__(self, name)
        self.prefix = prefix
        self.latch = latch
        self.mode = "latch" if latch else "pipeline"
        self.data_prefix = prefix + self.mode + "."
        self.cam_run = Channel(prefix + "camera.run", alias = "cam_run")
        self.cam_latch = Channel(prefix + "latch.capture", alias = "cam_latch")
        self.timestamp = Channel(self.data_prefix + "timestamp", alias = "timestamp") #[D1, D2, ...]
        
        self.cam_x = Channel(self.data_prefix + "x_stats.com", alias = "cam_x")
        self.cam_y = Channel(self.data_prefix + "y_stats.com", alias = "cam_y")
        self.profile_x = Channel(self.data_prefix + "profile.x", alias = "profile_x")
        self.profile_y = Channel(self.data_prefix + "profile.y", alias = "profile_y")
        self.cam_data = Channel(self.data_prefix + "image", alias = "cam_data")
        self.cam_shape = Channel(self.data_prefix + "image.shape", alias = "cam_image_shape") #[D1, D2, ...]
        self.cam_bg_image = Channel(self.data_prefix + "bg_image", alias = "cam_bg_image") #[D1, D2, ...]                
        
        self.cam_bg_en = Channel(self.prefix + "bg.enabled", type = 'l', alias = "cam_bg_en")
        self.cam_bg_capture = Channel(self.prefix + "bg.capture", type = 'l', alias = "cam_bg_capture")
        self.cam_bg_capture_remain = Channel(self.prefix + "bg.capture_remain", alias = "cam_bg_capture_remain")        

        self.image = CamToolImage(self)
        

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

    def stop(self):
        self.cam_run.write(0)    
    
    def capture(self):
        timestamp = self.timestamp.read()
        if not self.latch:
            self.cam_run.write(1)
        while(True):
            val = self.timestamp.read()
            if timestamp != val:
                self.setCache(val)
                break
            if self.latch:
                self.cam_latch.write(1)
            time.sleep(0.001)                        

    def doUpdate(self):
        self.capture()

    def enableBackground(self, value):
        self.cam_bg_en.write(1 if value else 0)
        
    def captureBackground(self, images):        
        self.start()
        self.cam_bg_capture.write(images)
        sleep(0.1)
        while( self.cam_bg_capture_remain.read() > 0):
            time.sleep(0.001)                
        #self.stop()        
        #self.cam_bg_capture.write(images)
        #while( self.cam_bg_capture_remain.read() > 0):
        #    self.capture()
        self.doInitialize()
    
    def doClose(self):        
        self.cam_run.close()
        self.cam_latch.close()
        self.timestamp.close()        
        self.cam_x.close()
        self.cam_y.close()
        self.profile_x.close()
        self.profile_y.close()
        self.cam_data.close()
        self.cam_shape.close()
        self.cam_bg_image.close()        
        self.cam_bg_en.close()
        self.cam_bg_capture.close()
        self.cam_bg_capture_remain.close()
        
        
    

if __name__ == "__builtin__":
    camera_tool = CamTool("camtool")

    camera_tool.enableBackground(False)
    camera_tool.enableBackground(True)
    camera_tool.captureBackground(5)
    for i in range (100):
        camera_tool.capture()
        print camera_tool.take()
    print camera_tool.cam_x.read(), camera_tool.cam_y.read() 
    
    add_device(camera_tool, True)